diff --git a/drivers/media/platform/Kconfig b/drivers/media/platform/Kconfig
new file mode 100644
index 0000000..ccbc974
--- /dev/null
+++ b/drivers/media/platform/Kconfig
@@ -0,0 +1,305 @@
+#
+# Platform drivers
+#	Most drivers here are currently for webcam support
+
+menuconfig V4L_PLATFORM_DRIVERS
+	bool "V4L platform devices"
+	depends on MEDIA_CAMERA_SUPPORT
+	default n
+	---help---
+	  Say Y here to enable support for platform-specific V4L drivers.
+
+if V4L_PLATFORM_DRIVERS
+
+source "drivers/media/platform/marvell-ccic/Kconfig"
+
+config VIDEO_VIA_CAMERA
+	tristate "VIAFB camera controller support"
+	depends on FB_VIA
+	select VIDEOBUF_DMA_SG
+	select VIDEO_OV7670
+	help
+	   Driver support for the integrated camera controller in VIA
+	   Chrome9 chipsets.  Currently only tested on OLPC xo-1.5 systems
+	   with ov7670 sensors.
+
+#
+# Platform multimedia device configuration
+#
+
+source "drivers/media/platform/davinci/Kconfig"
+
+source "drivers/media/platform/omap/Kconfig"
+
+source "drivers/media/platform/blackfin/Kconfig"
+
+config VIDEO_SH_VOU
+	tristate "SuperH VOU video output driver"
+	depends on MEDIA_CAMERA_SUPPORT
+	depends on VIDEO_DEV && I2C && HAS_DMA
+	depends on ARCH_SHMOBILE || COMPILE_TEST
+	select VIDEOBUF_DMA_CONTIG
+	help
+	  Support for the Video Output Unit (VOU) on SuperH SoCs.
+
+config VIDEO_VIU
+	tristate "Freescale VIU Video Driver"
+	depends on VIDEO_V4L2 && PPC_MPC512x
+	select VIDEOBUF_DMA_CONTIG
+	default y
+	---help---
+	  Support for Freescale VIU video driver. This device captures
+	  video data, or overlays video on DIU frame buffer.
+
+	  Say Y here if you want to enable VIU device on MPC5121e Rev2+.
+	  In doubt, say N.
+
+config VIDEO_TIMBERDALE
+	tristate "Support for timberdale Video In/LogiWIN"
+	depends on VIDEO_V4L2 && I2C && VIDEO_V4L2_SUBDEV_API && HAS_DMA
+	depends on (MFD_TIMBERDALE && TIMB_DMA) || COMPILE_TEST
+	select VIDEO_ADV7180
+	select VIDEOBUF_DMA_CONTIG
+	---help---
+	  Add support for the Video In peripherial of the timberdale FPGA.
+
+config VIDEO_M32R_AR
+	tristate "AR devices"
+	depends on VIDEO_V4L2
+	depends on M32R || COMPILE_TEST
+	---help---
+	  This is a video4linux driver for the Renesas AR (Artificial Retina)
+	  camera module.
+
+config VIDEO_M32R_AR_M64278
+	tristate "AR device with color module M64278(VGA)"
+	depends on PLAT_M32700UT
+	select VIDEO_M32R_AR
+	---help---
+	  This is a video4linux driver for the Renesas AR (Artificial
+	  Retina) with M64278E-800 camera module.
+	  This module supports VGA(640x480 pixels) resolutions.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called arv.
+
+config VIDEO_OMAP3
+	tristate "OMAP 3 Camera support"
+	depends on VIDEO_V4L2 && I2C && VIDEO_V4L2_SUBDEV_API && ARCH_OMAP3
+	depends on HAS_DMA && OF
+	depends on OMAP_IOMMU
+	select ARM_DMA_USE_IOMMU
+	select VIDEOBUF2_DMA_CONTIG
+	select MFD_SYSCON
+	---help---
+	  Driver for an OMAP 3 camera controller.
+
+config VIDEO_OMAP3_DEBUG
+	bool "OMAP 3 Camera debug messages"
+	depends on VIDEO_OMAP3
+	---help---
+	  Enable debug messages on OMAP 3 camera controller driver.
+
+config VIDEO_S3C_CAMIF
+	tristate "Samsung S3C24XX/S3C64XX SoC Camera Interface driver"
+	depends on VIDEO_V4L2 && I2C && VIDEO_V4L2_SUBDEV_API
+	depends on PM
+	depends on ARCH_S3C64XX || PLAT_S3C24XX || COMPILE_TEST
+	depends on HAS_DMA
+	select VIDEOBUF2_DMA_CONTIG
+	---help---
+	  This is a v4l2 driver for s3c24xx and s3c64xx SoC series camera
+	  host interface (CAMIF).
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called s3c-camif.
+
+source "drivers/media/platform/soc_camera/Kconfig"
+source "drivers/media/platform/exynos4-is/Kconfig"
+source "drivers/media/platform/s5p-tv/Kconfig"
+source "drivers/media/platform/am437x/Kconfig"
+source "drivers/media/platform/xilinx/Kconfig"
+
+endif # V4L_PLATFORM_DRIVERS
+
+menuconfig V4L_MEM2MEM_DRIVERS
+	bool "Memory-to-memory multimedia devices"
+	depends on VIDEO_V4L2
+	depends on MEDIA_CAMERA_SUPPORT
+	default n
+	---help---
+	  Say Y here to enable selecting drivers for V4L devices that
+	  use system memory for both source and destination buffers, as opposed
+	  to capture and output drivers, which use memory buffers for just
+	  one of those.
+
+if V4L_MEM2MEM_DRIVERS
+
+config VIDEO_CODA
+	tristate "Chips&Media Coda multi-standard codec IP"
+	depends on VIDEO_DEV && VIDEO_V4L2 && ARCH_MXC
+	depends on HAS_DMA
+	select SRAM
+	select VIDEOBUF2_DMA_CONTIG
+	select VIDEOBUF2_VMALLOC
+	select V4L2_MEM2MEM_DEV
+	select GENERIC_ALLOCATOR
+	---help---
+	   Coda is a range of video codec IPs that supports
+	   H.264, MPEG-4, and other video formats.
+
+config VIDEO_MEM2MEM_DEINTERLACE
+	tristate "Deinterlace support"
+	depends on VIDEO_DEV && VIDEO_V4L2 && DMA_ENGINE
+	depends on HAS_DMA
+	select VIDEOBUF2_DMA_CONTIG
+	select V4L2_MEM2MEM_DEV
+	help
+	    Generic deinterlacing V4L2 driver.
+
+config VIDEO_SAMSUNG_S5P_G2D
+	tristate "Samsung S5P and EXYNOS4 G2D 2d graphics accelerator driver"
+	depends on VIDEO_DEV && VIDEO_V4L2
+	depends on ARCH_S5PV210 || ARCH_EXYNOS || COMPILE_TEST
+	depends on HAS_DMA
+	select VIDEOBUF2_DMA_CONTIG
+	select V4L2_MEM2MEM_DEV
+	default n
+	---help---
+	  This is a v4l2 driver for Samsung S5P and EXYNOS4 G2D
+	  2d graphics accelerator.
+
+config VIDEO_SAMSUNG_S5P_JPEG
+	tristate "Samsung S5P/Exynos3250/Exynos4 JPEG codec driver"
+	depends on VIDEO_DEV && VIDEO_V4L2
+	depends on ARCH_S5PV210 || ARCH_EXYNOS || COMPILE_TEST
+	depends on HAS_DMA
+	select VIDEOBUF2_DMA_CONTIG
+	select V4L2_MEM2MEM_DEV
+	---help---
+	  This is a v4l2 driver for Samsung S5P, EXYNOS3250
+	  and EXYNOS4 JPEG codec
+
+config VIDEO_SAMSUNG_S5P_MFC
+	tristate "Samsung S5P MFC Video Codec"
+	depends on VIDEO_DEV && VIDEO_V4L2
+	depends on ARCH_S5PV210 || ARCH_EXYNOS || COMPILE_TEST
+	depends on HAS_DMA
+	select VIDEOBUF2_DMA_CONTIG
+	default n
+	help
+	    MFC 5.1 and 6.x driver for V4L2
+
+config VIDEO_MX2_EMMAPRP
+	tristate "MX2 eMMa-PrP support"
+	depends on VIDEO_DEV && VIDEO_V4L2
+	depends on SOC_IMX27 || COMPILE_TEST
+	depends on HAS_DMA
+	select VIDEOBUF2_DMA_CONTIG
+	select V4L2_MEM2MEM_DEV
+	help
+	    MX2X chips have a PrP that can be used to process buffers from
+	    memory to memory. Operations include resizing and format
+	    conversion.
+
+config VIDEO_SAMSUNG_EXYNOS_GSC
+	tristate "Samsung Exynos G-Scaler driver"
+	depends on VIDEO_DEV && VIDEO_V4L2
+	depends on ARCH_EXYNOS5 || COMPILE_TEST
+	depends on HAS_DMA
+	select VIDEOBUF2_DMA_CONTIG
+	select V4L2_MEM2MEM_DEV
+	help
+	  This is a v4l2 driver for Samsung EXYNOS5 SoC G-Scaler.
+
+config VIDEO_STI_BDISP
+	tristate "STMicroelectronics BDISP 2D blitter driver"
+	depends on VIDEO_DEV && VIDEO_V4L2
+	depends on ARCH_STI || COMPILE_TEST
+	depends on HAVE_DMA_ATTRS
+	select VIDEOBUF2_DMA_CONTIG
+	select V4L2_MEM2MEM_DEV
+	help
+	  This v4l2 mem2mem driver is a 2D blitter for STMicroelectronics SoC.
+
+config VIDEO_SH_VEU
+	tristate "SuperH VEU mem2mem video processing driver"
+	depends on VIDEO_DEV && VIDEO_V4L2 && HAS_DMA
+	select VIDEOBUF2_DMA_CONTIG
+	select V4L2_MEM2MEM_DEV
+	help
+	    Support for the Video Engine Unit (VEU) on SuperH and
+	    SH-Mobile SoCs.
+
+config VIDEO_RENESAS_JPU
+	tristate "Renesas JPEG Processing Unit"
+	depends on VIDEO_DEV && VIDEO_V4L2 && HAS_DMA
+	depends on ARCH_SHMOBILE || COMPILE_TEST
+	select VIDEOBUF2_DMA_CONTIG
+	select V4L2_MEM2MEM_DEV
+	---help---
+	  This is a V4L2 driver for the Renesas JPEG Processing Unit.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called rcar_jpu.
+
+config VIDEO_RENESAS_VSP1
+	tristate "Renesas VSP1 Video Processing Engine"
+	depends on VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API && HAS_DMA
+	depends on (ARCH_SHMOBILE && OF) || COMPILE_TEST
+	select VIDEOBUF2_DMA_CONTIG
+	---help---
+	  This is a V4L2 driver for the Renesas VSP1 video processing engine.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called vsp1.
+
+config VIDEO_TI_VPE
+	tristate "TI VPE (Video Processing Engine) driver"
+	depends on VIDEO_DEV && VIDEO_V4L2
+	depends on SOC_DRA7XX || COMPILE_TEST
+	depends on HAS_DMA
+	select VIDEOBUF2_DMA_CONTIG
+	select V4L2_MEM2MEM_DEV
+	default n
+	---help---
+	  Support for the TI VPE(Video Processing Engine) block
+	  found on DRA7XX SoC.
+
+config VIDEO_TI_VPE_DEBUG
+	bool "VPE debug messages"
+	depends on VIDEO_TI_VPE
+	---help---
+	  Enable debug messages on VPE driver.
+
+endif # V4L_MEM2MEM_DRIVERS
+
+menuconfig V4L_TEST_DRIVERS
+	bool "Media test drivers"
+	depends on MEDIA_CAMERA_SUPPORT
+
+if V4L_TEST_DRIVERS
+
+source "drivers/media/platform/vivid/Kconfig"
+
+config VIDEO_VIM2M
+	tristate "Virtual Memory-to-Memory Driver"
+	depends on VIDEO_DEV && VIDEO_V4L2
+	select VIDEOBUF2_VMALLOC
+	select V4L2_MEM2MEM_DEV
+	default n
+	---help---
+	  This is a virtual test device for the memory-to-memory driver
+	  framework.
+endif #V4L_TEST_DRIVERS
+
+menuconfig DVB_PLATFORM_DRIVERS
+	bool "DVB platform devices"
+	depends on MEDIA_DIGITAL_TV_SUPPORT
+	default n
+	---help---
+	  Say Y here to enable support for platform-specific Digital TV drivers.
+
+if DVB_PLATFORM_DRIVERS
+source "drivers/media/platform/sti/c8sectpfe/Kconfig"
+endif #DVB_PLATFORM_DRIVERS
diff --git a/drivers/media/platform/Makefile b/drivers/media/platform/Makefile
new file mode 100644
index 0000000..efa0295
--- /dev/null
+++ b/drivers/media/platform/Makefile
@@ -0,0 +1,57 @@
+#
+# Makefile for the video capture/playback device drivers.
+#
+
+obj-$(CONFIG_VIDEO_TIMBERDALE)	+= timblogiw.o
+obj-$(CONFIG_VIDEO_M32R_AR_M64278) += arv.o
+
+obj-$(CONFIG_VIDEO_VIA_CAMERA) += via-camera.o
+obj-$(CONFIG_VIDEO_CAFE_CCIC) += marvell-ccic/
+obj-$(CONFIG_VIDEO_MMP_CAMERA) += marvell-ccic/
+
+obj-$(CONFIG_VIDEO_OMAP3)	+= omap3isp/
+
+obj-$(CONFIG_VIDEO_VIU) += fsl-viu.o
+
+obj-$(CONFIG_VIDEO_VIVID)		+= vivid/
+obj-$(CONFIG_VIDEO_VIM2M)		+= vim2m.o
+
+obj-$(CONFIG_VIDEO_TI_VPE)		+= ti-vpe/
+
+obj-$(CONFIG_VIDEO_MX2_EMMAPRP)		+= mx2_emmaprp.o
+obj-$(CONFIG_VIDEO_CODA) 		+= coda/
+
+obj-$(CONFIG_VIDEO_SH_VEU)		+= sh_veu.o
+
+obj-$(CONFIG_VIDEO_MEM2MEM_DEINTERLACE)	+= m2m-deinterlace.o
+
+obj-$(CONFIG_VIDEO_S3C_CAMIF) 		+= s3c-camif/
+obj-$(CONFIG_VIDEO_SAMSUNG_EXYNOS4_IS) 	+= exynos4-is/
+obj-$(CONFIG_VIDEO_SAMSUNG_S5P_JPEG)	+= s5p-jpeg/
+obj-$(CONFIG_VIDEO_SAMSUNG_S5P_MFC)	+= s5p-mfc/
+obj-$(CONFIG_VIDEO_SAMSUNG_S5P_TV)	+= s5p-tv/
+
+obj-$(CONFIG_VIDEO_SAMSUNG_S5P_G2D)	+= s5p-g2d/
+obj-$(CONFIG_VIDEO_SAMSUNG_EXYNOS_GSC)	+= exynos-gsc/
+
+obj-$(CONFIG_VIDEO_STI_BDISP)		+= sti/bdisp/
+obj-$(CONFIG_DVB_C8SECTPFE)		+= sti/c8sectpfe/
+
+obj-$(CONFIG_BLACKFIN)                  += blackfin/
+
+obj-$(CONFIG_ARCH_DAVINCI)		+= davinci/
+
+obj-$(CONFIG_VIDEO_SH_VOU)		+= sh_vou.o
+
+obj-$(CONFIG_SOC_CAMERA)		+= soc_camera/
+
+obj-$(CONFIG_VIDEO_RENESAS_JPU) 	+= rcar_jpu.o
+obj-$(CONFIG_VIDEO_RENESAS_VSP1)	+= vsp1/
+
+obj-y	+= omap/
+
+obj-$(CONFIG_VIDEO_AM437X_VPFE)		+= am437x/
+
+obj-$(CONFIG_VIDEO_XILINX)		+= xilinx/
+
+ccflags-y += -I$(srctree)/drivers/media/i2c
diff --git a/drivers/media/platform/am437x/Kconfig b/drivers/media/platform/am437x/Kconfig
new file mode 100644
index 0000000..42d9c18
--- /dev/null
+++ b/drivers/media/platform/am437x/Kconfig
@@ -0,0 +1,11 @@
+config VIDEO_AM437X_VPFE
+	tristate "TI AM437x VPFE video capture driver"
+	depends on VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API && HAS_DMA
+	depends on SOC_AM43XX || COMPILE_TEST
+	select VIDEOBUF2_DMA_CONTIG
+	help
+	   Support for AM437x Video Processing Front End based Video
+	   Capture Driver.
+
+	   To compile this driver as a module, choose M here. The module
+	   will be called am437x-vpfe.
diff --git a/drivers/media/platform/am437x/Makefile b/drivers/media/platform/am437x/Makefile
new file mode 100644
index 0000000..d11fff1
--- /dev/null
+++ b/drivers/media/platform/am437x/Makefile
@@ -0,0 +1,3 @@
+# Makefile for AM437x VPFE driver
+
+obj-$(CONFIG_VIDEO_AM437X_VPFE) += am437x-vpfe.o
diff --git a/drivers/media/platform/am437x/am437x-vpfe.c b/drivers/media/platform/am437x/am437x-vpfe.c
new file mode 100644
index 0000000..572bc04
--- /dev/null
+++ b/drivers/media/platform/am437x/am437x-vpfe.c
@@ -0,0 +1,2772 @@
+/*
+ * TI VPFE capture Driver
+ *
+ * Copyright (C) 2013 - 2014 Texas Instruments, Inc.
+ *
+ * Benoit Parrot <bparrot@ti.com>
+ * Lad, Prabhakar <prabhakar.csengg@gmail.com>
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/videodev2.h>
+
+#include <media/v4l2-common.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-of.h>
+
+#include "am437x-vpfe.h"
+
+#define VPFE_MODULE_NAME	"vpfe"
+#define VPFE_VERSION		"0.1.0"
+
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "Debug level 0-8");
+
+#define vpfe_dbg(level, dev, fmt, arg...)	\
+		v4l2_dbg(level, debug, &dev->v4l2_dev, fmt, ##arg)
+#define vpfe_info(dev, fmt, arg...)	\
+		v4l2_info(&dev->v4l2_dev, fmt, ##arg)
+#define vpfe_err(dev, fmt, arg...)	\
+		v4l2_err(&dev->v4l2_dev, fmt, ##arg)
+
+/* standard information */
+struct vpfe_standard {
+	v4l2_std_id std_id;
+	unsigned int width;
+	unsigned int height;
+	struct v4l2_fract pixelaspect;
+	int frame_format;
+};
+
+static const struct vpfe_standard vpfe_standards[] = {
+	{V4L2_STD_525_60, 720, 480, {11, 10}, 1},
+	{V4L2_STD_625_50, 720, 576, {54, 59}, 1},
+};
+
+struct bus_format {
+	unsigned int width;
+	unsigned int bpp;
+};
+
+/*
+ * struct vpfe_fmt - VPFE media bus format information
+ * @name: V4L2 format description
+ * @code: V4L2 media bus format code
+ * @shifted: V4L2 media bus format code for the same pixel layout but
+ *	shifted to be 8 bits per pixel. =0 if format is not shiftable.
+ * @pixelformat: V4L2 pixel format FCC identifier
+ * @width: Bits per pixel (when transferred over a bus)
+ * @bpp: Bytes per pixel (when stored in memory)
+ * @supported: Indicates format supported by subdev
+ */
+struct vpfe_fmt {
+	const char *name;
+	u32 fourcc;
+	u32 code;
+	struct bus_format l;
+	struct bus_format s;
+	bool supported;
+	u32 index;
+};
+
+static struct vpfe_fmt formats[] = {
+	{
+		.name		= "YUV 4:2:2 packed, YCbYCr",
+		.fourcc		= V4L2_PIX_FMT_YUYV,
+		.code		= MEDIA_BUS_FMT_YUYV8_2X8,
+		.l.width	= 10,
+		.l.bpp		= 4,
+		.s.width	= 8,
+		.s.bpp		= 2,
+		.supported	= false,
+	}, {
+		.name		= "YUV 4:2:2 packed, CbYCrY",
+		.fourcc		= V4L2_PIX_FMT_UYVY,
+		.code		= MEDIA_BUS_FMT_UYVY8_2X8,
+		.l.width	= 10,
+		.l.bpp		= 4,
+		.s.width	= 8,
+		.s.bpp		= 2,
+		.supported	= false,
+	}, {
+		.name		= "YUV 4:2:2 packed, YCrYCb",
+		.fourcc		= V4L2_PIX_FMT_YVYU,
+		.code		= MEDIA_BUS_FMT_YVYU8_2X8,
+		.l.width	= 10,
+		.l.bpp		= 4,
+		.s.width	= 8,
+		.s.bpp		= 2,
+		.supported	= false,
+	}, {
+		.name		= "YUV 4:2:2 packed, CrYCbY",
+		.fourcc		= V4L2_PIX_FMT_VYUY,
+		.code		= MEDIA_BUS_FMT_VYUY8_2X8,
+		.l.width	= 10,
+		.l.bpp		= 4,
+		.s.width	= 8,
+		.s.bpp		= 2,
+		.supported	= false,
+	}, {
+		.name		= "RAW8 BGGR",
+		.fourcc		= V4L2_PIX_FMT_SBGGR8,
+		.code		= MEDIA_BUS_FMT_SBGGR8_1X8,
+		.l.width	= 10,
+		.l.bpp		= 2,
+		.s.width	= 8,
+		.s.bpp		= 1,
+		.supported	= false,
+	}, {
+		.name		= "RAW8 GBRG",
+		.fourcc		= V4L2_PIX_FMT_SGBRG8,
+		.code		= MEDIA_BUS_FMT_SGBRG8_1X8,
+		.l.width	= 10,
+		.l.bpp		= 2,
+		.s.width	= 8,
+		.s.bpp		= 1,
+		.supported	= false,
+	}, {
+		.name		= "RAW8 GRBG",
+		.fourcc		= V4L2_PIX_FMT_SGRBG8,
+		.code		= MEDIA_BUS_FMT_SGRBG8_1X8,
+		.l.width	= 10,
+		.l.bpp		= 2,
+		.s.width	= 8,
+		.s.bpp		= 1,
+		.supported	= false,
+	}, {
+		.name		= "RAW8 RGGB",
+		.fourcc		= V4L2_PIX_FMT_SRGGB8,
+		.code		= MEDIA_BUS_FMT_SRGGB8_1X8,
+		.l.width	= 10,
+		.l.bpp		= 2,
+		.s.width	= 8,
+		.s.bpp		= 1,
+		.supported	= false,
+	}, {
+		.name		= "RGB565 (LE)",
+		.fourcc		= V4L2_PIX_FMT_RGB565,
+		.code		= MEDIA_BUS_FMT_RGB565_2X8_LE,
+		.l.width	= 10,
+		.l.bpp		= 4,
+		.s.width	= 8,
+		.s.bpp		= 2,
+		.supported	= false,
+	}, {
+		.name		= "RGB565 (BE)",
+		.fourcc		= V4L2_PIX_FMT_RGB565X,
+		.code		= MEDIA_BUS_FMT_RGB565_2X8_BE,
+		.l.width	= 10,
+		.l.bpp		= 4,
+		.s.width	= 8,
+		.s.bpp		= 2,
+		.supported	= false,
+	},
+};
+
+static int
+__vpfe_get_format(struct vpfe_device *vpfe,
+		  struct v4l2_format *format, unsigned int *bpp);
+
+static struct vpfe_fmt *find_format_by_code(unsigned int code)
+{
+	struct vpfe_fmt *fmt;
+	unsigned int k;
+
+	for (k = 0; k < ARRAY_SIZE(formats); k++) {
+		fmt = &formats[k];
+		if (fmt->code == code)
+			return fmt;
+	}
+
+	return NULL;
+}
+
+static struct vpfe_fmt *find_format_by_pix(unsigned int pixelformat)
+{
+	struct vpfe_fmt *fmt;
+	unsigned int k;
+
+	for (k = 0; k < ARRAY_SIZE(formats); k++) {
+		fmt = &formats[k];
+		if (fmt->fourcc == pixelformat)
+			return fmt;
+	}
+
+	return NULL;
+}
+
+static void
+mbus_to_pix(struct vpfe_device *vpfe,
+	    const struct v4l2_mbus_framefmt *mbus,
+	    struct v4l2_pix_format *pix, unsigned int *bpp)
+{
+	struct vpfe_subdev_info *sdinfo = vpfe->current_subdev;
+	unsigned int bus_width = sdinfo->vpfe_param.bus_width;
+	struct vpfe_fmt *fmt;
+
+	fmt = find_format_by_code(mbus->code);
+	if (WARN_ON(fmt == NULL)) {
+		pr_err("Invalid mbus code set\n");
+		*bpp = 1;
+		return;
+	}
+
+	memset(pix, 0, sizeof(*pix));
+	v4l2_fill_pix_format(pix, mbus);
+	pix->pixelformat = fmt->fourcc;
+	*bpp = (bus_width == 10) ?  fmt->l.bpp : fmt->s.bpp;
+
+	/* pitch should be 32 bytes aligned */
+	pix->bytesperline = ALIGN(pix->width * *bpp, 32);
+	pix->sizeimage = pix->bytesperline * pix->height;
+}
+
+static void pix_to_mbus(struct vpfe_device *vpfe,
+			struct v4l2_pix_format *pix_fmt,
+			struct v4l2_mbus_framefmt *mbus_fmt)
+{
+	struct vpfe_fmt *fmt;
+
+	fmt = find_format_by_pix(pix_fmt->pixelformat);
+	if (!fmt) {
+		/* default to first entry */
+		vpfe_dbg(3, vpfe, "Invalid pixel code: %x, default used instead\n",
+			pix_fmt->pixelformat);
+		fmt = &formats[0];
+	}
+
+	memset(mbus_fmt, 0, sizeof(*mbus_fmt));
+	v4l2_fill_mbus_format(mbus_fmt, pix_fmt, fmt->code);
+}
+
+/*  Print Four-character-code (FOURCC) */
+static char *print_fourcc(u32 fmt)
+{
+	static char code[5];
+
+	code[0] = (unsigned char)(fmt & 0xff);
+	code[1] = (unsigned char)((fmt >> 8) & 0xff);
+	code[2] = (unsigned char)((fmt >> 16) & 0xff);
+	code[3] = (unsigned char)((fmt >> 24) & 0xff);
+	code[4] = '\0';
+
+	return code;
+}
+
+static int
+cmp_v4l2_format(const struct v4l2_format *lhs, const struct v4l2_format *rhs)
+{
+	return lhs->type == rhs->type &&
+		lhs->fmt.pix.width == rhs->fmt.pix.width &&
+		lhs->fmt.pix.height == rhs->fmt.pix.height &&
+		lhs->fmt.pix.pixelformat == rhs->fmt.pix.pixelformat &&
+		lhs->fmt.pix.field == rhs->fmt.pix.field &&
+		lhs->fmt.pix.colorspace == rhs->fmt.pix.colorspace &&
+		lhs->fmt.pix.ycbcr_enc == rhs->fmt.pix.ycbcr_enc &&
+		lhs->fmt.pix.quantization == rhs->fmt.pix.quantization &&
+		lhs->fmt.pix.xfer_func == rhs->fmt.pix.xfer_func;
+}
+
+static inline u32 vpfe_reg_read(struct vpfe_ccdc *ccdc, u32 offset)
+{
+	return ioread32(ccdc->ccdc_cfg.base_addr + offset);
+}
+
+static inline void vpfe_reg_write(struct vpfe_ccdc *ccdc, u32 val, u32 offset)
+{
+	iowrite32(val, ccdc->ccdc_cfg.base_addr + offset);
+}
+
+static inline struct vpfe_device *to_vpfe(struct vpfe_ccdc *ccdc)
+{
+	return container_of(ccdc, struct vpfe_device, ccdc);
+}
+
+static inline
+struct vpfe_cap_buffer *to_vpfe_buffer(struct vb2_v4l2_buffer *vb)
+{
+	return container_of(vb, struct vpfe_cap_buffer, vb);
+}
+
+static inline void vpfe_pcr_enable(struct vpfe_ccdc *ccdc, int flag)
+{
+	vpfe_reg_write(ccdc, !!flag, VPFE_PCR);
+}
+
+static void vpfe_config_enable(struct vpfe_ccdc *ccdc, int flag)
+{
+	unsigned int cfg;
+
+	if (!flag) {
+		cfg = vpfe_reg_read(ccdc, VPFE_CONFIG);
+		cfg &= ~(VPFE_CONFIG_EN_ENABLE << VPFE_CONFIG_EN_SHIFT);
+	} else {
+		cfg = VPFE_CONFIG_EN_ENABLE << VPFE_CONFIG_EN_SHIFT;
+	}
+
+	vpfe_reg_write(ccdc, cfg, VPFE_CONFIG);
+}
+
+static void vpfe_ccdc_setwin(struct vpfe_ccdc *ccdc,
+			     struct v4l2_rect *image_win,
+			     enum ccdc_frmfmt frm_fmt,
+			     int bpp)
+{
+	int horz_start, horz_nr_pixels;
+	int vert_start, vert_nr_lines;
+	int val, mid_img;
+
+	/*
+	 * ppc - per pixel count. indicates how many pixels per cell
+	 * output to SDRAM. example, for ycbcr, it is one y and one c, so 2.
+	 * raw capture this is 1
+	 */
+	horz_start = image_win->left * bpp;
+	horz_nr_pixels = (image_win->width * bpp) - 1;
+	vpfe_reg_write(ccdc, (horz_start << VPFE_HORZ_INFO_SPH_SHIFT) |
+				horz_nr_pixels, VPFE_HORZ_INFO);
+
+	vert_start = image_win->top;
+
+	if (frm_fmt == CCDC_FRMFMT_INTERLACED) {
+		vert_nr_lines = (image_win->height >> 1) - 1;
+		vert_start >>= 1;
+		/* Since first line doesn't have any data */
+		vert_start += 1;
+		/* configure VDINT0 */
+		val = (vert_start << VPFE_VDINT_VDINT0_SHIFT);
+	} else {
+		/* Since first line doesn't have any data */
+		vert_start += 1;
+		vert_nr_lines = image_win->height - 1;
+		/*
+		 * configure VDINT0 and VDINT1. VDINT1 will be at half
+		 * of image height
+		 */
+		mid_img = vert_start + (image_win->height / 2);
+		val = (vert_start << VPFE_VDINT_VDINT0_SHIFT) |
+				(mid_img & VPFE_VDINT_VDINT1_MASK);
+	}
+
+	vpfe_reg_write(ccdc, val, VPFE_VDINT);
+
+	vpfe_reg_write(ccdc, (vert_start << VPFE_VERT_START_SLV0_SHIFT) |
+				vert_start, VPFE_VERT_START);
+	vpfe_reg_write(ccdc, vert_nr_lines, VPFE_VERT_LINES);
+}
+
+static void vpfe_reg_dump(struct vpfe_ccdc *ccdc)
+{
+	struct vpfe_device *vpfe = to_vpfe(ccdc);
+
+	vpfe_dbg(3, vpfe, "ALAW: 0x%x\n", vpfe_reg_read(ccdc, VPFE_ALAW));
+	vpfe_dbg(3, vpfe, "CLAMP: 0x%x\n", vpfe_reg_read(ccdc, VPFE_CLAMP));
+	vpfe_dbg(3, vpfe, "DCSUB: 0x%x\n", vpfe_reg_read(ccdc, VPFE_DCSUB));
+	vpfe_dbg(3, vpfe, "BLKCMP: 0x%x\n", vpfe_reg_read(ccdc, VPFE_BLKCMP));
+	vpfe_dbg(3, vpfe, "COLPTN: 0x%x\n", vpfe_reg_read(ccdc, VPFE_COLPTN));
+	vpfe_dbg(3, vpfe, "SDOFST: 0x%x\n", vpfe_reg_read(ccdc, VPFE_SDOFST));
+	vpfe_dbg(3, vpfe, "SYN_MODE: 0x%x\n",
+		 vpfe_reg_read(ccdc, VPFE_SYNMODE));
+	vpfe_dbg(3, vpfe, "HSIZE_OFF: 0x%x\n",
+		 vpfe_reg_read(ccdc, VPFE_HSIZE_OFF));
+	vpfe_dbg(3, vpfe, "HORZ_INFO: 0x%x\n",
+		 vpfe_reg_read(ccdc, VPFE_HORZ_INFO));
+	vpfe_dbg(3, vpfe, "VERT_START: 0x%x\n",
+		 vpfe_reg_read(ccdc, VPFE_VERT_START));
+	vpfe_dbg(3, vpfe, "VERT_LINES: 0x%x\n",
+		 vpfe_reg_read(ccdc, VPFE_VERT_LINES));
+}
+
+static int
+vpfe_ccdc_validate_param(struct vpfe_ccdc *ccdc,
+			 struct vpfe_ccdc_config_params_raw *ccdcparam)
+{
+	struct vpfe_device *vpfe = to_vpfe(ccdc);
+	u8 max_gamma, max_data;
+
+	if (!ccdcparam->alaw.enable)
+		return 0;
+
+	max_gamma = ccdc_gamma_width_max_bit(ccdcparam->alaw.gamma_wd);
+	max_data = ccdc_data_size_max_bit(ccdcparam->data_sz);
+
+	if (ccdcparam->alaw.gamma_wd > VPFE_CCDC_GAMMA_BITS_09_0 ||
+	    ccdcparam->alaw.gamma_wd < VPFE_CCDC_GAMMA_BITS_15_6 ||
+	    max_gamma > max_data) {
+		vpfe_dbg(1, vpfe, "Invalid data line select\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void
+vpfe_ccdc_update_raw_params(struct vpfe_ccdc *ccdc,
+			    struct vpfe_ccdc_config_params_raw *raw_params)
+{
+	struct vpfe_ccdc_config_params_raw *config_params =
+				&ccdc->ccdc_cfg.bayer.config_params;
+
+	*config_params = *raw_params;
+}
+
+/*
+ * vpfe_ccdc_restore_defaults()
+ * This function will write defaults to all CCDC registers
+ */
+static void vpfe_ccdc_restore_defaults(struct vpfe_ccdc *ccdc)
+{
+	int i;
+
+	/* Disable CCDC */
+	vpfe_pcr_enable(ccdc, 0);
+
+	/* set all registers to default value */
+	for (i = 4; i <= 0x94; i += 4)
+		vpfe_reg_write(ccdc, 0,  i);
+
+	vpfe_reg_write(ccdc, VPFE_NO_CULLING, VPFE_CULLING);
+	vpfe_reg_write(ccdc, VPFE_CCDC_GAMMA_BITS_11_2, VPFE_ALAW);
+}
+
+static int vpfe_ccdc_close(struct vpfe_ccdc *ccdc, struct device *dev)
+{
+	int dma_cntl, i, pcr;
+
+	/* If the CCDC module is still busy wait for it to be done */
+	for (i = 0; i < 10; i++) {
+		usleep_range(5000, 6000);
+		pcr = vpfe_reg_read(ccdc, VPFE_PCR);
+		if (!pcr)
+			break;
+
+		/* make sure it it is disabled */
+		vpfe_pcr_enable(ccdc, 0);
+	}
+
+	/* Disable CCDC by resetting all register to default POR values */
+	vpfe_ccdc_restore_defaults(ccdc);
+
+	/* if DMA_CNTL overflow bit is set. Clear it
+	 *  It appears to take a while for this to become quiescent ~20ms
+	 */
+	for (i = 0; i < 10; i++) {
+		dma_cntl = vpfe_reg_read(ccdc, VPFE_DMA_CNTL);
+		if (!(dma_cntl & VPFE_DMA_CNTL_OVERFLOW))
+			break;
+
+		/* Clear the overflow bit */
+		vpfe_reg_write(ccdc, dma_cntl, VPFE_DMA_CNTL);
+		usleep_range(5000, 6000);
+	}
+
+	/* Disabled the module at the CONFIG level */
+	vpfe_config_enable(ccdc, 0);
+
+	pm_runtime_put_sync(dev);
+
+	return 0;
+}
+
+static int vpfe_ccdc_set_params(struct vpfe_ccdc *ccdc, void __user *params)
+{
+	struct vpfe_device *vpfe = container_of(ccdc, struct vpfe_device, ccdc);
+	struct vpfe_ccdc_config_params_raw raw_params;
+	int x;
+
+	if (ccdc->ccdc_cfg.if_type != VPFE_RAW_BAYER)
+		return -EINVAL;
+
+	x = copy_from_user(&raw_params, params, sizeof(raw_params));
+	if (x) {
+		vpfe_dbg(1, vpfe,
+			"vpfe_ccdc_set_params: error in copying ccdc params, %d\n",
+			x);
+		return -EFAULT;
+	}
+
+	if (!vpfe_ccdc_validate_param(ccdc, &raw_params)) {
+		vpfe_ccdc_update_raw_params(ccdc, &raw_params);
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+/*
+ * vpfe_ccdc_config_ycbcr()
+ * This function will configure CCDC for YCbCr video capture
+ */
+static void vpfe_ccdc_config_ycbcr(struct vpfe_ccdc *ccdc)
+{
+	struct vpfe_device *vpfe = container_of(ccdc, struct vpfe_device, ccdc);
+	struct ccdc_params_ycbcr *params = &ccdc->ccdc_cfg.ycbcr;
+	u32 syn_mode;
+
+	vpfe_dbg(3, vpfe, "vpfe_ccdc_config_ycbcr:\n");
+	/*
+	 * first restore the CCDC registers to default values
+	 * This is important since we assume default values to be set in
+	 * a lot of registers that we didn't touch
+	 */
+	vpfe_ccdc_restore_defaults(ccdc);
+
+	/*
+	 * configure pixel format, frame format, configure video frame
+	 * format, enable output to SDRAM, enable internal timing generator
+	 * and 8bit pack mode
+	 */
+	syn_mode = (((params->pix_fmt & VPFE_SYN_MODE_INPMOD_MASK) <<
+		    VPFE_SYN_MODE_INPMOD_SHIFT) |
+		    ((params->frm_fmt & VPFE_SYN_FLDMODE_MASK) <<
+		    VPFE_SYN_FLDMODE_SHIFT) | VPFE_VDHDEN_ENABLE |
+		    VPFE_WEN_ENABLE | VPFE_DATA_PACK_ENABLE);
+
+	/* setup BT.656 sync mode */
+	if (params->bt656_enable) {
+		vpfe_reg_write(ccdc, VPFE_REC656IF_BT656_EN, VPFE_REC656IF);
+
+		/*
+		 * configure the FID, VD, HD pin polarity,
+		 * fld,hd pol positive, vd negative, 8-bit data
+		 */
+		syn_mode |= VPFE_SYN_MODE_VD_POL_NEGATIVE;
+		if (ccdc->ccdc_cfg.if_type == VPFE_BT656_10BIT)
+			syn_mode |= VPFE_SYN_MODE_10BITS;
+		else
+			syn_mode |= VPFE_SYN_MODE_8BITS;
+	} else {
+		/* y/c external sync mode */
+		syn_mode |= (((params->fid_pol & VPFE_FID_POL_MASK) <<
+			     VPFE_FID_POL_SHIFT) |
+			     ((params->hd_pol & VPFE_HD_POL_MASK) <<
+			     VPFE_HD_POL_SHIFT) |
+			     ((params->vd_pol & VPFE_VD_POL_MASK) <<
+			     VPFE_VD_POL_SHIFT));
+	}
+	vpfe_reg_write(ccdc, syn_mode, VPFE_SYNMODE);
+
+	/* configure video window */
+	vpfe_ccdc_setwin(ccdc, &params->win,
+			 params->frm_fmt, params->bytesperpixel);
+
+	/*
+	 * configure the order of y cb cr in SDRAM, and disable latch
+	 * internal register on vsync
+	 */
+	if (ccdc->ccdc_cfg.if_type == VPFE_BT656_10BIT)
+		vpfe_reg_write(ccdc,
+			       (params->pix_order << VPFE_CCDCFG_Y8POS_SHIFT) |
+			       VPFE_LATCH_ON_VSYNC_DISABLE |
+			       VPFE_CCDCFG_BW656_10BIT, VPFE_CCDCFG);
+	else
+		vpfe_reg_write(ccdc,
+			       (params->pix_order << VPFE_CCDCFG_Y8POS_SHIFT) |
+			       VPFE_LATCH_ON_VSYNC_DISABLE, VPFE_CCDCFG);
+
+	/*
+	 * configure the horizontal line offset. This should be a
+	 * on 32 byte boundary. So clear LSB 5 bits
+	 */
+	vpfe_reg_write(ccdc, params->bytesperline, VPFE_HSIZE_OFF);
+
+	/* configure the memory line offset */
+	if (params->buf_type == CCDC_BUFTYPE_FLD_INTERLEAVED)
+		/* two fields are interleaved in memory */
+		vpfe_reg_write(ccdc, VPFE_SDOFST_FIELD_INTERLEAVED,
+			       VPFE_SDOFST);
+}
+
+static void
+vpfe_ccdc_config_black_clamp(struct vpfe_ccdc *ccdc,
+			     struct vpfe_ccdc_black_clamp *bclamp)
+{
+	u32 val;
+
+	if (!bclamp->enable) {
+		/* configure DCSub */
+		val = (bclamp->dc_sub) & VPFE_BLK_DC_SUB_MASK;
+		vpfe_reg_write(ccdc, val, VPFE_DCSUB);
+		vpfe_reg_write(ccdc, VPFE_CLAMP_DEFAULT_VAL, VPFE_CLAMP);
+		return;
+	}
+	/*
+	 * Configure gain,  Start pixel, No of line to be avg,
+	 * No of pixel/line to be avg, & Enable the Black clamping
+	 */
+	val = ((bclamp->sgain & VPFE_BLK_SGAIN_MASK) |
+	       ((bclamp->start_pixel & VPFE_BLK_ST_PXL_MASK) <<
+		VPFE_BLK_ST_PXL_SHIFT) |
+	       ((bclamp->sample_ln & VPFE_BLK_SAMPLE_LINE_MASK) <<
+		VPFE_BLK_SAMPLE_LINE_SHIFT) |
+	       ((bclamp->sample_pixel & VPFE_BLK_SAMPLE_LN_MASK) <<
+		VPFE_BLK_SAMPLE_LN_SHIFT) | VPFE_BLK_CLAMP_ENABLE);
+	vpfe_reg_write(ccdc, val, VPFE_CLAMP);
+	/* If Black clamping is enable then make dcsub 0 */
+	vpfe_reg_write(ccdc, VPFE_DCSUB_DEFAULT_VAL, VPFE_DCSUB);
+}
+
+static void
+vpfe_ccdc_config_black_compense(struct vpfe_ccdc *ccdc,
+				struct vpfe_ccdc_black_compensation *bcomp)
+{
+	u32 val;
+
+	val = ((bcomp->b & VPFE_BLK_COMP_MASK) |
+	      ((bcomp->gb & VPFE_BLK_COMP_MASK) <<
+	       VPFE_BLK_COMP_GB_COMP_SHIFT) |
+	      ((bcomp->gr & VPFE_BLK_COMP_MASK) <<
+	       VPFE_BLK_COMP_GR_COMP_SHIFT) |
+	      ((bcomp->r & VPFE_BLK_COMP_MASK) <<
+	       VPFE_BLK_COMP_R_COMP_SHIFT));
+	vpfe_reg_write(ccdc, val, VPFE_BLKCMP);
+}
+
+/*
+ * vpfe_ccdc_config_raw()
+ * This function will configure CCDC for Raw capture mode
+ */
+static void vpfe_ccdc_config_raw(struct vpfe_ccdc *ccdc)
+{
+	struct vpfe_device *vpfe = container_of(ccdc, struct vpfe_device, ccdc);
+	struct vpfe_ccdc_config_params_raw *config_params =
+				&ccdc->ccdc_cfg.bayer.config_params;
+	struct ccdc_params_raw *params = &ccdc->ccdc_cfg.bayer;
+	unsigned int syn_mode;
+	unsigned int val;
+
+	vpfe_dbg(3, vpfe, "vpfe_ccdc_config_raw:\n");
+
+	/* Reset CCDC */
+	vpfe_ccdc_restore_defaults(ccdc);
+
+	/* Disable latching function registers on VSYNC  */
+	vpfe_reg_write(ccdc, VPFE_LATCH_ON_VSYNC_DISABLE, VPFE_CCDCFG);
+
+	/*
+	 * Configure the vertical sync polarity(SYN_MODE.VDPOL),
+	 * horizontal sync polarity (SYN_MODE.HDPOL), frame id polarity
+	 * (SYN_MODE.FLDPOL), frame format(progressive or interlace),
+	 * data size(SYNMODE.DATSIZ), &pixel format (Input mode), output
+	 * SDRAM, enable internal timing generator
+	 */
+	syn_mode = (((params->vd_pol & VPFE_VD_POL_MASK) << VPFE_VD_POL_SHIFT) |
+		   ((params->hd_pol & VPFE_HD_POL_MASK) << VPFE_HD_POL_SHIFT) |
+		   ((params->fid_pol & VPFE_FID_POL_MASK) <<
+		   VPFE_FID_POL_SHIFT) | ((params->frm_fmt &
+		   VPFE_FRM_FMT_MASK) << VPFE_FRM_FMT_SHIFT) |
+		   ((config_params->data_sz & VPFE_DATA_SZ_MASK) <<
+		   VPFE_DATA_SZ_SHIFT) | ((params->pix_fmt &
+		   VPFE_PIX_FMT_MASK) << VPFE_PIX_FMT_SHIFT) |
+		   VPFE_WEN_ENABLE | VPFE_VDHDEN_ENABLE);
+
+	/* Enable and configure aLaw register if needed */
+	if (config_params->alaw.enable) {
+		val = ((config_params->alaw.gamma_wd &
+		      VPFE_ALAW_GAMMA_WD_MASK) | VPFE_ALAW_ENABLE);
+		vpfe_reg_write(ccdc, val, VPFE_ALAW);
+		vpfe_dbg(3, vpfe, "\nWriting 0x%x to ALAW...\n", val);
+	}
+
+	/* Configure video window */
+	vpfe_ccdc_setwin(ccdc, &params->win, params->frm_fmt,
+			 params->bytesperpixel);
+
+	/* Configure Black Clamp */
+	vpfe_ccdc_config_black_clamp(ccdc, &config_params->blk_clamp);
+
+	/* Configure Black level compensation */
+	vpfe_ccdc_config_black_compense(ccdc, &config_params->blk_comp);
+
+	/* If data size is 8 bit then pack the data */
+	if ((config_params->data_sz == VPFE_CCDC_DATA_8BITS) ||
+	    config_params->alaw.enable)
+		syn_mode |= VPFE_DATA_PACK_ENABLE;
+
+	/*
+	 * Configure Horizontal offset register. If pack 8 is enabled then
+	 * 1 pixel will take 1 byte
+	 */
+	vpfe_reg_write(ccdc, params->bytesperline, VPFE_HSIZE_OFF);
+
+	vpfe_dbg(3, vpfe, "Writing %d (%x) to HSIZE_OFF\n",
+		params->bytesperline, params->bytesperline);
+
+	/* Set value for SDOFST */
+	if (params->frm_fmt == CCDC_FRMFMT_INTERLACED) {
+		if (params->image_invert_enable) {
+			/* For interlace inverse mode */
+			vpfe_reg_write(ccdc, VPFE_INTERLACED_IMAGE_INVERT,
+				   VPFE_SDOFST);
+		} else {
+			/* For interlace non inverse mode */
+			vpfe_reg_write(ccdc, VPFE_INTERLACED_NO_IMAGE_INVERT,
+				   VPFE_SDOFST);
+		}
+	} else if (params->frm_fmt == CCDC_FRMFMT_PROGRESSIVE) {
+		vpfe_reg_write(ccdc, VPFE_PROGRESSIVE_NO_IMAGE_INVERT,
+			   VPFE_SDOFST);
+	}
+
+	vpfe_reg_write(ccdc, syn_mode, VPFE_SYNMODE);
+
+	vpfe_reg_dump(ccdc);
+}
+
+static inline int
+vpfe_ccdc_set_buftype(struct vpfe_ccdc *ccdc,
+		      enum ccdc_buftype buf_type)
+{
+	if (ccdc->ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		ccdc->ccdc_cfg.bayer.buf_type = buf_type;
+	else
+		ccdc->ccdc_cfg.ycbcr.buf_type = buf_type;
+
+	return 0;
+}
+
+static inline enum ccdc_buftype vpfe_ccdc_get_buftype(struct vpfe_ccdc *ccdc)
+{
+	if (ccdc->ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		return ccdc->ccdc_cfg.bayer.buf_type;
+
+	return ccdc->ccdc_cfg.ycbcr.buf_type;
+}
+
+static int vpfe_ccdc_set_pixel_format(struct vpfe_ccdc *ccdc, u32 pixfmt)
+{
+	struct vpfe_device *vpfe = container_of(ccdc, struct vpfe_device, ccdc);
+
+	vpfe_dbg(1, vpfe, "vpfe_ccdc_set_pixel_format: if_type: %d, pixfmt:%s\n",
+		 ccdc->ccdc_cfg.if_type, print_fourcc(pixfmt));
+
+	if (ccdc->ccdc_cfg.if_type == VPFE_RAW_BAYER) {
+		ccdc->ccdc_cfg.bayer.pix_fmt = CCDC_PIXFMT_RAW;
+		/*
+		 * Need to clear it in case it was left on
+		 * after the last capture.
+		 */
+		ccdc->ccdc_cfg.bayer.config_params.alaw.enable = 0;
+
+		switch (pixfmt) {
+		case V4L2_PIX_FMT_SBGGR8:
+			ccdc->ccdc_cfg.bayer.config_params.alaw.enable = 1;
+			break;
+
+		case V4L2_PIX_FMT_YUYV:
+		case V4L2_PIX_FMT_UYVY:
+		case V4L2_PIX_FMT_YUV420:
+		case V4L2_PIX_FMT_NV12:
+		case V4L2_PIX_FMT_RGB565X:
+			break;
+
+		case V4L2_PIX_FMT_SBGGR16:
+		default:
+			return -EINVAL;
+		}
+	} else {
+		switch (pixfmt) {
+		case V4L2_PIX_FMT_YUYV:
+			ccdc->ccdc_cfg.ycbcr.pix_order = CCDC_PIXORDER_YCBYCR;
+			break;
+
+		case V4L2_PIX_FMT_UYVY:
+			ccdc->ccdc_cfg.ycbcr.pix_order = CCDC_PIXORDER_CBYCRY;
+			break;
+
+		default:
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+static u32 vpfe_ccdc_get_pixel_format(struct vpfe_ccdc *ccdc)
+{
+	u32 pixfmt;
+
+	if (ccdc->ccdc_cfg.if_type == VPFE_RAW_BAYER) {
+		pixfmt = V4L2_PIX_FMT_YUYV;
+	} else {
+		if (ccdc->ccdc_cfg.ycbcr.pix_order == CCDC_PIXORDER_YCBYCR)
+			pixfmt = V4L2_PIX_FMT_YUYV;
+		else
+			pixfmt = V4L2_PIX_FMT_UYVY;
+	}
+
+	return pixfmt;
+}
+
+static int
+vpfe_ccdc_set_image_window(struct vpfe_ccdc *ccdc,
+			   struct v4l2_rect *win, unsigned int bpp)
+{
+	if (ccdc->ccdc_cfg.if_type == VPFE_RAW_BAYER) {
+		ccdc->ccdc_cfg.bayer.win = *win;
+		ccdc->ccdc_cfg.bayer.bytesperpixel = bpp;
+		ccdc->ccdc_cfg.bayer.bytesperline = ALIGN(win->width * bpp, 32);
+	} else {
+		ccdc->ccdc_cfg.ycbcr.win = *win;
+		ccdc->ccdc_cfg.ycbcr.bytesperpixel = bpp;
+		ccdc->ccdc_cfg.ycbcr.bytesperline = ALIGN(win->width * bpp, 32);
+	}
+
+	return 0;
+}
+
+static inline void
+vpfe_ccdc_get_image_window(struct vpfe_ccdc *ccdc,
+			   struct v4l2_rect *win)
+{
+	if (ccdc->ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		*win = ccdc->ccdc_cfg.bayer.win;
+	else
+		*win = ccdc->ccdc_cfg.ycbcr.win;
+}
+
+static inline unsigned int vpfe_ccdc_get_line_length(struct vpfe_ccdc *ccdc)
+{
+	if (ccdc->ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		return ccdc->ccdc_cfg.bayer.bytesperline;
+
+	return ccdc->ccdc_cfg.ycbcr.bytesperline;
+}
+
+static inline int
+vpfe_ccdc_set_frame_format(struct vpfe_ccdc *ccdc,
+			   enum ccdc_frmfmt frm_fmt)
+{
+	if (ccdc->ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		ccdc->ccdc_cfg.bayer.frm_fmt = frm_fmt;
+	else
+		ccdc->ccdc_cfg.ycbcr.frm_fmt = frm_fmt;
+
+	return 0;
+}
+
+static inline enum ccdc_frmfmt
+vpfe_ccdc_get_frame_format(struct vpfe_ccdc *ccdc)
+{
+	if (ccdc->ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		return ccdc->ccdc_cfg.bayer.frm_fmt;
+
+	return ccdc->ccdc_cfg.ycbcr.frm_fmt;
+}
+
+static inline int vpfe_ccdc_getfid(struct vpfe_ccdc *ccdc)
+{
+	return (vpfe_reg_read(ccdc, VPFE_SYNMODE) >> 15) & 1;
+}
+
+static inline void vpfe_set_sdr_addr(struct vpfe_ccdc *ccdc, unsigned long addr)
+{
+	vpfe_reg_write(ccdc, addr & 0xffffffe0, VPFE_SDR_ADDR);
+}
+
+static int vpfe_ccdc_set_hw_if_params(struct vpfe_ccdc *ccdc,
+				      struct vpfe_hw_if_param *params)
+{
+	struct vpfe_device *vpfe = container_of(ccdc, struct vpfe_device, ccdc);
+
+	ccdc->ccdc_cfg.if_type = params->if_type;
+
+	switch (params->if_type) {
+	case VPFE_BT656:
+	case VPFE_YCBCR_SYNC_16:
+	case VPFE_YCBCR_SYNC_8:
+	case VPFE_BT656_10BIT:
+		ccdc->ccdc_cfg.ycbcr.vd_pol = params->vdpol;
+		ccdc->ccdc_cfg.ycbcr.hd_pol = params->hdpol;
+		break;
+
+	case VPFE_RAW_BAYER:
+		ccdc->ccdc_cfg.bayer.vd_pol = params->vdpol;
+		ccdc->ccdc_cfg.bayer.hd_pol = params->hdpol;
+		if (params->bus_width == 10)
+			ccdc->ccdc_cfg.bayer.config_params.data_sz =
+				VPFE_CCDC_DATA_10BITS;
+		else
+			ccdc->ccdc_cfg.bayer.config_params.data_sz =
+				VPFE_CCDC_DATA_8BITS;
+		vpfe_dbg(1, vpfe, "params.bus_width: %d\n",
+			params->bus_width);
+		vpfe_dbg(1, vpfe, "config_params.data_sz: %d\n",
+			ccdc->ccdc_cfg.bayer.config_params.data_sz);
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void vpfe_clear_intr(struct vpfe_ccdc *ccdc, int vdint)
+{
+	unsigned int vpfe_int_status;
+
+	vpfe_int_status = vpfe_reg_read(ccdc, VPFE_IRQ_STS);
+
+	switch (vdint) {
+	/* VD0 interrupt */
+	case VPFE_VDINT0:
+		vpfe_int_status &= ~VPFE_VDINT0;
+		vpfe_int_status |= VPFE_VDINT0;
+		break;
+
+	/* VD1 interrupt */
+	case VPFE_VDINT1:
+		vpfe_int_status &= ~VPFE_VDINT1;
+		vpfe_int_status |= VPFE_VDINT1;
+		break;
+
+	/* VD2 interrupt */
+	case VPFE_VDINT2:
+		vpfe_int_status &= ~VPFE_VDINT2;
+		vpfe_int_status |= VPFE_VDINT2;
+		break;
+
+	/* Clear all interrupts */
+	default:
+		vpfe_int_status &= ~(VPFE_VDINT0 |
+				VPFE_VDINT1 |
+				VPFE_VDINT2);
+		vpfe_int_status |= (VPFE_VDINT0 |
+				VPFE_VDINT1 |
+				VPFE_VDINT2);
+		break;
+	}
+	/* Clear specific VDINT from the status register */
+	vpfe_reg_write(ccdc, vpfe_int_status, VPFE_IRQ_STS);
+
+	vpfe_int_status = vpfe_reg_read(ccdc, VPFE_IRQ_STS);
+
+	/* Acknowledge that we are done with all interrupts */
+	vpfe_reg_write(ccdc, 1, VPFE_IRQ_EOI);
+}
+
+static void vpfe_ccdc_config_defaults(struct vpfe_ccdc *ccdc)
+{
+	ccdc->ccdc_cfg.if_type = VPFE_RAW_BAYER;
+
+	ccdc->ccdc_cfg.ycbcr.pix_fmt = CCDC_PIXFMT_YCBCR_8BIT;
+	ccdc->ccdc_cfg.ycbcr.frm_fmt = CCDC_FRMFMT_INTERLACED;
+	ccdc->ccdc_cfg.ycbcr.fid_pol = VPFE_PINPOL_POSITIVE;
+	ccdc->ccdc_cfg.ycbcr.vd_pol = VPFE_PINPOL_POSITIVE;
+	ccdc->ccdc_cfg.ycbcr.hd_pol = VPFE_PINPOL_POSITIVE;
+	ccdc->ccdc_cfg.ycbcr.pix_order = CCDC_PIXORDER_CBYCRY;
+	ccdc->ccdc_cfg.ycbcr.buf_type = CCDC_BUFTYPE_FLD_INTERLEAVED;
+
+	ccdc->ccdc_cfg.ycbcr.win.left = 0;
+	ccdc->ccdc_cfg.ycbcr.win.top = 0;
+	ccdc->ccdc_cfg.ycbcr.win.width = 720;
+	ccdc->ccdc_cfg.ycbcr.win.height = 576;
+	ccdc->ccdc_cfg.ycbcr.bt656_enable = 1;
+
+	ccdc->ccdc_cfg.bayer.pix_fmt = CCDC_PIXFMT_RAW;
+	ccdc->ccdc_cfg.bayer.frm_fmt = CCDC_FRMFMT_PROGRESSIVE;
+	ccdc->ccdc_cfg.bayer.fid_pol = VPFE_PINPOL_POSITIVE;
+	ccdc->ccdc_cfg.bayer.vd_pol = VPFE_PINPOL_POSITIVE;
+	ccdc->ccdc_cfg.bayer.hd_pol = VPFE_PINPOL_POSITIVE;
+
+	ccdc->ccdc_cfg.bayer.win.left = 0;
+	ccdc->ccdc_cfg.bayer.win.top = 0;
+	ccdc->ccdc_cfg.bayer.win.width = 800;
+	ccdc->ccdc_cfg.bayer.win.height = 600;
+	ccdc->ccdc_cfg.bayer.config_params.data_sz = VPFE_CCDC_DATA_8BITS;
+	ccdc->ccdc_cfg.bayer.config_params.alaw.gamma_wd =
+						VPFE_CCDC_GAMMA_BITS_09_0;
+}
+
+/*
+ * vpfe_get_ccdc_image_format - Get image parameters based on CCDC settings
+ */
+static int vpfe_get_ccdc_image_format(struct vpfe_device *vpfe,
+				      struct v4l2_format *f)
+{
+	struct v4l2_rect image_win;
+	enum ccdc_buftype buf_type;
+	enum ccdc_frmfmt frm_fmt;
+
+	memset(f, 0, sizeof(*f));
+	f->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	vpfe_ccdc_get_image_window(&vpfe->ccdc, &image_win);
+	f->fmt.pix.width = image_win.width;
+	f->fmt.pix.height = image_win.height;
+	f->fmt.pix.bytesperline = vpfe_ccdc_get_line_length(&vpfe->ccdc);
+	f->fmt.pix.sizeimage = f->fmt.pix.bytesperline *
+				f->fmt.pix.height;
+	buf_type = vpfe_ccdc_get_buftype(&vpfe->ccdc);
+	f->fmt.pix.pixelformat = vpfe_ccdc_get_pixel_format(&vpfe->ccdc);
+	frm_fmt = vpfe_ccdc_get_frame_format(&vpfe->ccdc);
+
+	if (frm_fmt == CCDC_FRMFMT_PROGRESSIVE) {
+		f->fmt.pix.field = V4L2_FIELD_NONE;
+	} else if (frm_fmt == CCDC_FRMFMT_INTERLACED) {
+		if (buf_type == CCDC_BUFTYPE_FLD_INTERLEAVED) {
+			f->fmt.pix.field = V4L2_FIELD_INTERLACED;
+		 } else if (buf_type == CCDC_BUFTYPE_FLD_SEPARATED) {
+			f->fmt.pix.field = V4L2_FIELD_SEQ_TB;
+		} else {
+			vpfe_err(vpfe, "Invalid buf_type\n");
+			return -EINVAL;
+		}
+	} else {
+		vpfe_err(vpfe, "Invalid frm_fmt\n");
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int vpfe_config_ccdc_image_format(struct vpfe_device *vpfe)
+{
+	enum ccdc_frmfmt frm_fmt = CCDC_FRMFMT_INTERLACED;
+	int ret;
+
+	vpfe_dbg(2, vpfe, "vpfe_config_ccdc_image_format\n");
+
+	vpfe_dbg(1, vpfe, "pixelformat: %s\n",
+		print_fourcc(vpfe->fmt.fmt.pix.pixelformat));
+
+	if (vpfe_ccdc_set_pixel_format(&vpfe->ccdc,
+			vpfe->fmt.fmt.pix.pixelformat) < 0) {
+		vpfe_err(vpfe, "couldn't set pix format in ccdc\n");
+		return -EINVAL;
+	}
+
+	/* configure the image window */
+	vpfe_ccdc_set_image_window(&vpfe->ccdc, &vpfe->crop, vpfe->bpp);
+
+	switch (vpfe->fmt.fmt.pix.field) {
+	case V4L2_FIELD_INTERLACED:
+		/* do nothing, since it is default */
+		ret = vpfe_ccdc_set_buftype(
+				&vpfe->ccdc,
+				CCDC_BUFTYPE_FLD_INTERLEAVED);
+		break;
+
+	case V4L2_FIELD_NONE:
+		frm_fmt = CCDC_FRMFMT_PROGRESSIVE;
+		/* buffer type only applicable for interlaced scan */
+		break;
+
+	case V4L2_FIELD_SEQ_TB:
+		ret = vpfe_ccdc_set_buftype(
+				&vpfe->ccdc,
+				CCDC_BUFTYPE_FLD_SEPARATED);
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	if (ret)
+		return ret;
+
+	return vpfe_ccdc_set_frame_format(&vpfe->ccdc, frm_fmt);
+}
+
+/*
+ * vpfe_config_image_format()
+ * For a given standard, this functions sets up the default
+ * pix format & crop values in the vpfe device and ccdc.  It first
+ * starts with defaults based values from the standard table.
+ * It then checks if sub device supports get_fmt and then override the
+ * values based on that.Sets crop values to match with scan resolution
+ * starting at 0,0. It calls vpfe_config_ccdc_image_format() set the
+ * values in ccdc
+ */
+static int vpfe_config_image_format(struct vpfe_device *vpfe,
+				    v4l2_std_id std_id)
+{
+	struct v4l2_pix_format *pix = &vpfe->fmt.fmt.pix;
+	int i, ret;
+
+	for (i = 0; i < ARRAY_SIZE(vpfe_standards); i++) {
+		if (vpfe_standards[i].std_id & std_id) {
+			vpfe->std_info.active_pixels =
+					vpfe_standards[i].width;
+			vpfe->std_info.active_lines =
+					vpfe_standards[i].height;
+			vpfe->std_info.frame_format =
+					vpfe_standards[i].frame_format;
+			vpfe->std_index = i;
+
+			break;
+		}
+	}
+
+	if (i ==  ARRAY_SIZE(vpfe_standards)) {
+		vpfe_err(vpfe, "standard not supported\n");
+		return -EINVAL;
+	}
+
+	vpfe->crop.top = vpfe->crop.left = 0;
+	vpfe->crop.width = vpfe->std_info.active_pixels;
+	vpfe->crop.height = vpfe->std_info.active_lines;
+	pix->width = vpfe->crop.width;
+	pix->height = vpfe->crop.height;
+	pix->pixelformat = V4L2_PIX_FMT_YUYV;
+
+	/* first field and frame format based on standard frame format */
+	if (vpfe->std_info.frame_format)
+		pix->field = V4L2_FIELD_INTERLACED;
+	else
+		pix->field = V4L2_FIELD_NONE;
+
+	ret = __vpfe_get_format(vpfe, &vpfe->fmt, &vpfe->bpp);
+	if (ret)
+		return ret;
+
+	/* Update the crop window based on found values */
+	vpfe->crop.width = pix->width;
+	vpfe->crop.height = pix->height;
+
+	return vpfe_config_ccdc_image_format(vpfe);
+}
+
+static int vpfe_initialize_device(struct vpfe_device *vpfe)
+{
+	struct vpfe_subdev_info *sdinfo;
+	int ret;
+
+	sdinfo = &vpfe->cfg->sub_devs[0];
+	sdinfo->sd = vpfe->sd[0];
+	vpfe->current_input = 0;
+	vpfe->std_index = 0;
+	/* Configure the default format information */
+	ret = vpfe_config_image_format(vpfe,
+				       vpfe_standards[vpfe->std_index].std_id);
+	if (ret)
+		return ret;
+
+	pm_runtime_get_sync(vpfe->pdev);
+
+	vpfe_config_enable(&vpfe->ccdc, 1);
+
+	vpfe_ccdc_restore_defaults(&vpfe->ccdc);
+
+	/* Clear all VPFE interrupts */
+	vpfe_clear_intr(&vpfe->ccdc, -1);
+
+	return ret;
+}
+
+/*
+ * vpfe_release : This function is based on the vb2_fop_release
+ * helper function.
+ * It has been augmented to handle module power management,
+ * by disabling/enabling h/w module fcntl clock when necessary.
+ */
+static int vpfe_release(struct file *file)
+{
+	struct vpfe_device *vpfe = video_drvdata(file);
+	bool fh_singular;
+	int ret;
+
+	mutex_lock(&vpfe->lock);
+
+	/* Save the singular status before we call the clean-up helper */
+	fh_singular = v4l2_fh_is_singular_file(file);
+
+	/* the release helper will cleanup any on-going streaming */
+	ret = _vb2_fop_release(file, NULL);
+
+	/*
+	 * If this was the last open file.
+	 * Then de-initialize hw module.
+	 */
+	if (fh_singular)
+		vpfe_ccdc_close(&vpfe->ccdc, vpfe->pdev);
+
+	mutex_unlock(&vpfe->lock);
+
+	return ret;
+}
+
+/*
+ * vpfe_open : This function is based on the v4l2_fh_open helper function.
+ * It has been augmented to handle module power management,
+ * by disabling/enabling h/w module fcntl clock when necessary.
+ */
+static int vpfe_open(struct file *file)
+{
+	struct vpfe_device *vpfe = video_drvdata(file);
+	int ret;
+
+	mutex_lock(&vpfe->lock);
+
+	ret = v4l2_fh_open(file);
+	if (ret) {
+		vpfe_err(vpfe, "v4l2_fh_open failed\n");
+		goto unlock;
+	}
+
+	if (!v4l2_fh_is_singular_file(file))
+		goto unlock;
+
+	if (vpfe_initialize_device(vpfe)) {
+		v4l2_fh_release(file);
+		ret = -ENODEV;
+	}
+
+unlock:
+	mutex_unlock(&vpfe->lock);
+	return ret;
+}
+
+/**
+ * vpfe_schedule_next_buffer: set next buffer address for capture
+ * @vpfe : ptr to vpfe device
+ *
+ * This function will get next buffer from the dma queue and
+ * set the buffer address in the vpfe register for capture.
+ * the buffer is marked active
+ *
+ * Assumes caller is holding vpfe->dma_queue_lock already
+ */
+static inline void vpfe_schedule_next_buffer(struct vpfe_device *vpfe)
+{
+	vpfe->next_frm = list_entry(vpfe->dma_queue.next,
+				    struct vpfe_cap_buffer, list);
+	list_del(&vpfe->next_frm->list);
+
+	vpfe_set_sdr_addr(&vpfe->ccdc,
+	       vb2_dma_contig_plane_dma_addr(&vpfe->next_frm->vb.vb2_buf, 0));
+}
+
+static inline void vpfe_schedule_bottom_field(struct vpfe_device *vpfe)
+{
+	unsigned long addr;
+
+	addr = vb2_dma_contig_plane_dma_addr(&vpfe->next_frm->vb.vb2_buf, 0) +
+					vpfe->field_off;
+
+	vpfe_set_sdr_addr(&vpfe->ccdc, addr);
+}
+
+/*
+ * vpfe_process_buffer_complete: process a completed buffer
+ * @vpfe : ptr to vpfe device
+ *
+ * This function time stamp the buffer and mark it as DONE. It also
+ * wake up any process waiting on the QUEUE and set the next buffer
+ * as current
+ */
+static inline void vpfe_process_buffer_complete(struct vpfe_device *vpfe)
+{
+	v4l2_get_timestamp(&vpfe->cur_frm->vb.timestamp);
+	vpfe->cur_frm->vb.field = vpfe->fmt.fmt.pix.field;
+	vpfe->cur_frm->vb.sequence = vpfe->sequence++;
+	vb2_buffer_done(&vpfe->cur_frm->vb.vb2_buf, VB2_BUF_STATE_DONE);
+	vpfe->cur_frm = vpfe->next_frm;
+}
+
+/*
+ * vpfe_isr : ISR handler for vpfe capture (VINT0)
+ * @irq: irq number
+ * @dev_id: dev_id ptr
+ *
+ * It changes status of the captured buffer, takes next buffer from the queue
+ * and sets its address in VPFE registers
+ */
+static irqreturn_t vpfe_isr(int irq, void *dev)
+{
+	struct vpfe_device *vpfe = (struct vpfe_device *)dev;
+	enum v4l2_field field;
+	int intr_status;
+	int fid;
+
+	intr_status = vpfe_reg_read(&vpfe->ccdc, VPFE_IRQ_STS);
+
+	if (intr_status & VPFE_VDINT0) {
+		field = vpfe->fmt.fmt.pix.field;
+
+		if (field == V4L2_FIELD_NONE) {
+			/* handle progressive frame capture */
+			if (vpfe->cur_frm != vpfe->next_frm)
+				vpfe_process_buffer_complete(vpfe);
+			goto next_intr;
+		}
+
+		/* interlaced or TB capture check which field
+		   we are in hardware */
+		fid = vpfe_ccdc_getfid(&vpfe->ccdc);
+
+		/* switch the software maintained field id */
+		vpfe->field ^= 1;
+		if (fid == vpfe->field) {
+			/* we are in-sync here,continue */
+			if (fid == 0) {
+				/*
+				 * One frame is just being captured. If the
+				 * next frame is available, release the
+				 * current frame and move on
+				 */
+				if (vpfe->cur_frm != vpfe->next_frm)
+					vpfe_process_buffer_complete(vpfe);
+				/*
+				 * based on whether the two fields are stored
+				 * interleave or separately in memory,
+				 * reconfigure the CCDC memory address
+				 */
+				if (field == V4L2_FIELD_SEQ_TB)
+					vpfe_schedule_bottom_field(vpfe);
+
+				goto next_intr;
+			}
+			/*
+			 * if one field is just being captured configure
+			 * the next frame get the next frame from the empty
+			 * queue if no frame is available hold on to the
+			 * current buffer
+			 */
+			spin_lock(&vpfe->dma_queue_lock);
+			if (!list_empty(&vpfe->dma_queue) &&
+			    vpfe->cur_frm == vpfe->next_frm)
+				vpfe_schedule_next_buffer(vpfe);
+			spin_unlock(&vpfe->dma_queue_lock);
+		} else if (fid == 0) {
+			/*
+			 * out of sync. Recover from any hardware out-of-sync.
+			 * May loose one frame
+			 */
+			vpfe->field = fid;
+		}
+	}
+
+next_intr:
+	if (intr_status & VPFE_VDINT1) {
+		spin_lock(&vpfe->dma_queue_lock);
+		if (vpfe->fmt.fmt.pix.field == V4L2_FIELD_NONE &&
+		    !list_empty(&vpfe->dma_queue) &&
+		    vpfe->cur_frm == vpfe->next_frm)
+			vpfe_schedule_next_buffer(vpfe);
+		spin_unlock(&vpfe->dma_queue_lock);
+	}
+
+	vpfe_clear_intr(&vpfe->ccdc, intr_status);
+
+	return IRQ_HANDLED;
+}
+
+static inline void vpfe_detach_irq(struct vpfe_device *vpfe)
+{
+	unsigned int intr = VPFE_VDINT0;
+	enum ccdc_frmfmt frame_format;
+
+	frame_format = vpfe_ccdc_get_frame_format(&vpfe->ccdc);
+	if (frame_format == CCDC_FRMFMT_PROGRESSIVE)
+		intr |= VPFE_VDINT1;
+
+	vpfe_reg_write(&vpfe->ccdc, intr, VPFE_IRQ_EN_CLR);
+}
+
+static inline void vpfe_attach_irq(struct vpfe_device *vpfe)
+{
+	unsigned int intr = VPFE_VDINT0;
+	enum ccdc_frmfmt frame_format;
+
+	frame_format = vpfe_ccdc_get_frame_format(&vpfe->ccdc);
+	if (frame_format == CCDC_FRMFMT_PROGRESSIVE)
+		intr |= VPFE_VDINT1;
+
+	vpfe_reg_write(&vpfe->ccdc, intr, VPFE_IRQ_EN_SET);
+}
+
+static int vpfe_querycap(struct file *file, void  *priv,
+			 struct v4l2_capability *cap)
+{
+	struct vpfe_device *vpfe = video_drvdata(file);
+
+	vpfe_dbg(2, vpfe, "vpfe_querycap\n");
+
+	strlcpy(cap->driver, VPFE_MODULE_NAME, sizeof(cap->driver));
+	strlcpy(cap->card, "TI AM437x VPFE", sizeof(cap->card));
+	snprintf(cap->bus_info, sizeof(cap->bus_info),
+			"platform:%s", vpfe->v4l2_dev.name);
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING |
+			    V4L2_CAP_READWRITE;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+
+	return 0;
+}
+
+/* get the format set at output pad of the adjacent subdev */
+static int __vpfe_get_format(struct vpfe_device *vpfe,
+			     struct v4l2_format *format, unsigned int *bpp)
+{
+	struct v4l2_mbus_framefmt mbus_fmt;
+	struct vpfe_subdev_info *sdinfo;
+	struct v4l2_subdev_format fmt;
+	int ret;
+
+	sdinfo = vpfe->current_subdev;
+	if (!sdinfo->sd)
+		return -EINVAL;
+
+	fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	fmt.pad = 0;
+
+	ret = v4l2_subdev_call(sdinfo->sd, pad, get_fmt, NULL, &fmt);
+	if (ret && ret != -ENOIOCTLCMD && ret != -ENODEV)
+		return ret;
+
+	if (!ret) {
+		v4l2_fill_pix_format(&format->fmt.pix, &fmt.format);
+		mbus_to_pix(vpfe, &fmt.format, &format->fmt.pix, bpp);
+	} else {
+		ret = v4l2_device_call_until_err(&vpfe->v4l2_dev,
+						 sdinfo->grp_id,
+						 pad, get_fmt,
+						 NULL, &fmt);
+		if (ret && ret != -ENOIOCTLCMD && ret != -ENODEV)
+			return ret;
+		v4l2_fill_pix_format(&format->fmt.pix, &mbus_fmt);
+		mbus_to_pix(vpfe, &mbus_fmt, &format->fmt.pix, bpp);
+	}
+
+	format->type = vpfe->fmt.type;
+
+	vpfe_dbg(1, vpfe,
+		 "%s size %dx%d (%s) bytesperline = %d, size = %d, bpp = %d\n",
+		 __func__, format->fmt.pix.width, format->fmt.pix.height,
+		 print_fourcc(format->fmt.pix.pixelformat),
+		 format->fmt.pix.bytesperline, format->fmt.pix.sizeimage, *bpp);
+
+	return 0;
+}
+
+/* set the format at output pad of the adjacent subdev */
+static int __vpfe_set_format(struct vpfe_device *vpfe,
+			     struct v4l2_format *format, unsigned int *bpp)
+{
+	struct vpfe_subdev_info *sdinfo;
+	struct v4l2_subdev_format fmt;
+	int ret;
+
+	vpfe_dbg(2, vpfe, "__vpfe_set_format\n");
+
+	sdinfo = vpfe->current_subdev;
+	if (!sdinfo->sd)
+		return -EINVAL;
+
+	fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	fmt.pad = 0;
+
+	pix_to_mbus(vpfe, &format->fmt.pix, &fmt.format);
+
+	ret = v4l2_subdev_call(sdinfo->sd, pad, set_fmt, NULL, &fmt);
+	if (ret)
+		return ret;
+
+	v4l2_fill_pix_format(&format->fmt.pix, &fmt.format);
+	mbus_to_pix(vpfe, &fmt.format, &format->fmt.pix, bpp);
+
+	format->type = vpfe->fmt.type;
+
+	vpfe_dbg(1, vpfe,
+		 "%s size %dx%d (%s) bytesperline = %d, size = %d, bpp = %d\n",
+		 __func__,  format->fmt.pix.width, format->fmt.pix.height,
+		 print_fourcc(format->fmt.pix.pixelformat),
+		 format->fmt.pix.bytesperline, format->fmt.pix.sizeimage, *bpp);
+
+	return 0;
+}
+
+static int vpfe_g_fmt(struct file *file, void *priv,
+		      struct v4l2_format *fmt)
+{
+	struct vpfe_device *vpfe = video_drvdata(file);
+
+	vpfe_dbg(2, vpfe, "vpfe_g_fmt\n");
+
+	*fmt = vpfe->fmt;
+
+	return 0;
+}
+
+static int vpfe_enum_fmt(struct file *file, void  *priv,
+			 struct v4l2_fmtdesc *f)
+{
+	struct vpfe_device *vpfe = video_drvdata(file);
+	struct vpfe_subdev_info *sdinfo;
+	struct vpfe_fmt *fmt = NULL;
+	unsigned int k;
+
+	vpfe_dbg(2, vpfe, "vpfe_enum_format index:%d\n",
+		f->index);
+
+	sdinfo = vpfe->current_subdev;
+	if (!sdinfo->sd)
+		return -EINVAL;
+
+	if (f->index > ARRAY_SIZE(formats))
+		return -EINVAL;
+
+	for (k = 0; k < ARRAY_SIZE(formats); k++) {
+		if (formats[k].index == f->index) {
+			fmt = &formats[k];
+			break;
+		}
+	}
+	if (!fmt)
+		return -EINVAL;
+
+	strncpy(f->description, fmt->name, sizeof(f->description) - 1);
+	f->pixelformat = fmt->fourcc;
+	f->type = vpfe->fmt.type;
+
+	vpfe_dbg(1, vpfe, "vpfe_enum_format: mbus index: %d code: %x pixelformat: %s [%s]\n",
+		f->index, fmt->code, print_fourcc(fmt->fourcc), fmt->name);
+
+	return 0;
+}
+
+static int vpfe_try_fmt(struct file *file, void *priv,
+			struct v4l2_format *fmt)
+{
+	struct vpfe_device *vpfe = video_drvdata(file);
+	unsigned int bpp;
+
+	vpfe_dbg(2, vpfe, "vpfe_try_fmt\n");
+
+	return __vpfe_get_format(vpfe, fmt, &bpp);
+}
+
+static int vpfe_s_fmt(struct file *file, void *priv,
+		      struct v4l2_format *fmt)
+{
+	struct vpfe_device *vpfe = video_drvdata(file);
+	struct v4l2_format format;
+	unsigned int bpp;
+	int ret;
+
+	vpfe_dbg(2, vpfe, "vpfe_s_fmt\n");
+
+	/* If streaming is started, return error */
+	if (vb2_is_busy(&vpfe->buffer_queue)) {
+		vpfe_err(vpfe, "%s device busy\n", __func__);
+		return -EBUSY;
+	}
+
+	ret = __vpfe_get_format(vpfe, &format, &bpp);
+	if (ret)
+		return ret;
+
+
+	if (!cmp_v4l2_format(fmt, &format)) {
+		/* Sensor format is different from the requested format
+		 * so we need to change it
+		 */
+		ret = __vpfe_set_format(vpfe, fmt, &bpp);
+		if (ret)
+			return ret;
+	} else /* Just make sure all of the fields are consistent */
+		*fmt = format;
+
+	/* First detach any IRQ if currently attached */
+	vpfe_detach_irq(vpfe);
+	vpfe->fmt = *fmt;
+	vpfe->bpp = bpp;
+
+	/* Update the crop window based on found values */
+	vpfe->crop.width = fmt->fmt.pix.width;
+	vpfe->crop.height = fmt->fmt.pix.height;
+
+	/* set image capture parameters in the ccdc */
+	return vpfe_config_ccdc_image_format(vpfe);
+}
+
+static int vpfe_enum_size(struct file *file, void  *priv,
+			  struct v4l2_frmsizeenum *fsize)
+{
+	struct vpfe_device *vpfe = video_drvdata(file);
+	struct v4l2_subdev_frame_size_enum fse;
+	struct vpfe_subdev_info *sdinfo;
+	struct v4l2_mbus_framefmt mbus;
+	struct v4l2_pix_format pix;
+	struct vpfe_fmt *fmt;
+	int ret;
+
+	vpfe_dbg(2, vpfe, "vpfe_enum_size\n");
+
+	/* check for valid format */
+	fmt = find_format_by_pix(fsize->pixel_format);
+	if (!fmt) {
+		vpfe_dbg(3, vpfe, "Invalid pixel code: %x, default used instead\n",
+			fsize->pixel_format);
+		return -EINVAL;
+	}
+
+	memset(fsize->reserved, 0x0, sizeof(fsize->reserved));
+
+	sdinfo = vpfe->current_subdev;
+	if (!sdinfo->sd)
+		return -EINVAL;
+
+	memset(&pix, 0x0, sizeof(pix));
+	/* Construct pix from parameter and use default for the rest */
+	pix.pixelformat = fsize->pixel_format;
+	pix.width = 640;
+	pix.height = 480;
+	pix.colorspace = V4L2_COLORSPACE_SRGB;
+	pix.field = V4L2_FIELD_NONE;
+	pix_to_mbus(vpfe, &pix, &mbus);
+
+	memset(&fse, 0x0, sizeof(fse));
+	fse.index = fsize->index;
+	fse.pad = 0;
+	fse.code = mbus.code;
+	fse.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	ret = v4l2_subdev_call(sdinfo->sd, pad, enum_frame_size, NULL, &fse);
+	if (ret)
+		return -EINVAL;
+
+	vpfe_dbg(1, vpfe, "vpfe_enum_size: index: %d code: %x W:[%d,%d] H:[%d,%d]\n",
+		fse.index, fse.code, fse.min_width, fse.max_width,
+		fse.min_height, fse.max_height);
+
+	fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+	fsize->discrete.width = fse.max_width;
+	fsize->discrete.height = fse.max_height;
+
+	vpfe_dbg(1, vpfe, "vpfe_enum_size: index: %d pixformat: %s size: %dx%d\n",
+		fsize->index, print_fourcc(fsize->pixel_format),
+		fsize->discrete.width, fsize->discrete.height);
+
+	return 0;
+}
+
+/*
+ * vpfe_get_subdev_input_index - Get subdev index and subdev input index for a
+ * given app input index
+ */
+static int
+vpfe_get_subdev_input_index(struct vpfe_device *vpfe,
+			    int *subdev_index,
+			    int *subdev_input_index,
+			    int app_input_index)
+{
+	int i, j = 0;
+
+	for (i = 0; i < ARRAY_SIZE(vpfe->cfg->asd); i++) {
+		if (app_input_index < (j + 1)) {
+			*subdev_index = i;
+			*subdev_input_index = app_input_index - j;
+			return 0;
+		}
+		j++;
+	}
+	return -EINVAL;
+}
+
+/*
+ * vpfe_get_app_input - Get app input index for a given subdev input index
+ * driver stores the input index of the current sub device and translate it
+ * when application request the current input
+ */
+static int vpfe_get_app_input_index(struct vpfe_device *vpfe,
+				    int *app_input_index)
+{
+	struct vpfe_config *cfg = vpfe->cfg;
+	struct vpfe_subdev_info *sdinfo;
+	struct i2c_client *client;
+	struct i2c_client *curr_client;
+	int i, j = 0;
+
+	curr_client = v4l2_get_subdevdata(vpfe->current_subdev->sd);
+	for (i = 0; i < ARRAY_SIZE(vpfe->cfg->asd); i++) {
+		sdinfo = &cfg->sub_devs[i];
+		client = v4l2_get_subdevdata(sdinfo->sd);
+		if (client->addr == curr_client->addr &&
+		    client->adapter->nr == curr_client->adapter->nr) {
+			if (vpfe->current_input >= 1)
+				return -1;
+			*app_input_index = j + vpfe->current_input;
+			return 0;
+		}
+		j++;
+	}
+	return -EINVAL;
+}
+
+static int vpfe_enum_input(struct file *file, void *priv,
+			   struct v4l2_input *inp)
+{
+	struct vpfe_device *vpfe = video_drvdata(file);
+	struct vpfe_subdev_info *sdinfo;
+	int subdev, index;
+
+	vpfe_dbg(2, vpfe, "vpfe_enum_input\n");
+
+	if (vpfe_get_subdev_input_index(vpfe, &subdev, &index,
+					inp->index) < 0) {
+		vpfe_dbg(1, vpfe,
+			"input information not found for the subdev\n");
+		return -EINVAL;
+	}
+	sdinfo = &vpfe->cfg->sub_devs[subdev];
+	*inp = sdinfo->inputs[index];
+
+	return 0;
+}
+
+static int vpfe_g_input(struct file *file, void *priv, unsigned int *index)
+{
+	struct vpfe_device *vpfe = video_drvdata(file);
+
+	vpfe_dbg(2, vpfe, "vpfe_g_input\n");
+
+	return vpfe_get_app_input_index(vpfe, index);
+}
+
+/* Assumes caller is holding vpfe_dev->lock */
+static int vpfe_set_input(struct vpfe_device *vpfe, unsigned int index)
+{
+	int subdev_index = 0, inp_index = 0;
+	struct vpfe_subdev_info *sdinfo;
+	struct vpfe_route *route;
+	u32 input, output;
+	int ret;
+
+	vpfe_dbg(2, vpfe, "vpfe_set_input: index: %d\n", index);
+
+	/* If streaming is started, return error */
+	if (vb2_is_busy(&vpfe->buffer_queue)) {
+		vpfe_err(vpfe, "%s device busy\n", __func__);
+		return -EBUSY;
+	}
+	ret = vpfe_get_subdev_input_index(vpfe,
+					  &subdev_index,
+					  &inp_index,
+					  index);
+	if (ret < 0) {
+		vpfe_err(vpfe, "invalid input index: %d\n", index);
+		goto get_out;
+	}
+
+	sdinfo = &vpfe->cfg->sub_devs[subdev_index];
+	sdinfo->sd = vpfe->sd[subdev_index];
+	route = &sdinfo->routes[inp_index];
+	if (route && sdinfo->can_route) {
+		input = route->input;
+		output = route->output;
+		if (sdinfo->sd) {
+			ret = v4l2_subdev_call(sdinfo->sd, video,
+					s_routing, input, output, 0);
+			if (ret) {
+				vpfe_err(vpfe, "s_routing failed\n");
+				ret = -EINVAL;
+				goto get_out;
+			}
+		}
+
+	}
+
+	vpfe->current_subdev = sdinfo;
+	if (sdinfo->sd)
+		vpfe->v4l2_dev.ctrl_handler = sdinfo->sd->ctrl_handler;
+	vpfe->current_input = index;
+	vpfe->std_index = 0;
+
+	/* set the bus/interface parameter for the sub device in ccdc */
+	ret = vpfe_ccdc_set_hw_if_params(&vpfe->ccdc, &sdinfo->vpfe_param);
+	if (ret)
+		return ret;
+
+	/* set the default image parameters in the device */
+	return vpfe_config_image_format(vpfe,
+					vpfe_standards[vpfe->std_index].std_id);
+
+get_out:
+	return ret;
+}
+
+static int vpfe_s_input(struct file *file, void *priv, unsigned int index)
+{
+	struct vpfe_device *vpfe = video_drvdata(file);
+
+	vpfe_dbg(2, vpfe,
+		"vpfe_s_input: index: %d\n", index);
+
+	return vpfe_set_input(vpfe, index);
+}
+
+static int vpfe_querystd(struct file *file, void *priv, v4l2_std_id *std_id)
+{
+	struct vpfe_device *vpfe = video_drvdata(file);
+	struct vpfe_subdev_info *sdinfo;
+
+	vpfe_dbg(2, vpfe, "vpfe_querystd\n");
+
+	sdinfo = vpfe->current_subdev;
+	if (!(sdinfo->inputs[0].capabilities & V4L2_IN_CAP_STD))
+		return -ENODATA;
+
+	/* Call querystd function of decoder device */
+	return v4l2_device_call_until_err(&vpfe->v4l2_dev, sdinfo->grp_id,
+					 video, querystd, std_id);
+}
+
+static int vpfe_s_std(struct file *file, void *priv, v4l2_std_id std_id)
+{
+	struct vpfe_device *vpfe = video_drvdata(file);
+	struct vpfe_subdev_info *sdinfo;
+	int ret;
+
+	vpfe_dbg(2, vpfe, "vpfe_s_std\n");
+
+	sdinfo = vpfe->current_subdev;
+	if (!(sdinfo->inputs[0].capabilities & V4L2_IN_CAP_STD))
+		return -ENODATA;
+
+	/* If streaming is started, return error */
+	if (vb2_is_busy(&vpfe->buffer_queue)) {
+		vpfe_err(vpfe, "%s device busy\n", __func__);
+		ret = -EBUSY;
+		return ret;
+	}
+
+	ret = v4l2_device_call_until_err(&vpfe->v4l2_dev, sdinfo->grp_id,
+					 video, s_std, std_id);
+	if (ret < 0) {
+		vpfe_err(vpfe, "Failed to set standard\n");
+		return ret;
+	}
+	ret = vpfe_config_image_format(vpfe, std_id);
+
+	return ret;
+}
+
+static int vpfe_g_std(struct file *file, void *priv, v4l2_std_id *std_id)
+{
+	struct vpfe_device *vpfe = video_drvdata(file);
+	struct vpfe_subdev_info *sdinfo;
+
+	vpfe_dbg(2, vpfe, "vpfe_g_std\n");
+
+	sdinfo = vpfe->current_subdev;
+	if (sdinfo->inputs[0].capabilities != V4L2_IN_CAP_STD)
+		return -ENODATA;
+
+	*std_id = vpfe_standards[vpfe->std_index].std_id;
+
+	return 0;
+}
+
+/*
+ * vpfe_calculate_offsets : This function calculates buffers offset
+ * for top and bottom field
+ */
+static void vpfe_calculate_offsets(struct vpfe_device *vpfe)
+{
+	struct v4l2_rect image_win;
+
+	vpfe_dbg(2, vpfe, "vpfe_calculate_offsets\n");
+
+	vpfe_ccdc_get_image_window(&vpfe->ccdc, &image_win);
+	vpfe->field_off = image_win.height * image_win.width;
+}
+
+/*
+ * vpfe_queue_setup - Callback function for buffer setup.
+ * @vq: vb2_queue ptr
+ * @fmt: v4l2 format
+ * @nbuffers: ptr to number of buffers requested by application
+ * @nplanes:: contains number of distinct video planes needed to hold a frame
+ * @sizes[]: contains the size (in bytes) of each plane.
+ * @alloc_ctxs: ptr to allocation context
+ *
+ * This callback function is called when reqbuf() is called to adjust
+ * the buffer count and buffer size
+ */
+static int vpfe_queue_setup(struct vb2_queue *vq,
+			    const void *parg,
+			    unsigned int *nbuffers, unsigned int *nplanes,
+			    unsigned int sizes[], void *alloc_ctxs[])
+{
+	const struct v4l2_format *fmt = parg;
+	struct vpfe_device *vpfe = vb2_get_drv_priv(vq);
+
+	if (fmt && fmt->fmt.pix.sizeimage < vpfe->fmt.fmt.pix.sizeimage)
+		return -EINVAL;
+
+	if (vq->num_buffers + *nbuffers < 3)
+		*nbuffers = 3 - vq->num_buffers;
+
+	*nplanes = 1;
+	sizes[0] = fmt ? fmt->fmt.pix.sizeimage : vpfe->fmt.fmt.pix.sizeimage;
+	alloc_ctxs[0] = vpfe->alloc_ctx;
+
+	vpfe_dbg(1, vpfe,
+		"nbuffers=%d, size=%u\n", *nbuffers, sizes[0]);
+
+	/* Calculate field offset */
+	vpfe_calculate_offsets(vpfe);
+
+	return 0;
+}
+
+/*
+ * vpfe_buffer_prepare :  callback function for buffer prepare
+ * @vb: ptr to vb2_buffer
+ *
+ * This is the callback function for buffer prepare when vb2_qbuf()
+ * function is called. The buffer is prepared and user space virtual address
+ * or user address is converted into  physical address
+ */
+static int vpfe_buffer_prepare(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct vpfe_device *vpfe = vb2_get_drv_priv(vb->vb2_queue);
+
+	vb2_set_plane_payload(vb, 0, vpfe->fmt.fmt.pix.sizeimage);
+
+	if (vb2_get_plane_payload(vb, 0) > vb2_plane_size(vb, 0))
+		return -EINVAL;
+
+	vbuf->field = vpfe->fmt.fmt.pix.field;
+
+	return 0;
+}
+
+/*
+ * vpfe_buffer_queue : Callback function to add buffer to DMA queue
+ * @vb: ptr to vb2_buffer
+ */
+static void vpfe_buffer_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct vpfe_device *vpfe = vb2_get_drv_priv(vb->vb2_queue);
+	struct vpfe_cap_buffer *buf = to_vpfe_buffer(vbuf);
+	unsigned long flags = 0;
+
+	/* add the buffer to the DMA queue */
+	spin_lock_irqsave(&vpfe->dma_queue_lock, flags);
+	list_add_tail(&buf->list, &vpfe->dma_queue);
+	spin_unlock_irqrestore(&vpfe->dma_queue_lock, flags);
+}
+
+/*
+ * vpfe_start_streaming : Starts the DMA engine for streaming
+ * @vb: ptr to vb2_buffer
+ * @count: number of buffers
+ */
+static int vpfe_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct vpfe_device *vpfe = vb2_get_drv_priv(vq);
+	struct vpfe_cap_buffer *buf, *tmp;
+	struct vpfe_subdev_info *sdinfo;
+	unsigned long flags;
+	unsigned long addr;
+	int ret;
+
+	spin_lock_irqsave(&vpfe->dma_queue_lock, flags);
+
+	vpfe->field = 0;
+	vpfe->sequence = 0;
+
+	sdinfo = vpfe->current_subdev;
+
+	vpfe_attach_irq(vpfe);
+
+	if (vpfe->ccdc.ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		vpfe_ccdc_config_raw(&vpfe->ccdc);
+	else
+		vpfe_ccdc_config_ycbcr(&vpfe->ccdc);
+
+	/* Get the next frame from the buffer queue */
+	vpfe->next_frm = list_entry(vpfe->dma_queue.next,
+				    struct vpfe_cap_buffer, list);
+	vpfe->cur_frm = vpfe->next_frm;
+	/* Remove buffer from the buffer queue */
+	list_del(&vpfe->cur_frm->list);
+	spin_unlock_irqrestore(&vpfe->dma_queue_lock, flags);
+
+	addr = vb2_dma_contig_plane_dma_addr(&vpfe->cur_frm->vb.vb2_buf, 0);
+
+	vpfe_set_sdr_addr(&vpfe->ccdc, (unsigned long)(addr));
+
+	vpfe_pcr_enable(&vpfe->ccdc, 1);
+
+	ret = v4l2_subdev_call(sdinfo->sd, video, s_stream, 1);
+	if (ret < 0) {
+		vpfe_err(vpfe, "Error in attaching interrupt handle\n");
+		goto err;
+	}
+
+	return 0;
+
+err:
+	list_for_each_entry_safe(buf, tmp, &vpfe->dma_queue, list) {
+		list_del(&buf->list);
+		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_QUEUED);
+	}
+
+	return ret;
+}
+
+/*
+ * vpfe_stop_streaming : Stop the DMA engine
+ * @vq: ptr to vb2_queue
+ *
+ * This callback stops the DMA engine and any remaining buffers
+ * in the DMA queue are released.
+ */
+static void vpfe_stop_streaming(struct vb2_queue *vq)
+{
+	struct vpfe_device *vpfe = vb2_get_drv_priv(vq);
+	struct vpfe_subdev_info *sdinfo;
+	unsigned long flags;
+	int ret;
+
+	vpfe_pcr_enable(&vpfe->ccdc, 0);
+
+	vpfe_detach_irq(vpfe);
+
+	sdinfo = vpfe->current_subdev;
+	ret = v4l2_subdev_call(sdinfo->sd, video, s_stream, 0);
+	if (ret && ret != -ENOIOCTLCMD && ret != -ENODEV)
+		vpfe_dbg(1, vpfe, "stream off failed in subdev\n");
+
+	/* release all active buffers */
+	spin_lock_irqsave(&vpfe->dma_queue_lock, flags);
+	if (vpfe->cur_frm == vpfe->next_frm) {
+		vb2_buffer_done(&vpfe->cur_frm->vb.vb2_buf,
+				VB2_BUF_STATE_ERROR);
+	} else {
+		if (vpfe->cur_frm != NULL)
+			vb2_buffer_done(&vpfe->cur_frm->vb.vb2_buf,
+					VB2_BUF_STATE_ERROR);
+		if (vpfe->next_frm != NULL)
+			vb2_buffer_done(&vpfe->next_frm->vb.vb2_buf,
+					VB2_BUF_STATE_ERROR);
+	}
+
+	while (!list_empty(&vpfe->dma_queue)) {
+		vpfe->next_frm = list_entry(vpfe->dma_queue.next,
+						struct vpfe_cap_buffer, list);
+		list_del(&vpfe->next_frm->list);
+		vb2_buffer_done(&vpfe->next_frm->vb.vb2_buf,
+				VB2_BUF_STATE_ERROR);
+	}
+	spin_unlock_irqrestore(&vpfe->dma_queue_lock, flags);
+}
+
+static int vpfe_cropcap(struct file *file, void *priv,
+			struct v4l2_cropcap *crop)
+{
+	struct vpfe_device *vpfe = video_drvdata(file);
+
+	vpfe_dbg(2, vpfe, "vpfe_cropcap\n");
+
+	if (vpfe->std_index >= ARRAY_SIZE(vpfe_standards))
+		return -EINVAL;
+
+	memset(crop, 0, sizeof(struct v4l2_cropcap));
+
+	crop->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	crop->defrect.width = vpfe_standards[vpfe->std_index].width;
+	crop->bounds.width = crop->defrect.width;
+	crop->defrect.height = vpfe_standards[vpfe->std_index].height;
+	crop->bounds.height = crop->defrect.height;
+	crop->pixelaspect = vpfe_standards[vpfe->std_index].pixelaspect;
+
+	return 0;
+}
+
+static int
+vpfe_g_selection(struct file *file, void *fh, struct v4l2_selection *s)
+{
+	struct vpfe_device *vpfe = video_drvdata(file);
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		s->r.left = s->r.top = 0;
+		s->r.width = vpfe->crop.width;
+		s->r.height = vpfe->crop.height;
+		break;
+
+	case V4L2_SEL_TGT_CROP:
+		s->r = vpfe->crop;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int enclosed_rectangle(struct v4l2_rect *a, struct v4l2_rect *b)
+{
+	if (a->left < b->left || a->top < b->top)
+		return 0;
+
+	if (a->left + a->width > b->left + b->width)
+		return 0;
+
+	if (a->top + a->height > b->top + b->height)
+		return 0;
+
+	return 1;
+}
+
+static int
+vpfe_s_selection(struct file *file, void *fh, struct v4l2_selection *s)
+{
+	struct vpfe_device *vpfe = video_drvdata(file);
+	struct v4l2_rect cr = vpfe->crop;
+	struct v4l2_rect r = s->r;
+
+	/* If streaming is started, return error */
+	if (vb2_is_busy(&vpfe->buffer_queue)) {
+		vpfe_err(vpfe, "%s device busy\n", __func__);
+		return -EBUSY;
+	}
+
+	if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
+			s->target != V4L2_SEL_TGT_CROP)
+		return -EINVAL;
+
+	v4l_bound_align_image(&r.width, 0, cr.width, 0,
+			      &r.height, 0, cr.height, 0, 0);
+
+	r.left = clamp_t(unsigned int, r.left, 0, cr.width - r.width);
+	r.top  = clamp_t(unsigned int, r.top, 0, cr.height - r.height);
+
+	if (s->flags & V4L2_SEL_FLAG_LE && !enclosed_rectangle(&r, &s->r))
+		return -ERANGE;
+
+	if (s->flags & V4L2_SEL_FLAG_GE && !enclosed_rectangle(&s->r, &r))
+		return -ERANGE;
+
+	s->r = vpfe->crop = r;
+
+	vpfe_ccdc_set_image_window(&vpfe->ccdc, &r, vpfe->bpp);
+	vpfe->fmt.fmt.pix.width = r.width;
+	vpfe->fmt.fmt.pix.height = r.height;
+	vpfe->fmt.fmt.pix.bytesperline = vpfe_ccdc_get_line_length(&vpfe->ccdc);
+	vpfe->fmt.fmt.pix.sizeimage = vpfe->fmt.fmt.pix.bytesperline *
+						vpfe->fmt.fmt.pix.height;
+
+	vpfe_dbg(1, vpfe, "cropped (%d,%d)/%dx%d of %dx%d\n",
+		 r.left, r.top, r.width, r.height, cr.width, cr.height);
+
+	return 0;
+}
+
+static long vpfe_ioctl_default(struct file *file, void *priv,
+			       bool valid_prio, unsigned int cmd, void *param)
+{
+	struct vpfe_device *vpfe = video_drvdata(file);
+	int ret;
+
+	vpfe_dbg(2, vpfe, "vpfe_ioctl_default\n");
+
+	if (!valid_prio) {
+		vpfe_err(vpfe, "%s device busy\n", __func__);
+		return -EBUSY;
+	}
+
+	/* If streaming is started, return error */
+	if (vb2_is_busy(&vpfe->buffer_queue)) {
+		vpfe_err(vpfe, "%s device busy\n", __func__);
+		return -EBUSY;
+	}
+
+	switch (cmd) {
+	case VIDIOC_AM437X_CCDC_CFG:
+		ret = vpfe_ccdc_set_params(&vpfe->ccdc, (void __user *)param);
+		if (ret) {
+			vpfe_dbg(2, vpfe,
+				"Error setting parameters in CCDC\n");
+			return ret;
+		}
+		ret = vpfe_get_ccdc_image_format(vpfe,
+						 &vpfe->fmt);
+		if (ret < 0) {
+			vpfe_dbg(2, vpfe,
+				"Invalid image format at CCDC\n");
+			return ret;
+		}
+		break;
+
+	default:
+		ret = -ENOTTY;
+		break;
+	}
+
+	return ret;
+}
+
+static const struct vb2_ops vpfe_video_qops = {
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
+	.queue_setup		= vpfe_queue_setup,
+	.buf_prepare		= vpfe_buffer_prepare,
+	.buf_queue		= vpfe_buffer_queue,
+	.start_streaming	= vpfe_start_streaming,
+	.stop_streaming		= vpfe_stop_streaming,
+};
+
+/* vpfe capture driver file operations */
+static const struct v4l2_file_operations vpfe_fops = {
+	.owner		= THIS_MODULE,
+	.open		= vpfe_open,
+	.release	= vpfe_release,
+	.read		= vb2_fop_read,
+	.poll		= vb2_fop_poll,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap		= vb2_fop_mmap,
+};
+
+/* vpfe capture ioctl operations */
+static const struct v4l2_ioctl_ops vpfe_ioctl_ops = {
+	.vidioc_querycap		= vpfe_querycap,
+	.vidioc_enum_fmt_vid_cap	= vpfe_enum_fmt,
+	.vidioc_g_fmt_vid_cap		= vpfe_g_fmt,
+	.vidioc_s_fmt_vid_cap		= vpfe_s_fmt,
+	.vidioc_try_fmt_vid_cap		= vpfe_try_fmt,
+
+	.vidioc_enum_framesizes		= vpfe_enum_size,
+
+	.vidioc_enum_input		= vpfe_enum_input,
+	.vidioc_g_input			= vpfe_g_input,
+	.vidioc_s_input			= vpfe_s_input,
+
+	.vidioc_querystd		= vpfe_querystd,
+	.vidioc_s_std			= vpfe_s_std,
+	.vidioc_g_std			= vpfe_g_std,
+
+	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
+	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
+	.vidioc_prepare_buf		= vb2_ioctl_prepare_buf,
+	.vidioc_querybuf		= vb2_ioctl_querybuf,
+	.vidioc_qbuf			= vb2_ioctl_qbuf,
+	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
+	.vidioc_expbuf			= vb2_ioctl_expbuf,
+	.vidioc_streamon		= vb2_ioctl_streamon,
+	.vidioc_streamoff		= vb2_ioctl_streamoff,
+
+	.vidioc_log_status		= v4l2_ctrl_log_status,
+	.vidioc_subscribe_event		= v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
+
+	.vidioc_cropcap			= vpfe_cropcap,
+	.vidioc_g_selection		= vpfe_g_selection,
+	.vidioc_s_selection		= vpfe_s_selection,
+
+	.vidioc_default			= vpfe_ioctl_default,
+};
+
+static int
+vpfe_async_bound(struct v4l2_async_notifier *notifier,
+		 struct v4l2_subdev *subdev,
+		 struct v4l2_async_subdev *asd)
+{
+	struct vpfe_device *vpfe = container_of(notifier->v4l2_dev,
+					       struct vpfe_device, v4l2_dev);
+	struct v4l2_subdev_mbus_code_enum mbus_code;
+	struct vpfe_subdev_info *sdinfo;
+	bool found = false;
+	int i, j;
+
+	vpfe_dbg(1, vpfe, "vpfe_async_bound\n");
+
+	for (i = 0; i < ARRAY_SIZE(vpfe->cfg->asd); i++) {
+		if (vpfe->cfg->asd[i]->match.of.node == asd[i].match.of.node) {
+			sdinfo = &vpfe->cfg->sub_devs[i];
+			vpfe->sd[i] = subdev;
+			vpfe->sd[i]->grp_id = sdinfo->grp_id;
+			found = true;
+			break;
+		}
+	}
+
+	if (!found) {
+		vpfe_info(vpfe, "sub device (%s) not matched\n", subdev->name);
+		return -EINVAL;
+	}
+
+	vpfe->video_dev.tvnorms |= sdinfo->inputs[0].std;
+
+	/* setup the supported formats & indexes */
+	for (j = 0, i = 0; ; ++j) {
+		struct vpfe_fmt *fmt;
+		int ret;
+
+		memset(&mbus_code, 0, sizeof(mbus_code));
+		mbus_code.index = j;
+		mbus_code.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+		ret = v4l2_subdev_call(subdev, pad, enum_mbus_code,
+			       NULL, &mbus_code);
+		if (ret)
+			break;
+
+		fmt = find_format_by_code(mbus_code.code);
+		if (!fmt)
+			continue;
+
+		fmt->supported = true;
+		fmt->index = i++;
+	}
+
+	return 0;
+}
+
+static int vpfe_probe_complete(struct vpfe_device *vpfe)
+{
+	struct video_device *vdev;
+	struct vb2_queue *q;
+	int err;
+
+	spin_lock_init(&vpfe->dma_queue_lock);
+	mutex_init(&vpfe->lock);
+
+	vpfe->fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	/* set first sub device as current one */
+	vpfe->current_subdev = &vpfe->cfg->sub_devs[0];
+	vpfe->v4l2_dev.ctrl_handler = vpfe->sd[0]->ctrl_handler;
+
+	err = vpfe_set_input(vpfe, 0);
+	if (err)
+		goto probe_out;
+
+	/* Initialize videobuf2 queue as per the buffer type */
+	vpfe->alloc_ctx = vb2_dma_contig_init_ctx(vpfe->pdev);
+	if (IS_ERR(vpfe->alloc_ctx)) {
+		vpfe_err(vpfe, "Failed to get the context\n");
+		err = PTR_ERR(vpfe->alloc_ctx);
+		goto probe_out;
+	}
+
+	q = &vpfe->buffer_queue;
+	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	q->io_modes = VB2_MMAP | VB2_DMABUF | VB2_READ;
+	q->drv_priv = vpfe;
+	q->ops = &vpfe_video_qops;
+	q->mem_ops = &vb2_dma_contig_memops;
+	q->buf_struct_size = sizeof(struct vpfe_cap_buffer);
+	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	q->lock = &vpfe->lock;
+	q->min_buffers_needed = 1;
+
+	err = vb2_queue_init(q);
+	if (err) {
+		vpfe_err(vpfe, "vb2_queue_init() failed\n");
+		vb2_dma_contig_cleanup_ctx(vpfe->alloc_ctx);
+		goto probe_out;
+	}
+
+	INIT_LIST_HEAD(&vpfe->dma_queue);
+
+	vdev = &vpfe->video_dev;
+	strlcpy(vdev->name, VPFE_MODULE_NAME, sizeof(vdev->name));
+	vdev->release = video_device_release_empty;
+	vdev->fops = &vpfe_fops;
+	vdev->ioctl_ops = &vpfe_ioctl_ops;
+	vdev->v4l2_dev = &vpfe->v4l2_dev;
+	vdev->vfl_dir = VFL_DIR_RX;
+	vdev->queue = q;
+	vdev->lock = &vpfe->lock;
+	video_set_drvdata(vdev, vpfe);
+	err = video_register_device(&vpfe->video_dev, VFL_TYPE_GRABBER, -1);
+	if (err) {
+		vpfe_err(vpfe,
+			"Unable to register video device.\n");
+		goto probe_out;
+	}
+
+	return 0;
+
+probe_out:
+	v4l2_device_unregister(&vpfe->v4l2_dev);
+	return err;
+}
+
+static int vpfe_async_complete(struct v4l2_async_notifier *notifier)
+{
+	struct vpfe_device *vpfe = container_of(notifier->v4l2_dev,
+					struct vpfe_device, v4l2_dev);
+
+	return vpfe_probe_complete(vpfe);
+}
+
+static struct vpfe_config *
+vpfe_get_pdata(struct platform_device *pdev)
+{
+	struct device_node *endpoint = NULL;
+	struct v4l2_of_endpoint bus_cfg;
+	struct vpfe_subdev_info *sdinfo;
+	struct vpfe_config *pdata;
+	unsigned int flags;
+	unsigned int i;
+	int err;
+
+	dev_dbg(&pdev->dev, "vpfe_get_pdata\n");
+
+	if (!IS_ENABLED(CONFIG_OF) || !pdev->dev.of_node)
+		return pdev->dev.platform_data;
+
+	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return NULL;
+
+	for (i = 0; ; i++) {
+		struct device_node *rem;
+
+		endpoint = of_graph_get_next_endpoint(pdev->dev.of_node,
+						      endpoint);
+		if (!endpoint)
+			break;
+
+		sdinfo = &pdata->sub_devs[i];
+		sdinfo->grp_id = 0;
+
+		/* we only support camera */
+		sdinfo->inputs[0].index = i;
+		strcpy(sdinfo->inputs[0].name, "Camera");
+		sdinfo->inputs[0].type = V4L2_INPUT_TYPE_CAMERA;
+		sdinfo->inputs[0].std = V4L2_STD_ALL;
+		sdinfo->inputs[0].capabilities = V4L2_IN_CAP_STD;
+
+		sdinfo->can_route = 0;
+		sdinfo->routes = NULL;
+
+		of_property_read_u32(endpoint, "ti,am437x-vpfe-interface",
+				     &sdinfo->vpfe_param.if_type);
+		if (sdinfo->vpfe_param.if_type < 0 ||
+			sdinfo->vpfe_param.if_type > 4) {
+			sdinfo->vpfe_param.if_type = VPFE_RAW_BAYER;
+		}
+
+		err = v4l2_of_parse_endpoint(endpoint, &bus_cfg);
+		if (err) {
+			dev_err(&pdev->dev, "Could not parse the endpoint\n");
+			goto done;
+		}
+
+		sdinfo->vpfe_param.bus_width = bus_cfg.bus.parallel.bus_width;
+
+		if (sdinfo->vpfe_param.bus_width < 8 ||
+			sdinfo->vpfe_param.bus_width > 16) {
+			dev_err(&pdev->dev, "Invalid bus width.\n");
+			goto done;
+		}
+
+		flags = bus_cfg.bus.parallel.flags;
+
+		if (flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
+			sdinfo->vpfe_param.hdpol = 1;
+
+		if (flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH)
+			sdinfo->vpfe_param.vdpol = 1;
+
+		rem = of_graph_get_remote_port_parent(endpoint);
+		if (!rem) {
+			dev_err(&pdev->dev, "Remote device at %s not found\n",
+				endpoint->full_name);
+			goto done;
+		}
+
+		pdata->asd[i] = devm_kzalloc(&pdev->dev,
+					     sizeof(struct v4l2_async_subdev),
+					     GFP_KERNEL);
+		if (!pdata->asd[i]) {
+			of_node_put(rem);
+			pdata = NULL;
+			goto done;
+		}
+
+		pdata->asd[i]->match_type = V4L2_ASYNC_MATCH_OF;
+		pdata->asd[i]->match.of.node = rem;
+		of_node_put(rem);
+	}
+
+	of_node_put(endpoint);
+	return pdata;
+
+done:
+	of_node_put(endpoint);
+	return NULL;
+}
+
+/*
+ * vpfe_probe : This function creates device entries by register
+ * itself to the V4L2 driver and initializes fields of each
+ * device objects
+ */
+static int vpfe_probe(struct platform_device *pdev)
+{
+	struct vpfe_config *vpfe_cfg = vpfe_get_pdata(pdev);
+	struct vpfe_device *vpfe;
+	struct vpfe_ccdc *ccdc;
+	struct resource	*res;
+	int ret;
+
+	if (!vpfe_cfg) {
+		dev_err(&pdev->dev, "No platform data\n");
+		return -EINVAL;
+	}
+
+	vpfe = devm_kzalloc(&pdev->dev, sizeof(*vpfe), GFP_KERNEL);
+	if (!vpfe)
+		return -ENOMEM;
+
+	vpfe->pdev = &pdev->dev;
+	vpfe->cfg = vpfe_cfg;
+	ccdc = &vpfe->ccdc;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	ccdc->ccdc_cfg.base_addr = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(ccdc->ccdc_cfg.base_addr))
+		return PTR_ERR(ccdc->ccdc_cfg.base_addr);
+
+	ret = platform_get_irq(pdev, 0);
+	if (ret <= 0) {
+		dev_err(&pdev->dev, "No IRQ resource\n");
+		return -ENODEV;
+	}
+	vpfe->irq = ret;
+
+	ret = devm_request_irq(vpfe->pdev, vpfe->irq, vpfe_isr, 0,
+			       "vpfe_capture0", vpfe);
+	if (ret) {
+		dev_err(&pdev->dev, "Unable to request interrupt\n");
+		return -EINVAL;
+	}
+
+	ret = v4l2_device_register(&pdev->dev, &vpfe->v4l2_dev);
+	if (ret) {
+		vpfe_err(vpfe,
+			"Unable to register v4l2 device.\n");
+		return ret;
+	}
+
+	/* set the driver data in platform device */
+	platform_set_drvdata(pdev, vpfe);
+	/* Enabling module functional clock */
+	pm_runtime_enable(&pdev->dev);
+
+	/* for now just enable it here instead of waiting for the open */
+	pm_runtime_get_sync(&pdev->dev);
+
+	vpfe_ccdc_config_defaults(ccdc);
+
+	pm_runtime_put_sync(&pdev->dev);
+
+	vpfe->sd = devm_kzalloc(&pdev->dev, sizeof(struct v4l2_subdev *) *
+				ARRAY_SIZE(vpfe->cfg->asd), GFP_KERNEL);
+	if (!vpfe->sd) {
+		ret = -ENOMEM;
+		goto probe_out_v4l2_unregister;
+	}
+
+	vpfe->notifier.subdevs = vpfe->cfg->asd;
+	vpfe->notifier.num_subdevs = ARRAY_SIZE(vpfe->cfg->asd);
+	vpfe->notifier.bound = vpfe_async_bound;
+	vpfe->notifier.complete = vpfe_async_complete;
+	ret = v4l2_async_notifier_register(&vpfe->v4l2_dev,
+						&vpfe->notifier);
+	if (ret) {
+		vpfe_err(vpfe, "Error registering async notifier\n");
+		ret = -EINVAL;
+		goto probe_out_v4l2_unregister;
+	}
+
+	return 0;
+
+probe_out_v4l2_unregister:
+	v4l2_device_unregister(&vpfe->v4l2_dev);
+	return ret;
+}
+
+/*
+ * vpfe_remove : It un-register device from V4L2 driver
+ */
+static int vpfe_remove(struct platform_device *pdev)
+{
+	struct vpfe_device *vpfe = platform_get_drvdata(pdev);
+
+	vpfe_dbg(2, vpfe, "vpfe_remove\n");
+
+	pm_runtime_disable(&pdev->dev);
+
+	v4l2_async_notifier_unregister(&vpfe->notifier);
+	v4l2_device_unregister(&vpfe->v4l2_dev);
+	video_unregister_device(&vpfe->video_dev);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+
+static void vpfe_save_context(struct vpfe_ccdc *ccdc)
+{
+	ccdc->ccdc_ctx[VPFE_PCR >> 2] = vpfe_reg_read(ccdc, VPFE_PCR);
+	ccdc->ccdc_ctx[VPFE_SYNMODE >> 2] = vpfe_reg_read(ccdc, VPFE_SYNMODE);
+	ccdc->ccdc_ctx[VPFE_SDOFST >> 2] = vpfe_reg_read(ccdc, VPFE_SDOFST);
+	ccdc->ccdc_ctx[VPFE_SDR_ADDR >> 2] = vpfe_reg_read(ccdc, VPFE_SDR_ADDR);
+	ccdc->ccdc_ctx[VPFE_CLAMP >> 2] = vpfe_reg_read(ccdc, VPFE_CLAMP);
+	ccdc->ccdc_ctx[VPFE_DCSUB >> 2] = vpfe_reg_read(ccdc, VPFE_DCSUB);
+	ccdc->ccdc_ctx[VPFE_COLPTN >> 2] = vpfe_reg_read(ccdc, VPFE_COLPTN);
+	ccdc->ccdc_ctx[VPFE_BLKCMP >> 2] = vpfe_reg_read(ccdc, VPFE_BLKCMP);
+	ccdc->ccdc_ctx[VPFE_VDINT >> 2] = vpfe_reg_read(ccdc, VPFE_VDINT);
+	ccdc->ccdc_ctx[VPFE_ALAW >> 2] = vpfe_reg_read(ccdc, VPFE_ALAW);
+	ccdc->ccdc_ctx[VPFE_REC656IF >> 2] = vpfe_reg_read(ccdc, VPFE_REC656IF);
+	ccdc->ccdc_ctx[VPFE_CCDCFG >> 2] = vpfe_reg_read(ccdc, VPFE_CCDCFG);
+	ccdc->ccdc_ctx[VPFE_CULLING >> 2] = vpfe_reg_read(ccdc, VPFE_CULLING);
+	ccdc->ccdc_ctx[VPFE_HD_VD_WID >> 2] = vpfe_reg_read(ccdc,
+							    VPFE_HD_VD_WID);
+	ccdc->ccdc_ctx[VPFE_PIX_LINES >> 2] = vpfe_reg_read(ccdc,
+							    VPFE_PIX_LINES);
+	ccdc->ccdc_ctx[VPFE_HORZ_INFO >> 2] = vpfe_reg_read(ccdc,
+							    VPFE_HORZ_INFO);
+	ccdc->ccdc_ctx[VPFE_VERT_START >> 2] = vpfe_reg_read(ccdc,
+							     VPFE_VERT_START);
+	ccdc->ccdc_ctx[VPFE_VERT_LINES >> 2] = vpfe_reg_read(ccdc,
+							     VPFE_VERT_LINES);
+	ccdc->ccdc_ctx[VPFE_HSIZE_OFF >> 2] = vpfe_reg_read(ccdc,
+							    VPFE_HSIZE_OFF);
+}
+
+static int vpfe_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct vpfe_device *vpfe = platform_get_drvdata(pdev);
+	struct vpfe_ccdc *ccdc = &vpfe->ccdc;
+
+	/* if streaming has not started we don't care */
+	if (!vb2_start_streaming_called(&vpfe->buffer_queue))
+		return 0;
+
+	pm_runtime_get_sync(dev);
+	vpfe_config_enable(ccdc, 1);
+
+	/* Save VPFE context */
+	vpfe_save_context(ccdc);
+
+	/* Disable CCDC */
+	vpfe_pcr_enable(ccdc, 0);
+	vpfe_config_enable(ccdc, 0);
+
+	/* Disable both master and slave clock */
+	pm_runtime_put_sync(dev);
+
+	/* Select sleep pin state */
+	pinctrl_pm_select_sleep_state(dev);
+
+	return 0;
+}
+
+static void vpfe_restore_context(struct vpfe_ccdc *ccdc)
+{
+	vpfe_reg_write(ccdc, ccdc->ccdc_ctx[VPFE_SYNMODE >> 2], VPFE_SYNMODE);
+	vpfe_reg_write(ccdc, ccdc->ccdc_ctx[VPFE_CULLING >> 2], VPFE_CULLING);
+	vpfe_reg_write(ccdc, ccdc->ccdc_ctx[VPFE_SDOFST >> 2], VPFE_SDOFST);
+	vpfe_reg_write(ccdc, ccdc->ccdc_ctx[VPFE_SDR_ADDR >> 2], VPFE_SDR_ADDR);
+	vpfe_reg_write(ccdc, ccdc->ccdc_ctx[VPFE_CLAMP >> 2], VPFE_CLAMP);
+	vpfe_reg_write(ccdc, ccdc->ccdc_ctx[VPFE_DCSUB >> 2], VPFE_DCSUB);
+	vpfe_reg_write(ccdc, ccdc->ccdc_ctx[VPFE_COLPTN >> 2], VPFE_COLPTN);
+	vpfe_reg_write(ccdc, ccdc->ccdc_ctx[VPFE_BLKCMP >> 2], VPFE_BLKCMP);
+	vpfe_reg_write(ccdc, ccdc->ccdc_ctx[VPFE_VDINT >> 2], VPFE_VDINT);
+	vpfe_reg_write(ccdc, ccdc->ccdc_ctx[VPFE_ALAW >> 2], VPFE_ALAW);
+	vpfe_reg_write(ccdc, ccdc->ccdc_ctx[VPFE_REC656IF >> 2], VPFE_REC656IF);
+	vpfe_reg_write(ccdc, ccdc->ccdc_ctx[VPFE_CCDCFG >> 2], VPFE_CCDCFG);
+	vpfe_reg_write(ccdc, ccdc->ccdc_ctx[VPFE_PCR >> 2], VPFE_PCR);
+	vpfe_reg_write(ccdc, ccdc->ccdc_ctx[VPFE_HD_VD_WID >> 2],
+						VPFE_HD_VD_WID);
+	vpfe_reg_write(ccdc, ccdc->ccdc_ctx[VPFE_PIX_LINES >> 2],
+						VPFE_PIX_LINES);
+	vpfe_reg_write(ccdc, ccdc->ccdc_ctx[VPFE_HORZ_INFO >> 2],
+						VPFE_HORZ_INFO);
+	vpfe_reg_write(ccdc, ccdc->ccdc_ctx[VPFE_VERT_START >> 2],
+						VPFE_VERT_START);
+	vpfe_reg_write(ccdc, ccdc->ccdc_ctx[VPFE_VERT_LINES >> 2],
+						VPFE_VERT_LINES);
+	vpfe_reg_write(ccdc, ccdc->ccdc_ctx[VPFE_HSIZE_OFF >> 2],
+						VPFE_HSIZE_OFF);
+}
+
+static int vpfe_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct vpfe_device *vpfe = platform_get_drvdata(pdev);
+	struct vpfe_ccdc *ccdc = &vpfe->ccdc;
+
+	/* if streaming has not started we don't care */
+	if (!vb2_start_streaming_called(&vpfe->buffer_queue))
+		return 0;
+
+	/* Enable both master and slave clock */
+	pm_runtime_get_sync(dev);
+	vpfe_config_enable(ccdc, 1);
+
+	/* Restore VPFE context */
+	vpfe_restore_context(ccdc);
+
+	vpfe_config_enable(ccdc, 0);
+	pm_runtime_put_sync(dev);
+
+	/* Select default pin state */
+	pinctrl_pm_select_default_state(dev);
+
+	return 0;
+}
+
+#endif
+
+static SIMPLE_DEV_PM_OPS(vpfe_pm_ops, vpfe_suspend, vpfe_resume);
+
+static const struct of_device_id vpfe_of_match[] = {
+	{ .compatible = "ti,am437x-vpfe", },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, vpfe_of_match);
+
+static struct platform_driver vpfe_driver = {
+	.probe		= vpfe_probe,
+	.remove		= vpfe_remove,
+	.driver = {
+		.name	= VPFE_MODULE_NAME,
+		.pm	= &vpfe_pm_ops,
+		.of_match_table = of_match_ptr(vpfe_of_match),
+	},
+};
+
+module_platform_driver(vpfe_driver);
+
+MODULE_AUTHOR("Texas Instruments");
+MODULE_DESCRIPTION("TI AM437x VPFE driver");
+MODULE_LICENSE("GPL");
+MODULE_VERSION(VPFE_VERSION);
diff --git a/drivers/media/platform/am437x/am437x-vpfe.h b/drivers/media/platform/am437x/am437x-vpfe.h
new file mode 100644
index 0000000..777bf97
--- /dev/null
+++ b/drivers/media/platform/am437x/am437x-vpfe.h
@@ -0,0 +1,283 @@
+/*
+ * Copyright (C) 2013 - 2014 Texas Instruments, Inc.
+ *
+ * Benoit Parrot <bparrot@ti.com>
+ * Lad, Prabhakar <prabhakar.csengg@gmail.com>
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef AM437X_VPFE_H
+#define AM437X_VPFE_H
+
+#include <linux/am437x-vpfe.h>
+#include <linux/clk.h>
+#include <linux/device.h>
+#include <linux/io.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+
+#include <media/v4l2-dev.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "am437x-vpfe_regs.h"
+
+enum vpfe_pin_pol {
+	VPFE_PINPOL_POSITIVE = 0,
+	VPFE_PINPOL_NEGATIVE,
+};
+
+enum vpfe_hw_if_type {
+	/* Raw Bayer */
+	VPFE_RAW_BAYER = 0,
+	/* BT656 - 8 bit */
+	VPFE_BT656,
+	/* BT656 - 10 bit */
+	VPFE_BT656_10BIT,
+	/* YCbCr - 8 bit with external sync */
+	VPFE_YCBCR_SYNC_8,
+	/* YCbCr - 16 bit with external sync */
+	VPFE_YCBCR_SYNC_16,
+};
+
+/* interface description */
+struct vpfe_hw_if_param {
+	enum vpfe_hw_if_type if_type;
+	enum vpfe_pin_pol hdpol;
+	enum vpfe_pin_pol vdpol;
+	unsigned int bus_width;
+};
+
+#define VPFE_MAX_SUBDEV		1
+#define VPFE_MAX_INPUTS		1
+
+struct vpfe_pixel_format {
+	struct v4l2_fmtdesc fmtdesc;
+	/* bytes per pixel */
+	int bpp;
+};
+
+struct vpfe_std_info {
+	int active_pixels;
+	int active_lines;
+	/* current frame format */
+	int frame_format;
+};
+
+struct vpfe_route {
+	u32 input;
+	u32 output;
+};
+
+struct vpfe_subdev_info {
+	/* Sub device group id */
+	int grp_id;
+	/* inputs available at the sub device */
+	struct v4l2_input inputs[VPFE_MAX_INPUTS];
+	/* Sub dev routing information for each input */
+	struct vpfe_route *routes;
+	/* check if sub dev supports routing */
+	int can_route;
+	/* ccdc bus/interface configuration */
+	struct vpfe_hw_if_param vpfe_param;
+	struct v4l2_subdev *sd;
+};
+
+struct vpfe_config {
+	/* information about each subdev */
+	struct vpfe_subdev_info sub_devs[VPFE_MAX_SUBDEV];
+	/* Flat array, arranged in groups */
+	struct v4l2_async_subdev *asd[VPFE_MAX_SUBDEV];
+};
+
+struct vpfe_cap_buffer {
+	struct vb2_v4l2_buffer vb;
+	struct list_head list;
+};
+
+enum ccdc_pixfmt {
+	CCDC_PIXFMT_RAW = 0,
+	CCDC_PIXFMT_YCBCR_16BIT,
+	CCDC_PIXFMT_YCBCR_8BIT,
+};
+
+enum ccdc_frmfmt {
+	CCDC_FRMFMT_PROGRESSIVE = 0,
+	CCDC_FRMFMT_INTERLACED,
+};
+
+/* PIXEL ORDER IN MEMORY from LSB to MSB */
+/* only applicable for 8-bit input mode  */
+enum ccdc_pixorder {
+	CCDC_PIXORDER_YCBYCR,
+	CCDC_PIXORDER_CBYCRY,
+};
+
+enum ccdc_buftype {
+	CCDC_BUFTYPE_FLD_INTERLEAVED,
+	CCDC_BUFTYPE_FLD_SEPARATED
+};
+
+
+/* returns the highest bit used for the gamma */
+static inline u8 ccdc_gamma_width_max_bit(enum vpfe_ccdc_gamma_width width)
+{
+	return 15 - width;
+}
+
+/* returns the highest bit used for this data size */
+static inline u8 ccdc_data_size_max_bit(enum vpfe_ccdc_data_size sz)
+{
+	return sz == VPFE_CCDC_DATA_8BITS ? 7 : 15 - sz;
+}
+
+/* Structure for CCDC configuration parameters for raw capture mode */
+struct ccdc_params_raw {
+	/* pixel format */
+	enum ccdc_pixfmt pix_fmt;
+	/* progressive or interlaced frame */
+	enum ccdc_frmfmt frm_fmt;
+	struct v4l2_rect win;
+	/* Current Format Bytes Per Pixels */
+	unsigned int bytesperpixel;
+	/* Current Format Bytes per Lines
+	 * (Aligned to 32 bytes) used for HORZ_INFO
+	 */
+	unsigned int bytesperline;
+	/* field id polarity */
+	enum vpfe_pin_pol fid_pol;
+	/* vertical sync polarity */
+	enum vpfe_pin_pol vd_pol;
+	/* horizontal sync polarity */
+	enum vpfe_pin_pol hd_pol;
+	/* interleaved or separated fields */
+	enum ccdc_buftype buf_type;
+	/*
+	 * enable to store the image in inverse
+	 * order in memory(bottom to top)
+	 */
+	unsigned char image_invert_enable;
+	/* configurable parameters */
+	struct vpfe_ccdc_config_params_raw config_params;
+};
+
+struct ccdc_params_ycbcr {
+	/* pixel format */
+	enum ccdc_pixfmt pix_fmt;
+	/* progressive or interlaced frame */
+	enum ccdc_frmfmt frm_fmt;
+	struct v4l2_rect win;
+	/* Current Format Bytes Per Pixels */
+	unsigned int bytesperpixel;
+	/* Current Format Bytes per Lines
+	 * (Aligned to 32 bytes) used for HORZ_INFO
+	 */
+	unsigned int bytesperline;
+	/* field id polarity */
+	enum vpfe_pin_pol fid_pol;
+	/* vertical sync polarity */
+	enum vpfe_pin_pol vd_pol;
+	/* horizontal sync polarity */
+	enum vpfe_pin_pol hd_pol;
+	/* enable BT.656 embedded sync mode */
+	int bt656_enable;
+	/* cb:y:cr:y or y:cb:y:cr in memory */
+	enum ccdc_pixorder pix_order;
+	/* interleaved or separated fields  */
+	enum ccdc_buftype buf_type;
+};
+
+/*
+ * CCDC operational configuration
+ */
+struct ccdc_config {
+	/* CCDC interface type */
+	enum vpfe_hw_if_type if_type;
+	/* Raw Bayer configuration */
+	struct ccdc_params_raw bayer;
+	/* YCbCr configuration */
+	struct ccdc_params_ycbcr ycbcr;
+	/* ccdc base address */
+	void __iomem *base_addr;
+};
+
+struct vpfe_ccdc {
+	struct ccdc_config ccdc_cfg;
+	u32 ccdc_ctx[VPFE_REG_END / sizeof(u32)];
+};
+
+struct vpfe_device {
+	/* V4l2 specific parameters */
+	/* Identifies video device for this channel */
+	struct video_device video_dev;
+	/* sub devices */
+	struct v4l2_subdev **sd;
+	/* vpfe cfg */
+	struct vpfe_config *cfg;
+	/* V4l2 device */
+	struct v4l2_device v4l2_dev;
+	/* parent device */
+	struct device *pdev;
+	/* subdevice async Notifier */
+	struct v4l2_async_notifier notifier;
+	/* Indicates id of the field which is being displayed */
+	unsigned field;
+	unsigned sequence;
+	/* current interface type */
+	struct vpfe_hw_if_param vpfe_if_params;
+	/* ptr to currently selected sub device */
+	struct vpfe_subdev_info *current_subdev;
+	/* current input at the sub device */
+	int current_input;
+	/* Keeps track of the information about the standard */
+	struct vpfe_std_info std_info;
+	/* std index into std table */
+	int std_index;
+	/* IRQs used when CCDC output to SDRAM */
+	unsigned int irq;
+	/* Pointer pointing to current v4l2_buffer */
+	struct vpfe_cap_buffer *cur_frm;
+	/* Pointer pointing to next v4l2_buffer */
+	struct vpfe_cap_buffer *next_frm;
+	/* Used to store pixel format */
+	struct v4l2_format fmt;
+	/* Used to store current bytes per pixel based on current format */
+	unsigned int bpp;
+	/*
+	 * used when IMP is chained to store the crop window which
+	 * is different from the image window
+	 */
+	struct v4l2_rect crop;
+	/* Buffer queue used in video-buf */
+	struct vb2_queue buffer_queue;
+	/* Allocator-specific contexts for each plane */
+	struct vb2_alloc_ctx *alloc_ctx;
+	/* Queue of filled frames */
+	struct list_head dma_queue;
+	/* IRQ lock for DMA queue */
+	spinlock_t dma_queue_lock;
+	/* lock used to access this structure */
+	struct mutex lock;
+	/*
+	 * offset where second field starts from the starting of the
+	 * buffer for field separated YCbCr formats
+	 */
+	u32 field_off;
+	struct vpfe_ccdc ccdc;
+};
+
+#endif	/* AM437X_VPFE_H */
diff --git a/drivers/media/platform/am437x/am437x-vpfe_regs.h b/drivers/media/platform/am437x/am437x-vpfe_regs.h
new file mode 100644
index 0000000..4a0ed29
--- /dev/null
+++ b/drivers/media/platform/am437x/am437x-vpfe_regs.h
@@ -0,0 +1,140 @@
+/*
+ * TI AM437x Image Sensor Interface Registers
+ *
+ * Copyright (C) 2013 - 2014 Texas Instruments, Inc.
+ *
+ * Benoit Parrot <bparrot@ti.com>
+ * Lad, Prabhakar <prabhakar.csengg@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef AM437X_VPFE_REGS_H
+#define AM437X_VPFE_REGS_H
+
+/* VPFE module register offset */
+#define VPFE_REVISION				0x0
+#define VPFE_PCR				0x4
+#define VPFE_SYNMODE				0x8
+#define VPFE_HD_VD_WID				0xc
+#define VPFE_PIX_LINES				0x10
+#define VPFE_HORZ_INFO				0x14
+#define VPFE_VERT_START				0x18
+#define VPFE_VERT_LINES				0x1c
+#define VPFE_CULLING				0x20
+#define VPFE_HSIZE_OFF				0x24
+#define VPFE_SDOFST				0x28
+#define VPFE_SDR_ADDR				0x2c
+#define VPFE_CLAMP				0x30
+#define VPFE_DCSUB				0x34
+#define VPFE_COLPTN				0x38
+#define VPFE_BLKCMP				0x3c
+#define VPFE_VDINT				0x48
+#define VPFE_ALAW				0x4c
+#define VPFE_REC656IF				0x50
+#define VPFE_CCDCFG				0x54
+#define VPFE_DMA_CNTL				0x98
+#define VPFE_SYSCONFIG				0x104
+#define VPFE_CONFIG				0x108
+#define VPFE_IRQ_EOI				0x110
+#define VPFE_IRQ_STS_RAW			0x114
+#define VPFE_IRQ_STS				0x118
+#define VPFE_IRQ_EN_SET				0x11c
+#define VPFE_IRQ_EN_CLR				0x120
+#define VPFE_REG_END				0x124
+
+/* Define bit fields within selected registers */
+#define VPFE_FID_POL_MASK			1
+#define VPFE_FID_POL_SHIFT			4
+#define VPFE_HD_POL_MASK			1
+#define VPFE_HD_POL_SHIFT			3
+#define VPFE_VD_POL_MASK			1
+#define VPFE_VD_POL_SHIFT			2
+#define VPFE_HSIZE_OFF_MASK			0xffffffe0
+#define VPFE_32BYTE_ALIGN_VAL			31
+#define VPFE_FRM_FMT_MASK			0x1
+#define VPFE_FRM_FMT_SHIFT			7
+#define VPFE_DATA_SZ_MASK			7
+#define VPFE_DATA_SZ_SHIFT			8
+#define VPFE_PIX_FMT_MASK			3
+#define VPFE_PIX_FMT_SHIFT			12
+#define VPFE_VP2SDR_DISABLE			0xfffbffff
+#define VPFE_WEN_ENABLE				(1 << 17)
+#define VPFE_SDR2RSZ_DISABLE			0xfff7ffff
+#define VPFE_VDHDEN_ENABLE			(1 << 16)
+#define VPFE_LPF_ENABLE				(1 << 14)
+#define VPFE_ALAW_ENABLE			(1 << 3)
+#define VPFE_ALAW_GAMMA_WD_MASK			7
+#define VPFE_BLK_CLAMP_ENABLE			(1 << 31)
+#define VPFE_BLK_SGAIN_MASK			0x1f
+#define VPFE_BLK_ST_PXL_MASK			0x7fff
+#define VPFE_BLK_ST_PXL_SHIFT			10
+#define VPFE_BLK_SAMPLE_LN_MASK			7
+#define VPFE_BLK_SAMPLE_LN_SHIFT		28
+#define VPFE_BLK_SAMPLE_LINE_MASK		7
+#define VPFE_BLK_SAMPLE_LINE_SHIFT		25
+#define VPFE_BLK_DC_SUB_MASK			0x03fff
+#define VPFE_BLK_COMP_MASK			0xff
+#define VPFE_BLK_COMP_GB_COMP_SHIFT		8
+#define VPFE_BLK_COMP_GR_COMP_SHIFT		16
+#define VPFE_BLK_COMP_R_COMP_SHIFT		24
+#define VPFE_LATCH_ON_VSYNC_DISABLE		(1 << 15)
+#define VPFE_DATA_PACK_ENABLE			(1 << 11)
+#define VPFE_HORZ_INFO_SPH_SHIFT		16
+#define VPFE_VERT_START_SLV0_SHIFT		16
+#define VPFE_VDINT_VDINT0_SHIFT			16
+#define VPFE_VDINT_VDINT1_MASK			0xffff
+#define VPFE_PPC_RAW				1
+#define VPFE_DCSUB_DEFAULT_VAL			0
+#define VPFE_CLAMP_DEFAULT_VAL			0
+#define VPFE_COLPTN_VAL				0xbb11bb11
+#define VPFE_TWO_BYTES_PER_PIXEL		2
+#define VPFE_INTERLACED_IMAGE_INVERT		0x4b6d
+#define VPFE_INTERLACED_NO_IMAGE_INVERT		0x0249
+#define VPFE_PROGRESSIVE_IMAGE_INVERT		0x4000
+#define VPFE_PROGRESSIVE_NO_IMAGE_INVERT	0
+#define VPFE_INTERLACED_HEIGHT_SHIFT		1
+#define VPFE_SYN_MODE_INPMOD_SHIFT		12
+#define VPFE_SYN_MODE_INPMOD_MASK		3
+#define VPFE_SYN_MODE_8BITS			(7 << 8)
+#define VPFE_SYN_MODE_10BITS			(6 << 8)
+#define VPFE_SYN_MODE_11BITS			(5 << 8)
+#define VPFE_SYN_MODE_12BITS			(4 << 8)
+#define VPFE_SYN_MODE_13BITS			(3 << 8)
+#define VPFE_SYN_MODE_14BITS			(2 << 8)
+#define VPFE_SYN_MODE_15BITS			(1 << 8)
+#define VPFE_SYN_MODE_16BITS			(0 << 8)
+#define VPFE_SYN_FLDMODE_MASK			1
+#define VPFE_SYN_FLDMODE_SHIFT			7
+#define VPFE_REC656IF_BT656_EN			3
+#define VPFE_SYN_MODE_VD_POL_NEGATIVE		(1 << 2)
+#define VPFE_CCDCFG_Y8POS_SHIFT			11
+#define VPFE_CCDCFG_BW656_10BIT			(1 << 5)
+#define VPFE_SDOFST_FIELD_INTERLEAVED		0x249
+#define VPFE_NO_CULLING				0xffff00ff
+#define VPFE_VDINT0				(1 << 0)
+#define VPFE_VDINT1				(1 << 1)
+#define VPFE_VDINT2				(1 << 2)
+#define VPFE_DMA_CNTL_OVERFLOW			(1 << 31)
+
+#define VPFE_CONFIG_PCLK_INV_SHIFT		0
+#define VPFE_CONFIG_PCLK_INV_MASK		1
+#define VPFE_CONFIG_PCLK_INV_NOT_INV		0
+#define VPFE_CONFIG_PCLK_INV_INV		1
+#define VPFE_CONFIG_EN_SHIFT			1
+#define VPFE_CONFIG_EN_MASK			2
+#define VPFE_CONFIG_EN_DISABLE			0
+#define VPFE_CONFIG_EN_ENABLE			1
+#define VPFE_CONFIG_ST_SHIFT			2
+#define VPFE_CONFIG_ST_MASK			4
+#define VPFE_CONFIG_ST_OCP_ACTIVE		0
+#define VPFE_CONFIG_ST_OCP_STANDBY		1
+
+#endif		/* AM437X_VPFE_REGS_H */
diff --git a/drivers/media/platform/arv.c b/drivers/media/platform/arv.c
new file mode 100644
index 0000000..03c5098
--- /dev/null
+++ b/drivers/media/platform/arv.c
@@ -0,0 +1,884 @@
+/*
+ * Colour AR M64278(VGA) driver for Video4Linux
+ *
+ * Copyright (C) 2003	Takeo Takahashi <takahashi.takeo@renesas.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version
+ * 2 of the License, or (at your option) any later version.
+ *
+ * Some code is taken from AR driver sample program for M3T-M32700UT.
+ *
+ * AR driver sample (M32R SDK):
+ *     Copyright (c) 2003 RENESAS TECHNOROGY CORPORATION
+ *     AND RENESAS SOLUTIONS CORPORATION
+ *     All Rights Reserved.
+ *
+ * 2003-09-01:	Support w3cam by Takeo Takahashi
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/mm.h>
+#include <linux/sched.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-fh.h>
+#include <linux/mutex.h>
+
+#include <asm/uaccess.h>
+#include <asm/m32r.h>
+#include <asm/io.h>
+#include <asm/dma.h>
+#include <asm/byteorder.h>
+
+#if 0
+#define DEBUG(n, args...) printk(KERN_INFO args)
+#define CHECK_LOST	1
+#else
+#define DEBUG(n, args...)
+#define CHECK_LOST	0
+#endif
+
+/*
+ * USE_INT is always 0, interrupt mode is not available
+ * on linux due to lack of speed
+ */
+#define USE_INT		0	/* Don't modify */
+
+#define VERSION	"0.0.5"
+
+#define ar_inl(addr) 		inl((unsigned long)(addr))
+#define ar_outl(val, addr)	outl((unsigned long)(val), (unsigned long)(addr))
+
+extern struct cpuinfo_m32r	boot_cpu_data;
+
+/*
+ * CCD pixel size
+ *	Note that M32700UT does not support CIF mode, but QVGA is
+ *	supported by M32700UT hardware using VGA mode of AR LSI.
+ *
+ * 	Supported: VGA  (Normal mode, Interlace mode)
+ *		   QVGA (Always Interlace mode of VGA)
+ *
+ */
+#define AR_WIDTH_VGA		640
+#define AR_HEIGHT_VGA		480
+#define AR_WIDTH_QVGA		320
+#define AR_HEIGHT_QVGA		240
+#define MIN_AR_WIDTH		AR_WIDTH_QVGA
+#define MIN_AR_HEIGHT		AR_HEIGHT_QVGA
+#define MAX_AR_WIDTH		AR_WIDTH_VGA
+#define MAX_AR_HEIGHT		AR_HEIGHT_VGA
+
+/* bits & bytes per pixel */
+#define AR_BITS_PER_PIXEL	16
+#define AR_BYTES_PER_PIXEL	(AR_BITS_PER_PIXEL / 8)
+
+/* line buffer size */
+#define AR_LINE_BYTES_VGA	(AR_WIDTH_VGA * AR_BYTES_PER_PIXEL)
+#define AR_LINE_BYTES_QVGA	(AR_WIDTH_QVGA * AR_BYTES_PER_PIXEL)
+#define MAX_AR_LINE_BYTES	AR_LINE_BYTES_VGA
+
+/* frame size & type */
+#define AR_FRAME_BYTES_VGA \
+	(AR_WIDTH_VGA * AR_HEIGHT_VGA * AR_BYTES_PER_PIXEL)
+#define AR_FRAME_BYTES_QVGA \
+	(AR_WIDTH_QVGA * AR_HEIGHT_QVGA * AR_BYTES_PER_PIXEL)
+#define MAX_AR_FRAME_BYTES \
+	(MAX_AR_WIDTH * MAX_AR_HEIGHT * AR_BYTES_PER_PIXEL)
+
+#define AR_MAX_FRAME		15
+
+/* capture size */
+#define AR_SIZE_VGA		0
+#define AR_SIZE_QVGA		1
+
+/* capture mode */
+#define AR_MODE_INTERLACE	0
+#define AR_MODE_NORMAL		1
+
+struct ar {
+	struct v4l2_device v4l2_dev;
+	struct video_device vdev;
+	int start_capture;	/* duaring capture in INT. mode. */
+#if USE_INT
+	unsigned char *line_buff;	/* DMA line buffer */
+#endif
+	unsigned char *frame[MAX_AR_HEIGHT];	/* frame data */
+	short size;			/* capture size */
+	short mode;			/* capture mode */
+	int width, height;
+	int frame_bytes, line_bytes;
+	wait_queue_head_t wait;
+	struct mutex lock;
+};
+
+static struct ar ardev;
+
+static int video_nr = -1;	/* video device number (first free) */
+static unsigned char yuv[MAX_AR_FRAME_BYTES];
+
+/* module parameters */
+/* default frequency */
+#define DEFAULT_FREQ	50	/* 50 or 75 (MHz) is available as BCLK */
+static int freq = DEFAULT_FREQ;	/* BCLK: available 50 or 70 (MHz) */
+static int vga;			/* default mode(0:QVGA mode, other:VGA mode) */
+static int vga_interlace;	/* 0 is normal mode for, else interlace mode */
+module_param(freq, int, 0);
+module_param(vga, int, 0);
+module_param(vga_interlace, int, 0);
+
+static void wait_for_vsync(void)
+{
+	while (ar_inl(ARVCR0) & ARVCR0_VDS)	/* wait for VSYNC */
+		cpu_relax();
+	while (!(ar_inl(ARVCR0) & ARVCR0_VDS))	/* wait for VSYNC */
+		cpu_relax();
+}
+
+static void wait_acknowledge(void)
+{
+	int i;
+
+	for (i = 0; i < 1000; i++)
+		cpu_relax();
+	while (ar_inl(PLDI2CSTS) & PLDI2CSTS_NOACK)
+		cpu_relax();
+}
+
+/*******************************************************************
+ * I2C functions
+ *******************************************************************/
+static void iic(int n, unsigned long addr, unsigned long data1, unsigned long data2,
+	 unsigned long data3)
+{
+	int i;
+
+	/* Slave Address */
+	ar_outl(addr, PLDI2CDATA);
+	wait_for_vsync();
+
+	/* Start */
+	ar_outl(1, PLDI2CCND);
+	wait_acknowledge();
+
+	/* Transfer data 1 */
+	ar_outl(data1, PLDI2CDATA);
+	wait_for_vsync();
+	ar_outl(PLDI2CSTEN_STEN, PLDI2CSTEN);
+	wait_acknowledge();
+
+	/* Transfer data 2 */
+	ar_outl(data2, PLDI2CDATA);
+	wait_for_vsync();
+	ar_outl(PLDI2CSTEN_STEN, PLDI2CSTEN);
+	wait_acknowledge();
+
+	if (n == 3) {
+		/* Transfer data 3 */
+		ar_outl(data3, PLDI2CDATA);
+		wait_for_vsync();
+		ar_outl(PLDI2CSTEN_STEN, PLDI2CSTEN);
+		wait_acknowledge();
+	}
+
+	/* Stop */
+	for (i = 0; i < 100; i++)
+		cpu_relax();
+	ar_outl(2, PLDI2CCND);
+	ar_outl(2, PLDI2CCND);
+
+	while (ar_inl(PLDI2CSTS) & PLDI2CSTS_BB)
+		cpu_relax();
+}
+
+
+static void init_iic(void)
+{
+	DEBUG(1, "init_iic:\n");
+
+	/*
+	 * ICU Setting (iic)
+	 */
+	/* I2C Setting */
+	ar_outl(0x0, PLDI2CCR);      	/* I2CCR Disable                   */
+	ar_outl(0x0300, PLDI2CMOD); 	/* I2CMOD ACK/8b-data/7b-addr/auto */
+	ar_outl(0x1, PLDI2CACK);	/* I2CACK ACK                      */
+
+	/* I2C CLK */
+	/* 50MH-100k */
+	if (freq == 75)
+		ar_outl(369, PLDI2CFREQ);	/* BCLK = 75MHz */
+	else if (freq == 50)
+		ar_outl(244, PLDI2CFREQ);	/* BCLK = 50MHz */
+	else
+		ar_outl(244, PLDI2CFREQ);	/* default: BCLK = 50MHz */
+	ar_outl(0x1, PLDI2CCR); 	/* I2CCR Enable */
+}
+
+/**************************************************************************
+ *
+ * Video4Linux Interface functions
+ *
+ **************************************************************************/
+
+static inline void disable_dma(void)
+{
+	ar_outl(0x8000, M32R_DMAEN_PORTL);	/* disable DMA0 */
+}
+
+static inline void enable_dma(void)
+{
+	ar_outl(0x8080, M32R_DMAEN_PORTL);	/* enable DMA0 */
+}
+
+static inline void clear_dma_status(void)
+{
+	ar_outl(0x8000, M32R_DMAEDET_PORTL);	/* clear status */
+}
+
+static void wait_for_vertical_sync(struct ar *ar, int exp_line)
+{
+#if CHECK_LOST
+	int tmout = 10000;	/* FIXME */
+	int l;
+
+	/*
+	 * check HCOUNT because we cannot check vertical sync.
+	 */
+	for (; tmout >= 0; tmout--) {
+		l = ar_inl(ARVHCOUNT);
+		if (l == exp_line)
+			break;
+	}
+	if (tmout < 0)
+		v4l2_err(&ar->v4l2_dev, "lost %d -> %d\n", exp_line, l);
+#else
+	while (ar_inl(ARVHCOUNT) != exp_line)
+		cpu_relax();
+#endif
+}
+
+static ssize_t ar_read(struct file *file, char *buf, size_t count, loff_t *ppos)
+{
+	struct ar *ar = video_drvdata(file);
+	long ret = ar->frame_bytes;		/* return read bytes */
+	unsigned long arvcr1 = 0;
+	unsigned long flags;
+	unsigned char *p;
+	int h, w;
+	unsigned char *py, *pu, *pv;
+#if !USE_INT
+	int l;
+#endif
+
+	DEBUG(1, "ar_read()\n");
+
+	if (ar->size == AR_SIZE_QVGA)
+		arvcr1 |= ARVCR1_QVGA;
+	if (ar->mode == AR_MODE_NORMAL)
+		arvcr1 |= ARVCR1_NORMAL;
+
+	mutex_lock(&ar->lock);
+
+#if USE_INT
+	local_irq_save(flags);
+	disable_dma();
+	ar_outl(0xa1871300, M32R_DMA0CR0_PORTL);
+	ar_outl(0x01000000, M32R_DMA0CR1_PORTL);
+
+	/* set AR FIFO address as source(BSEL5) */
+	ar_outl(ARDATA32, M32R_DMA0CSA_PORTL);
+	ar_outl(ARDATA32, M32R_DMA0RSA_PORTL);
+	ar_outl(ar->line_buff, M32R_DMA0CDA_PORTL);	/* destination addr. */
+	ar_outl(ar->line_buff, M32R_DMA0RDA_PORTL); 	/* reload address */
+	ar_outl(ar->line_bytes, M32R_DMA0CBCUT_PORTL); 	/* byte count (bytes) */
+	ar_outl(ar->line_bytes, M32R_DMA0RBCUT_PORTL); 	/* reload count (bytes) */
+
+	/*
+	 * Okay, kick AR LSI to invoke an interrupt
+	 */
+	ar->start_capture = -1;
+	ar_outl(arvcr1 | ARVCR1_HIEN, ARVCR1);
+	local_irq_restore(flags);
+	/* .... AR interrupts .... */
+	wait_event_interruptible(ar->wait, ar->start_capture == 0);
+	if (signal_pending(current)) {
+		printk(KERN_ERR "arv: interrupted while get frame data.\n");
+		ret = -EINTR;
+		goto out_up;
+	}
+#else	/* ! USE_INT */
+	/* polling */
+	ar_outl(arvcr1, ARVCR1);
+	disable_dma();
+	ar_outl(0x8000, M32R_DMAEDET_PORTL);
+	ar_outl(0xa0861300, M32R_DMA0CR0_PORTL);
+	ar_outl(0x01000000, M32R_DMA0CR1_PORTL);
+	ar_outl(ARDATA32, M32R_DMA0CSA_PORTL);
+	ar_outl(ARDATA32, M32R_DMA0RSA_PORTL);
+	ar_outl(ar->line_bytes, M32R_DMA0CBCUT_PORTL);
+	ar_outl(ar->line_bytes, M32R_DMA0RBCUT_PORTL);
+
+	local_irq_save(flags);
+	while (ar_inl(ARVHCOUNT) != 0)		/* wait for 0 */
+		cpu_relax();
+	if (ar->mode == AR_MODE_INTERLACE && ar->size == AR_SIZE_VGA) {
+		for (h = 0; h < ar->height; h++) {
+			wait_for_vertical_sync(ar, h);
+			if (h < (AR_HEIGHT_VGA/2))
+				l = h << 1;
+			else
+				l = (((h - (AR_HEIGHT_VGA/2)) << 1) + 1);
+			ar_outl(virt_to_phys(ar->frame[l]), M32R_DMA0CDA_PORTL);
+			enable_dma();
+			while (!(ar_inl(M32R_DMAEDET_PORTL) & 0x8000))
+				cpu_relax();
+			disable_dma();
+			clear_dma_status();
+			ar_outl(0xa0861300, M32R_DMA0CR0_PORTL);
+		}
+	} else {
+		for (h = 0; h < ar->height; h++) {
+			wait_for_vertical_sync(ar, h);
+			ar_outl(virt_to_phys(ar->frame[h]), M32R_DMA0CDA_PORTL);
+			enable_dma();
+			while (!(ar_inl(M32R_DMAEDET_PORTL) & 0x8000))
+				cpu_relax();
+			disable_dma();
+			clear_dma_status();
+			ar_outl(0xa0861300, M32R_DMA0CR0_PORTL);
+		}
+	}
+	local_irq_restore(flags);
+#endif	/* ! USE_INT */
+
+	/*
+	 * convert YUV422 to YUV422P
+	 * 	+--------------------+
+	 *	|  Y0,Y1,...	     |
+	 *	|  ..............Yn  |
+	 *	+--------------------+
+	 *	|  U0,U1,........Un  |
+	 *	+--------------------+
+	 *	|  V0,V1,........Vn  |
+	 *	+--------------------+
+	 */
+	py = yuv;
+	pu = py + (ar->frame_bytes / 2);
+	pv = pu + (ar->frame_bytes / 4);
+	for (h = 0; h < ar->height; h++) {
+		p = ar->frame[h];
+		for (w = 0; w < ar->line_bytes; w += 4) {
+			*py++ = *p++;
+			*pu++ = *p++;
+			*py++ = *p++;
+			*pv++ = *p++;
+		}
+	}
+	if (copy_to_user(buf, yuv, ar->frame_bytes)) {
+		v4l2_err(&ar->v4l2_dev, "failed while copy_to_user yuv.\n");
+		ret = -EFAULT;
+		goto out_up;
+	}
+	DEBUG(1, "ret = %d\n", ret);
+out_up:
+	mutex_unlock(&ar->lock);
+	return ret;
+}
+
+static int ar_querycap(struct file *file, void  *priv,
+					struct v4l2_capability *vcap)
+{
+	struct ar *ar = video_drvdata(file);
+
+	strlcpy(vcap->driver, ar->vdev.name, sizeof(vcap->driver));
+	strlcpy(vcap->card, "Colour AR VGA", sizeof(vcap->card));
+	strlcpy(vcap->bus_info, "Platform", sizeof(vcap->bus_info));
+	vcap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE;
+	vcap->capabilities = vcap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	return 0;
+}
+
+static int ar_enum_input(struct file *file, void *fh, struct v4l2_input *vin)
+{
+	if (vin->index > 0)
+		return -EINVAL;
+	strlcpy(vin->name, "Camera", sizeof(vin->name));
+	vin->type = V4L2_INPUT_TYPE_CAMERA;
+	vin->audioset = 0;
+	vin->tuner = 0;
+	vin->std = V4L2_STD_ALL;
+	vin->status = 0;
+	return 0;
+}
+
+static int ar_g_input(struct file *file, void *fh, unsigned int *inp)
+{
+	*inp = 0;
+	return 0;
+}
+
+static int ar_s_input(struct file *file, void *fh, unsigned int inp)
+{
+	return inp ? -EINVAL : 0;
+}
+
+static int ar_g_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *fmt)
+{
+	struct ar *ar = video_drvdata(file);
+	struct v4l2_pix_format *pix = &fmt->fmt.pix;
+
+	pix->width = ar->width;
+	pix->height = ar->height;
+	pix->pixelformat = V4L2_PIX_FMT_YUV422P;
+	pix->field = (ar->mode == AR_MODE_NORMAL) ? V4L2_FIELD_NONE : V4L2_FIELD_INTERLACED;
+	pix->bytesperline = ar->width;
+	pix->sizeimage = 2 * ar->width * ar->height;
+	/* Just a guess */
+	pix->colorspace = V4L2_COLORSPACE_SMPTE170M;
+	return 0;
+}
+
+static int ar_try_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *fmt)
+{
+	struct ar *ar = video_drvdata(file);
+	struct v4l2_pix_format *pix = &fmt->fmt.pix;
+
+	if (pix->height <= AR_HEIGHT_QVGA || pix->width <= AR_WIDTH_QVGA) {
+		pix->height = AR_HEIGHT_QVGA;
+		pix->width = AR_WIDTH_QVGA;
+		pix->field = V4L2_FIELD_INTERLACED;
+	} else {
+		pix->height = AR_HEIGHT_VGA;
+		pix->width = AR_WIDTH_VGA;
+		pix->field = vga_interlace ? V4L2_FIELD_INTERLACED : V4L2_FIELD_NONE;
+	}
+	pix->pixelformat = V4L2_PIX_FMT_YUV422P;
+	pix->bytesperline = ar->width;
+	pix->sizeimage = 2 * ar->width * ar->height;
+	/* Just a guess */
+	pix->colorspace = V4L2_COLORSPACE_SMPTE170M;
+	return 0;
+}
+
+static int ar_s_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *fmt)
+{
+	struct ar *ar = video_drvdata(file);
+	struct v4l2_pix_format *pix = &fmt->fmt.pix;
+	int ret = ar_try_fmt_vid_cap(file, fh, fmt);
+
+	if (ret)
+		return ret;
+	mutex_lock(&ar->lock);
+	ar->width = pix->width;
+	ar->height = pix->height;
+	if (ar->width == AR_WIDTH_VGA) {
+		ar->size = AR_SIZE_VGA;
+		ar->frame_bytes = AR_FRAME_BYTES_VGA;
+		ar->line_bytes = AR_LINE_BYTES_VGA;
+		if (vga_interlace)
+			ar->mode = AR_MODE_INTERLACE;
+		else
+			ar->mode = AR_MODE_NORMAL;
+	} else {
+		ar->size = AR_SIZE_QVGA;
+		ar->frame_bytes = AR_FRAME_BYTES_QVGA;
+		ar->line_bytes = AR_LINE_BYTES_QVGA;
+		ar->mode = AR_MODE_INTERLACE;
+	}
+	/* Ok we figured out what to use from our wide choice */
+	mutex_unlock(&ar->lock);
+	return 0;
+}
+
+static int ar_enum_fmt_vid_cap(struct file *file, void *fh, struct v4l2_fmtdesc *fmt)
+{
+	static struct v4l2_fmtdesc formats[] = {
+		{ 0, 0, 0,
+		  "YUV 4:2:2 Planar", V4L2_PIX_FMT_YUV422P,
+		  { 0, 0, 0, 0 }
+		},
+	};
+	enum v4l2_buf_type type = fmt->type;
+
+	if (fmt->index > 0)
+		return -EINVAL;
+
+	*fmt = formats[fmt->index];
+	fmt->type = type;
+	return 0;
+}
+
+#if USE_INT
+/*
+ * Interrupt handler
+ */
+static void ar_interrupt(int irq, void *dev)
+{
+	struct ar *ar = dev;
+	unsigned int line_count;
+	unsigned int line_number;
+	unsigned int arvcr1;
+
+	line_count = ar_inl(ARVHCOUNT);			/* line number */
+	if (ar->mode == AR_MODE_INTERLACE && ar->size == AR_SIZE_VGA) {
+		/* operations for interlace mode */
+		if (line_count < (AR_HEIGHT_VGA / 2)) 	/* even line */
+			line_number = (line_count << 1);
+		else 					/* odd line */
+			line_number =
+			(((line_count - (AR_HEIGHT_VGA / 2)) << 1) + 1);
+	} else {
+		line_number = line_count;
+	}
+
+	if (line_number == 0) {
+		/*
+		 * It is an interrupt for line 0.
+		 * we have to start capture.
+		 */
+		disable_dma();
+#if 0
+		ar_outl(ar->line_buff, M32R_DMA0CDA_PORTL);	/* needless? */
+#endif
+		memcpy(ar->frame[0], ar->line_buff, ar->line_bytes);
+#if 0
+		ar_outl(0xa1861300, M32R_DMA0CR0_PORTL);
+#endif
+		enable_dma();
+		ar->start_capture = 1;			/* during capture */
+		return;
+	}
+
+	if (ar->start_capture == 1 && line_number <= (ar->height - 1)) {
+		disable_dma();
+		memcpy(ar->frame[line_number], ar->line_buff, ar->line_bytes);
+
+		/*
+		 * if captured all line of a frame, disable AR interrupt
+		 * and wake a process up.
+		 */
+		if (line_number == (ar->height - 1)) { 	/* end  of line */
+
+			ar->start_capture = 0;
+
+			/* disable AR interrupt request */
+			arvcr1 = ar_inl(ARVCR1);
+			arvcr1 &= ~ARVCR1_HIEN;		/* clear int. flag */
+			ar_outl(arvcr1, ARVCR1);	/* disable */
+			wake_up_interruptible(&ar->wait);
+		} else {
+#if 0
+			ar_outl(ar->line_buff, M32R_DMA0CDA_PORTL);
+			ar_outl(0xa1861300, M32R_DMA0CR0_PORTL);
+#endif
+			enable_dma();
+		}
+	}
+}
+#endif
+
+/*
+ * ar_initialize()
+ * 	ar_initialize() is called by video_register_device() and
+ *	initializes AR LSI and peripherals.
+ *
+ *	-1 is returned in all failures.
+ *	0 is returned in success.
+ *
+ */
+static int ar_initialize(struct ar *ar)
+{
+	unsigned long cr = 0;
+	int i, found = 0;
+
+	DEBUG(1, "ar_initialize:\n");
+
+	/*
+	 * initialize AR LSI
+	 */
+	ar_outl(0, ARVCR0);		/* assert reset of AR LSI */
+	for (i = 0; i < 0x18; i++)	/* wait for over 10 cycles @ 27MHz */
+		cpu_relax();
+	ar_outl(ARVCR0_RST, ARVCR0);	/* negate reset of AR LSI (enable) */
+	for (i = 0; i < 0x40d; i++)	/* wait for over 420 cycles @ 27MHz */
+		cpu_relax();
+
+	/* AR uses INT3 of CPU as interrupt pin. */
+	ar_outl(ARINTSEL_INT3, ARINTSEL);
+
+	if (ar->size == AR_SIZE_QVGA)
+		cr |= ARVCR1_QVGA;
+	if (ar->mode == AR_MODE_NORMAL)
+		cr |= ARVCR1_NORMAL;
+	ar_outl(cr, ARVCR1);
+
+	/*
+	 * Initialize IIC so that CPU can communicate with AR LSI,
+	 * and send boot commands to AR LSI.
+	 */
+	init_iic();
+
+	for (i = 0; i < 0x100000; i++) {	/* > 0xa1d10,  56ms */
+		if ((ar_inl(ARVCR0) & ARVCR0_VDS)) {	/* VSYNC */
+			found = 1;
+			break;
+		}
+	}
+
+	if (found == 0)
+		return -ENODEV;
+
+	v4l2_info(&ar->v4l2_dev, "Initializing ");
+
+	iic(2, 0x78, 0x11, 0x01, 0x00);	/* start */
+	iic(3, 0x78, 0x12, 0x00, 0x06);
+	iic(3, 0x78, 0x12, 0x12, 0x30);
+	iic(3, 0x78, 0x12, 0x15, 0x58);
+	iic(3, 0x78, 0x12, 0x17, 0x30);
+	printk(KERN_CONT ".");
+	iic(3, 0x78, 0x12, 0x1a, 0x97);
+	iic(3, 0x78, 0x12, 0x1b, 0xff);
+	iic(3, 0x78, 0x12, 0x1c, 0xff);
+	iic(3, 0x78, 0x12, 0x26, 0x10);
+	iic(3, 0x78, 0x12, 0x27, 0x00);
+	printk(KERN_CONT ".");
+	iic(2, 0x78, 0x34, 0x02, 0x00);
+	iic(2, 0x78, 0x7a, 0x10, 0x00);
+	iic(2, 0x78, 0x80, 0x39, 0x00);
+	iic(2, 0x78, 0x81, 0xe6, 0x00);
+	iic(2, 0x78, 0x8d, 0x00, 0x00);
+	printk(KERN_CONT ".");
+	iic(2, 0x78, 0x8e, 0x0c, 0x00);
+	iic(2, 0x78, 0x8f, 0x00, 0x00);
+#if 0
+	iic(2, 0x78, 0x90, 0x00, 0x00);	/* AWB on=1 off=0 */
+#endif
+	iic(2, 0x78, 0x93, 0x01, 0x00);
+	iic(2, 0x78, 0x94, 0xcd, 0x00);
+	iic(2, 0x78, 0x95, 0x00, 0x00);
+	printk(KERN_CONT ".");
+	iic(2, 0x78, 0x96, 0xa0, 0x00);
+	iic(2, 0x78, 0x97, 0x00, 0x00);
+	iic(2, 0x78, 0x98, 0x60, 0x00);
+	iic(2, 0x78, 0x99, 0x01, 0x00);
+	iic(2, 0x78, 0x9a, 0x19, 0x00);
+	printk(KERN_CONT ".");
+	iic(2, 0x78, 0x9b, 0x02, 0x00);
+	iic(2, 0x78, 0x9c, 0xe8, 0x00);
+	iic(2, 0x78, 0x9d, 0x02, 0x00);
+	iic(2, 0x78, 0x9e, 0x2e, 0x00);
+	iic(2, 0x78, 0xb8, 0x78, 0x00);
+	iic(2, 0x78, 0xba, 0x05, 0x00);
+#if 0
+	iic(2, 0x78, 0x83, 0x8c, 0x00);	/* brightness */
+#endif
+	printk(KERN_CONT ".");
+
+	/* color correction */
+	iic(3, 0x78, 0x49, 0x00, 0x95);	/* a		*/
+	iic(3, 0x78, 0x49, 0x01, 0x96);	/* b		*/
+	iic(3, 0x78, 0x49, 0x03, 0x85);	/* c		*/
+	iic(3, 0x78, 0x49, 0x04, 0x97);	/* d		*/
+	iic(3, 0x78, 0x49, 0x02, 0x7e);	/* e(Lo)	*/
+	iic(3, 0x78, 0x49, 0x05, 0xa4);	/* f(Lo)	*/
+	iic(3, 0x78, 0x49, 0x06, 0x04);	/* e(Hi)	*/
+	iic(3, 0x78, 0x49, 0x07, 0x04);	/* e(Hi)	*/
+	iic(2, 0x78, 0x48, 0x01, 0x00);	/* on=1 off=0	*/
+
+	printk(KERN_CONT ".");
+	iic(2, 0x78, 0x11, 0x00, 0x00);	/* end */
+	printk(KERN_CONT " done\n");
+	return 0;
+}
+
+
+/****************************************************************************
+ *
+ * Video4Linux Module functions
+ *
+ ****************************************************************************/
+
+static const struct v4l2_file_operations ar_fops = {
+	.owner		= THIS_MODULE,
+	.open		= v4l2_fh_open,
+	.release	= v4l2_fh_release,
+	.read		= ar_read,
+	.unlocked_ioctl	= video_ioctl2,
+};
+
+static const struct v4l2_ioctl_ops ar_ioctl_ops = {
+	.vidioc_querycap    		    = ar_querycap,
+	.vidioc_g_input      		    = ar_g_input,
+	.vidioc_s_input      		    = ar_s_input,
+	.vidioc_enum_input   		    = ar_enum_input,
+	.vidioc_enum_fmt_vid_cap 	    = ar_enum_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap 		    = ar_g_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap  		    = ar_s_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap  	    = ar_try_fmt_vid_cap,
+};
+
+#define ALIGN4(x)	((((int)(x)) & 0x3) == 0)
+
+static int __init ar_init(void)
+{
+	struct ar *ar;
+	struct v4l2_device *v4l2_dev;
+	int ret;
+	int i;
+
+	ar = &ardev;
+	v4l2_dev = &ar->v4l2_dev;
+	strlcpy(v4l2_dev->name, "arv", sizeof(v4l2_dev->name));
+	v4l2_info(v4l2_dev, "Colour AR VGA driver %s\n", VERSION);
+
+	ret = v4l2_device_register(NULL, v4l2_dev);
+	if (ret < 0) {
+		v4l2_err(v4l2_dev, "Could not register v4l2_device\n");
+		return ret;
+	}
+	ret = -EIO;
+
+#if USE_INT
+	/* allocate a DMA buffer for 1 line.  */
+	ar->line_buff = kmalloc(MAX_AR_LINE_BYTES, GFP_KERNEL | GFP_DMA);
+	if (ar->line_buff == NULL || !ALIGN4(ar->line_buff)) {
+		v4l2_err(v4l2_dev, "buffer allocation failed for DMA.\n");
+		ret = -ENOMEM;
+		goto out_end;
+	}
+#endif
+	/* allocate buffers for a frame */
+	for (i = 0; i < MAX_AR_HEIGHT; i++) {
+		ar->frame[i] = kmalloc(MAX_AR_LINE_BYTES, GFP_KERNEL);
+		if (ar->frame[i] == NULL || !ALIGN4(ar->frame[i])) {
+			v4l2_err(v4l2_dev, "buffer allocation failed for frame.\n");
+			ret = -ENOMEM;
+			goto out_line_buff;
+		}
+	}
+
+	strlcpy(ar->vdev.name, "Colour AR VGA", sizeof(ar->vdev.name));
+	ar->vdev.v4l2_dev = v4l2_dev;
+	ar->vdev.fops = &ar_fops;
+	ar->vdev.ioctl_ops = &ar_ioctl_ops;
+	ar->vdev.release = video_device_release_empty;
+	video_set_drvdata(&ar->vdev, ar);
+
+	if (vga) {
+		ar->width 	= AR_WIDTH_VGA;
+		ar->height 	= AR_HEIGHT_VGA;
+		ar->size 	= AR_SIZE_VGA;
+		ar->frame_bytes = AR_FRAME_BYTES_VGA;
+		ar->line_bytes	= AR_LINE_BYTES_VGA;
+		if (vga_interlace)
+			ar->mode = AR_MODE_INTERLACE;
+		else
+			ar->mode = AR_MODE_NORMAL;
+	} else {
+		ar->width 	= AR_WIDTH_QVGA;
+		ar->height 	= AR_HEIGHT_QVGA;
+		ar->size 	= AR_SIZE_QVGA;
+		ar->frame_bytes = AR_FRAME_BYTES_QVGA;
+		ar->line_bytes	= AR_LINE_BYTES_QVGA;
+		ar->mode	= AR_MODE_INTERLACE;
+	}
+	mutex_init(&ar->lock);
+	init_waitqueue_head(&ar->wait);
+
+#if USE_INT
+	if (request_irq(M32R_IRQ_INT3, ar_interrupt, 0, "arv", ar)) {
+		v4l2_err("request_irq(%d) failed.\n", M32R_IRQ_INT3);
+		ret = -EIO;
+		goto out_irq;
+	}
+#endif
+
+	if (ar_initialize(ar) != 0) {
+		v4l2_err(v4l2_dev, "M64278 not found.\n");
+		ret = -ENODEV;
+		goto out_dev;
+	}
+
+	/*
+	 * ok, we can initialize h/w according to parameters,
+	 * so register video device as a frame grabber type.
+	 * device is named "video[0-64]".
+	 * video_register_device() initializes h/w using ar_initialize().
+	 */
+	if (video_register_device(&ar->vdev, VFL_TYPE_GRABBER, video_nr) != 0) {
+		/* return -1, -ENFILE(full) or others */
+		v4l2_err(v4l2_dev, "register video (Colour AR) failed.\n");
+		ret = -ENODEV;
+		goto out_dev;
+	}
+
+	v4l2_info(v4l2_dev, "%s: Found M64278 VGA (IRQ %d, Freq %dMHz).\n",
+		video_device_node_name(&ar->vdev), M32R_IRQ_INT3, freq);
+
+	return 0;
+
+out_dev:
+#if USE_INT
+	free_irq(M32R_IRQ_INT3, ar);
+
+out_irq:
+#endif
+	for (i = 0; i < MAX_AR_HEIGHT; i++)
+		kfree(ar->frame[i]);
+
+out_line_buff:
+#if USE_INT
+	kfree(ar->line_buff);
+
+out_end:
+#endif
+	v4l2_device_unregister(&ar->v4l2_dev);
+	return ret;
+}
+
+
+static int __init ar_init_module(void)
+{
+	freq = (boot_cpu_data.bus_clock / 1000000);
+	printk(KERN_INFO "arv: Bus clock %d\n", freq);
+	if (freq != 50 && freq != 75)
+		freq = DEFAULT_FREQ;
+	return ar_init();
+}
+
+static void __exit ar_cleanup_module(void)
+{
+	struct ar *ar;
+	int i;
+
+	ar = &ardev;
+	video_unregister_device(&ar->vdev);
+#if USE_INT
+	free_irq(M32R_IRQ_INT3, ar);
+#endif
+	for (i = 0; i < MAX_AR_HEIGHT; i++)
+		kfree(ar->frame[i]);
+#if USE_INT
+	kfree(ar->line_buff);
+#endif
+	v4l2_device_unregister(&ar->v4l2_dev);
+}
+
+module_init(ar_init_module);
+module_exit(ar_cleanup_module);
+
+MODULE_AUTHOR("Takeo Takahashi <takahashi.takeo@renesas.com>");
+MODULE_DESCRIPTION("Colour AR M64278(VGA) for Video4Linux");
+MODULE_LICENSE("GPL");
+MODULE_VERSION(VERSION);
diff --git a/drivers/media/platform/blackfin/Kconfig b/drivers/media/platform/blackfin/Kconfig
new file mode 100644
index 0000000..68fa901
--- /dev/null
+++ b/drivers/media/platform/blackfin/Kconfig
@@ -0,0 +1,16 @@
+config VIDEO_BLACKFIN_CAPTURE
+	tristate "Blackfin Video Capture Driver"
+	depends on VIDEO_V4L2 && BLACKFIN && I2C
+	depends on HAS_DMA
+	select VIDEOBUF2_DMA_CONTIG
+	help
+	  V4L2 bridge driver for Blackfin video capture device.
+	  Choose PPI or EPPI as its interface.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called bfin_capture.
+
+config VIDEO_BLACKFIN_PPI
+	tristate
+	depends on VIDEO_BLACKFIN_CAPTURE
+	default VIDEO_BLACKFIN_CAPTURE
diff --git a/drivers/media/platform/blackfin/Makefile b/drivers/media/platform/blackfin/Makefile
new file mode 100644
index 0000000..30421bc
--- /dev/null
+++ b/drivers/media/platform/blackfin/Makefile
@@ -0,0 +1,2 @@
+obj-$(CONFIG_VIDEO_BLACKFIN_CAPTURE) += bfin_capture.o
+obj-$(CONFIG_VIDEO_BLACKFIN_PPI)     += ppi.o
diff --git a/drivers/media/platform/blackfin/bfin_capture.c b/drivers/media/platform/blackfin/bfin_capture.c
new file mode 100644
index 0000000..7764b9c
--- /dev/null
+++ b/drivers/media/platform/blackfin/bfin_capture.c
@@ -0,0 +1,1008 @@
+/*
+ * Analog Devices video capture driver
+ *
+ * Copyright (c) 2011 Analog Devices Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/completion.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/mm.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/time.h>
+#include <linux/types.h>
+
+#include <media/v4l2-common.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include <asm/dma.h>
+
+#include <media/blackfin/bfin_capture.h>
+#include <media/blackfin/ppi.h>
+
+#define CAPTURE_DRV_NAME        "bfin_capture"
+
+struct bcap_format {
+	char *desc;
+	u32 pixelformat;
+	u32 mbus_code;
+	int bpp; /* bits per pixel */
+	int dlen; /* data length for ppi in bits */
+};
+
+struct bcap_buffer {
+	struct vb2_v4l2_buffer vb;
+	struct list_head list;
+};
+
+struct bcap_device {
+	/* capture device instance */
+	struct v4l2_device v4l2_dev;
+	/* v4l2 control handler */
+	struct v4l2_ctrl_handler ctrl_handler;
+	/* device node data */
+	struct video_device video_dev;
+	/* sub device instance */
+	struct v4l2_subdev *sd;
+	/* capture config */
+	struct bfin_capture_config *cfg;
+	/* ppi interface */
+	struct ppi_if *ppi;
+	/* current input */
+	unsigned int cur_input;
+	/* current selected standard */
+	v4l2_std_id std;
+	/* current selected dv_timings */
+	struct v4l2_dv_timings dv_timings;
+	/* used to store pixel format */
+	struct v4l2_pix_format fmt;
+	/* bits per pixel*/
+	int bpp;
+	/* data length for ppi in bits */
+	int dlen;
+	/* used to store sensor supported format */
+	struct bcap_format *sensor_formats;
+	/* number of sensor formats array */
+	int num_sensor_formats;
+	/* pointing to current video buffer */
+	struct bcap_buffer *cur_frm;
+	/* buffer queue used in videobuf2 */
+	struct vb2_queue buffer_queue;
+	/* allocator-specific contexts for each plane */
+	struct vb2_alloc_ctx *alloc_ctx;
+	/* queue of filled frames */
+	struct list_head dma_queue;
+	/* used in videobuf2 callback */
+	spinlock_t lock;
+	/* used to access capture device */
+	struct mutex mutex;
+	/* used to wait ppi to complete one transfer */
+	struct completion comp;
+	/* prepare to stop */
+	bool stop;
+	/* vb2 buffer sequence counter */
+	unsigned sequence;
+};
+
+static const struct bcap_format bcap_formats[] = {
+	{
+		.desc        = "YCbCr 4:2:2 Interleaved UYVY",
+		.pixelformat = V4L2_PIX_FMT_UYVY,
+		.mbus_code   = MEDIA_BUS_FMT_UYVY8_2X8,
+		.bpp         = 16,
+		.dlen        = 8,
+	},
+	{
+		.desc        = "YCbCr 4:2:2 Interleaved YUYV",
+		.pixelformat = V4L2_PIX_FMT_YUYV,
+		.mbus_code   = MEDIA_BUS_FMT_YUYV8_2X8,
+		.bpp         = 16,
+		.dlen        = 8,
+	},
+	{
+		.desc        = "YCbCr 4:2:2 Interleaved UYVY",
+		.pixelformat = V4L2_PIX_FMT_UYVY,
+		.mbus_code   = MEDIA_BUS_FMT_UYVY8_1X16,
+		.bpp         = 16,
+		.dlen        = 16,
+	},
+	{
+		.desc        = "RGB 565",
+		.pixelformat = V4L2_PIX_FMT_RGB565,
+		.mbus_code   = MEDIA_BUS_FMT_RGB565_2X8_LE,
+		.bpp         = 16,
+		.dlen        = 8,
+	},
+	{
+		.desc        = "RGB 444",
+		.pixelformat = V4L2_PIX_FMT_RGB444,
+		.mbus_code   = MEDIA_BUS_FMT_RGB444_2X8_PADHI_LE,
+		.bpp         = 16,
+		.dlen        = 8,
+	},
+
+};
+#define BCAP_MAX_FMTS ARRAY_SIZE(bcap_formats)
+
+static irqreturn_t bcap_isr(int irq, void *dev_id);
+
+static struct bcap_buffer *to_bcap_vb(struct vb2_v4l2_buffer *vb)
+{
+	return container_of(vb, struct bcap_buffer, vb);
+}
+
+static int bcap_init_sensor_formats(struct bcap_device *bcap_dev)
+{
+	struct v4l2_subdev_mbus_code_enum code = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct bcap_format *sf;
+	unsigned int num_formats = 0;
+	int i, j;
+
+	while (!v4l2_subdev_call(bcap_dev->sd, pad,
+				enum_mbus_code, NULL, &code)) {
+		num_formats++;
+		code.index++;
+	}
+	if (!num_formats)
+		return -ENXIO;
+
+	sf = kzalloc(num_formats * sizeof(*sf), GFP_KERNEL);
+	if (!sf)
+		return -ENOMEM;
+
+	for (i = 0; i < num_formats; i++) {
+		code.index = i;
+		v4l2_subdev_call(bcap_dev->sd, pad,
+				enum_mbus_code, NULL, &code);
+		for (j = 0; j < BCAP_MAX_FMTS; j++)
+			if (code.code == bcap_formats[j].mbus_code)
+				break;
+		if (j == BCAP_MAX_FMTS) {
+			/* we don't allow this sensor working with our bridge */
+			kfree(sf);
+			return -EINVAL;
+		}
+		sf[i] = bcap_formats[j];
+	}
+	bcap_dev->sensor_formats = sf;
+	bcap_dev->num_sensor_formats = num_formats;
+	return 0;
+}
+
+static void bcap_free_sensor_formats(struct bcap_device *bcap_dev)
+{
+	bcap_dev->num_sensor_formats = 0;
+	kfree(bcap_dev->sensor_formats);
+	bcap_dev->sensor_formats = NULL;
+}
+
+static int bcap_queue_setup(struct vb2_queue *vq,
+				const void *parg,
+				unsigned int *nbuffers, unsigned int *nplanes,
+				unsigned int sizes[], void *alloc_ctxs[])
+{
+	const struct v4l2_format *fmt = parg;
+	struct bcap_device *bcap_dev = vb2_get_drv_priv(vq);
+
+	if (fmt && fmt->fmt.pix.sizeimage < bcap_dev->fmt.sizeimage)
+		return -EINVAL;
+
+	if (vq->num_buffers + *nbuffers < 2)
+		*nbuffers = 2;
+
+	*nplanes = 1;
+	sizes[0] = fmt ? fmt->fmt.pix.sizeimage : bcap_dev->fmt.sizeimage;
+	alloc_ctxs[0] = bcap_dev->alloc_ctx;
+
+	return 0;
+}
+
+static int bcap_buffer_prepare(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct bcap_device *bcap_dev = vb2_get_drv_priv(vb->vb2_queue);
+	unsigned long size = bcap_dev->fmt.sizeimage;
+
+	if (vb2_plane_size(vb, 0) < size) {
+		v4l2_err(&bcap_dev->v4l2_dev, "buffer too small (%lu < %lu)\n",
+				vb2_plane_size(vb, 0), size);
+		return -EINVAL;
+	}
+	vb2_set_plane_payload(vb, 0, size);
+
+	vbuf->field = bcap_dev->fmt.field;
+
+	return 0;
+}
+
+static void bcap_buffer_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct bcap_device *bcap_dev = vb2_get_drv_priv(vb->vb2_queue);
+	struct bcap_buffer *buf = to_bcap_vb(vbuf);
+	unsigned long flags;
+
+	spin_lock_irqsave(&bcap_dev->lock, flags);
+	list_add_tail(&buf->list, &bcap_dev->dma_queue);
+	spin_unlock_irqrestore(&bcap_dev->lock, flags);
+}
+
+static void bcap_buffer_cleanup(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct bcap_device *bcap_dev = vb2_get_drv_priv(vb->vb2_queue);
+	struct bcap_buffer *buf = to_bcap_vb(vbuf);
+	unsigned long flags;
+
+	spin_lock_irqsave(&bcap_dev->lock, flags);
+	list_del_init(&buf->list);
+	spin_unlock_irqrestore(&bcap_dev->lock, flags);
+}
+
+static int bcap_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct bcap_device *bcap_dev = vb2_get_drv_priv(vq);
+	struct ppi_if *ppi = bcap_dev->ppi;
+	struct bcap_buffer *buf, *tmp;
+	struct ppi_params params;
+	dma_addr_t addr;
+	int ret;
+
+	/* enable streamon on the sub device */
+	ret = v4l2_subdev_call(bcap_dev->sd, video, s_stream, 1);
+	if (ret && (ret != -ENOIOCTLCMD)) {
+		v4l2_err(&bcap_dev->v4l2_dev, "stream on failed in subdev\n");
+		goto err;
+	}
+
+	/* set ppi params */
+	params.width = bcap_dev->fmt.width;
+	params.height = bcap_dev->fmt.height;
+	params.bpp = bcap_dev->bpp;
+	params.dlen = bcap_dev->dlen;
+	params.ppi_control = bcap_dev->cfg->ppi_control;
+	params.int_mask = bcap_dev->cfg->int_mask;
+	if (bcap_dev->cfg->inputs[bcap_dev->cur_input].capabilities
+			& V4L2_IN_CAP_DV_TIMINGS) {
+		struct v4l2_bt_timings *bt = &bcap_dev->dv_timings.bt;
+
+		params.hdelay = bt->hsync + bt->hbackporch;
+		params.vdelay = bt->vsync + bt->vbackporch;
+		params.line = V4L2_DV_BT_FRAME_WIDTH(bt);
+		params.frame = V4L2_DV_BT_FRAME_HEIGHT(bt);
+	} else if (bcap_dev->cfg->inputs[bcap_dev->cur_input].capabilities
+			& V4L2_IN_CAP_STD) {
+		params.hdelay = 0;
+		params.vdelay = 0;
+		if (bcap_dev->std & V4L2_STD_525_60) {
+			params.line = 858;
+			params.frame = 525;
+		} else {
+			params.line = 864;
+			params.frame = 625;
+		}
+	} else {
+		params.hdelay = 0;
+		params.vdelay = 0;
+		params.line = params.width + bcap_dev->cfg->blank_pixels;
+		params.frame = params.height;
+	}
+	ret = ppi->ops->set_params(ppi, &params);
+	if (ret < 0) {
+		v4l2_err(&bcap_dev->v4l2_dev,
+				"Error in setting ppi params\n");
+		goto err;
+	}
+
+	/* attach ppi DMA irq handler */
+	ret = ppi->ops->attach_irq(ppi, bcap_isr);
+	if (ret < 0) {
+		v4l2_err(&bcap_dev->v4l2_dev,
+				"Error in attaching interrupt handler\n");
+		goto err;
+	}
+
+	bcap_dev->sequence = 0;
+
+	reinit_completion(&bcap_dev->comp);
+	bcap_dev->stop = false;
+
+	/* get the next frame from the dma queue */
+	bcap_dev->cur_frm = list_entry(bcap_dev->dma_queue.next,
+					struct bcap_buffer, list);
+	/* remove buffer from the dma queue */
+	list_del_init(&bcap_dev->cur_frm->list);
+	addr = vb2_dma_contig_plane_dma_addr(&bcap_dev->cur_frm->vb.vb2_buf,
+						0);
+	/* update DMA address */
+	ppi->ops->update_addr(ppi, (unsigned long)addr);
+	/* enable ppi */
+	ppi->ops->start(ppi);
+
+	return 0;
+
+err:
+	list_for_each_entry_safe(buf, tmp, &bcap_dev->dma_queue, list) {
+		list_del(&buf->list);
+		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_QUEUED);
+	}
+
+	return ret;
+}
+
+static void bcap_stop_streaming(struct vb2_queue *vq)
+{
+	struct bcap_device *bcap_dev = vb2_get_drv_priv(vq);
+	struct ppi_if *ppi = bcap_dev->ppi;
+	int ret;
+
+	bcap_dev->stop = true;
+	wait_for_completion(&bcap_dev->comp);
+	ppi->ops->stop(ppi);
+	ppi->ops->detach_irq(ppi);
+	ret = v4l2_subdev_call(bcap_dev->sd, video, s_stream, 0);
+	if (ret && (ret != -ENOIOCTLCMD))
+		v4l2_err(&bcap_dev->v4l2_dev,
+				"stream off failed in subdev\n");
+
+	/* release all active buffers */
+	if (bcap_dev->cur_frm)
+		vb2_buffer_done(&bcap_dev->cur_frm->vb.vb2_buf,
+				VB2_BUF_STATE_ERROR);
+
+	while (!list_empty(&bcap_dev->dma_queue)) {
+		bcap_dev->cur_frm = list_entry(bcap_dev->dma_queue.next,
+						struct bcap_buffer, list);
+		list_del_init(&bcap_dev->cur_frm->list);
+		vb2_buffer_done(&bcap_dev->cur_frm->vb.vb2_buf,
+				VB2_BUF_STATE_ERROR);
+	}
+}
+
+static struct vb2_ops bcap_video_qops = {
+	.queue_setup            = bcap_queue_setup,
+	.buf_prepare            = bcap_buffer_prepare,
+	.buf_cleanup            = bcap_buffer_cleanup,
+	.buf_queue              = bcap_buffer_queue,
+	.wait_prepare           = vb2_ops_wait_prepare,
+	.wait_finish            = vb2_ops_wait_finish,
+	.start_streaming        = bcap_start_streaming,
+	.stop_streaming         = bcap_stop_streaming,
+};
+
+static irqreturn_t bcap_isr(int irq, void *dev_id)
+{
+	struct ppi_if *ppi = dev_id;
+	struct bcap_device *bcap_dev = ppi->priv;
+	struct vb2_v4l2_buffer *vbuf = &bcap_dev->cur_frm->vb;
+	struct vb2_buffer *vb = &vbuf->vb2_buf;
+	dma_addr_t addr;
+
+	spin_lock(&bcap_dev->lock);
+
+	if (!list_empty(&bcap_dev->dma_queue)) {
+		v4l2_get_timestamp(&vbuf->timestamp);
+		if (ppi->err) {
+			vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
+			ppi->err = false;
+		} else {
+			vbuf->sequence = bcap_dev->sequence++;
+			vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
+		}
+		bcap_dev->cur_frm = list_entry(bcap_dev->dma_queue.next,
+				struct bcap_buffer, list);
+		list_del_init(&bcap_dev->cur_frm->list);
+	} else {
+		/* clear error flag, we will get a new frame */
+		if (ppi->err)
+			ppi->err = false;
+	}
+
+	ppi->ops->stop(ppi);
+
+	if (bcap_dev->stop) {
+		complete(&bcap_dev->comp);
+	} else {
+		addr = vb2_dma_contig_plane_dma_addr(
+				&bcap_dev->cur_frm->vb.vb2_buf, 0);
+		ppi->ops->update_addr(ppi, (unsigned long)addr);
+		ppi->ops->start(ppi);
+	}
+
+	spin_unlock(&bcap_dev->lock);
+
+	return IRQ_HANDLED;
+}
+
+static int bcap_querystd(struct file *file, void *priv, v4l2_std_id *std)
+{
+	struct bcap_device *bcap_dev = video_drvdata(file);
+	struct v4l2_input input;
+
+	input = bcap_dev->cfg->inputs[bcap_dev->cur_input];
+	if (!(input.capabilities & V4L2_IN_CAP_STD))
+		return -ENODATA;
+
+	return v4l2_subdev_call(bcap_dev->sd, video, querystd, std);
+}
+
+static int bcap_g_std(struct file *file, void *priv, v4l2_std_id *std)
+{
+	struct bcap_device *bcap_dev = video_drvdata(file);
+	struct v4l2_input input;
+
+	input = bcap_dev->cfg->inputs[bcap_dev->cur_input];
+	if (!(input.capabilities & V4L2_IN_CAP_STD))
+		return -ENODATA;
+
+	*std = bcap_dev->std;
+	return 0;
+}
+
+static int bcap_s_std(struct file *file, void *priv, v4l2_std_id std)
+{
+	struct bcap_device *bcap_dev = video_drvdata(file);
+	struct v4l2_input input;
+	int ret;
+
+	input = bcap_dev->cfg->inputs[bcap_dev->cur_input];
+	if (!(input.capabilities & V4L2_IN_CAP_STD))
+		return -ENODATA;
+
+	if (vb2_is_busy(&bcap_dev->buffer_queue))
+		return -EBUSY;
+
+	ret = v4l2_subdev_call(bcap_dev->sd, video, s_std, std);
+	if (ret < 0)
+		return ret;
+
+	bcap_dev->std = std;
+	return 0;
+}
+
+static int bcap_enum_dv_timings(struct file *file, void *priv,
+				struct v4l2_enum_dv_timings *timings)
+{
+	struct bcap_device *bcap_dev = video_drvdata(file);
+	struct v4l2_input input;
+
+	input = bcap_dev->cfg->inputs[bcap_dev->cur_input];
+	if (!(input.capabilities & V4L2_IN_CAP_DV_TIMINGS))
+		return -ENODATA;
+
+	timings->pad = 0;
+
+	return v4l2_subdev_call(bcap_dev->sd, pad,
+			enum_dv_timings, timings);
+}
+
+static int bcap_query_dv_timings(struct file *file, void *priv,
+				struct v4l2_dv_timings *timings)
+{
+	struct bcap_device *bcap_dev = video_drvdata(file);
+	struct v4l2_input input;
+
+	input = bcap_dev->cfg->inputs[bcap_dev->cur_input];
+	if (!(input.capabilities & V4L2_IN_CAP_DV_TIMINGS))
+		return -ENODATA;
+
+	return v4l2_subdev_call(bcap_dev->sd, video,
+				query_dv_timings, timings);
+}
+
+static int bcap_g_dv_timings(struct file *file, void *priv,
+				struct v4l2_dv_timings *timings)
+{
+	struct bcap_device *bcap_dev = video_drvdata(file);
+	struct v4l2_input input;
+
+	input = bcap_dev->cfg->inputs[bcap_dev->cur_input];
+	if (!(input.capabilities & V4L2_IN_CAP_DV_TIMINGS))
+		return -ENODATA;
+
+	*timings = bcap_dev->dv_timings;
+	return 0;
+}
+
+static int bcap_s_dv_timings(struct file *file, void *priv,
+				struct v4l2_dv_timings *timings)
+{
+	struct bcap_device *bcap_dev = video_drvdata(file);
+	struct v4l2_input input;
+	int ret;
+
+	input = bcap_dev->cfg->inputs[bcap_dev->cur_input];
+	if (!(input.capabilities & V4L2_IN_CAP_DV_TIMINGS))
+		return -ENODATA;
+
+	if (vb2_is_busy(&bcap_dev->buffer_queue))
+		return -EBUSY;
+
+	ret = v4l2_subdev_call(bcap_dev->sd, video, s_dv_timings, timings);
+	if (ret < 0)
+		return ret;
+
+	bcap_dev->dv_timings = *timings;
+	return 0;
+}
+
+static int bcap_enum_input(struct file *file, void *priv,
+				struct v4l2_input *input)
+{
+	struct bcap_device *bcap_dev = video_drvdata(file);
+	struct bfin_capture_config *config = bcap_dev->cfg;
+	int ret;
+	u32 status;
+
+	if (input->index >= config->num_inputs)
+		return -EINVAL;
+
+	*input = config->inputs[input->index];
+	/* get input status */
+	ret = v4l2_subdev_call(bcap_dev->sd, video, g_input_status, &status);
+	if (!ret)
+		input->status = status;
+	return 0;
+}
+
+static int bcap_g_input(struct file *file, void *priv, unsigned int *index)
+{
+	struct bcap_device *bcap_dev = video_drvdata(file);
+
+	*index = bcap_dev->cur_input;
+	return 0;
+}
+
+static int bcap_s_input(struct file *file, void *priv, unsigned int index)
+{
+	struct bcap_device *bcap_dev = video_drvdata(file);
+	struct bfin_capture_config *config = bcap_dev->cfg;
+	struct bcap_route *route;
+	int ret;
+
+	if (vb2_is_busy(&bcap_dev->buffer_queue))
+		return -EBUSY;
+
+	if (index >= config->num_inputs)
+		return -EINVAL;
+
+	route = &config->routes[index];
+	ret = v4l2_subdev_call(bcap_dev->sd, video, s_routing,
+				route->input, route->output, 0);
+	if ((ret < 0) && (ret != -ENOIOCTLCMD)) {
+		v4l2_err(&bcap_dev->v4l2_dev, "Failed to set input\n");
+		return ret;
+	}
+	bcap_dev->cur_input = index;
+	/* if this route has specific config, update ppi control */
+	if (route->ppi_control)
+		config->ppi_control = route->ppi_control;
+	return 0;
+}
+
+static int bcap_try_format(struct bcap_device *bcap,
+				struct v4l2_pix_format *pixfmt,
+				struct bcap_format *bcap_fmt)
+{
+	struct bcap_format *sf = bcap->sensor_formats;
+	struct bcap_format *fmt = NULL;
+	struct v4l2_subdev_pad_config pad_cfg;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+	};
+	int ret, i;
+
+	for (i = 0; i < bcap->num_sensor_formats; i++) {
+		fmt = &sf[i];
+		if (pixfmt->pixelformat == fmt->pixelformat)
+			break;
+	}
+	if (i == bcap->num_sensor_formats)
+		fmt = &sf[0];
+
+	v4l2_fill_mbus_format(&format.format, pixfmt, fmt->mbus_code);
+	ret = v4l2_subdev_call(bcap->sd, pad, set_fmt, &pad_cfg,
+				&format);
+	if (ret < 0)
+		return ret;
+	v4l2_fill_pix_format(pixfmt, &format.format);
+	if (bcap_fmt) {
+		for (i = 0; i < bcap->num_sensor_formats; i++) {
+			fmt = &sf[i];
+			if (format.format.code == fmt->mbus_code)
+				break;
+		}
+		*bcap_fmt = *fmt;
+	}
+	pixfmt->bytesperline = pixfmt->width * fmt->bpp / 8;
+	pixfmt->sizeimage = pixfmt->bytesperline * pixfmt->height;
+	return 0;
+}
+
+static int bcap_enum_fmt_vid_cap(struct file *file, void  *priv,
+					struct v4l2_fmtdesc *fmt)
+{
+	struct bcap_device *bcap_dev = video_drvdata(file);
+	struct bcap_format *sf = bcap_dev->sensor_formats;
+
+	if (fmt->index >= bcap_dev->num_sensor_formats)
+		return -EINVAL;
+
+	fmt->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	strlcpy(fmt->description,
+		sf[fmt->index].desc,
+		sizeof(fmt->description));
+	fmt->pixelformat = sf[fmt->index].pixelformat;
+	return 0;
+}
+
+static int bcap_try_fmt_vid_cap(struct file *file, void *priv,
+					struct v4l2_format *fmt)
+{
+	struct bcap_device *bcap_dev = video_drvdata(file);
+	struct v4l2_pix_format *pixfmt = &fmt->fmt.pix;
+
+	return bcap_try_format(bcap_dev, pixfmt, NULL);
+}
+
+static int bcap_g_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *fmt)
+{
+	struct bcap_device *bcap_dev = video_drvdata(file);
+
+	fmt->fmt.pix = bcap_dev->fmt;
+	return 0;
+}
+
+static int bcap_s_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *fmt)
+{
+	struct bcap_device *bcap_dev = video_drvdata(file);
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct bcap_format bcap_fmt;
+	struct v4l2_pix_format *pixfmt = &fmt->fmt.pix;
+	int ret;
+
+	if (vb2_is_busy(&bcap_dev->buffer_queue))
+		return -EBUSY;
+
+	/* see if format works */
+	ret = bcap_try_format(bcap_dev, pixfmt, &bcap_fmt);
+	if (ret < 0)
+		return ret;
+
+	v4l2_fill_mbus_format(&format.format, pixfmt, bcap_fmt.mbus_code);
+	ret = v4l2_subdev_call(bcap_dev->sd, pad, set_fmt, NULL, &format);
+	if (ret < 0)
+		return ret;
+	bcap_dev->fmt = *pixfmt;
+	bcap_dev->bpp = bcap_fmt.bpp;
+	bcap_dev->dlen = bcap_fmt.dlen;
+	return 0;
+}
+
+static int bcap_querycap(struct file *file, void  *priv,
+				struct v4l2_capability *cap)
+{
+	struct bcap_device *bcap_dev = video_drvdata(file);
+
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	strlcpy(cap->driver, CAPTURE_DRV_NAME, sizeof(cap->driver));
+	strlcpy(cap->bus_info, "Blackfin Platform", sizeof(cap->bus_info));
+	strlcpy(cap->card, bcap_dev->cfg->card_name, sizeof(cap->card));
+	return 0;
+}
+
+static int bcap_g_parm(struct file *file, void *fh,
+				struct v4l2_streamparm *a)
+{
+	struct bcap_device *bcap_dev = video_drvdata(file);
+
+	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+	return v4l2_subdev_call(bcap_dev->sd, video, g_parm, a);
+}
+
+static int bcap_s_parm(struct file *file, void *fh,
+				struct v4l2_streamparm *a)
+{
+	struct bcap_device *bcap_dev = video_drvdata(file);
+
+	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+	return v4l2_subdev_call(bcap_dev->sd, video, s_parm, a);
+}
+
+static int bcap_log_status(struct file *file, void *priv)
+{
+	struct bcap_device *bcap_dev = video_drvdata(file);
+	/* status for sub devices */
+	v4l2_device_call_all(&bcap_dev->v4l2_dev, 0, core, log_status);
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops bcap_ioctl_ops = {
+	.vidioc_querycap         = bcap_querycap,
+	.vidioc_g_fmt_vid_cap    = bcap_g_fmt_vid_cap,
+	.vidioc_enum_fmt_vid_cap = bcap_enum_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap    = bcap_s_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap  = bcap_try_fmt_vid_cap,
+	.vidioc_enum_input       = bcap_enum_input,
+	.vidioc_g_input          = bcap_g_input,
+	.vidioc_s_input          = bcap_s_input,
+	.vidioc_querystd         = bcap_querystd,
+	.vidioc_s_std            = bcap_s_std,
+	.vidioc_g_std            = bcap_g_std,
+	.vidioc_s_dv_timings     = bcap_s_dv_timings,
+	.vidioc_g_dv_timings     = bcap_g_dv_timings,
+	.vidioc_query_dv_timings = bcap_query_dv_timings,
+	.vidioc_enum_dv_timings  = bcap_enum_dv_timings,
+	.vidioc_reqbufs          = vb2_ioctl_reqbufs,
+	.vidioc_create_bufs      = vb2_ioctl_create_bufs,
+	.vidioc_querybuf         = vb2_ioctl_querybuf,
+	.vidioc_qbuf             = vb2_ioctl_qbuf,
+	.vidioc_dqbuf            = vb2_ioctl_dqbuf,
+	.vidioc_expbuf           = vb2_ioctl_expbuf,
+	.vidioc_streamon         = vb2_ioctl_streamon,
+	.vidioc_streamoff        = vb2_ioctl_streamoff,
+	.vidioc_g_parm           = bcap_g_parm,
+	.vidioc_s_parm           = bcap_s_parm,
+	.vidioc_log_status       = bcap_log_status,
+};
+
+static struct v4l2_file_operations bcap_fops = {
+	.owner = THIS_MODULE,
+	.open = v4l2_fh_open,
+	.release = vb2_fop_release,
+	.unlocked_ioctl = video_ioctl2,
+	.mmap = vb2_fop_mmap,
+#ifndef CONFIG_MMU
+	.get_unmapped_area = vb2_fop_get_unmapped_area,
+#endif
+	.poll = vb2_fop_poll
+};
+
+static int bcap_probe(struct platform_device *pdev)
+{
+	struct bcap_device *bcap_dev;
+	struct video_device *vfd;
+	struct i2c_adapter *i2c_adap;
+	struct bfin_capture_config *config;
+	struct vb2_queue *q;
+	struct bcap_route *route;
+	int ret;
+
+	config = pdev->dev.platform_data;
+	if (!config || !config->num_inputs) {
+		v4l2_err(pdev->dev.driver, "Unable to get board config\n");
+		return -ENODEV;
+	}
+
+	bcap_dev = kzalloc(sizeof(*bcap_dev), GFP_KERNEL);
+	if (!bcap_dev) {
+		v4l2_err(pdev->dev.driver, "Unable to alloc bcap_dev\n");
+		return -ENOMEM;
+	}
+
+	bcap_dev->cfg = config;
+
+	bcap_dev->ppi = ppi_create_instance(pdev, config->ppi_info);
+	if (!bcap_dev->ppi) {
+		v4l2_err(pdev->dev.driver, "Unable to create ppi\n");
+		ret = -ENODEV;
+		goto err_free_dev;
+	}
+	bcap_dev->ppi->priv = bcap_dev;
+
+	bcap_dev->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
+	if (IS_ERR(bcap_dev->alloc_ctx)) {
+		ret = PTR_ERR(bcap_dev->alloc_ctx);
+		goto err_free_ppi;
+	}
+
+	vfd = &bcap_dev->video_dev;
+	/* initialize field of video device */
+	vfd->release            = video_device_release_empty;
+	vfd->fops               = &bcap_fops;
+	vfd->ioctl_ops          = &bcap_ioctl_ops;
+	vfd->tvnorms            = 0;
+	vfd->v4l2_dev           = &bcap_dev->v4l2_dev;
+	strncpy(vfd->name, CAPTURE_DRV_NAME, sizeof(vfd->name));
+
+	ret = v4l2_device_register(&pdev->dev, &bcap_dev->v4l2_dev);
+	if (ret) {
+		v4l2_err(pdev->dev.driver,
+				"Unable to register v4l2 device\n");
+		goto err_cleanup_ctx;
+	}
+	v4l2_info(&bcap_dev->v4l2_dev, "v4l2 device registered\n");
+
+	bcap_dev->v4l2_dev.ctrl_handler = &bcap_dev->ctrl_handler;
+	ret = v4l2_ctrl_handler_init(&bcap_dev->ctrl_handler, 0);
+	if (ret) {
+		v4l2_err(&bcap_dev->v4l2_dev,
+				"Unable to init control handler\n");
+		goto err_unreg_v4l2;
+	}
+
+	spin_lock_init(&bcap_dev->lock);
+	/* initialize queue */
+	q = &bcap_dev->buffer_queue;
+	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	q->io_modes = VB2_MMAP | VB2_DMABUF;
+	q->drv_priv = bcap_dev;
+	q->buf_struct_size = sizeof(struct bcap_buffer);
+	q->ops = &bcap_video_qops;
+	q->mem_ops = &vb2_dma_contig_memops;
+	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	q->lock = &bcap_dev->mutex;
+	q->min_buffers_needed = 1;
+
+	ret = vb2_queue_init(q);
+	if (ret)
+		goto err_free_handler;
+
+	mutex_init(&bcap_dev->mutex);
+	init_completion(&bcap_dev->comp);
+
+	/* init video dma queues */
+	INIT_LIST_HEAD(&bcap_dev->dma_queue);
+
+	vfd->lock = &bcap_dev->mutex;
+	vfd->queue = q;
+
+	/* register video device */
+	ret = video_register_device(&bcap_dev->video_dev, VFL_TYPE_GRABBER, -1);
+	if (ret) {
+		v4l2_err(&bcap_dev->v4l2_dev,
+				"Unable to register video device\n");
+		goto err_free_handler;
+	}
+	video_set_drvdata(&bcap_dev->video_dev, bcap_dev);
+	v4l2_info(&bcap_dev->v4l2_dev, "video device registered as: %s\n",
+			video_device_node_name(vfd));
+
+	/* load up the subdevice */
+	i2c_adap = i2c_get_adapter(config->i2c_adapter_id);
+	if (!i2c_adap) {
+		v4l2_err(&bcap_dev->v4l2_dev,
+				"Unable to find i2c adapter\n");
+		ret = -ENODEV;
+		goto err_unreg_vdev;
+
+	}
+	bcap_dev->sd = v4l2_i2c_new_subdev_board(&bcap_dev->v4l2_dev,
+						 i2c_adap,
+						 &config->board_info,
+						 NULL);
+	if (bcap_dev->sd) {
+		int i;
+
+		/* update tvnorms from the sub devices */
+		for (i = 0; i < config->num_inputs; i++)
+			vfd->tvnorms |= config->inputs[i].std;
+	} else {
+		v4l2_err(&bcap_dev->v4l2_dev,
+				"Unable to register sub device\n");
+		ret = -ENODEV;
+		goto err_unreg_vdev;
+	}
+
+	v4l2_info(&bcap_dev->v4l2_dev, "v4l2 sub device registered\n");
+
+	/*
+	 * explicitly set input, otherwise some boards
+	 * may not work at the state as we expected
+	 */
+	route = &config->routes[0];
+	ret = v4l2_subdev_call(bcap_dev->sd, video, s_routing,
+				route->input, route->output, 0);
+	if ((ret < 0) && (ret != -ENOIOCTLCMD)) {
+		v4l2_err(&bcap_dev->v4l2_dev, "Failed to set input\n");
+		goto err_unreg_vdev;
+	}
+	bcap_dev->cur_input = 0;
+	/* if this route has specific config, update ppi control */
+	if (route->ppi_control)
+		config->ppi_control = route->ppi_control;
+
+	/* now we can probe the default state */
+	if (config->inputs[0].capabilities & V4L2_IN_CAP_STD) {
+		v4l2_std_id std;
+		ret = v4l2_subdev_call(bcap_dev->sd, video, g_std, &std);
+		if (ret) {
+			v4l2_err(&bcap_dev->v4l2_dev,
+					"Unable to get std\n");
+			goto err_unreg_vdev;
+		}
+		bcap_dev->std = std;
+	}
+	if (config->inputs[0].capabilities & V4L2_IN_CAP_DV_TIMINGS) {
+		struct v4l2_dv_timings dv_timings;
+		ret = v4l2_subdev_call(bcap_dev->sd, video,
+				g_dv_timings, &dv_timings);
+		if (ret) {
+			v4l2_err(&bcap_dev->v4l2_dev,
+					"Unable to get dv timings\n");
+			goto err_unreg_vdev;
+		}
+		bcap_dev->dv_timings = dv_timings;
+	}
+	ret = bcap_init_sensor_formats(bcap_dev);
+	if (ret) {
+		v4l2_err(&bcap_dev->v4l2_dev,
+				"Unable to create sensor formats table\n");
+		goto err_unreg_vdev;
+	}
+	return 0;
+err_unreg_vdev:
+	video_unregister_device(&bcap_dev->video_dev);
+err_free_handler:
+	v4l2_ctrl_handler_free(&bcap_dev->ctrl_handler);
+err_unreg_v4l2:
+	v4l2_device_unregister(&bcap_dev->v4l2_dev);
+err_cleanup_ctx:
+	vb2_dma_contig_cleanup_ctx(bcap_dev->alloc_ctx);
+err_free_ppi:
+	ppi_delete_instance(bcap_dev->ppi);
+err_free_dev:
+	kfree(bcap_dev);
+	return ret;
+}
+
+static int bcap_remove(struct platform_device *pdev)
+{
+	struct v4l2_device *v4l2_dev = platform_get_drvdata(pdev);
+	struct bcap_device *bcap_dev = container_of(v4l2_dev,
+						struct bcap_device, v4l2_dev);
+
+	bcap_free_sensor_formats(bcap_dev);
+	video_unregister_device(&bcap_dev->video_dev);
+	v4l2_ctrl_handler_free(&bcap_dev->ctrl_handler);
+	v4l2_device_unregister(v4l2_dev);
+	vb2_dma_contig_cleanup_ctx(bcap_dev->alloc_ctx);
+	ppi_delete_instance(bcap_dev->ppi);
+	kfree(bcap_dev);
+	return 0;
+}
+
+static struct platform_driver bcap_driver = {
+	.driver = {
+		.name  = CAPTURE_DRV_NAME,
+	},
+	.probe = bcap_probe,
+	.remove = bcap_remove,
+};
+module_platform_driver(bcap_driver);
+
+MODULE_DESCRIPTION("Analog Devices blackfin video capture driver");
+MODULE_AUTHOR("Scott Jiang <Scott.Jiang.Linux@gmail.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/platform/blackfin/ppi.c b/drivers/media/platform/blackfin/ppi.c
new file mode 100644
index 0000000..b8f3d9f
--- /dev/null
+++ b/drivers/media/platform/blackfin/ppi.c
@@ -0,0 +1,366 @@
+/*
+ * ppi.c Analog Devices Parallel Peripheral Interface driver
+ *
+ * Copyright (c) 2011 Analog Devices Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+
+#include <asm/bfin_ppi.h>
+#include <asm/blackfin.h>
+#include <asm/cacheflush.h>
+#include <asm/dma.h>
+#include <asm/portmux.h>
+
+#include <media/blackfin/ppi.h>
+
+static int ppi_attach_irq(struct ppi_if *ppi, irq_handler_t handler);
+static void ppi_detach_irq(struct ppi_if *ppi);
+static int ppi_start(struct ppi_if *ppi);
+static int ppi_stop(struct ppi_if *ppi);
+static int ppi_set_params(struct ppi_if *ppi, struct ppi_params *params);
+static void ppi_update_addr(struct ppi_if *ppi, unsigned long addr);
+
+static const struct ppi_ops ppi_ops = {
+	.attach_irq = ppi_attach_irq,
+	.detach_irq = ppi_detach_irq,
+	.start = ppi_start,
+	.stop = ppi_stop,
+	.set_params = ppi_set_params,
+	.update_addr = ppi_update_addr,
+};
+
+static irqreturn_t ppi_irq_err(int irq, void *dev_id)
+{
+	struct ppi_if *ppi = dev_id;
+	const struct ppi_info *info = ppi->info;
+
+	switch (info->type) {
+	case PPI_TYPE_PPI:
+	{
+		struct bfin_ppi_regs *reg = info->base;
+		unsigned short status;
+
+		/* register on bf561 is cleared when read 
+		 * others are W1C
+		 */
+		status = bfin_read16(&reg->status);
+		if (status & 0x3000)
+			ppi->err = true;
+		bfin_write16(&reg->status, 0xff00);
+		break;
+	}
+	case PPI_TYPE_EPPI:
+	{
+		struct bfin_eppi_regs *reg = info->base;
+		unsigned short status;
+
+		status = bfin_read16(&reg->status);
+		if (status & 0x2)
+			ppi->err = true;
+		bfin_write16(&reg->status, 0xffff);
+		break;
+	}
+	case PPI_TYPE_EPPI3:
+	{
+		struct bfin_eppi3_regs *reg = info->base;
+		unsigned long stat;
+
+		stat = bfin_read32(&reg->stat);
+		if (stat & 0x2)
+			ppi->err = true;
+		bfin_write32(&reg->stat, 0xc0ff);
+		break;
+	}
+	default:
+		break;
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int ppi_attach_irq(struct ppi_if *ppi, irq_handler_t handler)
+{
+	const struct ppi_info *info = ppi->info;
+	int ret;
+
+	ret = request_dma(info->dma_ch, "PPI_DMA");
+
+	if (ret) {
+		pr_err("Unable to allocate DMA channel for PPI\n");
+		return ret;
+	}
+	set_dma_callback(info->dma_ch, handler, ppi);
+
+	if (ppi->err_int) {
+		ret = request_irq(info->irq_err, ppi_irq_err, 0, "PPI ERROR", ppi);
+		if (ret) {
+			pr_err("Unable to allocate IRQ for PPI\n");
+			free_dma(info->dma_ch);
+		}
+	}
+	return ret;
+}
+
+static void ppi_detach_irq(struct ppi_if *ppi)
+{
+	const struct ppi_info *info = ppi->info;
+
+	if (ppi->err_int)
+		free_irq(info->irq_err, ppi);
+	free_dma(info->dma_ch);
+}
+
+static int ppi_start(struct ppi_if *ppi)
+{
+	const struct ppi_info *info = ppi->info;
+
+	/* enable DMA */
+	enable_dma(info->dma_ch);
+
+	/* enable PPI */
+	ppi->ppi_control |= PORT_EN;
+	switch (info->type) {
+	case PPI_TYPE_PPI:
+	{
+		struct bfin_ppi_regs *reg = info->base;
+		bfin_write16(&reg->control, ppi->ppi_control);
+		break;
+	}
+	case PPI_TYPE_EPPI:
+	{
+		struct bfin_eppi_regs *reg = info->base;
+		bfin_write32(&reg->control, ppi->ppi_control);
+		break;
+	}
+	case PPI_TYPE_EPPI3:
+	{
+		struct bfin_eppi3_regs *reg = info->base;
+		bfin_write32(&reg->ctl, ppi->ppi_control);
+		break;
+	}
+	default:
+		return -EINVAL;
+	}
+
+	SSYNC();
+	return 0;
+}
+
+static int ppi_stop(struct ppi_if *ppi)
+{
+	const struct ppi_info *info = ppi->info;
+
+	/* disable PPI */
+	ppi->ppi_control &= ~PORT_EN;
+	switch (info->type) {
+	case PPI_TYPE_PPI:
+	{
+		struct bfin_ppi_regs *reg = info->base;
+		bfin_write16(&reg->control, ppi->ppi_control);
+		break;
+	}
+	case PPI_TYPE_EPPI:
+	{
+		struct bfin_eppi_regs *reg = info->base;
+		bfin_write32(&reg->control, ppi->ppi_control);
+		break;
+	}
+	case PPI_TYPE_EPPI3:
+	{
+		struct bfin_eppi3_regs *reg = info->base;
+		bfin_write32(&reg->ctl, ppi->ppi_control);
+		break;
+	}
+	default:
+		return -EINVAL;
+	}
+
+	/* disable DMA */
+	clear_dma_irqstat(info->dma_ch);
+	disable_dma(info->dma_ch);
+
+	SSYNC();
+	return 0;
+}
+
+static int ppi_set_params(struct ppi_if *ppi, struct ppi_params *params)
+{
+	const struct ppi_info *info = ppi->info;
+	int dma32 = 0;
+	int dma_config, bytes_per_line;
+	int hcount, hdelay, samples_per_line;
+
+#ifdef CONFIG_PINCTRL
+	static const char * const pin_state[] = {"8bit", "16bit", "24bit"};
+	struct pinctrl *pctrl;
+	struct pinctrl_state *pstate;
+
+	if (params->dlen > 24 || params->dlen <= 0)
+		return -EINVAL;
+	pctrl = devm_pinctrl_get(ppi->dev);
+	if (IS_ERR(pctrl))
+		return PTR_ERR(pctrl);
+	pstate = pinctrl_lookup_state(pctrl,
+				      pin_state[(params->dlen + 7) / 8 - 1]);
+	if (pinctrl_select_state(pctrl, pstate))
+		return -EINVAL;
+#endif
+
+	bytes_per_line = params->width * params->bpp / 8;
+	/* convert parameters unit from pixels to samples */
+	hcount = params->width * params->bpp / params->dlen;
+	hdelay = params->hdelay * params->bpp / params->dlen;
+	samples_per_line = params->line * params->bpp / params->dlen;
+	if (params->int_mask == 0xFFFFFFFF)
+		ppi->err_int = false;
+	else
+		ppi->err_int = true;
+
+	dma_config = (DMA_FLOW_STOP | RESTART | DMA2D | DI_EN_Y);
+	ppi->ppi_control = params->ppi_control & ~PORT_EN;
+	if (!(ppi->ppi_control & PORT_DIR))
+		dma_config |= WNR;
+	switch (info->type) {
+	case PPI_TYPE_PPI:
+	{
+		struct bfin_ppi_regs *reg = info->base;
+
+		if (params->ppi_control & DMA32)
+			dma32 = 1;
+
+		bfin_write16(&reg->control, ppi->ppi_control);
+		bfin_write16(&reg->count, samples_per_line - 1);
+		bfin_write16(&reg->frame, params->frame);
+		break;
+	}
+	case PPI_TYPE_EPPI:
+	{
+		struct bfin_eppi_regs *reg = info->base;
+
+		if ((params->ppi_control & PACK_EN)
+			|| (params->ppi_control & 0x38000) > DLEN_16)
+			dma32 = 1;
+
+		bfin_write32(&reg->control, ppi->ppi_control);
+		bfin_write16(&reg->line, samples_per_line);
+		bfin_write16(&reg->frame, params->frame);
+		bfin_write16(&reg->hdelay, hdelay);
+		bfin_write16(&reg->vdelay, params->vdelay);
+		bfin_write16(&reg->hcount, hcount);
+		bfin_write16(&reg->vcount, params->height);
+		break;
+	}
+	case PPI_TYPE_EPPI3:
+	{
+		struct bfin_eppi3_regs *reg = info->base;
+
+		if ((params->ppi_control & PACK_EN)
+			|| (params->ppi_control & 0x70000) > DLEN_16)
+			dma32 = 1;
+
+		bfin_write32(&reg->ctl, ppi->ppi_control);
+		bfin_write32(&reg->line, samples_per_line);
+		bfin_write32(&reg->frame, params->frame);
+		bfin_write32(&reg->hdly, hdelay);
+		bfin_write32(&reg->vdly, params->vdelay);
+		bfin_write32(&reg->hcnt, hcount);
+		bfin_write32(&reg->vcnt, params->height);
+		if (params->int_mask)
+			bfin_write32(&reg->imsk, params->int_mask & 0xFF);
+		if (ppi->ppi_control & PORT_DIR) {
+			u32 hsync_width, vsync_width, vsync_period;
+
+			hsync_width = params->hsync
+					* params->bpp / params->dlen;
+			vsync_width = params->vsync * samples_per_line;
+			vsync_period = samples_per_line * params->frame;
+			bfin_write32(&reg->fs1_wlhb, hsync_width);
+			bfin_write32(&reg->fs1_paspl, samples_per_line);
+			bfin_write32(&reg->fs2_wlvb, vsync_width);
+			bfin_write32(&reg->fs2_palpf, vsync_period);
+		}
+		break;
+	}
+	default:
+		return -EINVAL;
+	}
+
+	if (dma32) {
+		dma_config |= WDSIZE_32 | PSIZE_32;
+		set_dma_x_count(info->dma_ch, bytes_per_line >> 2);
+		set_dma_x_modify(info->dma_ch, 4);
+		set_dma_y_modify(info->dma_ch, 4);
+	} else {
+		dma_config |= WDSIZE_16 | PSIZE_16;
+		set_dma_x_count(info->dma_ch, bytes_per_line >> 1);
+		set_dma_x_modify(info->dma_ch, 2);
+		set_dma_y_modify(info->dma_ch, 2);
+	}
+	set_dma_y_count(info->dma_ch, params->height);
+	set_dma_config(info->dma_ch, dma_config);
+
+	SSYNC();
+	return 0;
+}
+
+static void ppi_update_addr(struct ppi_if *ppi, unsigned long addr)
+{
+	set_dma_start_addr(ppi->info->dma_ch, addr);
+}
+
+struct ppi_if *ppi_create_instance(struct platform_device *pdev,
+			const struct ppi_info *info)
+{
+	struct ppi_if *ppi;
+
+	if (!info || !info->pin_req)
+		return NULL;
+
+#ifndef CONFIG_PINCTRL
+	if (peripheral_request_list(info->pin_req, KBUILD_MODNAME)) {
+		dev_err(&pdev->dev, "request peripheral failed\n");
+		return NULL;
+	}
+#endif
+
+	ppi = kzalloc(sizeof(*ppi), GFP_KERNEL);
+	if (!ppi) {
+		peripheral_free_list(info->pin_req);
+		dev_err(&pdev->dev, "unable to allocate memory for ppi handle\n");
+		return NULL;
+	}
+	ppi->ops = &ppi_ops;
+	ppi->info = info;
+	ppi->dev = &pdev->dev;
+
+	pr_info("ppi probe success\n");
+	return ppi;
+}
+EXPORT_SYMBOL(ppi_create_instance);
+
+void ppi_delete_instance(struct ppi_if *ppi)
+{
+	peripheral_free_list(ppi->info->pin_req);
+	kfree(ppi);
+}
+EXPORT_SYMBOL(ppi_delete_instance);
+
+MODULE_DESCRIPTION("Analog Devices PPI driver");
+MODULE_AUTHOR("Scott Jiang <Scott.Jiang.Linux@gmail.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/platform/coda/Makefile b/drivers/media/platform/coda/Makefile
new file mode 100644
index 0000000..9342ac5
--- /dev/null
+++ b/drivers/media/platform/coda/Makefile
@@ -0,0 +1,5 @@
+ccflags-y += -I$(src)
+
+coda-objs := coda-common.o coda-bit.o coda-gdi.o coda-h264.o coda-jpeg.o
+
+obj-$(CONFIG_VIDEO_CODA) += coda.o
diff --git a/drivers/media/platform/coda/coda-bit.c b/drivers/media/platform/coda/coda-bit.c
new file mode 100644
index 0000000..d76511c
--- /dev/null
+++ b/drivers/media/platform/coda/coda-bit.c
@@ -0,0 +1,2121 @@
+/*
+ * Coda multi-standard codec IP - BIT processor functions
+ *
+ * Copyright (C) 2012 Vista Silicon S.L.
+ *    Javier Martin, <javier.martin@vista-silicon.com>
+ *    Xavier Duret
+ * Copyright (C) 2012-2014 Philipp Zabel, Pengutronix
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/clk.h>
+#include <linux/irqreturn.h>
+#include <linux/kernel.h>
+#include <linux/log2.h>
+#include <linux/platform_device.h>
+#include <linux/reset.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+
+#include <media/v4l2-common.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/videobuf2-dma-contig.h>
+#include <media/videobuf2-vmalloc.h>
+
+#include "coda.h"
+#define CREATE_TRACE_POINTS
+#include "trace.h"
+
+#define CODA_PARA_BUF_SIZE	(10 * 1024)
+#define CODA7_PS_BUF_SIZE	0x28000
+#define CODA9_PS_SAVE_SIZE	(512 * 1024)
+
+#define CODA_DEFAULT_GAMMA	4096
+#define CODA9_DEFAULT_GAMMA	24576	/* 0.75 * 32768 */
+
+static void coda_free_bitstream_buffer(struct coda_ctx *ctx);
+
+static inline int coda_is_initialized(struct coda_dev *dev)
+{
+	return coda_read(dev, CODA_REG_BIT_CUR_PC) != 0;
+}
+
+static inline unsigned long coda_isbusy(struct coda_dev *dev)
+{
+	return coda_read(dev, CODA_REG_BIT_BUSY);
+}
+
+static int coda_wait_timeout(struct coda_dev *dev)
+{
+	unsigned long timeout = jiffies + msecs_to_jiffies(1000);
+
+	while (coda_isbusy(dev)) {
+		if (time_after(jiffies, timeout))
+			return -ETIMEDOUT;
+	}
+	return 0;
+}
+
+static void coda_command_async(struct coda_ctx *ctx, int cmd)
+{
+	struct coda_dev *dev = ctx->dev;
+
+	if (dev->devtype->product == CODA_960 ||
+	    dev->devtype->product == CODA_7541) {
+		/* Restore context related registers to CODA */
+		coda_write(dev, ctx->bit_stream_param,
+				CODA_REG_BIT_BIT_STREAM_PARAM);
+		coda_write(dev, ctx->frm_dis_flg,
+				CODA_REG_BIT_FRM_DIS_FLG(ctx->reg_idx));
+		coda_write(dev, ctx->frame_mem_ctrl,
+				CODA_REG_BIT_FRAME_MEM_CTRL);
+		coda_write(dev, ctx->workbuf.paddr, CODA_REG_BIT_WORK_BUF_ADDR);
+	}
+
+	if (dev->devtype->product == CODA_960) {
+		coda_write(dev, 1, CODA9_GDI_WPROT_ERR_CLR);
+		coda_write(dev, 0, CODA9_GDI_WPROT_RGN_EN);
+	}
+
+	coda_write(dev, CODA_REG_BIT_BUSY_FLAG, CODA_REG_BIT_BUSY);
+
+	coda_write(dev, ctx->idx, CODA_REG_BIT_RUN_INDEX);
+	coda_write(dev, ctx->params.codec_mode, CODA_REG_BIT_RUN_COD_STD);
+	coda_write(dev, ctx->params.codec_mode_aux, CODA7_REG_BIT_RUN_AUX_STD);
+
+	trace_coda_bit_run(ctx, cmd);
+
+	coda_write(dev, cmd, CODA_REG_BIT_RUN_COMMAND);
+}
+
+static int coda_command_sync(struct coda_ctx *ctx, int cmd)
+{
+	struct coda_dev *dev = ctx->dev;
+	int ret;
+
+	coda_command_async(ctx, cmd);
+	ret = coda_wait_timeout(dev);
+	trace_coda_bit_done(ctx);
+
+	return ret;
+}
+
+int coda_hw_reset(struct coda_ctx *ctx)
+{
+	struct coda_dev *dev = ctx->dev;
+	unsigned long timeout;
+	unsigned int idx;
+	int ret;
+
+	if (!dev->rstc)
+		return -ENOENT;
+
+	idx = coda_read(dev, CODA_REG_BIT_RUN_INDEX);
+
+	if (dev->devtype->product == CODA_960) {
+		timeout = jiffies + msecs_to_jiffies(100);
+		coda_write(dev, 0x11, CODA9_GDI_BUS_CTRL);
+		while (coda_read(dev, CODA9_GDI_BUS_STATUS) != 0x77) {
+			if (time_after(jiffies, timeout))
+				return -ETIME;
+			cpu_relax();
+		}
+	}
+
+	ret = reset_control_reset(dev->rstc);
+	if (ret < 0)
+		return ret;
+
+	if (dev->devtype->product == CODA_960)
+		coda_write(dev, 0x00, CODA9_GDI_BUS_CTRL);
+	coda_write(dev, CODA_REG_BIT_BUSY_FLAG, CODA_REG_BIT_BUSY);
+	coda_write(dev, CODA_REG_RUN_ENABLE, CODA_REG_BIT_CODE_RUN);
+	ret = coda_wait_timeout(dev);
+	coda_write(dev, idx, CODA_REG_BIT_RUN_INDEX);
+
+	return ret;
+}
+
+static void coda_kfifo_sync_from_device(struct coda_ctx *ctx)
+{
+	struct __kfifo *kfifo = &ctx->bitstream_fifo.kfifo;
+	struct coda_dev *dev = ctx->dev;
+	u32 rd_ptr;
+
+	rd_ptr = coda_read(dev, CODA_REG_BIT_RD_PTR(ctx->reg_idx));
+	kfifo->out = (kfifo->in & ~kfifo->mask) |
+		      (rd_ptr - ctx->bitstream.paddr);
+	if (kfifo->out > kfifo->in)
+		kfifo->out -= kfifo->mask + 1;
+}
+
+static void coda_kfifo_sync_to_device_full(struct coda_ctx *ctx)
+{
+	struct __kfifo *kfifo = &ctx->bitstream_fifo.kfifo;
+	struct coda_dev *dev = ctx->dev;
+	u32 rd_ptr, wr_ptr;
+
+	rd_ptr = ctx->bitstream.paddr + (kfifo->out & kfifo->mask);
+	coda_write(dev, rd_ptr, CODA_REG_BIT_RD_PTR(ctx->reg_idx));
+	wr_ptr = ctx->bitstream.paddr + (kfifo->in & kfifo->mask);
+	coda_write(dev, wr_ptr, CODA_REG_BIT_WR_PTR(ctx->reg_idx));
+}
+
+static void coda_kfifo_sync_to_device_write(struct coda_ctx *ctx)
+{
+	struct __kfifo *kfifo = &ctx->bitstream_fifo.kfifo;
+	struct coda_dev *dev = ctx->dev;
+	u32 wr_ptr;
+
+	wr_ptr = ctx->bitstream.paddr + (kfifo->in & kfifo->mask);
+	coda_write(dev, wr_ptr, CODA_REG_BIT_WR_PTR(ctx->reg_idx));
+}
+
+static int coda_bitstream_queue(struct coda_ctx *ctx,
+				struct vb2_v4l2_buffer *src_buf)
+{
+	u32 src_size = vb2_get_plane_payload(&src_buf->vb2_buf, 0);
+	u32 n;
+
+	n = kfifo_in(&ctx->bitstream_fifo,
+			vb2_plane_vaddr(&src_buf->vb2_buf, 0), src_size);
+	if (n < src_size)
+		return -ENOSPC;
+
+	src_buf->sequence = ctx->qsequence++;
+
+	return 0;
+}
+
+static bool coda_bitstream_try_queue(struct coda_ctx *ctx,
+				     struct vb2_v4l2_buffer *src_buf)
+{
+	int ret;
+
+	if (coda_get_bitstream_payload(ctx) +
+	    vb2_get_plane_payload(&src_buf->vb2_buf, 0) + 512 >=
+	    ctx->bitstream.size)
+		return false;
+
+	if (vb2_plane_vaddr(&src_buf->vb2_buf, 0) == NULL) {
+		v4l2_err(&ctx->dev->v4l2_dev, "trying to queue empty buffer\n");
+		return true;
+	}
+
+	ret = coda_bitstream_queue(ctx, src_buf);
+	if (ret < 0) {
+		v4l2_err(&ctx->dev->v4l2_dev, "bitstream buffer overflow\n");
+		return false;
+	}
+	/* Sync read pointer to device */
+	if (ctx == v4l2_m2m_get_curr_priv(ctx->dev->m2m_dev))
+		coda_kfifo_sync_to_device_write(ctx);
+
+	ctx->hold = false;
+
+	return true;
+}
+
+void coda_fill_bitstream(struct coda_ctx *ctx, bool streaming)
+{
+	struct vb2_v4l2_buffer *src_buf;
+	struct coda_buffer_meta *meta;
+	unsigned long flags;
+	u32 start;
+
+	if (ctx->bit_stream_param & CODA_BIT_STREAM_END_FLAG)
+		return;
+
+	while (v4l2_m2m_num_src_bufs_ready(ctx->fh.m2m_ctx) > 0) {
+		/*
+		 * Only queue a single JPEG into the bitstream buffer, except
+		 * to increase payload over 512 bytes or if in hold state.
+		 */
+		if (ctx->codec->src_fourcc == V4L2_PIX_FMT_JPEG &&
+		    (coda_get_bitstream_payload(ctx) >= 512) && !ctx->hold)
+			break;
+
+		src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+
+		/* Drop frames that do not start/end with a SOI/EOI markers */
+		if (ctx->codec->src_fourcc == V4L2_PIX_FMT_JPEG &&
+		    !coda_jpeg_check_buffer(ctx, src_buf)) {
+			v4l2_err(&ctx->dev->v4l2_dev,
+				 "dropping invalid JPEG frame %d\n",
+				 ctx->qsequence);
+			src_buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+			v4l2_m2m_buf_done(src_buf, streaming ?
+					  VB2_BUF_STATE_ERROR :
+					  VB2_BUF_STATE_QUEUED);
+			continue;
+		}
+
+		/* Dump empty buffers */
+		if (!vb2_get_plane_payload(&src_buf->vb2_buf, 0)) {
+			src_buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+			v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_DONE);
+			continue;
+		}
+
+		/* Buffer start position */
+		start = ctx->bitstream_fifo.kfifo.in &
+			ctx->bitstream_fifo.kfifo.mask;
+
+		if (coda_bitstream_try_queue(ctx, src_buf)) {
+			/*
+			 * Source buffer is queued in the bitstream ringbuffer;
+			 * queue the timestamp and mark source buffer as done
+			 */
+			src_buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+
+			meta = kmalloc(sizeof(*meta), GFP_KERNEL);
+			if (meta) {
+				meta->sequence = src_buf->sequence;
+				meta->timecode = src_buf->timecode;
+				meta->timestamp = src_buf->timestamp;
+				meta->start = start;
+				meta->end = ctx->bitstream_fifo.kfifo.in &
+					    ctx->bitstream_fifo.kfifo.mask;
+				spin_lock_irqsave(&ctx->buffer_meta_lock,
+						  flags);
+				list_add_tail(&meta->list,
+					      &ctx->buffer_meta_list);
+				ctx->num_metas++;
+				spin_unlock_irqrestore(&ctx->buffer_meta_lock,
+						       flags);
+
+				trace_coda_bit_queue(ctx, src_buf, meta);
+			}
+
+			v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_DONE);
+		} else {
+			break;
+		}
+	}
+}
+
+void coda_bit_stream_end_flag(struct coda_ctx *ctx)
+{
+	struct coda_dev *dev = ctx->dev;
+
+	ctx->bit_stream_param |= CODA_BIT_STREAM_END_FLAG;
+
+	/* If this context is currently running, update the hardware flag */
+	if ((dev->devtype->product == CODA_960) &&
+	    coda_isbusy(dev) &&
+	    (ctx->idx == coda_read(dev, CODA_REG_BIT_RUN_INDEX))) {
+		coda_write(dev, ctx->bit_stream_param,
+			   CODA_REG_BIT_BIT_STREAM_PARAM);
+	}
+}
+
+static void coda_parabuf_write(struct coda_ctx *ctx, int index, u32 value)
+{
+	struct coda_dev *dev = ctx->dev;
+	u32 *p = ctx->parabuf.vaddr;
+
+	if (dev->devtype->product == CODA_DX6)
+		p[index] = value;
+	else
+		p[index ^ 1] = value;
+}
+
+static inline int coda_alloc_context_buf(struct coda_ctx *ctx,
+					 struct coda_aux_buf *buf, size_t size,
+					 const char *name)
+{
+	return coda_alloc_aux_buf(ctx->dev, buf, size, name, ctx->debugfs_entry);
+}
+
+
+static void coda_free_framebuffers(struct coda_ctx *ctx)
+{
+	int i;
+
+	for (i = 0; i < CODA_MAX_FRAMEBUFFERS; i++)
+		coda_free_aux_buf(ctx->dev, &ctx->internal_frames[i]);
+}
+
+static int coda_alloc_framebuffers(struct coda_ctx *ctx,
+				   struct coda_q_data *q_data, u32 fourcc)
+{
+	struct coda_dev *dev = ctx->dev;
+	int width, height;
+	int ysize;
+	int ret;
+	int i;
+
+	if (ctx->codec && (ctx->codec->src_fourcc == V4L2_PIX_FMT_H264 ||
+	     ctx->codec->dst_fourcc == V4L2_PIX_FMT_H264)) {
+		width = round_up(q_data->width, 16);
+		height = round_up(q_data->height, 16);
+	} else {
+		width = round_up(q_data->width, 8);
+		height = q_data->height;
+	}
+	ysize = width * height;
+
+	/* Allocate frame buffers */
+	for (i = 0; i < ctx->num_internal_frames; i++) {
+		size_t size;
+		char *name;
+
+		if (ctx->tiled_map_type == GDI_TILED_FRAME_MB_RASTER_MAP)
+			size = round_up(ysize, 4096) + ysize / 2;
+		else
+			size = ysize + ysize / 2;
+		if (ctx->codec->src_fourcc == V4L2_PIX_FMT_H264 &&
+		    dev->devtype->product != CODA_DX6)
+			size += ysize / 4;
+		name = kasprintf(GFP_KERNEL, "fb%d", i);
+		ret = coda_alloc_context_buf(ctx, &ctx->internal_frames[i],
+					     size, name);
+		kfree(name);
+		if (ret < 0) {
+			coda_free_framebuffers(ctx);
+			return ret;
+		}
+	}
+
+	/* Register frame buffers in the parameter buffer */
+	for (i = 0; i < ctx->num_internal_frames; i++) {
+		u32 y, cb, cr;
+
+		/* Start addresses of Y, Cb, Cr planes */
+		y = ctx->internal_frames[i].paddr;
+		cb = y + ysize;
+		cr = y + ysize + ysize/4;
+		if (ctx->tiled_map_type == GDI_TILED_FRAME_MB_RASTER_MAP) {
+			cb = round_up(cb, 4096);
+			cr = 0;
+			/* Packed 20-bit MSB of base addresses */
+			/* YYYYYCCC, CCyyyyyc, cccc.... */
+			y = (y & 0xfffff000) | cb >> 20;
+			cb = (cb & 0x000ff000) << 12;
+		}
+		coda_parabuf_write(ctx, i * 3 + 0, y);
+		coda_parabuf_write(ctx, i * 3 + 1, cb);
+		coda_parabuf_write(ctx, i * 3 + 2, cr);
+
+		/* mvcol buffer for h.264 */
+		if (ctx->codec->src_fourcc == V4L2_PIX_FMT_H264 &&
+		    dev->devtype->product != CODA_DX6)
+			coda_parabuf_write(ctx, 96 + i,
+					   ctx->internal_frames[i].paddr +
+					   ysize + ysize/4 + ysize/4);
+	}
+
+	/* mvcol buffer for mpeg4 */
+	if ((dev->devtype->product != CODA_DX6) &&
+	    (ctx->codec->src_fourcc == V4L2_PIX_FMT_MPEG4))
+		coda_parabuf_write(ctx, 97, ctx->internal_frames[0].paddr +
+					    ysize + ysize/4 + ysize/4);
+
+	return 0;
+}
+
+static void coda_free_context_buffers(struct coda_ctx *ctx)
+{
+	struct coda_dev *dev = ctx->dev;
+
+	coda_free_aux_buf(dev, &ctx->slicebuf);
+	coda_free_aux_buf(dev, &ctx->psbuf);
+	if (dev->devtype->product != CODA_DX6)
+		coda_free_aux_buf(dev, &ctx->workbuf);
+	coda_free_aux_buf(dev, &ctx->parabuf);
+}
+
+static int coda_alloc_context_buffers(struct coda_ctx *ctx,
+				      struct coda_q_data *q_data)
+{
+	struct coda_dev *dev = ctx->dev;
+	size_t size;
+	int ret;
+
+	if (!ctx->parabuf.vaddr) {
+		ret = coda_alloc_context_buf(ctx, &ctx->parabuf,
+					     CODA_PARA_BUF_SIZE, "parabuf");
+		if (ret < 0)
+			return ret;
+	}
+
+	if (dev->devtype->product == CODA_DX6)
+		return 0;
+
+	if (!ctx->slicebuf.vaddr && q_data->fourcc == V4L2_PIX_FMT_H264) {
+		/* worst case slice size */
+		size = (DIV_ROUND_UP(q_data->width, 16) *
+			DIV_ROUND_UP(q_data->height, 16)) * 3200 / 8 + 512;
+		ret = coda_alloc_context_buf(ctx, &ctx->slicebuf, size,
+					     "slicebuf");
+		if (ret < 0)
+			goto err;
+	}
+
+	if (!ctx->psbuf.vaddr && dev->devtype->product == CODA_7541) {
+		ret = coda_alloc_context_buf(ctx, &ctx->psbuf,
+					     CODA7_PS_BUF_SIZE, "psbuf");
+		if (ret < 0)
+			goto err;
+	}
+
+	if (!ctx->workbuf.vaddr) {
+		size = dev->devtype->workbuf_size;
+		if (dev->devtype->product == CODA_960 &&
+		    q_data->fourcc == V4L2_PIX_FMT_H264)
+			size += CODA9_PS_SAVE_SIZE;
+		ret = coda_alloc_context_buf(ctx, &ctx->workbuf, size,
+					     "workbuf");
+		if (ret < 0)
+			goto err;
+	}
+
+	return 0;
+
+err:
+	coda_free_context_buffers(ctx);
+	return ret;
+}
+
+static int coda_encode_header(struct coda_ctx *ctx, struct vb2_v4l2_buffer *buf,
+			      int header_code, u8 *header, int *size)
+{
+	struct vb2_buffer *vb = &buf->vb2_buf;
+	struct coda_dev *dev = ctx->dev;
+	size_t bufsize;
+	int ret;
+	int i;
+
+	if (dev->devtype->product == CODA_960)
+		memset(vb2_plane_vaddr(vb, 0), 0, 64);
+
+	coda_write(dev, vb2_dma_contig_plane_dma_addr(vb, 0),
+		   CODA_CMD_ENC_HEADER_BB_START);
+	bufsize = vb2_plane_size(vb, 0);
+	if (dev->devtype->product == CODA_960)
+		bufsize /= 1024;
+	coda_write(dev, bufsize, CODA_CMD_ENC_HEADER_BB_SIZE);
+	coda_write(dev, header_code, CODA_CMD_ENC_HEADER_CODE);
+	ret = coda_command_sync(ctx, CODA_COMMAND_ENCODE_HEADER);
+	if (ret < 0) {
+		v4l2_err(&dev->v4l2_dev, "CODA_COMMAND_ENCODE_HEADER timeout\n");
+		return ret;
+	}
+
+	if (dev->devtype->product == CODA_960) {
+		for (i = 63; i > 0; i--)
+			if (((char *)vb2_plane_vaddr(vb, 0))[i] != 0)
+				break;
+		*size = i + 1;
+	} else {
+		*size = coda_read(dev, CODA_REG_BIT_WR_PTR(ctx->reg_idx)) -
+			coda_read(dev, CODA_CMD_ENC_HEADER_BB_START);
+	}
+	memcpy(header, vb2_plane_vaddr(vb, 0), *size);
+
+	return 0;
+}
+
+static phys_addr_t coda_iram_alloc(struct coda_iram_info *iram, size_t size)
+{
+	phys_addr_t ret;
+
+	size = round_up(size, 1024);
+	if (size > iram->remaining)
+		return 0;
+	iram->remaining -= size;
+
+	ret = iram->next_paddr;
+	iram->next_paddr += size;
+
+	return ret;
+}
+
+static void coda_setup_iram(struct coda_ctx *ctx)
+{
+	struct coda_iram_info *iram_info = &ctx->iram_info;
+	struct coda_dev *dev = ctx->dev;
+	int w64, w128;
+	int mb_width;
+	int dbk_bits;
+	int bit_bits;
+	int ip_bits;
+
+	memset(iram_info, 0, sizeof(*iram_info));
+	iram_info->next_paddr = dev->iram.paddr;
+	iram_info->remaining = dev->iram.size;
+
+	if (!dev->iram.vaddr)
+		return;
+
+	switch (dev->devtype->product) {
+	case CODA_7541:
+		dbk_bits = CODA7_USE_HOST_DBK_ENABLE | CODA7_USE_DBK_ENABLE;
+		bit_bits = CODA7_USE_HOST_BIT_ENABLE | CODA7_USE_BIT_ENABLE;
+		ip_bits = CODA7_USE_HOST_IP_ENABLE | CODA7_USE_IP_ENABLE;
+		break;
+	case CODA_960:
+		dbk_bits = CODA9_USE_HOST_DBK_ENABLE | CODA9_USE_DBK_ENABLE;
+		bit_bits = CODA9_USE_HOST_BIT_ENABLE | CODA7_USE_BIT_ENABLE;
+		ip_bits = CODA9_USE_HOST_IP_ENABLE | CODA7_USE_IP_ENABLE;
+		break;
+	default: /* CODA_DX6 */
+		return;
+	}
+
+	if (ctx->inst_type == CODA_INST_ENCODER) {
+		struct coda_q_data *q_data_src;
+
+		q_data_src = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
+		mb_width = DIV_ROUND_UP(q_data_src->width, 16);
+		w128 = mb_width * 128;
+		w64 = mb_width * 64;
+
+		/* Prioritize in case IRAM is too small for everything */
+		if (dev->devtype->product == CODA_7541) {
+			iram_info->search_ram_size = round_up(mb_width * 16 *
+							      36 + 2048, 1024);
+			iram_info->search_ram_paddr = coda_iram_alloc(iram_info,
+						iram_info->search_ram_size);
+			if (!iram_info->search_ram_paddr) {
+				pr_err("IRAM is smaller than the search ram size\n");
+				goto out;
+			}
+			iram_info->axi_sram_use |= CODA7_USE_HOST_ME_ENABLE |
+						   CODA7_USE_ME_ENABLE;
+		}
+
+		/* Only H.264BP and H.263P3 are considered */
+		iram_info->buf_dbk_y_use = coda_iram_alloc(iram_info, w64);
+		iram_info->buf_dbk_c_use = coda_iram_alloc(iram_info, w64);
+		if (!iram_info->buf_dbk_c_use)
+			goto out;
+		iram_info->axi_sram_use |= dbk_bits;
+
+		iram_info->buf_bit_use = coda_iram_alloc(iram_info, w128);
+		if (!iram_info->buf_bit_use)
+			goto out;
+		iram_info->axi_sram_use |= bit_bits;
+
+		iram_info->buf_ip_ac_dc_use = coda_iram_alloc(iram_info, w128);
+		if (!iram_info->buf_ip_ac_dc_use)
+			goto out;
+		iram_info->axi_sram_use |= ip_bits;
+
+		/* OVL and BTP disabled for encoder */
+	} else if (ctx->inst_type == CODA_INST_DECODER) {
+		struct coda_q_data *q_data_dst;
+
+		q_data_dst = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
+		mb_width = DIV_ROUND_UP(q_data_dst->width, 16);
+		w128 = mb_width * 128;
+
+		iram_info->buf_dbk_y_use = coda_iram_alloc(iram_info, w128);
+		iram_info->buf_dbk_c_use = coda_iram_alloc(iram_info, w128);
+		if (!iram_info->buf_dbk_c_use)
+			goto out;
+		iram_info->axi_sram_use |= dbk_bits;
+
+		iram_info->buf_bit_use = coda_iram_alloc(iram_info, w128);
+		if (!iram_info->buf_bit_use)
+			goto out;
+		iram_info->axi_sram_use |= bit_bits;
+
+		iram_info->buf_ip_ac_dc_use = coda_iram_alloc(iram_info, w128);
+		if (!iram_info->buf_ip_ac_dc_use)
+			goto out;
+		iram_info->axi_sram_use |= ip_bits;
+
+		/* OVL and BTP unused as there is no VC1 support yet */
+	}
+
+out:
+	if (!(iram_info->axi_sram_use & CODA7_USE_HOST_IP_ENABLE))
+		v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
+			 "IRAM smaller than needed\n");
+
+	if (dev->devtype->product == CODA_7541) {
+		/* TODO - Enabling these causes picture errors on CODA7541 */
+		if (ctx->inst_type == CODA_INST_DECODER) {
+			/* fw 1.4.50 */
+			iram_info->axi_sram_use &= ~(CODA7_USE_HOST_IP_ENABLE |
+						     CODA7_USE_IP_ENABLE);
+		} else {
+			/* fw 13.4.29 */
+			iram_info->axi_sram_use &= ~(CODA7_USE_HOST_IP_ENABLE |
+						     CODA7_USE_HOST_DBK_ENABLE |
+						     CODA7_USE_IP_ENABLE |
+						     CODA7_USE_DBK_ENABLE);
+		}
+	}
+}
+
+static u32 coda_supported_firmwares[] = {
+	CODA_FIRMWARE_VERNUM(CODA_DX6, 2, 2, 5),
+	CODA_FIRMWARE_VERNUM(CODA_7541, 1, 4, 50),
+	CODA_FIRMWARE_VERNUM(CODA_960, 2, 1, 5),
+};
+
+static bool coda_firmware_supported(u32 vernum)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(coda_supported_firmwares); i++)
+		if (vernum == coda_supported_firmwares[i])
+			return true;
+	return false;
+}
+
+int coda_check_firmware(struct coda_dev *dev)
+{
+	u16 product, major, minor, release;
+	u32 data;
+	int ret;
+
+	ret = clk_prepare_enable(dev->clk_per);
+	if (ret)
+		goto err_clk_per;
+
+	ret = clk_prepare_enable(dev->clk_ahb);
+	if (ret)
+		goto err_clk_ahb;
+
+	coda_write(dev, 0, CODA_CMD_FIRMWARE_VERNUM);
+	coda_write(dev, CODA_REG_BIT_BUSY_FLAG, CODA_REG_BIT_BUSY);
+	coda_write(dev, 0, CODA_REG_BIT_RUN_INDEX);
+	coda_write(dev, 0, CODA_REG_BIT_RUN_COD_STD);
+	coda_write(dev, CODA_COMMAND_FIRMWARE_GET, CODA_REG_BIT_RUN_COMMAND);
+	if (coda_wait_timeout(dev)) {
+		v4l2_err(&dev->v4l2_dev, "firmware get command error\n");
+		ret = -EIO;
+		goto err_run_cmd;
+	}
+
+	if (dev->devtype->product == CODA_960) {
+		data = coda_read(dev, CODA9_CMD_FIRMWARE_CODE_REV);
+		v4l2_info(&dev->v4l2_dev, "Firmware code revision: %d\n",
+			  data);
+	}
+
+	/* Check we are compatible with the loaded firmware */
+	data = coda_read(dev, CODA_CMD_FIRMWARE_VERNUM);
+	product = CODA_FIRMWARE_PRODUCT(data);
+	major = CODA_FIRMWARE_MAJOR(data);
+	minor = CODA_FIRMWARE_MINOR(data);
+	release = CODA_FIRMWARE_RELEASE(data);
+
+	clk_disable_unprepare(dev->clk_per);
+	clk_disable_unprepare(dev->clk_ahb);
+
+	if (product != dev->devtype->product) {
+		v4l2_err(&dev->v4l2_dev,
+			 "Wrong firmware. Hw: %s, Fw: %s, Version: %u.%u.%u\n",
+			 coda_product_name(dev->devtype->product),
+			 coda_product_name(product), major, minor, release);
+		return -EINVAL;
+	}
+
+	v4l2_info(&dev->v4l2_dev, "Initialized %s.\n",
+		  coda_product_name(product));
+
+	if (coda_firmware_supported(data)) {
+		v4l2_info(&dev->v4l2_dev, "Firmware version: %u.%u.%u\n",
+			  major, minor, release);
+	} else {
+		v4l2_warn(&dev->v4l2_dev,
+			  "Unsupported firmware version: %u.%u.%u\n",
+			  major, minor, release);
+	}
+
+	return 0;
+
+err_run_cmd:
+	clk_disable_unprepare(dev->clk_ahb);
+err_clk_ahb:
+	clk_disable_unprepare(dev->clk_per);
+err_clk_per:
+	return ret;
+}
+
+static void coda9_set_frame_cache(struct coda_ctx *ctx, u32 fourcc)
+{
+	u32 cache_size, cache_config;
+
+	if (ctx->tiled_map_type == GDI_LINEAR_FRAME_MAP) {
+		/* Luma 2x0 page, 2x6 cache, chroma 2x0 page, 2x4 cache size */
+		cache_size = 0x20262024;
+		cache_config = 2 << CODA9_CACHE_PAGEMERGE_OFFSET;
+	} else {
+		/* Luma 0x2 page, 4x4 cache, chroma 0x2 page, 4x3 cache size */
+		cache_size = 0x02440243;
+		cache_config = 1 << CODA9_CACHE_PAGEMERGE_OFFSET;
+	}
+	coda_write(ctx->dev, cache_size, CODA9_CMD_SET_FRAME_CACHE_SIZE);
+	if (fourcc == V4L2_PIX_FMT_NV12) {
+		cache_config |= 32 << CODA9_CACHE_LUMA_BUFFER_SIZE_OFFSET |
+				16 << CODA9_CACHE_CR_BUFFER_SIZE_OFFSET |
+				0 << CODA9_CACHE_CB_BUFFER_SIZE_OFFSET;
+	} else {
+		cache_config |= 32 << CODA9_CACHE_LUMA_BUFFER_SIZE_OFFSET |
+				8 << CODA9_CACHE_CR_BUFFER_SIZE_OFFSET |
+				8 << CODA9_CACHE_CB_BUFFER_SIZE_OFFSET;
+	}
+	coda_write(ctx->dev, cache_config, CODA9_CMD_SET_FRAME_CACHE_CONFIG);
+}
+
+/*
+ * Encoder context operations
+ */
+
+static int coda_encoder_reqbufs(struct coda_ctx *ctx,
+				struct v4l2_requestbuffers *rb)
+{
+	struct coda_q_data *q_data_src;
+	int ret;
+
+	if (rb->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		return 0;
+
+	if (rb->count) {
+		q_data_src = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
+		ret = coda_alloc_context_buffers(ctx, q_data_src);
+		if (ret < 0)
+			return ret;
+	} else {
+		coda_free_context_buffers(ctx);
+	}
+
+	return 0;
+}
+
+static int coda_start_encoding(struct coda_ctx *ctx)
+{
+	struct coda_dev *dev = ctx->dev;
+	struct v4l2_device *v4l2_dev = &dev->v4l2_dev;
+	struct coda_q_data *q_data_src, *q_data_dst;
+	u32 bitstream_buf, bitstream_size;
+	struct vb2_v4l2_buffer *buf;
+	int gamma, ret, value;
+	u32 dst_fourcc;
+	int num_fb;
+	u32 stride;
+
+	q_data_src = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
+	q_data_dst = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
+	dst_fourcc = q_data_dst->fourcc;
+
+	buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
+	bitstream_buf = vb2_dma_contig_plane_dma_addr(&buf->vb2_buf, 0);
+	bitstream_size = q_data_dst->sizeimage;
+
+	if (!coda_is_initialized(dev)) {
+		v4l2_err(v4l2_dev, "coda is not initialized.\n");
+		return -EFAULT;
+	}
+
+	if (dst_fourcc == V4L2_PIX_FMT_JPEG) {
+		if (!ctx->params.jpeg_qmat_tab[0])
+			ctx->params.jpeg_qmat_tab[0] = kmalloc(64, GFP_KERNEL);
+		if (!ctx->params.jpeg_qmat_tab[1])
+			ctx->params.jpeg_qmat_tab[1] = kmalloc(64, GFP_KERNEL);
+		coda_set_jpeg_compression_quality(ctx, ctx->params.jpeg_quality);
+	}
+
+	mutex_lock(&dev->coda_mutex);
+
+	coda_write(dev, ctx->parabuf.paddr, CODA_REG_BIT_PARA_BUF_ADDR);
+	coda_write(dev, bitstream_buf, CODA_REG_BIT_RD_PTR(ctx->reg_idx));
+	coda_write(dev, bitstream_buf, CODA_REG_BIT_WR_PTR(ctx->reg_idx));
+	switch (dev->devtype->product) {
+	case CODA_DX6:
+		coda_write(dev, CODADX6_STREAM_BUF_DYNALLOC_EN |
+			CODADX6_STREAM_BUF_PIC_RESET, CODA_REG_BIT_STREAM_CTRL);
+		break;
+	case CODA_960:
+		coda_write(dev, 0, CODA9_GDI_WPROT_RGN_EN);
+		/* fallthrough */
+	case CODA_7541:
+		coda_write(dev, CODA7_STREAM_BUF_DYNALLOC_EN |
+			CODA7_STREAM_BUF_PIC_RESET, CODA_REG_BIT_STREAM_CTRL);
+		break;
+	}
+
+	ctx->frame_mem_ctrl &= ~(CODA_FRAME_CHROMA_INTERLEAVE | (0x3 << 9) |
+				 CODA9_FRAME_TILED2LINEAR);
+	if (q_data_src->fourcc == V4L2_PIX_FMT_NV12)
+		ctx->frame_mem_ctrl |= CODA_FRAME_CHROMA_INTERLEAVE;
+	if (ctx->tiled_map_type == GDI_TILED_FRAME_MB_RASTER_MAP)
+		ctx->frame_mem_ctrl |= (0x3 << 9) | CODA9_FRAME_TILED2LINEAR;
+	coda_write(dev, ctx->frame_mem_ctrl, CODA_REG_BIT_FRAME_MEM_CTRL);
+
+	if (dev->devtype->product == CODA_DX6) {
+		/* Configure the coda */
+		coda_write(dev, dev->iram.paddr,
+			   CODADX6_REG_BIT_SEARCH_RAM_BASE_ADDR);
+	}
+
+	/* Could set rotation here if needed */
+	value = 0;
+	switch (dev->devtype->product) {
+	case CODA_DX6:
+		value = (q_data_src->width & CODADX6_PICWIDTH_MASK)
+			<< CODADX6_PICWIDTH_OFFSET;
+		value |= (q_data_src->height & CODADX6_PICHEIGHT_MASK)
+			 << CODA_PICHEIGHT_OFFSET;
+		break;
+	case CODA_7541:
+		if (dst_fourcc == V4L2_PIX_FMT_H264) {
+			value = (round_up(q_data_src->width, 16) &
+				 CODA7_PICWIDTH_MASK) << CODA7_PICWIDTH_OFFSET;
+			value |= (round_up(q_data_src->height, 16) &
+				 CODA7_PICHEIGHT_MASK) << CODA_PICHEIGHT_OFFSET;
+			break;
+		}
+		/* fallthrough */
+	case CODA_960:
+		value = (q_data_src->width & CODA7_PICWIDTH_MASK)
+			<< CODA7_PICWIDTH_OFFSET;
+		value |= (q_data_src->height & CODA7_PICHEIGHT_MASK)
+			 << CODA_PICHEIGHT_OFFSET;
+	}
+	coda_write(dev, value, CODA_CMD_ENC_SEQ_SRC_SIZE);
+	if (dst_fourcc == V4L2_PIX_FMT_JPEG)
+		ctx->params.framerate = 0;
+	coda_write(dev, ctx->params.framerate,
+		   CODA_CMD_ENC_SEQ_SRC_F_RATE);
+
+	ctx->params.codec_mode = ctx->codec->mode;
+	switch (dst_fourcc) {
+	case V4L2_PIX_FMT_MPEG4:
+		if (dev->devtype->product == CODA_960)
+			coda_write(dev, CODA9_STD_MPEG4,
+				   CODA_CMD_ENC_SEQ_COD_STD);
+		else
+			coda_write(dev, CODA_STD_MPEG4,
+				   CODA_CMD_ENC_SEQ_COD_STD);
+		coda_write(dev, 0, CODA_CMD_ENC_SEQ_MP4_PARA);
+		break;
+	case V4L2_PIX_FMT_H264:
+		if (dev->devtype->product == CODA_960)
+			coda_write(dev, CODA9_STD_H264,
+				   CODA_CMD_ENC_SEQ_COD_STD);
+		else
+			coda_write(dev, CODA_STD_H264,
+				   CODA_CMD_ENC_SEQ_COD_STD);
+		if (ctx->params.h264_deblk_enabled) {
+			value = ((ctx->params.h264_deblk_alpha &
+				  CODA_264PARAM_DEBLKFILTEROFFSETALPHA_MASK) <<
+				 CODA_264PARAM_DEBLKFILTEROFFSETALPHA_OFFSET) |
+				((ctx->params.h264_deblk_beta &
+				  CODA_264PARAM_DEBLKFILTEROFFSETBETA_MASK) <<
+				 CODA_264PARAM_DEBLKFILTEROFFSETBETA_OFFSET);
+		} else {
+			value = 1 << CODA_264PARAM_DISABLEDEBLK_OFFSET;
+		}
+		coda_write(dev, value, CODA_CMD_ENC_SEQ_264_PARA);
+		break;
+	case V4L2_PIX_FMT_JPEG:
+		coda_write(dev, 0, CODA_CMD_ENC_SEQ_JPG_PARA);
+		coda_write(dev, ctx->params.jpeg_restart_interval,
+				CODA_CMD_ENC_SEQ_JPG_RST_INTERVAL);
+		coda_write(dev, 0, CODA_CMD_ENC_SEQ_JPG_THUMB_EN);
+		coda_write(dev, 0, CODA_CMD_ENC_SEQ_JPG_THUMB_SIZE);
+		coda_write(dev, 0, CODA_CMD_ENC_SEQ_JPG_THUMB_OFFSET);
+
+		coda_jpeg_write_tables(ctx);
+		break;
+	default:
+		v4l2_err(v4l2_dev,
+			 "dst format (0x%08x) invalid.\n", dst_fourcc);
+		ret = -EINVAL;
+		goto out;
+	}
+
+	/*
+	 * slice mode and GOP size registers are used for thumb size/offset
+	 * in JPEG mode
+	 */
+	if (dst_fourcc != V4L2_PIX_FMT_JPEG) {
+		switch (ctx->params.slice_mode) {
+		case V4L2_MPEG_VIDEO_MULTI_SLICE_MODE_SINGLE:
+			value = 0;
+			break;
+		case V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_MB:
+			value  = (ctx->params.slice_max_mb &
+				  CODA_SLICING_SIZE_MASK)
+				 << CODA_SLICING_SIZE_OFFSET;
+			value |= (1 & CODA_SLICING_UNIT_MASK)
+				 << CODA_SLICING_UNIT_OFFSET;
+			value |=  1 & CODA_SLICING_MODE_MASK;
+			break;
+		case V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_BYTES:
+			value  = (ctx->params.slice_max_bits &
+				  CODA_SLICING_SIZE_MASK)
+				 << CODA_SLICING_SIZE_OFFSET;
+			value |= (0 & CODA_SLICING_UNIT_MASK)
+				 << CODA_SLICING_UNIT_OFFSET;
+			value |=  1 & CODA_SLICING_MODE_MASK;
+			break;
+		}
+		coda_write(dev, value, CODA_CMD_ENC_SEQ_SLICE_MODE);
+		value = ctx->params.gop_size & CODA_GOP_SIZE_MASK;
+		coda_write(dev, value, CODA_CMD_ENC_SEQ_GOP_SIZE);
+	}
+
+	if (ctx->params.bitrate) {
+		/* Rate control enabled */
+		value = (ctx->params.bitrate & CODA_RATECONTROL_BITRATE_MASK)
+			<< CODA_RATECONTROL_BITRATE_OFFSET;
+		value |=  1 & CODA_RATECONTROL_ENABLE_MASK;
+		value |= (ctx->params.vbv_delay &
+			  CODA_RATECONTROL_INITIALDELAY_MASK)
+			 << CODA_RATECONTROL_INITIALDELAY_OFFSET;
+		if (dev->devtype->product == CODA_960)
+			value |= BIT(31); /* disable autoskip */
+	} else {
+		value = 0;
+	}
+	coda_write(dev, value, CODA_CMD_ENC_SEQ_RC_PARA);
+
+	coda_write(dev, ctx->params.vbv_size, CODA_CMD_ENC_SEQ_RC_BUF_SIZE);
+	coda_write(dev, ctx->params.intra_refresh,
+		   CODA_CMD_ENC_SEQ_INTRA_REFRESH);
+
+	coda_write(dev, bitstream_buf, CODA_CMD_ENC_SEQ_BB_START);
+	coda_write(dev, bitstream_size / 1024, CODA_CMD_ENC_SEQ_BB_SIZE);
+
+
+	value = 0;
+	if (dev->devtype->product == CODA_960)
+		gamma = CODA9_DEFAULT_GAMMA;
+	else
+		gamma = CODA_DEFAULT_GAMMA;
+	if (gamma > 0) {
+		coda_write(dev, (gamma & CODA_GAMMA_MASK) << CODA_GAMMA_OFFSET,
+			   CODA_CMD_ENC_SEQ_RC_GAMMA);
+	}
+
+	if (ctx->params.h264_min_qp || ctx->params.h264_max_qp) {
+		coda_write(dev,
+			   ctx->params.h264_min_qp << CODA_QPMIN_OFFSET |
+			   ctx->params.h264_max_qp << CODA_QPMAX_OFFSET,
+			   CODA_CMD_ENC_SEQ_RC_QP_MIN_MAX);
+	}
+	if (dev->devtype->product == CODA_960) {
+		if (ctx->params.h264_max_qp)
+			value |= 1 << CODA9_OPTION_RCQPMAX_OFFSET;
+		if (CODA_DEFAULT_GAMMA > 0)
+			value |= 1 << CODA9_OPTION_GAMMA_OFFSET;
+	} else {
+		if (CODA_DEFAULT_GAMMA > 0) {
+			if (dev->devtype->product == CODA_DX6)
+				value |= 1 << CODADX6_OPTION_GAMMA_OFFSET;
+			else
+				value |= 1 << CODA7_OPTION_GAMMA_OFFSET;
+		}
+		if (ctx->params.h264_min_qp)
+			value |= 1 << CODA7_OPTION_RCQPMIN_OFFSET;
+		if (ctx->params.h264_max_qp)
+			value |= 1 << CODA7_OPTION_RCQPMAX_OFFSET;
+	}
+	coda_write(dev, value, CODA_CMD_ENC_SEQ_OPTION);
+
+	coda_write(dev, 0, CODA_CMD_ENC_SEQ_RC_INTERVAL_MODE);
+
+	coda_setup_iram(ctx);
+
+	if (dst_fourcc == V4L2_PIX_FMT_H264) {
+		switch (dev->devtype->product) {
+		case CODA_DX6:
+			value = FMO_SLICE_SAVE_BUF_SIZE << 7;
+			coda_write(dev, value, CODADX6_CMD_ENC_SEQ_FMO);
+			break;
+		case CODA_7541:
+			coda_write(dev, ctx->iram_info.search_ram_paddr,
+					CODA7_CMD_ENC_SEQ_SEARCH_BASE);
+			coda_write(dev, ctx->iram_info.search_ram_size,
+					CODA7_CMD_ENC_SEQ_SEARCH_SIZE);
+			break;
+		case CODA_960:
+			coda_write(dev, 0, CODA9_CMD_ENC_SEQ_ME_OPTION);
+			coda_write(dev, 0, CODA9_CMD_ENC_SEQ_INTRA_WEIGHT);
+		}
+	}
+
+	ret = coda_command_sync(ctx, CODA_COMMAND_SEQ_INIT);
+	if (ret < 0) {
+		v4l2_err(v4l2_dev, "CODA_COMMAND_SEQ_INIT timeout\n");
+		goto out;
+	}
+
+	if (coda_read(dev, CODA_RET_ENC_SEQ_SUCCESS) == 0) {
+		v4l2_err(v4l2_dev, "CODA_COMMAND_SEQ_INIT failed\n");
+		ret = -EFAULT;
+		goto out;
+	}
+	ctx->initialized = 1;
+
+	if (dst_fourcc != V4L2_PIX_FMT_JPEG) {
+		if (dev->devtype->product == CODA_960)
+			ctx->num_internal_frames = 4;
+		else
+			ctx->num_internal_frames = 2;
+		ret = coda_alloc_framebuffers(ctx, q_data_src, dst_fourcc);
+		if (ret < 0) {
+			v4l2_err(v4l2_dev, "failed to allocate framebuffers\n");
+			goto out;
+		}
+		num_fb = 2;
+		stride = q_data_src->bytesperline;
+	} else {
+		ctx->num_internal_frames = 0;
+		num_fb = 0;
+		stride = 0;
+	}
+	coda_write(dev, num_fb, CODA_CMD_SET_FRAME_BUF_NUM);
+	coda_write(dev, stride, CODA_CMD_SET_FRAME_BUF_STRIDE);
+
+	if (dev->devtype->product == CODA_7541) {
+		coda_write(dev, q_data_src->bytesperline,
+				CODA7_CMD_SET_FRAME_SOURCE_BUF_STRIDE);
+	}
+	if (dev->devtype->product != CODA_DX6) {
+		coda_write(dev, ctx->iram_info.buf_bit_use,
+				CODA7_CMD_SET_FRAME_AXI_BIT_ADDR);
+		coda_write(dev, ctx->iram_info.buf_ip_ac_dc_use,
+				CODA7_CMD_SET_FRAME_AXI_IPACDC_ADDR);
+		coda_write(dev, ctx->iram_info.buf_dbk_y_use,
+				CODA7_CMD_SET_FRAME_AXI_DBKY_ADDR);
+		coda_write(dev, ctx->iram_info.buf_dbk_c_use,
+				CODA7_CMD_SET_FRAME_AXI_DBKC_ADDR);
+		coda_write(dev, ctx->iram_info.buf_ovl_use,
+				CODA7_CMD_SET_FRAME_AXI_OVL_ADDR);
+		if (dev->devtype->product == CODA_960) {
+			coda_write(dev, ctx->iram_info.buf_btp_use,
+					CODA9_CMD_SET_FRAME_AXI_BTP_ADDR);
+
+			coda9_set_frame_cache(ctx, q_data_src->fourcc);
+
+			/* FIXME */
+			coda_write(dev, ctx->internal_frames[2].paddr,
+				   CODA9_CMD_SET_FRAME_SUBSAMP_A);
+			coda_write(dev, ctx->internal_frames[3].paddr,
+				   CODA9_CMD_SET_FRAME_SUBSAMP_B);
+		}
+	}
+
+	ret = coda_command_sync(ctx, CODA_COMMAND_SET_FRAME_BUF);
+	if (ret < 0) {
+		v4l2_err(v4l2_dev, "CODA_COMMAND_SET_FRAME_BUF timeout\n");
+		goto out;
+	}
+
+	/* Save stream headers */
+	buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
+	switch (dst_fourcc) {
+	case V4L2_PIX_FMT_H264:
+		/*
+		 * Get SPS in the first frame and copy it to an
+		 * intermediate buffer.
+		 */
+		ret = coda_encode_header(ctx, buf, CODA_HEADER_H264_SPS,
+					 &ctx->vpu_header[0][0],
+					 &ctx->vpu_header_size[0]);
+		if (ret < 0)
+			goto out;
+
+		/*
+		 * Get PPS in the first frame and copy it to an
+		 * intermediate buffer.
+		 */
+		ret = coda_encode_header(ctx, buf, CODA_HEADER_H264_PPS,
+					 &ctx->vpu_header[1][0],
+					 &ctx->vpu_header_size[1]);
+		if (ret < 0)
+			goto out;
+
+		/*
+		 * Length of H.264 headers is variable and thus it might not be
+		 * aligned for the coda to append the encoded frame. In that is
+		 * the case a filler NAL must be added to header 2.
+		 */
+		ctx->vpu_header_size[2] = coda_h264_padding(
+					(ctx->vpu_header_size[0] +
+					 ctx->vpu_header_size[1]),
+					 ctx->vpu_header[2]);
+		break;
+	case V4L2_PIX_FMT_MPEG4:
+		/*
+		 * Get VOS in the first frame and copy it to an
+		 * intermediate buffer
+		 */
+		ret = coda_encode_header(ctx, buf, CODA_HEADER_MP4V_VOS,
+					 &ctx->vpu_header[0][0],
+					 &ctx->vpu_header_size[0]);
+		if (ret < 0)
+			goto out;
+
+		ret = coda_encode_header(ctx, buf, CODA_HEADER_MP4V_VIS,
+					 &ctx->vpu_header[1][0],
+					 &ctx->vpu_header_size[1]);
+		if (ret < 0)
+			goto out;
+
+		ret = coda_encode_header(ctx, buf, CODA_HEADER_MP4V_VOL,
+					 &ctx->vpu_header[2][0],
+					 &ctx->vpu_header_size[2]);
+		if (ret < 0)
+			goto out;
+		break;
+	default:
+		/* No more formats need to save headers at the moment */
+		break;
+	}
+
+out:
+	mutex_unlock(&dev->coda_mutex);
+	return ret;
+}
+
+static int coda_prepare_encode(struct coda_ctx *ctx)
+{
+	struct coda_q_data *q_data_src, *q_data_dst;
+	struct vb2_v4l2_buffer *src_buf, *dst_buf;
+	struct coda_dev *dev = ctx->dev;
+	int force_ipicture;
+	int quant_param = 0;
+	u32 pic_stream_buffer_addr, pic_stream_buffer_size;
+	u32 rot_mode = 0;
+	u32 dst_fourcc;
+	u32 reg;
+
+	src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+	dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
+	q_data_src = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
+	q_data_dst = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
+	dst_fourcc = q_data_dst->fourcc;
+
+	src_buf->sequence = ctx->osequence;
+	dst_buf->sequence = ctx->osequence;
+	ctx->osequence++;
+
+	/*
+	 * Workaround coda firmware BUG that only marks the first
+	 * frame as IDR. This is a problem for some decoders that can't
+	 * recover when a frame is lost.
+	 */
+	if (src_buf->sequence % ctx->params.gop_size) {
+		src_buf->flags |= V4L2_BUF_FLAG_PFRAME;
+		src_buf->flags &= ~V4L2_BUF_FLAG_KEYFRAME;
+	} else {
+		src_buf->flags |= V4L2_BUF_FLAG_KEYFRAME;
+		src_buf->flags &= ~V4L2_BUF_FLAG_PFRAME;
+	}
+
+	if (dev->devtype->product == CODA_960)
+		coda_set_gdi_regs(ctx);
+
+	/*
+	 * Copy headers at the beginning of the first frame for H.264 only.
+	 * In MPEG4 they are already copied by the coda.
+	 */
+	if (src_buf->sequence == 0) {
+		pic_stream_buffer_addr =
+			vb2_dma_contig_plane_dma_addr(&dst_buf->vb2_buf, 0) +
+			ctx->vpu_header_size[0] +
+			ctx->vpu_header_size[1] +
+			ctx->vpu_header_size[2];
+		pic_stream_buffer_size = q_data_dst->sizeimage -
+			ctx->vpu_header_size[0] -
+			ctx->vpu_header_size[1] -
+			ctx->vpu_header_size[2];
+		memcpy(vb2_plane_vaddr(&dst_buf->vb2_buf, 0),
+		       &ctx->vpu_header[0][0], ctx->vpu_header_size[0]);
+		memcpy(vb2_plane_vaddr(&dst_buf->vb2_buf, 0)
+			+ ctx->vpu_header_size[0], &ctx->vpu_header[1][0],
+			ctx->vpu_header_size[1]);
+		memcpy(vb2_plane_vaddr(&dst_buf->vb2_buf, 0)
+			+ ctx->vpu_header_size[0] + ctx->vpu_header_size[1],
+			&ctx->vpu_header[2][0], ctx->vpu_header_size[2]);
+	} else {
+		pic_stream_buffer_addr =
+			vb2_dma_contig_plane_dma_addr(&dst_buf->vb2_buf, 0);
+		pic_stream_buffer_size = q_data_dst->sizeimage;
+	}
+
+	if (src_buf->flags & V4L2_BUF_FLAG_KEYFRAME) {
+		force_ipicture = 1;
+		switch (dst_fourcc) {
+		case V4L2_PIX_FMT_H264:
+			quant_param = ctx->params.h264_intra_qp;
+			break;
+		case V4L2_PIX_FMT_MPEG4:
+			quant_param = ctx->params.mpeg4_intra_qp;
+			break;
+		case V4L2_PIX_FMT_JPEG:
+			quant_param = 30;
+			break;
+		default:
+			v4l2_warn(&ctx->dev->v4l2_dev,
+				"cannot set intra qp, fmt not supported\n");
+			break;
+		}
+	} else {
+		force_ipicture = 0;
+		switch (dst_fourcc) {
+		case V4L2_PIX_FMT_H264:
+			quant_param = ctx->params.h264_inter_qp;
+			break;
+		case V4L2_PIX_FMT_MPEG4:
+			quant_param = ctx->params.mpeg4_inter_qp;
+			break;
+		default:
+			v4l2_warn(&ctx->dev->v4l2_dev,
+				"cannot set inter qp, fmt not supported\n");
+			break;
+		}
+	}
+
+	/* submit */
+	if (ctx->params.rot_mode)
+		rot_mode = CODA_ROT_MIR_ENABLE | ctx->params.rot_mode;
+	coda_write(dev, rot_mode, CODA_CMD_ENC_PIC_ROT_MODE);
+	coda_write(dev, quant_param, CODA_CMD_ENC_PIC_QS);
+
+	if (dev->devtype->product == CODA_960) {
+		coda_write(dev, 4/*FIXME: 0*/, CODA9_CMD_ENC_PIC_SRC_INDEX);
+		coda_write(dev, q_data_src->width, CODA9_CMD_ENC_PIC_SRC_STRIDE);
+		coda_write(dev, 0, CODA9_CMD_ENC_PIC_SUB_FRAME_SYNC);
+
+		reg = CODA9_CMD_ENC_PIC_SRC_ADDR_Y;
+	} else {
+		reg = CODA_CMD_ENC_PIC_SRC_ADDR_Y;
+	}
+	coda_write_base(ctx, q_data_src, src_buf, reg);
+
+	coda_write(dev, force_ipicture << 1 & 0x2,
+		   CODA_CMD_ENC_PIC_OPTION);
+
+	coda_write(dev, pic_stream_buffer_addr, CODA_CMD_ENC_PIC_BB_START);
+	coda_write(dev, pic_stream_buffer_size / 1024,
+		   CODA_CMD_ENC_PIC_BB_SIZE);
+
+	if (!ctx->streamon_out) {
+		/* After streamoff on the output side, set stream end flag */
+		ctx->bit_stream_param |= CODA_BIT_STREAM_END_FLAG;
+		coda_write(dev, ctx->bit_stream_param,
+			   CODA_REG_BIT_BIT_STREAM_PARAM);
+	}
+
+	if (dev->devtype->product != CODA_DX6)
+		coda_write(dev, ctx->iram_info.axi_sram_use,
+				CODA7_REG_BIT_AXI_SRAM_USE);
+
+	trace_coda_enc_pic_run(ctx, src_buf);
+
+	coda_command_async(ctx, CODA_COMMAND_PIC_RUN);
+
+	return 0;
+}
+
+static void coda_finish_encode(struct coda_ctx *ctx)
+{
+	struct vb2_v4l2_buffer *src_buf, *dst_buf;
+	struct coda_dev *dev = ctx->dev;
+	u32 wr_ptr, start_ptr;
+
+	src_buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+	dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
+
+	trace_coda_enc_pic_done(ctx, dst_buf);
+
+	/* Get results from the coda */
+	start_ptr = coda_read(dev, CODA_CMD_ENC_PIC_BB_START);
+	wr_ptr = coda_read(dev, CODA_REG_BIT_WR_PTR(ctx->reg_idx));
+
+	/* Calculate bytesused field */
+	if (dst_buf->sequence == 0) {
+		vb2_set_plane_payload(&dst_buf->vb2_buf, 0, wr_ptr - start_ptr +
+					ctx->vpu_header_size[0] +
+					ctx->vpu_header_size[1] +
+					ctx->vpu_header_size[2]);
+	} else {
+		vb2_set_plane_payload(&dst_buf->vb2_buf, 0, wr_ptr - start_ptr);
+	}
+
+	v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev, "frame size = %u\n",
+		 wr_ptr - start_ptr);
+
+	coda_read(dev, CODA_RET_ENC_PIC_SLICE_NUM);
+	coda_read(dev, CODA_RET_ENC_PIC_FLAG);
+
+	if (coda_read(dev, CODA_RET_ENC_PIC_TYPE) == 0) {
+		dst_buf->flags |= V4L2_BUF_FLAG_KEYFRAME;
+		dst_buf->flags &= ~V4L2_BUF_FLAG_PFRAME;
+	} else {
+		dst_buf->flags |= V4L2_BUF_FLAG_PFRAME;
+		dst_buf->flags &= ~V4L2_BUF_FLAG_KEYFRAME;
+	}
+
+	dst_buf->timestamp = src_buf->timestamp;
+	dst_buf->flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+	dst_buf->flags |=
+		src_buf->flags & V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+	dst_buf->timecode = src_buf->timecode;
+
+	v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_DONE);
+
+	dst_buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
+	coda_m2m_buf_done(ctx, dst_buf, VB2_BUF_STATE_DONE);
+
+	ctx->gopcounter--;
+	if (ctx->gopcounter < 0)
+		ctx->gopcounter = ctx->params.gop_size - 1;
+
+	v4l2_dbg(1, coda_debug, &dev->v4l2_dev,
+		"job finished: encoding frame (%d) (%s)\n",
+		dst_buf->sequence,
+		(dst_buf->flags & V4L2_BUF_FLAG_KEYFRAME) ?
+		"KEYFRAME" : "PFRAME");
+}
+
+static void coda_seq_end_work(struct work_struct *work)
+{
+	struct coda_ctx *ctx = container_of(work, struct coda_ctx, seq_end_work);
+	struct coda_dev *dev = ctx->dev;
+
+	mutex_lock(&ctx->buffer_mutex);
+	mutex_lock(&dev->coda_mutex);
+
+	if (ctx->initialized == 0)
+		goto out;
+
+	v4l2_dbg(1, coda_debug, &dev->v4l2_dev,
+		 "%d: %s: sent command 'SEQ_END' to coda\n", ctx->idx,
+		 __func__);
+	if (coda_command_sync(ctx, CODA_COMMAND_SEQ_END)) {
+		v4l2_err(&dev->v4l2_dev,
+			 "CODA_COMMAND_SEQ_END failed\n");
+	}
+
+	/*
+	 * FIXME: Sometimes h.264 encoding fails with 8-byte sequences missing
+	 * from the output stream after the h.264 decoder has run. Resetting the
+	 * hardware after the decoder has finished seems to help.
+	 */
+	if (dev->devtype->product == CODA_960)
+		coda_hw_reset(ctx);
+
+	kfifo_init(&ctx->bitstream_fifo,
+		ctx->bitstream.vaddr, ctx->bitstream.size);
+
+	coda_free_framebuffers(ctx);
+
+	ctx->initialized = 0;
+
+out:
+	mutex_unlock(&dev->coda_mutex);
+	mutex_unlock(&ctx->buffer_mutex);
+}
+
+static void coda_bit_release(struct coda_ctx *ctx)
+{
+	mutex_lock(&ctx->buffer_mutex);
+	coda_free_framebuffers(ctx);
+	coda_free_context_buffers(ctx);
+	coda_free_bitstream_buffer(ctx);
+	mutex_unlock(&ctx->buffer_mutex);
+}
+
+const struct coda_context_ops coda_bit_encode_ops = {
+	.queue_init = coda_encoder_queue_init,
+	.reqbufs = coda_encoder_reqbufs,
+	.start_streaming = coda_start_encoding,
+	.prepare_run = coda_prepare_encode,
+	.finish_run = coda_finish_encode,
+	.seq_end_work = coda_seq_end_work,
+	.release = coda_bit_release,
+};
+
+/*
+ * Decoder context operations
+ */
+
+static int coda_alloc_bitstream_buffer(struct coda_ctx *ctx,
+				       struct coda_q_data *q_data)
+{
+	if (ctx->bitstream.vaddr)
+		return 0;
+
+	ctx->bitstream.size = roundup_pow_of_two(q_data->sizeimage * 2);
+	ctx->bitstream.vaddr = dma_alloc_writecombine(
+			&ctx->dev->plat_dev->dev, ctx->bitstream.size,
+			&ctx->bitstream.paddr, GFP_KERNEL);
+	if (!ctx->bitstream.vaddr) {
+		v4l2_err(&ctx->dev->v4l2_dev,
+			 "failed to allocate bitstream ringbuffer");
+		return -ENOMEM;
+	}
+	kfifo_init(&ctx->bitstream_fifo,
+		   ctx->bitstream.vaddr, ctx->bitstream.size);
+
+	return 0;
+}
+
+static void coda_free_bitstream_buffer(struct coda_ctx *ctx)
+{
+	if (ctx->bitstream.vaddr == NULL)
+		return;
+
+	dma_free_writecombine(&ctx->dev->plat_dev->dev, ctx->bitstream.size,
+			      ctx->bitstream.vaddr, ctx->bitstream.paddr);
+	ctx->bitstream.vaddr = NULL;
+	kfifo_init(&ctx->bitstream_fifo, NULL, 0);
+}
+
+static int coda_decoder_reqbufs(struct coda_ctx *ctx,
+				struct v4l2_requestbuffers *rb)
+{
+	struct coda_q_data *q_data_src;
+	int ret;
+
+	if (rb->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		return 0;
+
+	if (rb->count) {
+		q_data_src = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
+		ret = coda_alloc_context_buffers(ctx, q_data_src);
+		if (ret < 0)
+			return ret;
+		ret = coda_alloc_bitstream_buffer(ctx, q_data_src);
+		if (ret < 0) {
+			coda_free_context_buffers(ctx);
+			return ret;
+		}
+	} else {
+		coda_free_bitstream_buffer(ctx);
+		coda_free_context_buffers(ctx);
+	}
+
+	return 0;
+}
+
+static int __coda_start_decoding(struct coda_ctx *ctx)
+{
+	struct coda_q_data *q_data_src, *q_data_dst;
+	u32 bitstream_buf, bitstream_size;
+	struct coda_dev *dev = ctx->dev;
+	int width, height;
+	u32 src_fourcc, dst_fourcc;
+	u32 val;
+	int ret;
+
+	/* Start decoding */
+	q_data_src = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
+	q_data_dst = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
+	bitstream_buf = ctx->bitstream.paddr;
+	bitstream_size = ctx->bitstream.size;
+	src_fourcc = q_data_src->fourcc;
+	dst_fourcc = q_data_dst->fourcc;
+
+	coda_write(dev, ctx->parabuf.paddr, CODA_REG_BIT_PARA_BUF_ADDR);
+
+	/* Update coda bitstream read and write pointers from kfifo */
+	coda_kfifo_sync_to_device_full(ctx);
+
+	ctx->frame_mem_ctrl &= ~(CODA_FRAME_CHROMA_INTERLEAVE | (0x3 << 9) |
+				 CODA9_FRAME_TILED2LINEAR);
+	if (dst_fourcc == V4L2_PIX_FMT_NV12)
+		ctx->frame_mem_ctrl |= CODA_FRAME_CHROMA_INTERLEAVE;
+	if (ctx->tiled_map_type == GDI_TILED_FRAME_MB_RASTER_MAP)
+		ctx->frame_mem_ctrl |= (0x3 << 9) | CODA9_FRAME_TILED2LINEAR;
+	coda_write(dev, ctx->frame_mem_ctrl, CODA_REG_BIT_FRAME_MEM_CTRL);
+
+	ctx->display_idx = -1;
+	ctx->frm_dis_flg = 0;
+	coda_write(dev, 0, CODA_REG_BIT_FRM_DIS_FLG(ctx->reg_idx));
+
+	coda_write(dev, CODA_BIT_DEC_SEQ_INIT_ESCAPE,
+			CODA_REG_BIT_BIT_STREAM_PARAM);
+
+	coda_write(dev, bitstream_buf, CODA_CMD_DEC_SEQ_BB_START);
+	coda_write(dev, bitstream_size / 1024, CODA_CMD_DEC_SEQ_BB_SIZE);
+	val = 0;
+	if ((dev->devtype->product == CODA_7541) ||
+	    (dev->devtype->product == CODA_960))
+		val |= CODA_REORDER_ENABLE;
+	if (ctx->codec->src_fourcc == V4L2_PIX_FMT_JPEG)
+		val |= CODA_NO_INT_ENABLE;
+	coda_write(dev, val, CODA_CMD_DEC_SEQ_OPTION);
+
+	ctx->params.codec_mode = ctx->codec->mode;
+	if (dev->devtype->product == CODA_960 &&
+	    src_fourcc == V4L2_PIX_FMT_MPEG4)
+		ctx->params.codec_mode_aux = CODA_MP4_AUX_MPEG4;
+	else
+		ctx->params.codec_mode_aux = 0;
+	if (src_fourcc == V4L2_PIX_FMT_H264) {
+		if (dev->devtype->product == CODA_7541) {
+			coda_write(dev, ctx->psbuf.paddr,
+					CODA_CMD_DEC_SEQ_PS_BB_START);
+			coda_write(dev, (CODA7_PS_BUF_SIZE / 1024),
+					CODA_CMD_DEC_SEQ_PS_BB_SIZE);
+		}
+		if (dev->devtype->product == CODA_960) {
+			coda_write(dev, 0, CODA_CMD_DEC_SEQ_X264_MV_EN);
+			coda_write(dev, 512, CODA_CMD_DEC_SEQ_SPP_CHUNK_SIZE);
+		}
+	}
+	if (dev->devtype->product != CODA_960)
+		coda_write(dev, 0, CODA_CMD_DEC_SEQ_SRC_SIZE);
+
+	if (coda_command_sync(ctx, CODA_COMMAND_SEQ_INIT)) {
+		v4l2_err(&dev->v4l2_dev, "CODA_COMMAND_SEQ_INIT timeout\n");
+		coda_write(dev, 0, CODA_REG_BIT_BIT_STREAM_PARAM);
+		return -ETIMEDOUT;
+	}
+	ctx->initialized = 1;
+
+	/* Update kfifo out pointer from coda bitstream read pointer */
+	coda_kfifo_sync_from_device(ctx);
+
+	coda_write(dev, 0, CODA_REG_BIT_BIT_STREAM_PARAM);
+
+	if (coda_read(dev, CODA_RET_DEC_SEQ_SUCCESS) == 0) {
+		v4l2_err(&dev->v4l2_dev,
+			"CODA_COMMAND_SEQ_INIT failed, error code = %d\n",
+			coda_read(dev, CODA_RET_DEC_SEQ_ERR_REASON));
+		return -EAGAIN;
+	}
+
+	val = coda_read(dev, CODA_RET_DEC_SEQ_SRC_SIZE);
+	if (dev->devtype->product == CODA_DX6) {
+		width = (val >> CODADX6_PICWIDTH_OFFSET) & CODADX6_PICWIDTH_MASK;
+		height = val & CODADX6_PICHEIGHT_MASK;
+	} else {
+		width = (val >> CODA7_PICWIDTH_OFFSET) & CODA7_PICWIDTH_MASK;
+		height = val & CODA7_PICHEIGHT_MASK;
+	}
+
+	if (width > q_data_dst->bytesperline || height > q_data_dst->height) {
+		v4l2_err(&dev->v4l2_dev, "stream is %dx%d, not %dx%d\n",
+			 width, height, q_data_dst->bytesperline,
+			 q_data_dst->height);
+		return -EINVAL;
+	}
+
+	width = round_up(width, 16);
+	height = round_up(height, 16);
+
+	v4l2_dbg(1, coda_debug, &dev->v4l2_dev, "%s instance %d now: %dx%d\n",
+		 __func__, ctx->idx, width, height);
+
+	ctx->num_internal_frames = coda_read(dev, CODA_RET_DEC_SEQ_FRAME_NEED);
+	if (ctx->num_internal_frames > CODA_MAX_FRAMEBUFFERS) {
+		v4l2_err(&dev->v4l2_dev,
+			 "not enough framebuffers to decode (%d < %d)\n",
+			 CODA_MAX_FRAMEBUFFERS, ctx->num_internal_frames);
+		return -EINVAL;
+	}
+
+	if (src_fourcc == V4L2_PIX_FMT_H264) {
+		u32 left_right;
+		u32 top_bottom;
+
+		left_right = coda_read(dev, CODA_RET_DEC_SEQ_CROP_LEFT_RIGHT);
+		top_bottom = coda_read(dev, CODA_RET_DEC_SEQ_CROP_TOP_BOTTOM);
+
+		q_data_dst->rect.left = (left_right >> 10) & 0x3ff;
+		q_data_dst->rect.top = (top_bottom >> 10) & 0x3ff;
+		q_data_dst->rect.width = width - q_data_dst->rect.left -
+					 (left_right & 0x3ff);
+		q_data_dst->rect.height = height - q_data_dst->rect.top -
+					  (top_bottom & 0x3ff);
+	}
+
+	ret = coda_alloc_framebuffers(ctx, q_data_dst, src_fourcc);
+	if (ret < 0) {
+		v4l2_err(&dev->v4l2_dev, "failed to allocate framebuffers\n");
+		return ret;
+	}
+
+	/* Tell the decoder how many frame buffers we allocated. */
+	coda_write(dev, ctx->num_internal_frames, CODA_CMD_SET_FRAME_BUF_NUM);
+	coda_write(dev, width, CODA_CMD_SET_FRAME_BUF_STRIDE);
+
+	if (dev->devtype->product != CODA_DX6) {
+		/* Set secondary AXI IRAM */
+		coda_setup_iram(ctx);
+
+		coda_write(dev, ctx->iram_info.buf_bit_use,
+				CODA7_CMD_SET_FRAME_AXI_BIT_ADDR);
+		coda_write(dev, ctx->iram_info.buf_ip_ac_dc_use,
+				CODA7_CMD_SET_FRAME_AXI_IPACDC_ADDR);
+		coda_write(dev, ctx->iram_info.buf_dbk_y_use,
+				CODA7_CMD_SET_FRAME_AXI_DBKY_ADDR);
+		coda_write(dev, ctx->iram_info.buf_dbk_c_use,
+				CODA7_CMD_SET_FRAME_AXI_DBKC_ADDR);
+		coda_write(dev, ctx->iram_info.buf_ovl_use,
+				CODA7_CMD_SET_FRAME_AXI_OVL_ADDR);
+		if (dev->devtype->product == CODA_960) {
+			coda_write(dev, ctx->iram_info.buf_btp_use,
+					CODA9_CMD_SET_FRAME_AXI_BTP_ADDR);
+
+			coda_write(dev, -1, CODA9_CMD_SET_FRAME_DELAY);
+			coda9_set_frame_cache(ctx, dst_fourcc);
+		}
+	}
+
+	if (src_fourcc == V4L2_PIX_FMT_H264) {
+		coda_write(dev, ctx->slicebuf.paddr,
+				CODA_CMD_SET_FRAME_SLICE_BB_START);
+		coda_write(dev, ctx->slicebuf.size / 1024,
+				CODA_CMD_SET_FRAME_SLICE_BB_SIZE);
+	}
+
+	if (dev->devtype->product == CODA_7541) {
+		int max_mb_x = 1920 / 16;
+		int max_mb_y = 1088 / 16;
+		int max_mb_num = max_mb_x * max_mb_y;
+
+		coda_write(dev, max_mb_num << 16 | max_mb_x << 8 | max_mb_y,
+				CODA7_CMD_SET_FRAME_MAX_DEC_SIZE);
+	} else if (dev->devtype->product == CODA_960) {
+		int max_mb_x = 1920 / 16;
+		int max_mb_y = 1088 / 16;
+		int max_mb_num = max_mb_x * max_mb_y;
+
+		coda_write(dev, max_mb_num << 16 | max_mb_x << 8 | max_mb_y,
+				CODA9_CMD_SET_FRAME_MAX_DEC_SIZE);
+	}
+
+	if (coda_command_sync(ctx, CODA_COMMAND_SET_FRAME_BUF)) {
+		v4l2_err(&ctx->dev->v4l2_dev,
+			 "CODA_COMMAND_SET_FRAME_BUF timeout\n");
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static int coda_start_decoding(struct coda_ctx *ctx)
+{
+	struct coda_dev *dev = ctx->dev;
+	int ret;
+
+	mutex_lock(&dev->coda_mutex);
+	ret = __coda_start_decoding(ctx);
+	mutex_unlock(&dev->coda_mutex);
+
+	return ret;
+}
+
+static int coda_prepare_decode(struct coda_ctx *ctx)
+{
+	struct vb2_v4l2_buffer *dst_buf;
+	struct coda_dev *dev = ctx->dev;
+	struct coda_q_data *q_data_dst;
+	struct coda_buffer_meta *meta;
+	unsigned long flags;
+	u32 reg_addr, reg_stride;
+
+	dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
+	q_data_dst = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
+
+	/* Try to copy source buffer contents into the bitstream ringbuffer */
+	mutex_lock(&ctx->bitstream_mutex);
+	coda_fill_bitstream(ctx, true);
+	mutex_unlock(&ctx->bitstream_mutex);
+
+	if (coda_get_bitstream_payload(ctx) < 512 &&
+	    (!(ctx->bit_stream_param & CODA_BIT_STREAM_END_FLAG))) {
+		v4l2_dbg(1, coda_debug, &dev->v4l2_dev,
+			 "bitstream payload: %d, skipping\n",
+			 coda_get_bitstream_payload(ctx));
+		v4l2_m2m_job_finish(ctx->dev->m2m_dev, ctx->fh.m2m_ctx);
+		return -EAGAIN;
+	}
+
+	/* Run coda_start_decoding (again) if not yet initialized */
+	if (!ctx->initialized) {
+		int ret = __coda_start_decoding(ctx);
+
+		if (ret < 0) {
+			v4l2_err(&dev->v4l2_dev, "failed to start decoding\n");
+			v4l2_m2m_job_finish(ctx->dev->m2m_dev, ctx->fh.m2m_ctx);
+			return -EAGAIN;
+		} else {
+			ctx->initialized = 1;
+		}
+	}
+
+	if (dev->devtype->product == CODA_960)
+		coda_set_gdi_regs(ctx);
+
+	if (dev->devtype->product == CODA_960) {
+		/*
+		 * The CODA960 seems to have an internal list of buffers with
+		 * 64 entries that includes the registered frame buffers as
+		 * well as the rotator buffer output.
+		 * ROT_INDEX needs to be < 0x40, but > ctx->num_internal_frames.
+		 */
+		coda_write(dev, CODA_MAX_FRAMEBUFFERS + dst_buf->vb2_buf.index,
+				CODA9_CMD_DEC_PIC_ROT_INDEX);
+
+		reg_addr = CODA9_CMD_DEC_PIC_ROT_ADDR_Y;
+		reg_stride = CODA9_CMD_DEC_PIC_ROT_STRIDE;
+	} else {
+		reg_addr = CODA_CMD_DEC_PIC_ROT_ADDR_Y;
+		reg_stride = CODA_CMD_DEC_PIC_ROT_STRIDE;
+	}
+	coda_write_base(ctx, q_data_dst, dst_buf, reg_addr);
+	coda_write(dev, q_data_dst->bytesperline, reg_stride);
+
+	coda_write(dev, CODA_ROT_MIR_ENABLE | ctx->params.rot_mode,
+			CODA_CMD_DEC_PIC_ROT_MODE);
+
+	switch (dev->devtype->product) {
+	case CODA_DX6:
+		/* TBD */
+	case CODA_7541:
+		coda_write(dev, CODA_PRE_SCAN_EN, CODA_CMD_DEC_PIC_OPTION);
+		break;
+	case CODA_960:
+		/* 'hardcode to use interrupt disable mode'? */
+		coda_write(dev, (1 << 10), CODA_CMD_DEC_PIC_OPTION);
+		break;
+	}
+
+	coda_write(dev, 0, CODA_CMD_DEC_PIC_SKIP_NUM);
+
+	coda_write(dev, 0, CODA_CMD_DEC_PIC_BB_START);
+	coda_write(dev, 0, CODA_CMD_DEC_PIC_START_BYTE);
+
+	if (dev->devtype->product != CODA_DX6)
+		coda_write(dev, ctx->iram_info.axi_sram_use,
+				CODA7_REG_BIT_AXI_SRAM_USE);
+
+	spin_lock_irqsave(&ctx->buffer_meta_lock, flags);
+	meta = list_first_entry_or_null(&ctx->buffer_meta_list,
+					struct coda_buffer_meta, list);
+
+	if (meta && ctx->codec->src_fourcc == V4L2_PIX_FMT_JPEG) {
+
+		/* If this is the last buffer in the bitstream, add padding */
+		if (meta->end == (ctx->bitstream_fifo.kfifo.in &
+				  ctx->bitstream_fifo.kfifo.mask)) {
+			static unsigned char buf[512];
+			unsigned int pad;
+
+			/* Pad to multiple of 256 and then add 256 more */
+			pad = ((0 - meta->end) & 0xff) + 256;
+
+			memset(buf, 0xff, sizeof(buf));
+
+			kfifo_in(&ctx->bitstream_fifo, buf, pad);
+		}
+	}
+	spin_unlock_irqrestore(&ctx->buffer_meta_lock, flags);
+
+	coda_kfifo_sync_to_device_full(ctx);
+
+	/* Clear decode success flag */
+	coda_write(dev, 0, CODA_RET_DEC_PIC_SUCCESS);
+
+	trace_coda_dec_pic_run(ctx, meta);
+
+	coda_command_async(ctx, CODA_COMMAND_PIC_RUN);
+
+	return 0;
+}
+
+static void coda_finish_decode(struct coda_ctx *ctx)
+{
+	struct coda_dev *dev = ctx->dev;
+	struct coda_q_data *q_data_src;
+	struct coda_q_data *q_data_dst;
+	struct vb2_v4l2_buffer *dst_buf;
+	struct coda_buffer_meta *meta;
+	unsigned long payload;
+	unsigned long flags;
+	int width, height;
+	int decoded_idx;
+	int display_idx;
+	u32 src_fourcc;
+	int success;
+	u32 err_mb;
+	u32 val;
+
+	/* Update kfifo out pointer from coda bitstream read pointer */
+	coda_kfifo_sync_from_device(ctx);
+
+	/*
+	 * in stream-end mode, the read pointer can overshoot the write pointer
+	 * by up to 512 bytes
+	 */
+	if (ctx->bit_stream_param & CODA_BIT_STREAM_END_FLAG) {
+		if (coda_get_bitstream_payload(ctx) >= ctx->bitstream.size - 512)
+			kfifo_init(&ctx->bitstream_fifo,
+				ctx->bitstream.vaddr, ctx->bitstream.size);
+	}
+
+	q_data_src = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
+	src_fourcc = q_data_src->fourcc;
+
+	val = coda_read(dev, CODA_RET_DEC_PIC_SUCCESS);
+	if (val != 1)
+		pr_err("DEC_PIC_SUCCESS = %d\n", val);
+
+	success = val & 0x1;
+	if (!success)
+		v4l2_err(&dev->v4l2_dev, "decode failed\n");
+
+	if (src_fourcc == V4L2_PIX_FMT_H264) {
+		if (val & (1 << 3))
+			v4l2_err(&dev->v4l2_dev,
+				 "insufficient PS buffer space (%d bytes)\n",
+				 ctx->psbuf.size);
+		if (val & (1 << 2))
+			v4l2_err(&dev->v4l2_dev,
+				 "insufficient slice buffer space (%d bytes)\n",
+				 ctx->slicebuf.size);
+	}
+
+	val = coda_read(dev, CODA_RET_DEC_PIC_SIZE);
+	width = (val >> 16) & 0xffff;
+	height = val & 0xffff;
+
+	q_data_dst = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
+
+	/* frame crop information */
+	if (src_fourcc == V4L2_PIX_FMT_H264) {
+		u32 left_right;
+		u32 top_bottom;
+
+		left_right = coda_read(dev, CODA_RET_DEC_PIC_CROP_LEFT_RIGHT);
+		top_bottom = coda_read(dev, CODA_RET_DEC_PIC_CROP_TOP_BOTTOM);
+
+		if (left_right == 0xffffffff && top_bottom == 0xffffffff) {
+			/* Keep current crop information */
+		} else {
+			struct v4l2_rect *rect = &q_data_dst->rect;
+
+			rect->left = left_right >> 16 & 0xffff;
+			rect->top = top_bottom >> 16 & 0xffff;
+			rect->width = width - rect->left -
+				      (left_right & 0xffff);
+			rect->height = height - rect->top -
+				       (top_bottom & 0xffff);
+		}
+	} else {
+		/* no cropping */
+	}
+
+	err_mb = coda_read(dev, CODA_RET_DEC_PIC_ERR_MB);
+	if (err_mb > 0)
+		v4l2_err(&dev->v4l2_dev,
+			 "errors in %d macroblocks\n", err_mb);
+
+	if (dev->devtype->product == CODA_7541) {
+		val = coda_read(dev, CODA_RET_DEC_PIC_OPTION);
+		if (val == 0) {
+			/* not enough bitstream data */
+			v4l2_dbg(1, coda_debug, &dev->v4l2_dev,
+				 "prescan failed: %d\n", val);
+			ctx->hold = true;
+			return;
+		}
+	}
+
+	ctx->frm_dis_flg = coda_read(dev,
+				     CODA_REG_BIT_FRM_DIS_FLG(ctx->reg_idx));
+
+	/*
+	 * The previous display frame was copied out by the rotator,
+	 * now it can be overwritten again
+	 */
+	if (ctx->display_idx >= 0 &&
+	    ctx->display_idx < ctx->num_internal_frames) {
+		ctx->frm_dis_flg &= ~(1 << ctx->display_idx);
+		coda_write(dev, ctx->frm_dis_flg,
+				CODA_REG_BIT_FRM_DIS_FLG(ctx->reg_idx));
+	}
+
+	/*
+	 * The index of the last decoded frame, not necessarily in
+	 * display order, and the index of the next display frame.
+	 * The latter could have been decoded in a previous run.
+	 */
+	decoded_idx = coda_read(dev, CODA_RET_DEC_PIC_CUR_IDX);
+	display_idx = coda_read(dev, CODA_RET_DEC_PIC_FRAME_IDX);
+
+	if (decoded_idx == -1) {
+		/* no frame was decoded, but we might have a display frame */
+		if (display_idx >= 0 && display_idx < ctx->num_internal_frames)
+			ctx->sequence_offset++;
+		else if (ctx->display_idx < 0)
+			ctx->hold = true;
+	} else if (decoded_idx == -2) {
+		/* no frame was decoded, we still return remaining buffers */
+	} else if (decoded_idx < 0 || decoded_idx >= ctx->num_internal_frames) {
+		v4l2_err(&dev->v4l2_dev,
+			 "decoded frame index out of range: %d\n", decoded_idx);
+	} else {
+		val = coda_read(dev, CODA_RET_DEC_PIC_FRAME_NUM) - 1;
+		val -= ctx->sequence_offset;
+		spin_lock_irqsave(&ctx->buffer_meta_lock, flags);
+		if (!list_empty(&ctx->buffer_meta_list)) {
+			meta = list_first_entry(&ctx->buffer_meta_list,
+					      struct coda_buffer_meta, list);
+			list_del(&meta->list);
+			ctx->num_metas--;
+			spin_unlock_irqrestore(&ctx->buffer_meta_lock, flags);
+			/*
+			 * Clamp counters to 16 bits for comparison, as the HW
+			 * counter rolls over at this point for h.264. This
+			 * may be different for other formats, but using 16 bits
+			 * should be enough to detect most errors and saves us
+			 * from doing different things based on the format.
+			 */
+			if ((val & 0xffff) != (meta->sequence & 0xffff)) {
+				v4l2_err(&dev->v4l2_dev,
+					 "sequence number mismatch (%d(%d) != %d)\n",
+					 val, ctx->sequence_offset,
+					 meta->sequence);
+			}
+			ctx->frame_metas[decoded_idx] = *meta;
+			kfree(meta);
+		} else {
+			spin_unlock_irqrestore(&ctx->buffer_meta_lock, flags);
+			v4l2_err(&dev->v4l2_dev, "empty timestamp list!\n");
+			memset(&ctx->frame_metas[decoded_idx], 0,
+			       sizeof(struct coda_buffer_meta));
+			ctx->frame_metas[decoded_idx].sequence = val;
+			ctx->sequence_offset++;
+		}
+
+		trace_coda_dec_pic_done(ctx, &ctx->frame_metas[decoded_idx]);
+
+		val = coda_read(dev, CODA_RET_DEC_PIC_TYPE) & 0x7;
+		if (val == 0)
+			ctx->frame_types[decoded_idx] = V4L2_BUF_FLAG_KEYFRAME;
+		else if (val == 1)
+			ctx->frame_types[decoded_idx] = V4L2_BUF_FLAG_PFRAME;
+		else
+			ctx->frame_types[decoded_idx] = V4L2_BUF_FLAG_BFRAME;
+
+		ctx->frame_errors[decoded_idx] = err_mb;
+	}
+
+	if (display_idx == -1) {
+		/*
+		 * no more frames to be decoded, but there could still
+		 * be rotator output to dequeue
+		 */
+		ctx->hold = true;
+	} else if (display_idx == -3) {
+		/* possibly prescan failure */
+	} else if (display_idx < 0 || display_idx >= ctx->num_internal_frames) {
+		v4l2_err(&dev->v4l2_dev,
+			 "presentation frame index out of range: %d\n",
+			 display_idx);
+	}
+
+	/* If a frame was copied out, return it */
+	if (ctx->display_idx >= 0 &&
+	    ctx->display_idx < ctx->num_internal_frames) {
+		dst_buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
+		dst_buf->sequence = ctx->osequence++;
+
+		dst_buf->flags &= ~(V4L2_BUF_FLAG_KEYFRAME |
+					     V4L2_BUF_FLAG_PFRAME |
+					     V4L2_BUF_FLAG_BFRAME);
+		dst_buf->flags |= ctx->frame_types[ctx->display_idx];
+		meta = &ctx->frame_metas[ctx->display_idx];
+		dst_buf->timecode = meta->timecode;
+		dst_buf->timestamp = meta->timestamp;
+
+		trace_coda_dec_rot_done(ctx, dst_buf, meta);
+
+		switch (q_data_dst->fourcc) {
+		case V4L2_PIX_FMT_YUV420:
+		case V4L2_PIX_FMT_YVU420:
+		case V4L2_PIX_FMT_NV12:
+		default:
+			payload = width * height * 3 / 2;
+			break;
+		case V4L2_PIX_FMT_YUV422P:
+			payload = width * height * 2;
+			break;
+		}
+		vb2_set_plane_payload(&dst_buf->vb2_buf, 0, payload);
+
+		coda_m2m_buf_done(ctx, dst_buf, ctx->frame_errors[display_idx] ?
+				  VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+
+		v4l2_dbg(1, coda_debug, &dev->v4l2_dev,
+			"job finished: decoding frame (%d) (%s)\n",
+			dst_buf->sequence,
+			(dst_buf->flags & V4L2_BUF_FLAG_KEYFRAME) ?
+			"KEYFRAME" : "PFRAME");
+	} else {
+		v4l2_dbg(1, coda_debug, &dev->v4l2_dev,
+			"job finished: no frame decoded\n");
+	}
+
+	/* The rotator will copy the current display frame next time */
+	ctx->display_idx = display_idx;
+}
+
+const struct coda_context_ops coda_bit_decode_ops = {
+	.queue_init = coda_decoder_queue_init,
+	.reqbufs = coda_decoder_reqbufs,
+	.start_streaming = coda_start_decoding,
+	.prepare_run = coda_prepare_decode,
+	.finish_run = coda_finish_decode,
+	.seq_end_work = coda_seq_end_work,
+	.release = coda_bit_release,
+};
+
+irqreturn_t coda_irq_handler(int irq, void *data)
+{
+	struct coda_dev *dev = data;
+	struct coda_ctx *ctx;
+
+	/* read status register to attend the IRQ */
+	coda_read(dev, CODA_REG_BIT_INT_STATUS);
+	coda_write(dev, CODA_REG_BIT_INT_CLEAR_SET,
+		      CODA_REG_BIT_INT_CLEAR);
+
+	ctx = v4l2_m2m_get_curr_priv(dev->m2m_dev);
+	if (ctx == NULL) {
+		v4l2_err(&dev->v4l2_dev,
+			 "Instance released before the end of transaction\n");
+		mutex_unlock(&dev->coda_mutex);
+		return IRQ_HANDLED;
+	}
+
+	trace_coda_bit_done(ctx);
+
+	if (ctx->aborting) {
+		v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
+			 "task has been aborted\n");
+	}
+
+	if (coda_isbusy(ctx->dev)) {
+		v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
+			 "coda is still busy!!!!\n");
+		return IRQ_NONE;
+	}
+
+	complete(&ctx->completion);
+
+	return IRQ_HANDLED;
+}
diff --git a/drivers/media/platform/coda/coda-common.c b/drivers/media/platform/coda/coda-common.c
new file mode 100644
index 0000000..323aad3
--- /dev/null
+++ b/drivers/media/platform/coda/coda-common.c
@@ -0,0 +1,2317 @@
+/*
+ * Coda multi-standard codec IP
+ *
+ * Copyright (C) 2012 Vista Silicon S.L.
+ *    Javier Martin, <javier.martin@vista-silicon.com>
+ *    Xavier Duret
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/clk.h>
+#include <linux/debugfs.h>
+#include <linux/delay.h>
+#include <linux/firmware.h>
+#include <linux/gcd.h>
+#include <linux/genalloc.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/kfifo.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <linux/of.h>
+#include <linux/platform_data/coda.h>
+#include <linux/reset.h>
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/videobuf2-dma-contig.h>
+#include <media/videobuf2-vmalloc.h>
+
+#include "coda.h"
+
+#define CODA_NAME		"coda"
+
+#define CODADX6_MAX_INSTANCES	4
+#define CODA_MAX_FORMATS	4
+
+#define CODA_ISRAM_SIZE	(2048 * 2)
+
+#define MIN_W 176
+#define MIN_H 144
+
+#define S_ALIGN		1 /* multiple of 2 */
+#define W_ALIGN		1 /* multiple of 2 */
+#define H_ALIGN		1 /* multiple of 2 */
+
+#define fh_to_ctx(__fh)	container_of(__fh, struct coda_ctx, fh)
+
+int coda_debug;
+module_param(coda_debug, int, 0644);
+MODULE_PARM_DESC(coda_debug, "Debug level (0-2)");
+
+static int disable_tiling;
+module_param(disable_tiling, int, 0644);
+MODULE_PARM_DESC(disable_tiling, "Disable tiled frame buffers");
+
+void coda_write(struct coda_dev *dev, u32 data, u32 reg)
+{
+	v4l2_dbg(2, coda_debug, &dev->v4l2_dev,
+		 "%s: data=0x%x, reg=0x%x\n", __func__, data, reg);
+	writel(data, dev->regs_base + reg);
+}
+
+unsigned int coda_read(struct coda_dev *dev, u32 reg)
+{
+	u32 data;
+
+	data = readl(dev->regs_base + reg);
+	v4l2_dbg(2, coda_debug, &dev->v4l2_dev,
+		 "%s: data=0x%x, reg=0x%x\n", __func__, data, reg);
+	return data;
+}
+
+void coda_write_base(struct coda_ctx *ctx, struct coda_q_data *q_data,
+		     struct vb2_v4l2_buffer *buf, unsigned int reg_y)
+{
+	u32 base_y = vb2_dma_contig_plane_dma_addr(&buf->vb2_buf, 0);
+	u32 base_cb, base_cr;
+
+	switch (q_data->fourcc) {
+	case V4L2_PIX_FMT_NV12:
+	case V4L2_PIX_FMT_YUV420:
+	default:
+		base_cb = base_y + q_data->bytesperline * q_data->height;
+		base_cr = base_cb + q_data->bytesperline * q_data->height / 4;
+		break;
+	case V4L2_PIX_FMT_YVU420:
+		/* Switch Cb and Cr for YVU420 format */
+		base_cr = base_y + q_data->bytesperline * q_data->height;
+		base_cb = base_cr + q_data->bytesperline * q_data->height / 4;
+		break;
+	case V4L2_PIX_FMT_YUV422P:
+		base_cb = base_y + q_data->bytesperline * q_data->height;
+		base_cr = base_cb + q_data->bytesperline * q_data->height / 2;
+	}
+
+	coda_write(ctx->dev, base_y, reg_y);
+	coda_write(ctx->dev, base_cb, reg_y + 4);
+	coda_write(ctx->dev, base_cr, reg_y + 8);
+}
+
+#define CODA_CODEC(mode, src_fourcc, dst_fourcc, max_w, max_h) \
+	{ mode, src_fourcc, dst_fourcc, max_w, max_h }
+
+/*
+ * Arrays of codecs supported by each given version of Coda:
+ *  i.MX27 -> codadx6
+ *  i.MX5x -> coda7
+ *  i.MX6  -> coda960
+ * Use V4L2_PIX_FMT_YUV420 as placeholder for all supported YUV 4:2:0 variants
+ */
+static const struct coda_codec codadx6_codecs[] = {
+	CODA_CODEC(CODADX6_MODE_ENCODE_H264, V4L2_PIX_FMT_YUV420, V4L2_PIX_FMT_H264,  720, 576),
+	CODA_CODEC(CODADX6_MODE_ENCODE_MP4,  V4L2_PIX_FMT_YUV420, V4L2_PIX_FMT_MPEG4, 720, 576),
+};
+
+static const struct coda_codec coda7_codecs[] = {
+	CODA_CODEC(CODA7_MODE_ENCODE_H264, V4L2_PIX_FMT_YUV420, V4L2_PIX_FMT_H264,   1280, 720),
+	CODA_CODEC(CODA7_MODE_ENCODE_MP4,  V4L2_PIX_FMT_YUV420, V4L2_PIX_FMT_MPEG4,  1280, 720),
+	CODA_CODEC(CODA7_MODE_ENCODE_MJPG, V4L2_PIX_FMT_YUV420, V4L2_PIX_FMT_JPEG,   8192, 8192),
+	CODA_CODEC(CODA7_MODE_DECODE_H264, V4L2_PIX_FMT_H264,   V4L2_PIX_FMT_YUV420, 1920, 1088),
+	CODA_CODEC(CODA7_MODE_DECODE_MP4,  V4L2_PIX_FMT_MPEG4,  V4L2_PIX_FMT_YUV420, 1920, 1088),
+	CODA_CODEC(CODA7_MODE_DECODE_MJPG, V4L2_PIX_FMT_JPEG,   V4L2_PIX_FMT_YUV420, 8192, 8192),
+};
+
+static const struct coda_codec coda9_codecs[] = {
+	CODA_CODEC(CODA9_MODE_ENCODE_H264, V4L2_PIX_FMT_YUV420, V4L2_PIX_FMT_H264,   1920, 1088),
+	CODA_CODEC(CODA9_MODE_ENCODE_MP4,  V4L2_PIX_FMT_YUV420, V4L2_PIX_FMT_MPEG4,  1920, 1088),
+	CODA_CODEC(CODA9_MODE_DECODE_H264, V4L2_PIX_FMT_H264,   V4L2_PIX_FMT_YUV420, 1920, 1088),
+	CODA_CODEC(CODA9_MODE_DECODE_MP4,  V4L2_PIX_FMT_MPEG4,  V4L2_PIX_FMT_YUV420, 1920, 1088),
+};
+
+struct coda_video_device {
+	const char *name;
+	enum coda_inst_type type;
+	const struct coda_context_ops *ops;
+	bool direct;
+	u32 src_formats[CODA_MAX_FORMATS];
+	u32 dst_formats[CODA_MAX_FORMATS];
+};
+
+static const struct coda_video_device coda_bit_encoder = {
+	.name = "coda-encoder",
+	.type = CODA_INST_ENCODER,
+	.ops = &coda_bit_encode_ops,
+	.src_formats = {
+		V4L2_PIX_FMT_NV12,
+		V4L2_PIX_FMT_YUV420,
+		V4L2_PIX_FMT_YVU420,
+	},
+	.dst_formats = {
+		V4L2_PIX_FMT_H264,
+		V4L2_PIX_FMT_MPEG4,
+	},
+};
+
+static const struct coda_video_device coda_bit_jpeg_encoder = {
+	.name = "coda-jpeg-encoder",
+	.type = CODA_INST_ENCODER,
+	.ops = &coda_bit_encode_ops,
+	.src_formats = {
+		V4L2_PIX_FMT_NV12,
+		V4L2_PIX_FMT_YUV420,
+		V4L2_PIX_FMT_YVU420,
+		V4L2_PIX_FMT_YUV422P,
+	},
+	.dst_formats = {
+		V4L2_PIX_FMT_JPEG,
+	},
+};
+
+static const struct coda_video_device coda_bit_decoder = {
+	.name = "coda-decoder",
+	.type = CODA_INST_DECODER,
+	.ops = &coda_bit_decode_ops,
+	.src_formats = {
+		V4L2_PIX_FMT_H264,
+		V4L2_PIX_FMT_MPEG4,
+	},
+	.dst_formats = {
+		V4L2_PIX_FMT_NV12,
+		V4L2_PIX_FMT_YUV420,
+		V4L2_PIX_FMT_YVU420,
+	},
+};
+
+static const struct coda_video_device coda_bit_jpeg_decoder = {
+	.name = "coda-jpeg-decoder",
+	.type = CODA_INST_DECODER,
+	.ops = &coda_bit_decode_ops,
+	.src_formats = {
+		V4L2_PIX_FMT_JPEG,
+	},
+	.dst_formats = {
+		V4L2_PIX_FMT_NV12,
+		V4L2_PIX_FMT_YUV420,
+		V4L2_PIX_FMT_YVU420,
+		V4L2_PIX_FMT_YUV422P,
+	},
+};
+
+static const struct coda_video_device *codadx6_video_devices[] = {
+	&coda_bit_encoder,
+};
+
+static const struct coda_video_device *coda7_video_devices[] = {
+	&coda_bit_jpeg_encoder,
+	&coda_bit_jpeg_decoder,
+	&coda_bit_encoder,
+	&coda_bit_decoder,
+};
+
+static const struct coda_video_device *coda9_video_devices[] = {
+	&coda_bit_encoder,
+	&coda_bit_decoder,
+};
+
+/*
+ * Normalize all supported YUV 4:2:0 formats to the value used in the codec
+ * tables.
+ */
+static u32 coda_format_normalize_yuv(u32 fourcc)
+{
+	switch (fourcc) {
+	case V4L2_PIX_FMT_NV12:
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_YVU420:
+	case V4L2_PIX_FMT_YUV422P:
+		return V4L2_PIX_FMT_YUV420;
+	default:
+		return fourcc;
+	}
+}
+
+static const struct coda_codec *coda_find_codec(struct coda_dev *dev,
+						int src_fourcc, int dst_fourcc)
+{
+	const struct coda_codec *codecs = dev->devtype->codecs;
+	int num_codecs = dev->devtype->num_codecs;
+	int k;
+
+	src_fourcc = coda_format_normalize_yuv(src_fourcc);
+	dst_fourcc = coda_format_normalize_yuv(dst_fourcc);
+	if (src_fourcc == dst_fourcc)
+		return NULL;
+
+	for (k = 0; k < num_codecs; k++) {
+		if (codecs[k].src_fourcc == src_fourcc &&
+		    codecs[k].dst_fourcc == dst_fourcc)
+			break;
+	}
+
+	if (k == num_codecs)
+		return NULL;
+
+	return &codecs[k];
+}
+
+static void coda_get_max_dimensions(struct coda_dev *dev,
+				    const struct coda_codec *codec,
+				    int *max_w, int *max_h)
+{
+	const struct coda_codec *codecs = dev->devtype->codecs;
+	int num_codecs = dev->devtype->num_codecs;
+	unsigned int w, h;
+	int k;
+
+	if (codec) {
+		w = codec->max_w;
+		h = codec->max_h;
+	} else {
+		for (k = 0, w = 0, h = 0; k < num_codecs; k++) {
+			w = max(w, codecs[k].max_w);
+			h = max(h, codecs[k].max_h);
+		}
+	}
+
+	if (max_w)
+		*max_w = w;
+	if (max_h)
+		*max_h = h;
+}
+
+const struct coda_video_device *to_coda_video_device(struct video_device *vdev)
+{
+	struct coda_dev *dev = video_get_drvdata(vdev);
+	unsigned int i = vdev - dev->vfd;
+
+	if (i >= dev->devtype->num_vdevs)
+		return NULL;
+
+	return dev->devtype->vdevs[i];
+}
+
+const char *coda_product_name(int product)
+{
+	static char buf[9];
+
+	switch (product) {
+	case CODA_DX6:
+		return "CodaDx6";
+	case CODA_7541:
+		return "CODA7541";
+	case CODA_960:
+		return "CODA960";
+	default:
+		snprintf(buf, sizeof(buf), "(0x%04x)", product);
+		return buf;
+	}
+}
+
+/*
+ * V4L2 ioctl() operations.
+ */
+static int coda_querycap(struct file *file, void *priv,
+			 struct v4l2_capability *cap)
+{
+	struct coda_ctx *ctx = fh_to_ctx(priv);
+
+	strlcpy(cap->driver, CODA_NAME, sizeof(cap->driver));
+	strlcpy(cap->card, coda_product_name(ctx->dev->devtype->product),
+		sizeof(cap->card));
+	strlcpy(cap->bus_info, "platform:" CODA_NAME, sizeof(cap->bus_info));
+	cap->device_caps = V4L2_CAP_VIDEO_M2M | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+
+	return 0;
+}
+
+static int coda_enum_fmt(struct file *file, void *priv,
+			 struct v4l2_fmtdesc *f)
+{
+	struct video_device *vdev = video_devdata(file);
+	const struct coda_video_device *cvd = to_coda_video_device(vdev);
+	const u32 *formats;
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		formats = cvd->src_formats;
+	else if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		formats = cvd->dst_formats;
+	else
+		return -EINVAL;
+
+	if (f->index >= CODA_MAX_FORMATS || formats[f->index] == 0)
+		return -EINVAL;
+
+	f->pixelformat = formats[f->index];
+
+	return 0;
+}
+
+static int coda_g_fmt(struct file *file, void *priv,
+		      struct v4l2_format *f)
+{
+	struct coda_q_data *q_data;
+	struct coda_ctx *ctx = fh_to_ctx(priv);
+
+	q_data = get_q_data(ctx, f->type);
+	if (!q_data)
+		return -EINVAL;
+
+	f->fmt.pix.field	= V4L2_FIELD_NONE;
+	f->fmt.pix.pixelformat	= q_data->fourcc;
+	f->fmt.pix.width	= q_data->width;
+	f->fmt.pix.height	= q_data->height;
+	f->fmt.pix.bytesperline = q_data->bytesperline;
+
+	f->fmt.pix.sizeimage	= q_data->sizeimage;
+	if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_JPEG)
+		f->fmt.pix.colorspace = V4L2_COLORSPACE_JPEG;
+	else
+		f->fmt.pix.colorspace = ctx->colorspace;
+
+	return 0;
+}
+
+static int coda_try_pixelformat(struct coda_ctx *ctx, struct v4l2_format *f)
+{
+	struct coda_q_data *q_data;
+	const u32 *formats;
+	int i;
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		formats = ctx->cvd->src_formats;
+	else if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		formats = ctx->cvd->dst_formats;
+	else
+		return -EINVAL;
+
+	for (i = 0; i < CODA_MAX_FORMATS; i++) {
+		if (formats[i] == f->fmt.pix.pixelformat) {
+			f->fmt.pix.pixelformat = formats[i];
+			return 0;
+		}
+	}
+
+	/* Fall back to currently set pixelformat */
+	q_data = get_q_data(ctx, f->type);
+	f->fmt.pix.pixelformat = q_data->fourcc;
+
+	return 0;
+}
+
+static unsigned int coda_estimate_sizeimage(struct coda_ctx *ctx, u32 sizeimage,
+					    u32 width, u32 height)
+{
+	/*
+	 * This is a rough estimate for sensible compressed buffer
+	 * sizes (between 1 and 16 bits per pixel). This could be
+	 * improved by better format specific worst case estimates.
+	 */
+	return round_up(clamp(sizeimage, width * height / 8,
+					 width * height * 2), PAGE_SIZE);
+}
+
+static int coda_try_fmt(struct coda_ctx *ctx, const struct coda_codec *codec,
+			struct v4l2_format *f)
+{
+	struct coda_dev *dev = ctx->dev;
+	unsigned int max_w, max_h;
+	enum v4l2_field field;
+
+	field = f->fmt.pix.field;
+	if (field == V4L2_FIELD_ANY)
+		field = V4L2_FIELD_NONE;
+	else if (V4L2_FIELD_NONE != field)
+		return -EINVAL;
+
+	/* V4L2 specification suggests the driver corrects the format struct
+	 * if any of the dimensions is unsupported */
+	f->fmt.pix.field = field;
+
+	coda_get_max_dimensions(dev, codec, &max_w, &max_h);
+	v4l_bound_align_image(&f->fmt.pix.width, MIN_W, max_w, W_ALIGN,
+			      &f->fmt.pix.height, MIN_H, max_h, H_ALIGN,
+			      S_ALIGN);
+
+	switch (f->fmt.pix.pixelformat) {
+	case V4L2_PIX_FMT_NV12:
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_YVU420:
+		/*
+		 * Frame stride must be at least multiple of 8,
+		 * but multiple of 16 for h.264 or JPEG 4:2:x
+		 */
+		f->fmt.pix.bytesperline = round_up(f->fmt.pix.width, 16);
+		f->fmt.pix.sizeimage = f->fmt.pix.bytesperline *
+					f->fmt.pix.height * 3 / 2;
+		break;
+	case V4L2_PIX_FMT_YUV422P:
+		f->fmt.pix.bytesperline = round_up(f->fmt.pix.width, 16);
+		f->fmt.pix.sizeimage = f->fmt.pix.bytesperline *
+					f->fmt.pix.height * 2;
+		break;
+	case V4L2_PIX_FMT_JPEG:
+		f->fmt.pix.colorspace = V4L2_COLORSPACE_JPEG;
+		/* fallthrough */
+	case V4L2_PIX_FMT_H264:
+	case V4L2_PIX_FMT_MPEG4:
+		f->fmt.pix.bytesperline = 0;
+		f->fmt.pix.sizeimage = coda_estimate_sizeimage(ctx,
+							f->fmt.pix.sizeimage,
+							f->fmt.pix.width,
+							f->fmt.pix.height);
+		break;
+	default:
+		BUG();
+	}
+
+	return 0;
+}
+
+static int coda_try_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	struct coda_ctx *ctx = fh_to_ctx(priv);
+	const struct coda_q_data *q_data_src;
+	const struct coda_codec *codec;
+	struct vb2_queue *src_vq;
+	int ret;
+
+	ret = coda_try_pixelformat(ctx, f);
+	if (ret < 0)
+		return ret;
+
+	q_data_src = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
+
+	/*
+	 * If the source format is already fixed, only allow the same output
+	 * resolution
+	 */
+	src_vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
+	if (vb2_is_streaming(src_vq)) {
+		f->fmt.pix.width = q_data_src->width;
+		f->fmt.pix.height = q_data_src->height;
+	}
+
+	f->fmt.pix.colorspace = ctx->colorspace;
+
+	q_data_src = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
+	codec = coda_find_codec(ctx->dev, q_data_src->fourcc,
+				f->fmt.pix.pixelformat);
+	if (!codec)
+		return -EINVAL;
+
+	ret = coda_try_fmt(ctx, codec, f);
+	if (ret < 0)
+		return ret;
+
+	/* The h.264 decoder only returns complete 16x16 macroblocks */
+	if (codec && codec->src_fourcc == V4L2_PIX_FMT_H264) {
+		f->fmt.pix.width = f->fmt.pix.width;
+		f->fmt.pix.height = round_up(f->fmt.pix.height, 16);
+		f->fmt.pix.bytesperline = round_up(f->fmt.pix.width, 16);
+		f->fmt.pix.sizeimage = f->fmt.pix.bytesperline *
+				       f->fmt.pix.height * 3 / 2;
+	}
+
+	return 0;
+}
+
+static int coda_try_fmt_vid_out(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	struct coda_ctx *ctx = fh_to_ctx(priv);
+	struct coda_dev *dev = ctx->dev;
+	const struct coda_q_data *q_data_dst;
+	const struct coda_codec *codec;
+	int ret;
+
+	ret = coda_try_pixelformat(ctx, f);
+	if (ret < 0)
+		return ret;
+
+	switch (f->fmt.pix.colorspace) {
+	case V4L2_COLORSPACE_REC709:
+	case V4L2_COLORSPACE_JPEG:
+		break;
+	default:
+		if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_JPEG)
+			f->fmt.pix.colorspace = V4L2_COLORSPACE_JPEG;
+		else
+			f->fmt.pix.colorspace = V4L2_COLORSPACE_REC709;
+	}
+
+	q_data_dst = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
+	codec = coda_find_codec(dev, f->fmt.pix.pixelformat, q_data_dst->fourcc);
+
+	return coda_try_fmt(ctx, codec, f);
+}
+
+static int coda_s_fmt(struct coda_ctx *ctx, struct v4l2_format *f)
+{
+	struct coda_q_data *q_data;
+	struct vb2_queue *vq;
+
+	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
+	if (!vq)
+		return -EINVAL;
+
+	q_data = get_q_data(ctx, f->type);
+	if (!q_data)
+		return -EINVAL;
+
+	if (vb2_is_busy(vq)) {
+		v4l2_err(&ctx->dev->v4l2_dev, "%s queue busy\n", __func__);
+		return -EBUSY;
+	}
+
+	q_data->fourcc = f->fmt.pix.pixelformat;
+	q_data->width = f->fmt.pix.width;
+	q_data->height = f->fmt.pix.height;
+	q_data->bytesperline = f->fmt.pix.bytesperline;
+	q_data->sizeimage = f->fmt.pix.sizeimage;
+	q_data->rect.left = 0;
+	q_data->rect.top = 0;
+	q_data->rect.width = f->fmt.pix.width;
+	q_data->rect.height = f->fmt.pix.height;
+
+	switch (f->fmt.pix.pixelformat) {
+	case V4L2_PIX_FMT_NV12:
+		if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+			ctx->tiled_map_type = GDI_TILED_FRAME_MB_RASTER_MAP;
+			if (!disable_tiling)
+				break;
+		}
+		/* else fall through */
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_YVU420:
+		ctx->tiled_map_type = GDI_LINEAR_FRAME_MAP;
+		break;
+	default:
+		break;
+	}
+
+	v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
+		"Setting format for type %d, wxh: %dx%d, fmt: %d\n",
+		f->type, q_data->width, q_data->height, q_data->fourcc);
+
+	return 0;
+}
+
+static int coda_s_fmt_vid_cap(struct file *file, void *priv,
+			      struct v4l2_format *f)
+{
+	struct coda_ctx *ctx = fh_to_ctx(priv);
+	int ret;
+
+	ret = coda_try_fmt_vid_cap(file, priv, f);
+	if (ret)
+		return ret;
+
+	return coda_s_fmt(ctx, f);
+}
+
+static int coda_s_fmt_vid_out(struct file *file, void *priv,
+			      struct v4l2_format *f)
+{
+	struct coda_ctx *ctx = fh_to_ctx(priv);
+	struct v4l2_format f_cap;
+	int ret;
+
+	ret = coda_try_fmt_vid_out(file, priv, f);
+	if (ret)
+		return ret;
+
+	ret = coda_s_fmt(ctx, f);
+	if (ret)
+		return ret;
+
+	ctx->colorspace = f->fmt.pix.colorspace;
+
+	memset(&f_cap, 0, sizeof(f_cap));
+	f_cap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	coda_g_fmt(file, priv, &f_cap);
+	f_cap.fmt.pix.width = f->fmt.pix.width;
+	f_cap.fmt.pix.height = f->fmt.pix.height;
+
+	ret = coda_try_fmt_vid_cap(file, priv, &f_cap);
+	if (ret)
+		return ret;
+
+	return coda_s_fmt(ctx, &f_cap);
+}
+
+static int coda_reqbufs(struct file *file, void *priv,
+			struct v4l2_requestbuffers *rb)
+{
+	struct coda_ctx *ctx = fh_to_ctx(priv);
+	int ret;
+
+	ret = v4l2_m2m_reqbufs(file, ctx->fh.m2m_ctx, rb);
+	if (ret)
+		return ret;
+
+	/*
+	 * Allow to allocate instance specific per-context buffers, such as
+	 * bitstream ringbuffer, slice buffer, work buffer, etc. if needed.
+	 */
+	if (rb->type == V4L2_BUF_TYPE_VIDEO_OUTPUT && ctx->ops->reqbufs)
+		return ctx->ops->reqbufs(ctx, rb);
+
+	return 0;
+}
+
+static int coda_qbuf(struct file *file, void *priv,
+		     struct v4l2_buffer *buf)
+{
+	struct coda_ctx *ctx = fh_to_ctx(priv);
+
+	return v4l2_m2m_qbuf(file, ctx->fh.m2m_ctx, buf);
+}
+
+static bool coda_buf_is_end_of_stream(struct coda_ctx *ctx,
+				      struct vb2_v4l2_buffer *buf)
+{
+	struct vb2_queue *src_vq;
+
+	src_vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
+
+	return ((ctx->bit_stream_param & CODA_BIT_STREAM_END_FLAG) &&
+		(buf->sequence == (ctx->qsequence - 1)));
+}
+
+void coda_m2m_buf_done(struct coda_ctx *ctx, struct vb2_v4l2_buffer *buf,
+		       enum vb2_buffer_state state)
+{
+	const struct v4l2_event eos_event = {
+		.type = V4L2_EVENT_EOS
+	};
+
+	if (coda_buf_is_end_of_stream(ctx, buf)) {
+		buf->flags |= V4L2_BUF_FLAG_LAST;
+
+		v4l2_event_queue_fh(&ctx->fh, &eos_event);
+	}
+
+	v4l2_m2m_buf_done(buf, state);
+}
+
+static int coda_g_selection(struct file *file, void *fh,
+			    struct v4l2_selection *s)
+{
+	struct coda_ctx *ctx = fh_to_ctx(fh);
+	struct coda_q_data *q_data;
+	struct v4l2_rect r, *rsel;
+
+	q_data = get_q_data(ctx, s->type);
+	if (!q_data)
+		return -EINVAL;
+
+	r.left = 0;
+	r.top = 0;
+	r.width = q_data->width;
+	r.height = q_data->height;
+	rsel = &q_data->rect;
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		rsel = &r;
+		/* fallthrough */
+	case V4L2_SEL_TGT_CROP:
+		if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+			return -EINVAL;
+		break;
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+	case V4L2_SEL_TGT_COMPOSE_PADDED:
+		rsel = &r;
+		/* fallthrough */
+	case V4L2_SEL_TGT_COMPOSE:
+	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+		if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+			return -EINVAL;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	s->r = *rsel;
+
+	return 0;
+}
+
+static int coda_try_decoder_cmd(struct file *file, void *fh,
+				struct v4l2_decoder_cmd *dc)
+{
+	if (dc->cmd != V4L2_DEC_CMD_STOP)
+		return -EINVAL;
+
+	if (dc->flags & V4L2_DEC_CMD_STOP_TO_BLACK)
+		return -EINVAL;
+
+	if (!(dc->flags & V4L2_DEC_CMD_STOP_IMMEDIATELY) && (dc->stop.pts != 0))
+		return -EINVAL;
+
+	return 0;
+}
+
+static int coda_decoder_cmd(struct file *file, void *fh,
+			    struct v4l2_decoder_cmd *dc)
+{
+	struct coda_ctx *ctx = fh_to_ctx(fh);
+	int ret;
+
+	ret = coda_try_decoder_cmd(file, fh, dc);
+	if (ret < 0)
+		return ret;
+
+	/* Ignore decoder stop command silently in encoder context */
+	if (ctx->inst_type != CODA_INST_DECODER)
+		return 0;
+
+	/* Set the stream-end flag on this context */
+	coda_bit_stream_end_flag(ctx);
+	ctx->hold = false;
+	v4l2_m2m_try_schedule(ctx->fh.m2m_ctx);
+
+	return 0;
+}
+
+static int coda_g_parm(struct file *file, void *fh, struct v4l2_streamparm *a)
+{
+	struct coda_ctx *ctx = fh_to_ctx(fh);
+	struct v4l2_fract *tpf;
+
+	if (a->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		return -EINVAL;
+
+	a->parm.output.capability = V4L2_CAP_TIMEPERFRAME;
+	tpf = &a->parm.output.timeperframe;
+	tpf->denominator = ctx->params.framerate & CODA_FRATE_RES_MASK;
+	tpf->numerator = 1 + (ctx->params.framerate >>
+			      CODA_FRATE_DIV_OFFSET);
+
+	return 0;
+}
+
+/*
+ * Approximate timeperframe v4l2_fract with values that can be written
+ * into the 16-bit CODA_FRATE_DIV and CODA_FRATE_RES fields.
+ */
+static void coda_approximate_timeperframe(struct v4l2_fract *timeperframe)
+{
+	struct v4l2_fract s = *timeperframe;
+	struct v4l2_fract f0;
+	struct v4l2_fract f1 = { 1, 0 };
+	struct v4l2_fract f2 = { 0, 1 };
+	unsigned int i, div, s_denominator;
+
+	/* Lower bound is 1/65535 */
+	if (s.numerator == 0 || s.denominator / s.numerator > 65535) {
+		timeperframe->numerator = 1;
+		timeperframe->denominator = 65535;
+		return;
+	}
+
+	/* Upper bound is 65536/1, map everything above to infinity */
+	if (s.denominator == 0 || s.numerator / s.denominator > 65536) {
+		timeperframe->numerator = 1;
+		timeperframe->denominator = 0;
+		return;
+	}
+
+	/* Reduce fraction to lowest terms */
+	div = gcd(s.numerator, s.denominator);
+	if (div > 1) {
+		s.numerator /= div;
+		s.denominator /= div;
+	}
+
+	if (s.numerator <= 65536 && s.denominator < 65536) {
+		*timeperframe = s;
+		return;
+	}
+
+	/* Find successive convergents from continued fraction expansion */
+	while (f2.numerator <= 65536 && f2.denominator < 65536) {
+		f0 = f1;
+		f1 = f2;
+
+		/* Stop when f2 exactly equals timeperframe */
+		if (s.numerator == 0)
+			break;
+
+		i = s.denominator / s.numerator;
+
+		f2.numerator = f0.numerator + i * f1.numerator;
+		f2.denominator = f0.denominator + i * f2.denominator;
+
+		s_denominator = s.numerator;
+		s.numerator = s.denominator % s.numerator;
+		s.denominator = s_denominator;
+	}
+
+	*timeperframe = f1;
+}
+
+static uint32_t coda_timeperframe_to_frate(struct v4l2_fract *timeperframe)
+{
+	return ((timeperframe->numerator - 1) << CODA_FRATE_DIV_OFFSET) |
+		timeperframe->denominator;
+}
+
+static int coda_s_parm(struct file *file, void *fh, struct v4l2_streamparm *a)
+{
+	struct coda_ctx *ctx = fh_to_ctx(fh);
+	struct v4l2_fract *tpf;
+
+	if (a->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		return -EINVAL;
+
+	tpf = &a->parm.output.timeperframe;
+	coda_approximate_timeperframe(tpf);
+	ctx->params.framerate = coda_timeperframe_to_frate(tpf);
+
+	return 0;
+}
+
+static int coda_subscribe_event(struct v4l2_fh *fh,
+				const struct v4l2_event_subscription *sub)
+{
+	switch (sub->type) {
+	case V4L2_EVENT_EOS:
+		return v4l2_event_subscribe(fh, sub, 0, NULL);
+	default:
+		return v4l2_ctrl_subscribe_event(fh, sub);
+	}
+}
+
+static const struct v4l2_ioctl_ops coda_ioctl_ops = {
+	.vidioc_querycap	= coda_querycap,
+
+	.vidioc_enum_fmt_vid_cap = coda_enum_fmt,
+	.vidioc_g_fmt_vid_cap	= coda_g_fmt,
+	.vidioc_try_fmt_vid_cap	= coda_try_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap	= coda_s_fmt_vid_cap,
+
+	.vidioc_enum_fmt_vid_out = coda_enum_fmt,
+	.vidioc_g_fmt_vid_out	= coda_g_fmt,
+	.vidioc_try_fmt_vid_out	= coda_try_fmt_vid_out,
+	.vidioc_s_fmt_vid_out	= coda_s_fmt_vid_out,
+
+	.vidioc_reqbufs		= coda_reqbufs,
+	.vidioc_querybuf	= v4l2_m2m_ioctl_querybuf,
+
+	.vidioc_qbuf		= coda_qbuf,
+	.vidioc_expbuf		= v4l2_m2m_ioctl_expbuf,
+	.vidioc_dqbuf		= v4l2_m2m_ioctl_dqbuf,
+	.vidioc_create_bufs	= v4l2_m2m_ioctl_create_bufs,
+
+	.vidioc_streamon	= v4l2_m2m_ioctl_streamon,
+	.vidioc_streamoff	= v4l2_m2m_ioctl_streamoff,
+
+	.vidioc_g_selection	= coda_g_selection,
+
+	.vidioc_try_decoder_cmd	= coda_try_decoder_cmd,
+	.vidioc_decoder_cmd	= coda_decoder_cmd,
+
+	.vidioc_g_parm		= coda_g_parm,
+	.vidioc_s_parm		= coda_s_parm,
+
+	.vidioc_subscribe_event = coda_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+};
+
+/*
+ * Mem-to-mem operations.
+ */
+
+static void coda_device_run(void *m2m_priv)
+{
+	struct coda_ctx *ctx = m2m_priv;
+	struct coda_dev *dev = ctx->dev;
+
+	queue_work(dev->workqueue, &ctx->pic_run_work);
+}
+
+static void coda_pic_run_work(struct work_struct *work)
+{
+	struct coda_ctx *ctx = container_of(work, struct coda_ctx, pic_run_work);
+	struct coda_dev *dev = ctx->dev;
+	int ret;
+
+	mutex_lock(&ctx->buffer_mutex);
+	mutex_lock(&dev->coda_mutex);
+
+	ret = ctx->ops->prepare_run(ctx);
+	if (ret < 0 && ctx->inst_type == CODA_INST_DECODER) {
+		mutex_unlock(&dev->coda_mutex);
+		mutex_unlock(&ctx->buffer_mutex);
+		/* job_finish scheduled by prepare_decode */
+		return;
+	}
+
+	if (!wait_for_completion_timeout(&ctx->completion,
+					 msecs_to_jiffies(1000))) {
+		dev_err(&dev->plat_dev->dev, "CODA PIC_RUN timeout\n");
+
+		ctx->hold = true;
+
+		coda_hw_reset(ctx);
+	} else if (!ctx->aborting) {
+		ctx->ops->finish_run(ctx);
+	}
+
+	if ((ctx->aborting || (!ctx->streamon_cap && !ctx->streamon_out)) &&
+	    ctx->ops->seq_end_work)
+		queue_work(dev->workqueue, &ctx->seq_end_work);
+
+	mutex_unlock(&dev->coda_mutex);
+	mutex_unlock(&ctx->buffer_mutex);
+
+	v4l2_m2m_job_finish(ctx->dev->m2m_dev, ctx->fh.m2m_ctx);
+}
+
+static int coda_job_ready(void *m2m_priv)
+{
+	struct coda_ctx *ctx = m2m_priv;
+	int src_bufs = v4l2_m2m_num_src_bufs_ready(ctx->fh.m2m_ctx);
+
+	/*
+	 * For both 'P' and 'key' frame cases 1 picture
+	 * and 1 frame are needed. In the decoder case,
+	 * the compressed frame can be in the bitstream.
+	 */
+	if (!src_bufs && ctx->inst_type != CODA_INST_DECODER) {
+		v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
+			 "not ready: not enough video buffers.\n");
+		return 0;
+	}
+
+	if (!v4l2_m2m_num_dst_bufs_ready(ctx->fh.m2m_ctx)) {
+		v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
+			 "not ready: not enough video capture buffers.\n");
+		return 0;
+	}
+
+	if (ctx->inst_type == CODA_INST_DECODER && ctx->use_bit) {
+		bool stream_end = ctx->bit_stream_param &
+				  CODA_BIT_STREAM_END_FLAG;
+		int num_metas = ctx->num_metas;
+
+		if (ctx->hold && !src_bufs) {
+			v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
+				 "%d: not ready: on hold for more buffers.\n",
+				 ctx->idx);
+			return 0;
+		}
+
+		if (!stream_end && (num_metas + src_bufs) < 2) {
+			v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
+				 "%d: not ready: need 2 buffers available (%d, %d)\n",
+				 ctx->idx, num_metas, src_bufs);
+			return 0;
+		}
+
+
+		if (!src_bufs && !stream_end &&
+		    (coda_get_bitstream_payload(ctx) < 512)) {
+			v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
+				 "%d: not ready: not enough bitstream data (%d).\n",
+				 ctx->idx, coda_get_bitstream_payload(ctx));
+			return 0;
+		}
+	}
+
+	if (ctx->aborting) {
+		v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
+			 "not ready: aborting\n");
+		return 0;
+	}
+
+	v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
+			"job ready\n");
+
+	return 1;
+}
+
+static void coda_job_abort(void *priv)
+{
+	struct coda_ctx *ctx = priv;
+
+	ctx->aborting = 1;
+
+	v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
+		 "Aborting task\n");
+}
+
+static void coda_lock(void *m2m_priv)
+{
+	struct coda_ctx *ctx = m2m_priv;
+	struct coda_dev *pcdev = ctx->dev;
+
+	mutex_lock(&pcdev->dev_mutex);
+}
+
+static void coda_unlock(void *m2m_priv)
+{
+	struct coda_ctx *ctx = m2m_priv;
+	struct coda_dev *pcdev = ctx->dev;
+
+	mutex_unlock(&pcdev->dev_mutex);
+}
+
+static const struct v4l2_m2m_ops coda_m2m_ops = {
+	.device_run	= coda_device_run,
+	.job_ready	= coda_job_ready,
+	.job_abort	= coda_job_abort,
+	.lock		= coda_lock,
+	.unlock		= coda_unlock,
+};
+
+static void set_default_params(struct coda_ctx *ctx)
+{
+	unsigned int max_w, max_h, usize, csize;
+
+	ctx->codec = coda_find_codec(ctx->dev, ctx->cvd->src_formats[0],
+				     ctx->cvd->dst_formats[0]);
+	max_w = min(ctx->codec->max_w, 1920U);
+	max_h = min(ctx->codec->max_h, 1088U);
+	usize = max_w * max_h * 3 / 2;
+	csize = coda_estimate_sizeimage(ctx, usize, max_w, max_h);
+
+	ctx->params.codec_mode = ctx->codec->mode;
+	ctx->colorspace = V4L2_COLORSPACE_REC709;
+	ctx->params.framerate = 30;
+
+	/* Default formats for output and input queues */
+	ctx->q_data[V4L2_M2M_SRC].fourcc = ctx->cvd->src_formats[0];
+	ctx->q_data[V4L2_M2M_DST].fourcc = ctx->cvd->dst_formats[0];
+	ctx->q_data[V4L2_M2M_SRC].width = max_w;
+	ctx->q_data[V4L2_M2M_SRC].height = max_h;
+	ctx->q_data[V4L2_M2M_DST].width = max_w;
+	ctx->q_data[V4L2_M2M_DST].height = max_h;
+	if (ctx->codec->src_fourcc == V4L2_PIX_FMT_YUV420) {
+		ctx->q_data[V4L2_M2M_SRC].bytesperline = max_w;
+		ctx->q_data[V4L2_M2M_SRC].sizeimage = usize;
+		ctx->q_data[V4L2_M2M_DST].bytesperline = 0;
+		ctx->q_data[V4L2_M2M_DST].sizeimage = csize;
+	} else {
+		ctx->q_data[V4L2_M2M_SRC].bytesperline = 0;
+		ctx->q_data[V4L2_M2M_SRC].sizeimage = csize;
+		ctx->q_data[V4L2_M2M_DST].bytesperline = max_w;
+		ctx->q_data[V4L2_M2M_DST].sizeimage = usize;
+	}
+	ctx->q_data[V4L2_M2M_SRC].rect.width = max_w;
+	ctx->q_data[V4L2_M2M_SRC].rect.height = max_h;
+	ctx->q_data[V4L2_M2M_DST].rect.width = max_w;
+	ctx->q_data[V4L2_M2M_DST].rect.height = max_h;
+
+	/*
+	 * Since the RBC2AXI logic only supports a single chroma plane,
+	 * macroblock tiling only works for to NV12 pixel format.
+	 */
+	ctx->tiled_map_type = GDI_LINEAR_FRAME_MAP;
+}
+
+/*
+ * Queue operations
+ */
+static int coda_queue_setup(struct vb2_queue *vq, const void *parg,
+				unsigned int *nbuffers, unsigned int *nplanes,
+				unsigned int sizes[], void *alloc_ctxs[])
+{
+	struct coda_ctx *ctx = vb2_get_drv_priv(vq);
+	struct coda_q_data *q_data;
+	unsigned int size;
+
+	q_data = get_q_data(ctx, vq->type);
+	size = q_data->sizeimage;
+
+	*nplanes = 1;
+	sizes[0] = size;
+
+	/* Set to vb2-dma-contig allocator context, ignored by vb2-vmalloc */
+	alloc_ctxs[0] = ctx->dev->alloc_ctx;
+
+	v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
+		 "get %d buffer(s) of size %d each.\n", *nbuffers, size);
+
+	return 0;
+}
+
+static int coda_buf_prepare(struct vb2_buffer *vb)
+{
+	struct coda_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct coda_q_data *q_data;
+
+	q_data = get_q_data(ctx, vb->vb2_queue->type);
+
+	if (vb2_plane_size(vb, 0) < q_data->sizeimage) {
+		v4l2_warn(&ctx->dev->v4l2_dev,
+			  "%s data will not fit into plane (%lu < %lu)\n",
+			  __func__, vb2_plane_size(vb, 0),
+			  (long)q_data->sizeimage);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void coda_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct coda_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct vb2_queue *vq = vb->vb2_queue;
+	struct coda_q_data *q_data;
+
+	q_data = get_q_data(ctx, vb->vb2_queue->type);
+
+	/*
+	 * In the decoder case, immediately try to copy the buffer into the
+	 * bitstream ringbuffer and mark it as ready to be dequeued.
+	 */
+	if (ctx->bitstream.size && vq->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+		/*
+		 * For backwards compatibility, queuing an empty buffer marks
+		 * the stream end
+		 */
+		if (vb2_get_plane_payload(vb, 0) == 0)
+			coda_bit_stream_end_flag(ctx);
+		mutex_lock(&ctx->bitstream_mutex);
+		v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
+		if (vb2_is_streaming(vb->vb2_queue))
+			coda_fill_bitstream(ctx, true);
+		mutex_unlock(&ctx->bitstream_mutex);
+	} else {
+		v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
+	}
+}
+
+int coda_alloc_aux_buf(struct coda_dev *dev, struct coda_aux_buf *buf,
+		       size_t size, const char *name, struct dentry *parent)
+{
+	buf->vaddr = dma_alloc_coherent(&dev->plat_dev->dev, size, &buf->paddr,
+					GFP_KERNEL);
+	if (!buf->vaddr) {
+		v4l2_err(&dev->v4l2_dev,
+			 "Failed to allocate %s buffer of size %u\n",
+			 name, size);
+		return -ENOMEM;
+	}
+
+	buf->size = size;
+
+	if (name && parent) {
+		buf->blob.data = buf->vaddr;
+		buf->blob.size = size;
+		buf->dentry = debugfs_create_blob(name, 0644, parent,
+						  &buf->blob);
+		if (!buf->dentry)
+			dev_warn(&dev->plat_dev->dev,
+				 "failed to create debugfs entry %s\n", name);
+	}
+
+	return 0;
+}
+
+void coda_free_aux_buf(struct coda_dev *dev,
+		       struct coda_aux_buf *buf)
+{
+	if (buf->vaddr) {
+		dma_free_coherent(&dev->plat_dev->dev, buf->size,
+				  buf->vaddr, buf->paddr);
+		buf->vaddr = NULL;
+		buf->size = 0;
+		debugfs_remove(buf->dentry);
+		buf->dentry = NULL;
+	}
+}
+
+static int coda_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct coda_ctx *ctx = vb2_get_drv_priv(q);
+	struct v4l2_device *v4l2_dev = &ctx->dev->v4l2_dev;
+	struct coda_q_data *q_data_src, *q_data_dst;
+	struct vb2_v4l2_buffer *buf;
+	int ret = 0;
+
+	q_data_src = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
+	if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+		if (ctx->inst_type == CODA_INST_DECODER && ctx->use_bit) {
+			/* copy the buffers that were queued before streamon */
+			mutex_lock(&ctx->bitstream_mutex);
+			coda_fill_bitstream(ctx, false);
+			mutex_unlock(&ctx->bitstream_mutex);
+
+			if (coda_get_bitstream_payload(ctx) < 512) {
+				ret = -EINVAL;
+				goto err;
+			}
+		} else {
+			if (count < 1) {
+				ret = -EINVAL;
+				goto err;
+			}
+		}
+
+		ctx->streamon_out = 1;
+	} else {
+		if (count < 1) {
+			ret = -EINVAL;
+			goto err;
+		}
+
+		ctx->streamon_cap = 1;
+	}
+
+	/* Don't start the coda unless both queues are on */
+	if (!(ctx->streamon_out & ctx->streamon_cap))
+		return 0;
+
+	q_data_dst = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
+	if ((q_data_src->width != q_data_dst->width &&
+	     round_up(q_data_src->width, 16) != q_data_dst->width) ||
+	    (q_data_src->height != q_data_dst->height &&
+	     round_up(q_data_src->height, 16) != q_data_dst->height)) {
+		v4l2_err(v4l2_dev, "can't convert %dx%d to %dx%d\n",
+			 q_data_src->width, q_data_src->height,
+			 q_data_dst->width, q_data_dst->height);
+		ret = -EINVAL;
+		goto err;
+	}
+
+	/* Allow BIT decoder device_run with no new buffers queued */
+	if (ctx->inst_type == CODA_INST_DECODER && ctx->use_bit)
+		v4l2_m2m_set_src_buffered(ctx->fh.m2m_ctx, true);
+
+	ctx->gopcounter = ctx->params.gop_size - 1;
+
+	ctx->codec = coda_find_codec(ctx->dev, q_data_src->fourcc,
+				     q_data_dst->fourcc);
+	if (!ctx->codec) {
+		v4l2_err(v4l2_dev, "couldn't tell instance type.\n");
+		ret = -EINVAL;
+		goto err;
+	}
+
+	if (q_data_dst->fourcc == V4L2_PIX_FMT_JPEG)
+		ctx->params.gop_size = 1;
+	ctx->gopcounter = ctx->params.gop_size - 1;
+
+	ret = ctx->ops->start_streaming(ctx);
+	if (ctx->inst_type == CODA_INST_DECODER) {
+		if (ret == -EAGAIN)
+			return 0;
+		else if (ret < 0)
+			goto err;
+	}
+
+	return ret;
+
+err:
+	if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+		while ((buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx)))
+			v4l2_m2m_buf_done(buf, VB2_BUF_STATE_QUEUED);
+	} else {
+		while ((buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx)))
+			v4l2_m2m_buf_done(buf, VB2_BUF_STATE_QUEUED);
+	}
+	return ret;
+}
+
+static void coda_stop_streaming(struct vb2_queue *q)
+{
+	struct coda_ctx *ctx = vb2_get_drv_priv(q);
+	struct coda_dev *dev = ctx->dev;
+	struct vb2_v4l2_buffer *buf;
+	unsigned long flags;
+	bool stop;
+
+	stop = ctx->streamon_out && ctx->streamon_cap;
+
+	if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+		v4l2_dbg(1, coda_debug, &dev->v4l2_dev,
+			 "%s: output\n", __func__);
+		ctx->streamon_out = 0;
+
+		coda_bit_stream_end_flag(ctx);
+
+		ctx->qsequence = 0;
+
+		while ((buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx)))
+			v4l2_m2m_buf_done(buf, VB2_BUF_STATE_ERROR);
+	} else {
+		v4l2_dbg(1, coda_debug, &dev->v4l2_dev,
+			 "%s: capture\n", __func__);
+		ctx->streamon_cap = 0;
+
+		ctx->osequence = 0;
+		ctx->sequence_offset = 0;
+
+		while ((buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx)))
+			v4l2_m2m_buf_done(buf, VB2_BUF_STATE_ERROR);
+	}
+
+	if (stop) {
+		struct coda_buffer_meta *meta;
+
+		if (ctx->ops->seq_end_work) {
+			queue_work(dev->workqueue, &ctx->seq_end_work);
+			flush_work(&ctx->seq_end_work);
+		}
+		spin_lock_irqsave(&ctx->buffer_meta_lock, flags);
+		while (!list_empty(&ctx->buffer_meta_list)) {
+			meta = list_first_entry(&ctx->buffer_meta_list,
+						struct coda_buffer_meta, list);
+			list_del(&meta->list);
+			kfree(meta);
+		}
+		ctx->num_metas = 0;
+		spin_unlock_irqrestore(&ctx->buffer_meta_lock, flags);
+		kfifo_init(&ctx->bitstream_fifo,
+			ctx->bitstream.vaddr, ctx->bitstream.size);
+		ctx->runcounter = 0;
+		ctx->aborting = 0;
+	}
+
+	if (!ctx->streamon_out && !ctx->streamon_cap)
+		ctx->bit_stream_param &= ~CODA_BIT_STREAM_END_FLAG;
+}
+
+static const struct vb2_ops coda_qops = {
+	.queue_setup		= coda_queue_setup,
+	.buf_prepare		= coda_buf_prepare,
+	.buf_queue		= coda_buf_queue,
+	.start_streaming	= coda_start_streaming,
+	.stop_streaming		= coda_stop_streaming,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
+};
+
+static int coda_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct coda_ctx *ctx =
+			container_of(ctrl->handler, struct coda_ctx, ctrls);
+
+	v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
+		 "s_ctrl: id = %d, val = %d\n", ctrl->id, ctrl->val);
+
+	switch (ctrl->id) {
+	case V4L2_CID_HFLIP:
+		if (ctrl->val)
+			ctx->params.rot_mode |= CODA_MIR_HOR;
+		else
+			ctx->params.rot_mode &= ~CODA_MIR_HOR;
+		break;
+	case V4L2_CID_VFLIP:
+		if (ctrl->val)
+			ctx->params.rot_mode |= CODA_MIR_VER;
+		else
+			ctx->params.rot_mode &= ~CODA_MIR_VER;
+		break;
+	case V4L2_CID_MPEG_VIDEO_BITRATE:
+		ctx->params.bitrate = ctrl->val / 1000;
+		break;
+	case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
+		ctx->params.gop_size = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_I_FRAME_QP:
+		ctx->params.h264_intra_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_P_FRAME_QP:
+		ctx->params.h264_inter_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_MIN_QP:
+		ctx->params.h264_min_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_MAX_QP:
+		ctx->params.h264_max_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_ALPHA:
+		ctx->params.h264_deblk_alpha = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_BETA:
+		ctx->params.h264_deblk_beta = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_MODE:
+		ctx->params.h264_deblk_enabled = (ctrl->val ==
+				V4L2_MPEG_VIDEO_H264_LOOP_FILTER_MODE_ENABLED);
+		break;
+	case V4L2_CID_MPEG_VIDEO_MPEG4_I_FRAME_QP:
+		ctx->params.mpeg4_intra_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_MPEG4_P_FRAME_QP:
+		ctx->params.mpeg4_inter_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MODE:
+		ctx->params.slice_mode = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MAX_MB:
+		ctx->params.slice_max_mb = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MAX_BYTES:
+		ctx->params.slice_max_bits = ctrl->val * 8;
+		break;
+	case V4L2_CID_MPEG_VIDEO_HEADER_MODE:
+		break;
+	case V4L2_CID_MPEG_VIDEO_CYCLIC_INTRA_REFRESH_MB:
+		ctx->params.intra_refresh = ctrl->val;
+		break;
+	case V4L2_CID_JPEG_COMPRESSION_QUALITY:
+		coda_set_jpeg_compression_quality(ctx, ctrl->val);
+		break;
+	case V4L2_CID_JPEG_RESTART_INTERVAL:
+		ctx->params.jpeg_restart_interval = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_VBV_DELAY:
+		ctx->params.vbv_delay = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_VBV_SIZE:
+		ctx->params.vbv_size = min(ctrl->val * 8192, 0x7fffffff);
+		break;
+	default:
+		v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
+			"Invalid control, id=%d, val=%d\n",
+			ctrl->id, ctrl->val);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops coda_ctrl_ops = {
+	.s_ctrl = coda_s_ctrl,
+};
+
+static void coda_encode_ctrls(struct coda_ctx *ctx)
+{
+	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_BITRATE, 0, 32767000, 1000, 0);
+	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_GOP_SIZE, 1, 60, 1, 16);
+	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_H264_I_FRAME_QP, 0, 51, 1, 25);
+	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_H264_P_FRAME_QP, 0, 51, 1, 25);
+	if (ctx->dev->devtype->product != CODA_960) {
+		v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
+			V4L2_CID_MPEG_VIDEO_H264_MIN_QP, 0, 51, 1, 12);
+	}
+	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_H264_MAX_QP, 0, 51, 1, 51);
+	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_ALPHA, 0, 15, 1, 0);
+	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_BETA, 0, 15, 1, 0);
+	v4l2_ctrl_new_std_menu(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_MODE,
+		V4L2_MPEG_VIDEO_H264_LOOP_FILTER_MODE_DISABLED, 0x0,
+		V4L2_MPEG_VIDEO_H264_LOOP_FILTER_MODE_ENABLED);
+	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_MPEG4_I_FRAME_QP, 1, 31, 1, 2);
+	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_MPEG4_P_FRAME_QP, 1, 31, 1, 2);
+	v4l2_ctrl_new_std_menu(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MODE,
+		V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_BYTES, 0x0,
+		V4L2_MPEG_VIDEO_MULTI_SLICE_MODE_SINGLE);
+	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MAX_MB, 1, 0x3fffffff, 1, 1);
+	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MAX_BYTES, 1, 0x3fffffff, 1,
+		500);
+	v4l2_ctrl_new_std_menu(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_HEADER_MODE,
+		V4L2_MPEG_VIDEO_HEADER_MODE_JOINED_WITH_1ST_FRAME,
+		(1 << V4L2_MPEG_VIDEO_HEADER_MODE_SEPARATE),
+		V4L2_MPEG_VIDEO_HEADER_MODE_JOINED_WITH_1ST_FRAME);
+	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_CYCLIC_INTRA_REFRESH_MB, 0,
+		1920 * 1088 / 256, 1, 0);
+	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_VBV_DELAY, 0, 0x7fff, 1, 0);
+	/*
+	 * The maximum VBV size value is 0x7fffffff bits,
+	 * one bit less than 262144 KiB
+	 */
+	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_VBV_SIZE, 0, 262144, 1, 0);
+}
+
+static void coda_jpeg_encode_ctrls(struct coda_ctx *ctx)
+{
+	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_JPEG_COMPRESSION_QUALITY, 5, 100, 1, 50);
+	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_JPEG_RESTART_INTERVAL, 0, 100, 1, 0);
+}
+
+static int coda_ctrls_setup(struct coda_ctx *ctx)
+{
+	v4l2_ctrl_handler_init(&ctx->ctrls, 2);
+
+	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_HFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_VFLIP, 0, 1, 1, 0);
+	if (ctx->inst_type == CODA_INST_ENCODER) {
+		if (ctx->cvd->dst_formats[0] == V4L2_PIX_FMT_JPEG)
+			coda_jpeg_encode_ctrls(ctx);
+		else
+			coda_encode_ctrls(ctx);
+	}
+
+	if (ctx->ctrls.error) {
+		v4l2_err(&ctx->dev->v4l2_dev,
+			"control initialization error (%d)",
+			ctx->ctrls.error);
+		return -EINVAL;
+	}
+
+	return v4l2_ctrl_handler_setup(&ctx->ctrls);
+}
+
+static int coda_queue_init(struct coda_ctx *ctx, struct vb2_queue *vq)
+{
+	vq->drv_priv = ctx;
+	vq->ops = &coda_qops;
+	vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	vq->lock = &ctx->dev->dev_mutex;
+	/* One way to indicate end-of-stream for coda is to set the
+	 * bytesused == 0. However by default videobuf2 handles bytesused
+	 * equal to 0 as a special case and changes its value to the size
+	 * of the buffer. Set the allow_zero_bytesused flag, so
+	 * that videobuf2 will keep the value of bytesused intact.
+	 */
+	vq->allow_zero_bytesused = 1;
+
+	return vb2_queue_init(vq);
+}
+
+int coda_encoder_queue_init(void *priv, struct vb2_queue *src_vq,
+			    struct vb2_queue *dst_vq)
+{
+	int ret;
+
+	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	src_vq->io_modes = VB2_DMABUF | VB2_MMAP;
+	src_vq->mem_ops = &vb2_dma_contig_memops;
+
+	ret = coda_queue_init(priv, src_vq);
+	if (ret)
+		return ret;
+
+	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	dst_vq->io_modes = VB2_DMABUF | VB2_MMAP;
+	dst_vq->mem_ops = &vb2_dma_contig_memops;
+
+	return coda_queue_init(priv, dst_vq);
+}
+
+int coda_decoder_queue_init(void *priv, struct vb2_queue *src_vq,
+			    struct vb2_queue *dst_vq)
+{
+	int ret;
+
+	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	src_vq->io_modes = VB2_DMABUF | VB2_MMAP | VB2_USERPTR;
+	src_vq->mem_ops = &vb2_vmalloc_memops;
+
+	ret = coda_queue_init(priv, src_vq);
+	if (ret)
+		return ret;
+
+	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	dst_vq->io_modes = VB2_DMABUF | VB2_MMAP;
+	dst_vq->mem_ops = &vb2_dma_contig_memops;
+
+	return coda_queue_init(priv, dst_vq);
+}
+
+static int coda_next_free_instance(struct coda_dev *dev)
+{
+	int idx = ffz(dev->instance_mask);
+
+	if ((idx < 0) ||
+	    (dev->devtype->product == CODA_DX6 && idx > CODADX6_MAX_INSTANCES))
+		return -EBUSY;
+
+	return idx;
+}
+
+/*
+ * File operations
+ */
+
+static int coda_open(struct file *file)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct coda_dev *dev = video_get_drvdata(vdev);
+	struct coda_ctx *ctx = NULL;
+	char *name;
+	int ret;
+	int idx;
+
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx)
+		return -ENOMEM;
+
+	idx = coda_next_free_instance(dev);
+	if (idx < 0) {
+		ret = idx;
+		goto err_coda_max;
+	}
+	set_bit(idx, &dev->instance_mask);
+
+	name = kasprintf(GFP_KERNEL, "context%d", idx);
+	if (!name) {
+		ret = -ENOMEM;
+		goto err_coda_name_init;
+	}
+
+	ctx->debugfs_entry = debugfs_create_dir(name, dev->debugfs_root);
+	kfree(name);
+
+	ctx->cvd = to_coda_video_device(vdev);
+	ctx->inst_type = ctx->cvd->type;
+	ctx->ops = ctx->cvd->ops;
+	ctx->use_bit = !ctx->cvd->direct;
+	init_completion(&ctx->completion);
+	INIT_WORK(&ctx->pic_run_work, coda_pic_run_work);
+	if (ctx->ops->seq_end_work)
+		INIT_WORK(&ctx->seq_end_work, ctx->ops->seq_end_work);
+	v4l2_fh_init(&ctx->fh, video_devdata(file));
+	file->private_data = &ctx->fh;
+	v4l2_fh_add(&ctx->fh);
+	ctx->dev = dev;
+	ctx->idx = idx;
+	switch (dev->devtype->product) {
+	case CODA_960:
+		ctx->frame_mem_ctrl = 1 << 12;
+		/* fallthrough */
+	case CODA_7541:
+		ctx->reg_idx = 0;
+		break;
+	default:
+		ctx->reg_idx = idx;
+	}
+
+	/* Power up and upload firmware if necessary */
+	ret = pm_runtime_get_sync(&dev->plat_dev->dev);
+	if (ret < 0) {
+		v4l2_err(&dev->v4l2_dev, "failed to power up: %d\n", ret);
+		goto err_pm_get;
+	}
+
+	ret = clk_prepare_enable(dev->clk_per);
+	if (ret)
+		goto err_clk_per;
+
+	ret = clk_prepare_enable(dev->clk_ahb);
+	if (ret)
+		goto err_clk_ahb;
+
+	set_default_params(ctx);
+	ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(dev->m2m_dev, ctx,
+					    ctx->ops->queue_init);
+	if (IS_ERR(ctx->fh.m2m_ctx)) {
+		ret = PTR_ERR(ctx->fh.m2m_ctx);
+
+		v4l2_err(&dev->v4l2_dev, "%s return error (%d)\n",
+			 __func__, ret);
+		goto err_ctx_init;
+	}
+
+	ret = coda_ctrls_setup(ctx);
+	if (ret) {
+		v4l2_err(&dev->v4l2_dev, "failed to setup coda controls\n");
+		goto err_ctrls_setup;
+	}
+
+	ctx->fh.ctrl_handler = &ctx->ctrls;
+
+	mutex_init(&ctx->bitstream_mutex);
+	mutex_init(&ctx->buffer_mutex);
+	INIT_LIST_HEAD(&ctx->buffer_meta_list);
+	spin_lock_init(&ctx->buffer_meta_lock);
+
+	coda_lock(ctx);
+	list_add(&ctx->list, &dev->instances);
+	coda_unlock(ctx);
+
+	v4l2_dbg(1, coda_debug, &dev->v4l2_dev, "Created instance %d (%p)\n",
+		 ctx->idx, ctx);
+
+	return 0;
+
+err_ctrls_setup:
+	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
+err_ctx_init:
+	clk_disable_unprepare(dev->clk_ahb);
+err_clk_ahb:
+	clk_disable_unprepare(dev->clk_per);
+err_clk_per:
+	pm_runtime_put_sync(&dev->plat_dev->dev);
+err_pm_get:
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	clear_bit(ctx->idx, &dev->instance_mask);
+err_coda_name_init:
+err_coda_max:
+	kfree(ctx);
+	return ret;
+}
+
+static int coda_release(struct file *file)
+{
+	struct coda_dev *dev = video_drvdata(file);
+	struct coda_ctx *ctx = fh_to_ctx(file->private_data);
+
+	v4l2_dbg(1, coda_debug, &dev->v4l2_dev, "Releasing instance %p\n",
+		 ctx);
+
+	if (ctx->inst_type == CODA_INST_DECODER && ctx->use_bit)
+		coda_bit_stream_end_flag(ctx);
+
+	/* If this instance is running, call .job_abort and wait for it to end */
+	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
+
+	/* In case the instance was not running, we still need to call SEQ_END */
+	if (ctx->ops->seq_end_work) {
+		queue_work(dev->workqueue, &ctx->seq_end_work);
+		flush_work(&ctx->seq_end_work);
+	}
+
+	coda_lock(ctx);
+	list_del(&ctx->list);
+	coda_unlock(ctx);
+
+	if (ctx->dev->devtype->product == CODA_DX6)
+		coda_free_aux_buf(dev, &ctx->workbuf);
+
+	v4l2_ctrl_handler_free(&ctx->ctrls);
+	clk_disable_unprepare(dev->clk_ahb);
+	clk_disable_unprepare(dev->clk_per);
+	pm_runtime_put_sync(&dev->plat_dev->dev);
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	clear_bit(ctx->idx, &dev->instance_mask);
+	if (ctx->ops->release)
+		ctx->ops->release(ctx);
+	debugfs_remove_recursive(ctx->debugfs_entry);
+	kfree(ctx);
+
+	return 0;
+}
+
+static const struct v4l2_file_operations coda_fops = {
+	.owner		= THIS_MODULE,
+	.open		= coda_open,
+	.release	= coda_release,
+	.poll		= v4l2_m2m_fop_poll,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap		= v4l2_m2m_fop_mmap,
+};
+
+static int coda_hw_init(struct coda_dev *dev)
+{
+	u32 data;
+	u16 *p;
+	int i, ret;
+
+	ret = clk_prepare_enable(dev->clk_per);
+	if (ret)
+		goto err_clk_per;
+
+	ret = clk_prepare_enable(dev->clk_ahb);
+	if (ret)
+		goto err_clk_ahb;
+
+	if (dev->rstc)
+		reset_control_reset(dev->rstc);
+
+	/*
+	 * Copy the first CODA_ISRAM_SIZE in the internal SRAM.
+	 * The 16-bit chars in the code buffer are in memory access
+	 * order, re-sort them to CODA order for register download.
+	 * Data in this SRAM survives a reboot.
+	 */
+	p = (u16 *)dev->codebuf.vaddr;
+	if (dev->devtype->product == CODA_DX6) {
+		for (i = 0; i < (CODA_ISRAM_SIZE / 2); i++)  {
+			data = CODA_DOWN_ADDRESS_SET(i) |
+				CODA_DOWN_DATA_SET(p[i ^ 1]);
+			coda_write(dev, data, CODA_REG_BIT_CODE_DOWN);
+		}
+	} else {
+		for (i = 0; i < (CODA_ISRAM_SIZE / 2); i++) {
+			data = CODA_DOWN_ADDRESS_SET(i) |
+				CODA_DOWN_DATA_SET(p[round_down(i, 4) +
+							3 - (i % 4)]);
+			coda_write(dev, data, CODA_REG_BIT_CODE_DOWN);
+		}
+	}
+
+	/* Clear registers */
+	for (i = 0; i < 64; i++)
+		coda_write(dev, 0, CODA_REG_BIT_CODE_BUF_ADDR + i * 4);
+
+	/* Tell the BIT where to find everything it needs */
+	if (dev->devtype->product == CODA_960 ||
+	    dev->devtype->product == CODA_7541) {
+		coda_write(dev, dev->tempbuf.paddr,
+				CODA_REG_BIT_TEMP_BUF_ADDR);
+		coda_write(dev, 0, CODA_REG_BIT_BIT_STREAM_PARAM);
+	} else {
+		coda_write(dev, dev->workbuf.paddr,
+			      CODA_REG_BIT_WORK_BUF_ADDR);
+	}
+	coda_write(dev, dev->codebuf.paddr,
+		      CODA_REG_BIT_CODE_BUF_ADDR);
+	coda_write(dev, 0, CODA_REG_BIT_CODE_RUN);
+
+	/* Set default values */
+	switch (dev->devtype->product) {
+	case CODA_DX6:
+		coda_write(dev, CODADX6_STREAM_BUF_PIC_FLUSH,
+			   CODA_REG_BIT_STREAM_CTRL);
+		break;
+	default:
+		coda_write(dev, CODA7_STREAM_BUF_PIC_FLUSH,
+			   CODA_REG_BIT_STREAM_CTRL);
+	}
+	if (dev->devtype->product == CODA_960)
+		coda_write(dev, 1 << 12, CODA_REG_BIT_FRAME_MEM_CTRL);
+	else
+		coda_write(dev, 0, CODA_REG_BIT_FRAME_MEM_CTRL);
+
+	if (dev->devtype->product != CODA_DX6)
+		coda_write(dev, 0, CODA7_REG_BIT_AXI_SRAM_USE);
+
+	coda_write(dev, CODA_INT_INTERRUPT_ENABLE,
+		      CODA_REG_BIT_INT_ENABLE);
+
+	/* Reset VPU and start processor */
+	data = coda_read(dev, CODA_REG_BIT_CODE_RESET);
+	data |= CODA_REG_RESET_ENABLE;
+	coda_write(dev, data, CODA_REG_BIT_CODE_RESET);
+	udelay(10);
+	data &= ~CODA_REG_RESET_ENABLE;
+	coda_write(dev, data, CODA_REG_BIT_CODE_RESET);
+	coda_write(dev, CODA_REG_RUN_ENABLE, CODA_REG_BIT_CODE_RUN);
+
+	clk_disable_unprepare(dev->clk_ahb);
+	clk_disable_unprepare(dev->clk_per);
+
+	return 0;
+
+err_clk_ahb:
+	clk_disable_unprepare(dev->clk_per);
+err_clk_per:
+	return ret;
+}
+
+static int coda_register_device(struct coda_dev *dev, int i)
+{
+	struct video_device *vfd = &dev->vfd[i];
+
+	if (i >= dev->devtype->num_vdevs)
+		return -EINVAL;
+
+	strlcpy(vfd->name, dev->devtype->vdevs[i]->name, sizeof(vfd->name));
+	vfd->fops	= &coda_fops;
+	vfd->ioctl_ops	= &coda_ioctl_ops;
+	vfd->release	= video_device_release_empty,
+	vfd->lock	= &dev->dev_mutex;
+	vfd->v4l2_dev	= &dev->v4l2_dev;
+	vfd->vfl_dir	= VFL_DIR_M2M;
+	video_set_drvdata(vfd, dev);
+
+	/* Not applicable, use the selection API instead */
+	v4l2_disable_ioctl(vfd, VIDIOC_CROPCAP);
+	v4l2_disable_ioctl(vfd, VIDIOC_G_CROP);
+	v4l2_disable_ioctl(vfd, VIDIOC_S_CROP);
+
+	return video_register_device(vfd, VFL_TYPE_GRABBER, 0);
+}
+
+static void coda_fw_callback(const struct firmware *fw, void *context)
+{
+	struct coda_dev *dev = context;
+	struct platform_device *pdev = dev->plat_dev;
+	int i, ret;
+
+	if (!fw) {
+		v4l2_err(&dev->v4l2_dev, "firmware request failed\n");
+		goto put_pm;
+	}
+
+	/* allocate auxiliary per-device code buffer for the BIT processor */
+	ret = coda_alloc_aux_buf(dev, &dev->codebuf, fw->size, "codebuf",
+				 dev->debugfs_root);
+	if (ret < 0)
+		goto put_pm;
+
+	/* Copy the whole firmware image to the code buffer */
+	memcpy(dev->codebuf.vaddr, fw->data, fw->size);
+	release_firmware(fw);
+
+	ret = coda_hw_init(dev);
+	if (ret < 0) {
+		v4l2_err(&dev->v4l2_dev, "HW initialization failed\n");
+		goto put_pm;
+	}
+
+	ret = coda_check_firmware(dev);
+	if (ret < 0)
+		goto put_pm;
+
+	dev->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
+	if (IS_ERR(dev->alloc_ctx)) {
+		v4l2_err(&dev->v4l2_dev, "Failed to alloc vb2 context\n");
+		goto put_pm;
+	}
+
+	dev->m2m_dev = v4l2_m2m_init(&coda_m2m_ops);
+	if (IS_ERR(dev->m2m_dev)) {
+		v4l2_err(&dev->v4l2_dev, "Failed to init mem2mem device\n");
+		goto rel_ctx;
+	}
+
+	for (i = 0; i < dev->devtype->num_vdevs; i++) {
+		ret = coda_register_device(dev, i);
+		if (ret) {
+			v4l2_err(&dev->v4l2_dev,
+				 "Failed to register %s video device: %d\n",
+				 dev->devtype->vdevs[i]->name, ret);
+			goto rel_vfd;
+		}
+	}
+
+	v4l2_info(&dev->v4l2_dev, "codec registered as /dev/video[%d-%d]\n",
+		  dev->vfd[0].num, dev->vfd[i - 1].num);
+
+	pm_runtime_put_sync(&pdev->dev);
+	return;
+
+rel_vfd:
+	while (--i >= 0)
+		video_unregister_device(&dev->vfd[i]);
+	v4l2_m2m_release(dev->m2m_dev);
+rel_ctx:
+	vb2_dma_contig_cleanup_ctx(dev->alloc_ctx);
+put_pm:
+	pm_runtime_put_sync(&pdev->dev);
+}
+
+static int coda_firmware_request(struct coda_dev *dev)
+{
+	char *fw = dev->devtype->firmware;
+
+	dev_dbg(&dev->plat_dev->dev, "requesting firmware '%s' for %s\n", fw,
+		coda_product_name(dev->devtype->product));
+
+	return request_firmware_nowait(THIS_MODULE, true,
+		fw, &dev->plat_dev->dev, GFP_KERNEL, dev, coda_fw_callback);
+}
+
+enum coda_platform {
+	CODA_IMX27,
+	CODA_IMX53,
+	CODA_IMX6Q,
+	CODA_IMX6DL,
+};
+
+static const struct coda_devtype coda_devdata[] = {
+	[CODA_IMX27] = {
+		.firmware     = "v4l-codadx6-imx27.bin",
+		.product      = CODA_DX6,
+		.codecs       = codadx6_codecs,
+		.num_codecs   = ARRAY_SIZE(codadx6_codecs),
+		.vdevs        = codadx6_video_devices,
+		.num_vdevs    = ARRAY_SIZE(codadx6_video_devices),
+		.workbuf_size = 288 * 1024 + FMO_SLICE_SAVE_BUF_SIZE * 8 * 1024,
+		.iram_size    = 0xb000,
+	},
+	[CODA_IMX53] = {
+		.firmware     = "v4l-coda7541-imx53.bin",
+		.product      = CODA_7541,
+		.codecs       = coda7_codecs,
+		.num_codecs   = ARRAY_SIZE(coda7_codecs),
+		.vdevs        = coda7_video_devices,
+		.num_vdevs    = ARRAY_SIZE(coda7_video_devices),
+		.workbuf_size = 128 * 1024,
+		.tempbuf_size = 304 * 1024,
+		.iram_size    = 0x14000,
+	},
+	[CODA_IMX6Q] = {
+		.firmware     = "v4l-coda960-imx6q.bin",
+		.product      = CODA_960,
+		.codecs       = coda9_codecs,
+		.num_codecs   = ARRAY_SIZE(coda9_codecs),
+		.vdevs        = coda9_video_devices,
+		.num_vdevs    = ARRAY_SIZE(coda9_video_devices),
+		.workbuf_size = 80 * 1024,
+		.tempbuf_size = 204 * 1024,
+		.iram_size    = 0x21000,
+	},
+	[CODA_IMX6DL] = {
+		.firmware     = "v4l-coda960-imx6dl.bin",
+		.product      = CODA_960,
+		.codecs       = coda9_codecs,
+		.num_codecs   = ARRAY_SIZE(coda9_codecs),
+		.vdevs        = coda9_video_devices,
+		.num_vdevs    = ARRAY_SIZE(coda9_video_devices),
+		.workbuf_size = 80 * 1024,
+		.tempbuf_size = 204 * 1024,
+		.iram_size    = 0x20000,
+	},
+};
+
+static struct platform_device_id coda_platform_ids[] = {
+	{ .name = "coda-imx27", .driver_data = CODA_IMX27 },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(platform, coda_platform_ids);
+
+#ifdef CONFIG_OF
+static const struct of_device_id coda_dt_ids[] = {
+	{ .compatible = "fsl,imx27-vpu", .data = &coda_devdata[CODA_IMX27] },
+	{ .compatible = "fsl,imx53-vpu", .data = &coda_devdata[CODA_IMX53] },
+	{ .compatible = "fsl,imx6q-vpu", .data = &coda_devdata[CODA_IMX6Q] },
+	{ .compatible = "fsl,imx6dl-vpu", .data = &coda_devdata[CODA_IMX6DL] },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, coda_dt_ids);
+#endif
+
+static int coda_probe(struct platform_device *pdev)
+{
+	const struct of_device_id *of_id =
+			of_match_device(of_match_ptr(coda_dt_ids), &pdev->dev);
+	const struct platform_device_id *pdev_id;
+	struct coda_platform_data *pdata = pdev->dev.platform_data;
+	struct device_node *np = pdev->dev.of_node;
+	struct gen_pool *pool;
+	struct coda_dev *dev;
+	struct resource *res;
+	int ret, irq;
+
+	dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
+	if (!dev)
+		return -ENOMEM;
+
+	pdev_id = of_id ? of_id->data : platform_get_device_id(pdev);
+
+	if (of_id)
+		dev->devtype = of_id->data;
+	else if (pdev_id)
+		dev->devtype = &coda_devdata[pdev_id->driver_data];
+	else
+		return -EINVAL;
+
+	spin_lock_init(&dev->irqlock);
+	INIT_LIST_HEAD(&dev->instances);
+
+	dev->plat_dev = pdev;
+	dev->clk_per = devm_clk_get(&pdev->dev, "per");
+	if (IS_ERR(dev->clk_per)) {
+		dev_err(&pdev->dev, "Could not get per clock\n");
+		return PTR_ERR(dev->clk_per);
+	}
+
+	dev->clk_ahb = devm_clk_get(&pdev->dev, "ahb");
+	if (IS_ERR(dev->clk_ahb)) {
+		dev_err(&pdev->dev, "Could not get ahb clock\n");
+		return PTR_ERR(dev->clk_ahb);
+	}
+
+	/* Get  memory for physical registers */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	dev->regs_base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(dev->regs_base))
+		return PTR_ERR(dev->regs_base);
+
+	/* IRQ */
+	irq = platform_get_irq_byname(pdev, "bit");
+	if (irq < 0)
+		irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		dev_err(&pdev->dev, "failed to get irq resource\n");
+		return irq;
+	}
+
+	ret = devm_request_threaded_irq(&pdev->dev, irq, NULL, coda_irq_handler,
+			IRQF_ONESHOT, dev_name(&pdev->dev), dev);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to request irq: %d\n", ret);
+		return ret;
+	}
+
+	dev->rstc = devm_reset_control_get_optional(&pdev->dev, NULL);
+	if (IS_ERR(dev->rstc)) {
+		ret = PTR_ERR(dev->rstc);
+		if (ret == -ENOENT || ret == -ENOSYS) {
+			dev->rstc = NULL;
+		} else {
+			dev_err(&pdev->dev, "failed get reset control: %d\n",
+				ret);
+			return ret;
+		}
+	}
+
+	/* Get IRAM pool from device tree or platform data */
+	pool = of_gen_pool_get(np, "iram", 0);
+	if (!pool && pdata)
+		pool = gen_pool_get(pdata->iram_dev, NULL);
+	if (!pool) {
+		dev_err(&pdev->dev, "iram pool not available\n");
+		return -ENOMEM;
+	}
+	dev->iram_pool = pool;
+
+	ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
+	if (ret)
+		return ret;
+
+	mutex_init(&dev->dev_mutex);
+	mutex_init(&dev->coda_mutex);
+
+	dev->debugfs_root = debugfs_create_dir("coda", NULL);
+	if (!dev->debugfs_root)
+		dev_warn(&pdev->dev, "failed to create debugfs root\n");
+
+	/* allocate auxiliary per-device buffers for the BIT processor */
+	if (dev->devtype->product == CODA_DX6) {
+		ret = coda_alloc_aux_buf(dev, &dev->workbuf,
+					 dev->devtype->workbuf_size, "workbuf",
+					 dev->debugfs_root);
+		if (ret < 0)
+			goto err_v4l2_register;
+	}
+
+	if (dev->devtype->tempbuf_size) {
+		ret = coda_alloc_aux_buf(dev, &dev->tempbuf,
+					 dev->devtype->tempbuf_size, "tempbuf",
+					 dev->debugfs_root);
+		if (ret < 0)
+			goto err_v4l2_register;
+	}
+
+	dev->iram.size = dev->devtype->iram_size;
+	dev->iram.vaddr = gen_pool_dma_alloc(dev->iram_pool, dev->iram.size,
+					     &dev->iram.paddr);
+	if (!dev->iram.vaddr) {
+		dev_warn(&pdev->dev, "unable to alloc iram\n");
+	} else {
+		memset(dev->iram.vaddr, 0, dev->iram.size);
+		dev->iram.blob.data = dev->iram.vaddr;
+		dev->iram.blob.size = dev->iram.size;
+		dev->iram.dentry = debugfs_create_blob("iram", 0644,
+						       dev->debugfs_root,
+						       &dev->iram.blob);
+	}
+
+	dev->workqueue = alloc_workqueue("coda", WQ_UNBOUND | WQ_MEM_RECLAIM, 1);
+	if (!dev->workqueue) {
+		dev_err(&pdev->dev, "unable to alloc workqueue\n");
+		ret = -ENOMEM;
+		goto err_v4l2_register;
+	}
+
+	platform_set_drvdata(pdev, dev);
+
+	/*
+	 * Start activated so we can directly call coda_hw_init in
+	 * coda_fw_callback regardless of whether CONFIG_PM is
+	 * enabled or whether the device is associated with a PM domain.
+	 */
+	pm_runtime_get_noresume(&pdev->dev);
+	pm_runtime_set_active(&pdev->dev);
+	pm_runtime_enable(&pdev->dev);
+
+	return coda_firmware_request(dev);
+
+err_v4l2_register:
+	v4l2_device_unregister(&dev->v4l2_dev);
+	return ret;
+}
+
+static int coda_remove(struct platform_device *pdev)
+{
+	struct coda_dev *dev = platform_get_drvdata(pdev);
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(dev->vfd); i++) {
+		if (video_get_drvdata(&dev->vfd[i]))
+			video_unregister_device(&dev->vfd[i]);
+	}
+	if (dev->m2m_dev)
+		v4l2_m2m_release(dev->m2m_dev);
+	pm_runtime_disable(&pdev->dev);
+	if (dev->alloc_ctx)
+		vb2_dma_contig_cleanup_ctx(dev->alloc_ctx);
+	v4l2_device_unregister(&dev->v4l2_dev);
+	destroy_workqueue(dev->workqueue);
+	if (dev->iram.vaddr)
+		gen_pool_free(dev->iram_pool, (unsigned long)dev->iram.vaddr,
+			      dev->iram.size);
+	coda_free_aux_buf(dev, &dev->codebuf);
+	coda_free_aux_buf(dev, &dev->tempbuf);
+	coda_free_aux_buf(dev, &dev->workbuf);
+	debugfs_remove_recursive(dev->debugfs_root);
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int coda_runtime_resume(struct device *dev)
+{
+	struct coda_dev *cdev = dev_get_drvdata(dev);
+	int ret = 0;
+
+	if (dev->pm_domain && cdev->codebuf.vaddr) {
+		ret = coda_hw_init(cdev);
+		if (ret)
+			v4l2_err(&cdev->v4l2_dev, "HW initialization failed\n");
+	}
+
+	return ret;
+}
+#endif
+
+static const struct dev_pm_ops coda_pm_ops = {
+	SET_RUNTIME_PM_OPS(NULL, coda_runtime_resume, NULL)
+};
+
+static struct platform_driver coda_driver = {
+	.probe	= coda_probe,
+	.remove	= coda_remove,
+	.driver	= {
+		.name	= CODA_NAME,
+		.of_match_table = of_match_ptr(coda_dt_ids),
+		.pm	= &coda_pm_ops,
+	},
+	.id_table = coda_platform_ids,
+};
+
+module_platform_driver(coda_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Javier Martin <javier.martin@vista-silicon.com>");
+MODULE_DESCRIPTION("Coda multi-standard codec V4L2 driver");
diff --git a/drivers/media/platform/coda/coda-gdi.c b/drivers/media/platform/coda/coda-gdi.c
new file mode 100644
index 0000000..aaa7afc
--- /dev/null
+++ b/drivers/media/platform/coda/coda-gdi.c
@@ -0,0 +1,150 @@
+/*
+ * Coda multi-standard codec IP
+ *
+ * Copyright (C) 2014 Philipp Zabel, Pengutronix
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/bitops.h>
+#include "coda.h"
+
+#define XY2_INVERT	BIT(7)
+#define XY2_ZERO	BIT(6)
+#define XY2_TB_XOR	BIT(5)
+#define XY2_XYSEL	BIT(4)
+#define XY2_Y		(1 << 4)
+#define XY2_X		(0 << 4)
+
+#define XY2(luma_sel, luma_bit, chroma_sel, chroma_bit) \
+	(((XY2_##luma_sel) | (luma_bit)) << 8 | \
+	 (XY2_##chroma_sel) | (chroma_bit))
+
+static const u16 xy2ca_zero_map[16] = {
+	XY2(ZERO, 0, ZERO, 0),
+	XY2(ZERO, 0, ZERO, 0),
+	XY2(ZERO, 0, ZERO, 0),
+	XY2(ZERO, 0, ZERO, 0),
+	XY2(ZERO, 0, ZERO, 0),
+	XY2(ZERO, 0, ZERO, 0),
+	XY2(ZERO, 0, ZERO, 0),
+	XY2(ZERO, 0, ZERO, 0),
+	XY2(ZERO, 0, ZERO, 0),
+	XY2(ZERO, 0, ZERO, 0),
+	XY2(ZERO, 0, ZERO, 0),
+	XY2(ZERO, 0, ZERO, 0),
+	XY2(ZERO, 0, ZERO, 0),
+	XY2(ZERO, 0, ZERO, 0),
+	XY2(ZERO, 0, ZERO, 0),
+	XY2(ZERO, 0, ZERO, 0),
+};
+
+static const u16 xy2ca_tiled_map[16] = {
+	XY2(Y,    0, Y,    0),
+	XY2(Y,    1, Y,    1),
+	XY2(Y,    2, Y,    2),
+	XY2(Y,    3, X,    3),
+	XY2(X,    3, ZERO, 0),
+	XY2(ZERO, 0, ZERO, 0),
+	XY2(ZERO, 0, ZERO, 0),
+	XY2(ZERO, 0, ZERO, 0),
+	XY2(ZERO, 0, ZERO, 0),
+	XY2(ZERO, 0, ZERO, 0),
+	XY2(ZERO, 0, ZERO, 0),
+	XY2(ZERO, 0, ZERO, 0),
+	XY2(ZERO, 0, ZERO, 0),
+	XY2(ZERO, 0, ZERO, 0),
+	XY2(ZERO, 0, ZERO, 0),
+	XY2(ZERO, 0, ZERO, 0),
+};
+
+/*
+ * RA[15:0], CA[15:8] are hardwired to contain the 24-bit macroblock
+ * start offset (macroblock size is 16x16 for luma, 16x8 for chroma).
+ * Bits CA[4:0] are set using XY2CA above. BA[3:0] seems to be unused.
+ */
+
+#define RBC_CA		(0 << 4)
+#define RBC_BA		(1 << 4)
+#define RBC_RA		(2 << 4)
+#define RBC_ZERO	(3 << 4)
+
+#define RBC(luma_sel, luma_bit, chroma_sel, chroma_bit) \
+	(((RBC_##luma_sel) | (luma_bit)) << 6 | \
+	 (RBC_##chroma_sel) | (chroma_bit))
+
+static const u16 rbc2axi_tiled_map[32] = {
+	RBC(ZERO, 0, ZERO, 0),
+	RBC(ZERO, 0, ZERO, 0),
+	RBC(ZERO, 0, ZERO, 0),
+	RBC(CA,   0, CA,   0),
+	RBC(CA,   1, CA,   1),
+	RBC(CA,   2, CA,   2),
+	RBC(CA,   3, CA,   3),
+	RBC(CA,   4, CA,   8),
+	RBC(CA,   8, CA,   9),
+	RBC(CA,   9, CA,  10),
+	RBC(CA,  10, CA,  11),
+	RBC(CA,  11, CA,  12),
+	RBC(CA,  12, CA,  13),
+	RBC(CA,  13, CA,  14),
+	RBC(CA,  14, CA,  15),
+	RBC(CA,  15, RA,   0),
+	RBC(RA,   0, RA,   1),
+	RBC(RA,   1, RA,   2),
+	RBC(RA,   2, RA,   3),
+	RBC(RA,   3, RA,   4),
+	RBC(RA,   4, RA,   5),
+	RBC(RA,   5, RA,   6),
+	RBC(RA,   6, RA,   7),
+	RBC(RA,   7, RA,   8),
+	RBC(RA,   8, RA,   9),
+	RBC(RA,   9, RA,  10),
+	RBC(RA,  10, RA,  11),
+	RBC(RA,  11, RA,  12),
+	RBC(RA,  12, RA,  13),
+	RBC(RA,  13, RA,  14),
+	RBC(RA,  14, RA,  15),
+	RBC(RA,  15, ZERO, 0),
+};
+
+void coda_set_gdi_regs(struct coda_ctx *ctx)
+{
+	struct coda_dev *dev = ctx->dev;
+	const u16 *xy2ca_map;
+	u32 xy2rbc_config;
+	int i;
+
+	switch (ctx->tiled_map_type) {
+	case GDI_LINEAR_FRAME_MAP:
+	default:
+		xy2ca_map = xy2ca_zero_map;
+		xy2rbc_config = 0;
+		break;
+	case GDI_TILED_FRAME_MB_RASTER_MAP:
+		xy2ca_map = xy2ca_tiled_map;
+		xy2rbc_config = CODA9_XY2RBC_TILED_MAP |
+				CODA9_XY2RBC_CA_INC_HOR |
+				(16 - 1) << 12 | (8 - 1) << 4;
+		break;
+	}
+
+	for (i = 0; i < 16; i++)
+		coda_write(dev, xy2ca_map[i],
+				CODA9_GDI_XY2_CAS_0 + 4 * i);
+	for (i = 0; i < 4; i++)
+		coda_write(dev, XY2(ZERO, 0, ZERO, 0),
+				CODA9_GDI_XY2_BA_0 + 4 * i);
+	for (i = 0; i < 16; i++)
+		coda_write(dev, XY2(ZERO, 0, ZERO, 0),
+				CODA9_GDI_XY2_RAS_0 + 4 * i);
+	coda_write(dev, xy2rbc_config, CODA9_GDI_XY2_RBC_CONFIG);
+	if (xy2rbc_config) {
+		for (i = 0; i < 32; i++)
+			coda_write(dev, rbc2axi_tiled_map[i],
+					CODA9_GDI_RBC2_AXI_0 + 4 * i);
+	}
+}
diff --git a/drivers/media/platform/coda/coda-h264.c b/drivers/media/platform/coda/coda-h264.c
new file mode 100644
index 0000000..456773a
--- /dev/null
+++ b/drivers/media/platform/coda/coda-h264.c
@@ -0,0 +1,37 @@
+/*
+ * Coda multi-standard codec IP - H.264 helper functions
+ *
+ * Copyright (C) 2012 Vista Silicon S.L.
+ *    Javier Martin, <javier.martin@vista-silicon.com>
+ *    Xavier Duret
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/kernel.h>
+#include <linux/string.h>
+
+static const u8 coda_filler_nal[14] = { 0x00, 0x00, 0x00, 0x01, 0x0c, 0xff,
+			0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x80 };
+static const u8 coda_filler_size[8] = { 0, 7, 14, 13, 12, 11, 10, 9 };
+
+int coda_h264_padding(int size, char *p)
+{
+	int nal_size;
+	int diff;
+
+	diff = size - (size & ~0x7);
+	if (diff == 0)
+		return 0;
+
+	nal_size = coda_filler_size[diff];
+	memcpy(p, coda_filler_nal, nal_size);
+
+	/* Add rbsp stop bit and trailing at the end */
+	*(p + nal_size - 1) = 0x80;
+
+	return nal_size;
+}
diff --git a/drivers/media/platform/coda/coda-jpeg.c b/drivers/media/platform/coda/coda-jpeg.c
new file mode 100644
index 0000000..96cd42a
--- /dev/null
+++ b/drivers/media/platform/coda/coda-jpeg.c
@@ -0,0 +1,239 @@
+/*
+ * Coda multi-standard codec IP - JPEG support functions
+ *
+ * Copyright (C) 2014 Philipp Zabel, Pengutronix
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/kernel.h>
+#include <linux/swab.h>
+
+#include "coda.h"
+#include "trace.h"
+
+#define SOI_MARKER	0xffd8
+#define EOI_MARKER	0xffd9
+
+/*
+ * Typical Huffman tables for 8-bit precision luminance and
+ * chrominance from JPEG ITU-T.81 (ISO/IEC 10918-1) Annex K.3
+ */
+
+static const unsigned char luma_dc_bits[16] = {
+	0x00, 0x01, 0x05, 0x01, 0x01, 0x01, 0x01, 0x01,
+	0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+};
+
+static const unsigned char luma_dc_value[12] = {
+	0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
+	0x08, 0x09, 0x0a, 0x0b,
+};
+
+static const unsigned char chroma_dc_bits[16] = {
+	0x00, 0x03, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01,
+	0x01, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00,
+};
+
+static const unsigned char chroma_dc_value[12] = {
+	0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
+	0x08, 0x09, 0x0a, 0x0b,
+};
+
+static const unsigned char luma_ac_bits[16] = {
+	0x00, 0x02, 0x01, 0x03, 0x03, 0x02, 0x04, 0x03,
+	0x05, 0x05, 0x04, 0x04, 0x00, 0x00, 0x01, 0x7d,
+};
+
+static const unsigned char luma_ac_value[162 + 2] = {
+	0x01, 0x02, 0x03, 0x00, 0x04, 0x11, 0x05, 0x12,
+	0x21, 0x31, 0x41, 0x06, 0x13, 0x51, 0x61, 0x07,
+	0x22, 0x71, 0x14, 0x32, 0x81, 0x91, 0xa1, 0x08,
+	0x23, 0x42, 0xb1, 0xc1, 0x15, 0x52, 0xd1, 0xf0,
+	0x24, 0x33, 0x62, 0x72, 0x82, 0x09, 0x0a, 0x16,
+	0x17, 0x18, 0x19, 0x1a, 0x25, 0x26, 0x27, 0x28,
+	0x29, 0x2a, 0x34, 0x35, 0x36, 0x37, 0x38, 0x39,
+	0x3a, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48, 0x49,
+	0x4a, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58, 0x59,
+	0x5a, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68, 0x69,
+	0x6a, 0x73, 0x74, 0x75, 0x76, 0x77, 0x78, 0x79,
+	0x7a, 0x83, 0x84, 0x85, 0x86, 0x87, 0x88, 0x89,
+	0x8a, 0x92, 0x93, 0x94, 0x95, 0x96, 0x97, 0x98,
+	0x99, 0x9a, 0xa2, 0xa3, 0xa4, 0xa5, 0xa6, 0xa7,
+	0xa8, 0xa9, 0xaa, 0xb2, 0xb3, 0xb4, 0xb5, 0xb6,
+	0xb7, 0xb8, 0xb9, 0xba, 0xc2, 0xc3, 0xc4, 0xc5,
+	0xc6, 0xc7, 0xc8, 0xc9, 0xca, 0xd2, 0xd3, 0xd4,
+	0xd5, 0xd6, 0xd7, 0xd8, 0xd9, 0xda, 0xe1, 0xe2,
+	0xe3, 0xe4, 0xe5, 0xe6, 0xe7, 0xe8, 0xe9, 0xea,
+	0xf1, 0xf2, 0xf3, 0xf4, 0xf5, 0xf6, 0xf7, 0xf8,
+	0xf9, 0xfa, /* padded to 32-bit */
+};
+
+static const unsigned char chroma_ac_bits[16] = {
+	0x00, 0x02, 0x01, 0x02, 0x04, 0x04, 0x03, 0x04,
+	0x07, 0x05, 0x04, 0x04, 0x00, 0x01, 0x02, 0x77,
+};
+
+static const unsigned char chroma_ac_value[162 + 2] = {
+	0x00, 0x01, 0x02, 0x03, 0x11, 0x04, 0x05, 0x21,
+	0x31, 0x06, 0x12, 0x41, 0x51, 0x07, 0x61, 0x71,
+	0x13, 0x22, 0x32, 0x81, 0x08, 0x14, 0x42, 0x91,
+	0xa1, 0xb1, 0xc1, 0x09, 0x23, 0x33, 0x52, 0xf0,
+	0x15, 0x62, 0x72, 0xd1, 0x0a, 0x16, 0x24, 0x34,
+	0xe1, 0x25, 0xf1, 0x17, 0x18, 0x19, 0x1a, 0x26,
+	0x27, 0x28, 0x29, 0x2a, 0x35, 0x36, 0x37, 0x38,
+	0x39, 0x3a, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48,
+	0x49, 0x4a, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58,
+	0x59, 0x5a, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68,
+	0x69, 0x6a, 0x73, 0x74, 0x75, 0x76, 0x77, 0x78,
+	0x79, 0x7a, 0x82, 0x83, 0x84, 0x85, 0x86, 0x87,
+	0x88, 0x89, 0x8a, 0x92, 0x93, 0x94, 0x95, 0x96,
+	0x97, 0x98, 0x99, 0x9a, 0xa2, 0xa3, 0xa4, 0xa5,
+	0xa6, 0xa7, 0xa8, 0xa9, 0xaa, 0xb2, 0xb3, 0xb4,
+	0xb5, 0xb6, 0xb7, 0xb8, 0xb9, 0xba, 0xc2, 0xc3,
+	0xc4, 0xc5, 0xc6, 0xc7, 0xc8, 0xc9, 0xca, 0xd2,
+	0xd3, 0xd4, 0xd5, 0xd6, 0xd7, 0xd8, 0xd9, 0xda,
+	0xe2, 0xe3, 0xe4, 0xe5, 0xe6, 0xe7, 0xe8, 0xe9,
+	0xea, 0xf2, 0xf3, 0xf4, 0xf5, 0xf6, 0xf7, 0xf8,
+	0xf9, 0xfa, /* padded to 32-bit */
+};
+
+/*
+ * Quantization tables for luminance and chrominance components in
+ * zig-zag scan order from the Freescale i.MX VPU libaries
+ */
+
+static unsigned char luma_q[64] = {
+	0x06, 0x04, 0x04, 0x04, 0x05, 0x04, 0x06, 0x05,
+	0x05, 0x06, 0x09, 0x06, 0x05, 0x06, 0x09, 0x0b,
+	0x08, 0x06, 0x06, 0x08, 0x0b, 0x0c, 0x0a, 0x0a,
+	0x0b, 0x0a, 0x0a, 0x0c, 0x10, 0x0c, 0x0c, 0x0c,
+	0x0c, 0x0c, 0x0c, 0x10, 0x0c, 0x0c, 0x0c, 0x0c,
+	0x0c, 0x0c, 0x0c, 0x0c, 0x0c, 0x0c, 0x0c, 0x0c,
+	0x0c, 0x0c, 0x0c, 0x0c, 0x0c, 0x0c, 0x0c, 0x0c,
+	0x0c, 0x0c, 0x0c, 0x0c, 0x0c, 0x0c, 0x0c, 0x0c,
+};
+
+static unsigned char chroma_q[64] = {
+	0x07, 0x07, 0x07, 0x0d, 0x0c, 0x0d, 0x18, 0x10,
+	0x10, 0x18, 0x14, 0x0e, 0x0e, 0x0e, 0x14, 0x14,
+	0x0e, 0x0e, 0x0e, 0x0e, 0x14, 0x11, 0x0c, 0x0c,
+	0x0c, 0x0c, 0x0c, 0x11, 0x11, 0x0c, 0x0c, 0x0c,
+	0x0c, 0x0c, 0x0c, 0x11, 0x0c, 0x0c, 0x0c, 0x0c,
+	0x0c, 0x0c, 0x0c, 0x0c, 0x0c, 0x0c, 0x0c, 0x0c,
+	0x0c, 0x0c, 0x0c, 0x0c, 0x0c, 0x0c, 0x0c, 0x0c,
+	0x0c, 0x0c, 0x0c, 0x0c, 0x0c, 0x0c, 0x0c, 0x0c,
+};
+
+struct coda_memcpy_desc {
+	int offset;
+	const void *src;
+	size_t len;
+};
+
+static void coda_memcpy_parabuf(void *parabuf,
+				const struct coda_memcpy_desc *desc)
+{
+	u32 *dst = parabuf + desc->offset;
+	const u32 *src = desc->src;
+	int len = desc->len / 4;
+	int i;
+
+	for (i = 0; i < len; i += 2) {
+		dst[i + 1] = swab32(src[i]);
+		dst[i] = swab32(src[i + 1]);
+	}
+}
+
+int coda_jpeg_write_tables(struct coda_ctx *ctx)
+{
+	int i;
+	static const struct coda_memcpy_desc huff[8] = {
+		{ 0,   luma_dc_bits,    sizeof(luma_dc_bits)    },
+		{ 16,  luma_dc_value,   sizeof(luma_dc_value)   },
+		{ 32,  luma_ac_bits,    sizeof(luma_ac_bits)    },
+		{ 48,  luma_ac_value,   sizeof(luma_ac_value)   },
+		{ 216, chroma_dc_bits,  sizeof(chroma_dc_bits)  },
+		{ 232, chroma_dc_value, sizeof(chroma_dc_value) },
+		{ 248, chroma_ac_bits,  sizeof(chroma_ac_bits)  },
+		{ 264, chroma_ac_value, sizeof(chroma_ac_value) },
+	};
+	struct coda_memcpy_desc qmat[3] = {
+		{ 512, ctx->params.jpeg_qmat_tab[0], 64 },
+		{ 576, ctx->params.jpeg_qmat_tab[1], 64 },
+		{ 640, ctx->params.jpeg_qmat_tab[1], 64 },
+	};
+
+	/* Write huffman tables to parameter memory */
+	for (i = 0; i < ARRAY_SIZE(huff); i++)
+		coda_memcpy_parabuf(ctx->parabuf.vaddr, huff + i);
+
+	/* Write Q-matrix to parameter memory */
+	for (i = 0; i < ARRAY_SIZE(qmat); i++)
+		coda_memcpy_parabuf(ctx->parabuf.vaddr, qmat + i);
+
+	return 0;
+}
+
+bool coda_jpeg_check_buffer(struct coda_ctx *ctx, struct vb2_v4l2_buffer *vb)
+{
+	void *vaddr = vb2_plane_vaddr(&vb->vb2_buf, 0);
+	u16 soi = be16_to_cpup((__be16 *)vaddr);
+	u16 eoi = be16_to_cpup((__be16 *)(vaddr +
+			  vb2_get_plane_payload(&vb->vb2_buf, 0) - 2));
+
+	return soi == SOI_MARKER && eoi == EOI_MARKER;
+}
+
+/*
+ * Scale quantization table using nonlinear scaling factor
+ * u8 qtab[64], scale [50,190]
+ */
+static void coda_scale_quant_table(u8 *q_tab, int scale)
+{
+	unsigned int temp;
+	int i;
+
+	for (i = 0; i < 64; i++) {
+		temp = DIV_ROUND_CLOSEST((unsigned int)q_tab[i] * scale, 100);
+		if (temp <= 0)
+			temp = 1;
+		if (temp > 255)
+			temp = 255;
+		q_tab[i] = (unsigned char)temp;
+	}
+}
+
+void coda_set_jpeg_compression_quality(struct coda_ctx *ctx, int quality)
+{
+	unsigned int scale;
+
+	ctx->params.jpeg_quality = quality;
+
+	/* Clip quality setting to [5,100] interval */
+	if (quality > 100)
+		quality = 100;
+	if (quality < 5)
+		quality = 5;
+
+	/*
+	 * Non-linear scaling factor:
+	 * [5,50] -> [1000..100], [51,100] -> [98..0]
+	 */
+	if (quality < 50)
+		scale = 5000 / quality;
+	else
+		scale = 200 - 2 * quality;
+
+	if (ctx->params.jpeg_qmat_tab[0]) {
+		memcpy(ctx->params.jpeg_qmat_tab[0], luma_q, 64);
+		coda_scale_quant_table(ctx->params.jpeg_qmat_tab[0], scale);
+	}
+	if (ctx->params.jpeg_qmat_tab[1]) {
+		memcpy(ctx->params.jpeg_qmat_tab[1], chroma_q, 64);
+		coda_scale_quant_table(ctx->params.jpeg_qmat_tab[1], scale);
+	}
+}
diff --git a/drivers/media/platform/coda/coda.h b/drivers/media/platform/coda/coda.h
new file mode 100644
index 0000000..96532b0
--- /dev/null
+++ b/drivers/media/platform/coda/coda.h
@@ -0,0 +1,301 @@
+/*
+ * Coda multi-standard codec IP
+ *
+ * Copyright (C) 2012 Vista Silicon S.L.
+ *    Javier Martin, <javier.martin@vista-silicon.com>
+ *    Xavier Duret
+ * Copyright (C) 2012-2014 Philipp Zabel, Pengutronix
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#ifndef __CODA_H__
+#define __CODA_H__
+
+#include <linux/debugfs.h>
+#include <linux/irqreturn.h>
+#include <linux/mutex.h>
+#include <linux/kfifo.h>
+#include <linux/videodev2.h>
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fh.h>
+#include <media/videobuf2-v4l2.h>
+
+#include "coda_regs.h"
+
+#define CODA_MAX_FRAMEBUFFERS	8
+#define FMO_SLICE_SAVE_BUF_SIZE	(32)
+
+enum {
+	V4L2_M2M_SRC = 0,
+	V4L2_M2M_DST = 1,
+};
+
+enum coda_inst_type {
+	CODA_INST_ENCODER,
+	CODA_INST_DECODER,
+};
+
+enum coda_product {
+	CODA_DX6 = 0xf001,
+	CODA_7541 = 0xf012,
+	CODA_960 = 0xf020,
+};
+
+struct coda_video_device;
+
+struct coda_devtype {
+	char			*firmware;
+	enum coda_product	product;
+	const struct coda_codec	*codecs;
+	unsigned int		num_codecs;
+	const struct coda_video_device **vdevs;
+	unsigned int		num_vdevs;
+	size_t			workbuf_size;
+	size_t			tempbuf_size;
+	size_t			iram_size;
+};
+
+struct coda_aux_buf {
+	void			*vaddr;
+	dma_addr_t		paddr;
+	u32			size;
+	struct debugfs_blob_wrapper blob;
+	struct dentry		*dentry;
+};
+
+struct coda_dev {
+	struct v4l2_device	v4l2_dev;
+	struct video_device	vfd[5];
+	struct platform_device	*plat_dev;
+	const struct coda_devtype *devtype;
+
+	void __iomem		*regs_base;
+	struct clk		*clk_per;
+	struct clk		*clk_ahb;
+	struct reset_control	*rstc;
+
+	struct coda_aux_buf	codebuf;
+	struct coda_aux_buf	tempbuf;
+	struct coda_aux_buf	workbuf;
+	struct gen_pool		*iram_pool;
+	struct coda_aux_buf	iram;
+
+	spinlock_t		irqlock;
+	struct mutex		dev_mutex;
+	struct mutex		coda_mutex;
+	struct workqueue_struct	*workqueue;
+	struct v4l2_m2m_dev	*m2m_dev;
+	struct vb2_alloc_ctx	*alloc_ctx;
+	struct list_head	instances;
+	unsigned long		instance_mask;
+	struct dentry		*debugfs_root;
+};
+
+struct coda_codec {
+	u32 mode;
+	u32 src_fourcc;
+	u32 dst_fourcc;
+	u32 max_w;
+	u32 max_h;
+};
+
+struct coda_huff_tab;
+
+struct coda_params {
+	u8			rot_mode;
+	u8			h264_intra_qp;
+	u8			h264_inter_qp;
+	u8			h264_min_qp;
+	u8			h264_max_qp;
+	u8			h264_deblk_enabled;
+	u8			h264_deblk_alpha;
+	u8			h264_deblk_beta;
+	u8			mpeg4_intra_qp;
+	u8			mpeg4_inter_qp;
+	u8			gop_size;
+	int			intra_refresh;
+	u8			jpeg_quality;
+	u8			jpeg_restart_interval;
+	u8			*jpeg_qmat_tab[3];
+	int			codec_mode;
+	int			codec_mode_aux;
+	enum v4l2_mpeg_video_multi_slice_mode slice_mode;
+	u32			framerate;
+	u16			bitrate;
+	u16			vbv_delay;
+	u32			vbv_size;
+	u32			slice_max_bits;
+	u32			slice_max_mb;
+};
+
+struct coda_buffer_meta {
+	struct list_head	list;
+	u32			sequence;
+	struct v4l2_timecode	timecode;
+	struct timeval		timestamp;
+	u32			start;
+	u32			end;
+};
+
+/* Per-queue, driver-specific private data */
+struct coda_q_data {
+	unsigned int		width;
+	unsigned int		height;
+	unsigned int		bytesperline;
+	unsigned int		sizeimage;
+	unsigned int		fourcc;
+	struct v4l2_rect	rect;
+};
+
+struct coda_iram_info {
+	u32		axi_sram_use;
+	phys_addr_t	buf_bit_use;
+	phys_addr_t	buf_ip_ac_dc_use;
+	phys_addr_t	buf_dbk_y_use;
+	phys_addr_t	buf_dbk_c_use;
+	phys_addr_t	buf_ovl_use;
+	phys_addr_t	buf_btp_use;
+	phys_addr_t	search_ram_paddr;
+	int		search_ram_size;
+	int		remaining;
+	phys_addr_t	next_paddr;
+};
+
+#define GDI_LINEAR_FRAME_MAP 0
+#define GDI_TILED_FRAME_MB_RASTER_MAP 1
+
+struct coda_ctx;
+
+struct coda_context_ops {
+	int (*queue_init)(void *priv, struct vb2_queue *src_vq,
+			  struct vb2_queue *dst_vq);
+	int (*reqbufs)(struct coda_ctx *ctx, struct v4l2_requestbuffers *rb);
+	int (*start_streaming)(struct coda_ctx *ctx);
+	int (*prepare_run)(struct coda_ctx *ctx);
+	void (*finish_run)(struct coda_ctx *ctx);
+	void (*seq_end_work)(struct work_struct *work);
+	void (*release)(struct coda_ctx *ctx);
+};
+
+struct coda_ctx {
+	struct coda_dev			*dev;
+	struct mutex			buffer_mutex;
+	struct list_head		list;
+	struct work_struct		pic_run_work;
+	struct work_struct		seq_end_work;
+	struct completion		completion;
+	const struct coda_video_device	*cvd;
+	const struct coda_context_ops	*ops;
+	int				aborting;
+	int				initialized;
+	int				streamon_out;
+	int				streamon_cap;
+	u32				qsequence;
+	u32				osequence;
+	u32				sequence_offset;
+	struct coda_q_data		q_data[2];
+	enum coda_inst_type		inst_type;
+	const struct coda_codec		*codec;
+	enum v4l2_colorspace		colorspace;
+	struct coda_params		params;
+	struct v4l2_ctrl_handler	ctrls;
+	struct v4l2_fh			fh;
+	int				gopcounter;
+	int				runcounter;
+	char				vpu_header[3][64];
+	int				vpu_header_size[3];
+	struct kfifo			bitstream_fifo;
+	struct mutex			bitstream_mutex;
+	struct coda_aux_buf		bitstream;
+	bool				hold;
+	struct coda_aux_buf		parabuf;
+	struct coda_aux_buf		psbuf;
+	struct coda_aux_buf		slicebuf;
+	struct coda_aux_buf		internal_frames[CODA_MAX_FRAMEBUFFERS];
+	u32				frame_types[CODA_MAX_FRAMEBUFFERS];
+	struct coda_buffer_meta		frame_metas[CODA_MAX_FRAMEBUFFERS];
+	u32				frame_errors[CODA_MAX_FRAMEBUFFERS];
+	struct list_head		buffer_meta_list;
+	spinlock_t			buffer_meta_lock;
+	int				num_metas;
+	struct coda_aux_buf		workbuf;
+	int				num_internal_frames;
+	int				idx;
+	int				reg_idx;
+	struct coda_iram_info		iram_info;
+	int				tiled_map_type;
+	u32				bit_stream_param;
+	u32				frm_dis_flg;
+	u32				frame_mem_ctrl;
+	int				display_idx;
+	struct dentry			*debugfs_entry;
+	bool				use_bit;
+};
+
+extern int coda_debug;
+
+void coda_write(struct coda_dev *dev, u32 data, u32 reg);
+unsigned int coda_read(struct coda_dev *dev, u32 reg);
+void coda_write_base(struct coda_ctx *ctx, struct coda_q_data *q_data,
+		     struct vb2_v4l2_buffer *buf, unsigned int reg_y);
+
+int coda_alloc_aux_buf(struct coda_dev *dev, struct coda_aux_buf *buf,
+		       size_t size, const char *name, struct dentry *parent);
+void coda_free_aux_buf(struct coda_dev *dev, struct coda_aux_buf *buf);
+
+int coda_encoder_queue_init(void *priv, struct vb2_queue *src_vq,
+			    struct vb2_queue *dst_vq);
+int coda_decoder_queue_init(void *priv, struct vb2_queue *src_vq,
+			    struct vb2_queue *dst_vq);
+
+int coda_hw_reset(struct coda_ctx *ctx);
+
+void coda_fill_bitstream(struct coda_ctx *ctx, bool streaming);
+
+void coda_set_gdi_regs(struct coda_ctx *ctx);
+
+static inline struct coda_q_data *get_q_data(struct coda_ctx *ctx,
+					     enum v4l2_buf_type type)
+{
+	switch (type) {
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+		return &(ctx->q_data[V4L2_M2M_SRC]);
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		return &(ctx->q_data[V4L2_M2M_DST]);
+	default:
+		return NULL;
+	}
+}
+
+const char *coda_product_name(int product);
+
+int coda_check_firmware(struct coda_dev *dev);
+
+static inline unsigned int coda_get_bitstream_payload(struct coda_ctx *ctx)
+{
+	return kfifo_len(&ctx->bitstream_fifo);
+}
+
+void coda_bit_stream_end_flag(struct coda_ctx *ctx);
+
+void coda_m2m_buf_done(struct coda_ctx *ctx, struct vb2_v4l2_buffer *buf,
+		       enum vb2_buffer_state state);
+
+int coda_h264_padding(int size, char *p);
+
+bool coda_jpeg_check_buffer(struct coda_ctx *ctx, struct vb2_v4l2_buffer *vb);
+int coda_jpeg_write_tables(struct coda_ctx *ctx);
+void coda_set_jpeg_compression_quality(struct coda_ctx *ctx, int quality);
+
+extern const struct coda_context_ops coda_bit_encode_ops;
+extern const struct coda_context_ops coda_bit_decode_ops;
+
+irqreturn_t coda_irq_handler(int irq, void *data);
+
+#endif /* __CODA_H__ */
diff --git a/drivers/media/platform/coda/coda_regs.h b/drivers/media/platform/coda/coda_regs.h
new file mode 100644
index 0000000..3490602
--- /dev/null
+++ b/drivers/media/platform/coda/coda_regs.h
@@ -0,0 +1,464 @@
+/*
+ * linux/drivers/media/platform/coda/coda_regs.h
+ *
+ * Copyright (C) 2012 Vista Silicon SL
+ *    Javier Martin <javier.martin@vista-silicon.com>
+ *    Xavier Duret
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#ifndef _REGS_CODA_H_
+#define _REGS_CODA_H_
+
+/* HW registers */
+#define CODA_REG_BIT_CODE_RUN			0x000
+#define		CODA_REG_RUN_ENABLE		(1 << 0)
+#define CODA_REG_BIT_CODE_DOWN			0x004
+#define		CODA_DOWN_ADDRESS_SET(x)	(((x) & 0xffff) << 16)
+#define		CODA_DOWN_DATA_SET(x)		((x) & 0xffff)
+#define CODA_REG_BIT_HOST_IN_REQ		0x008
+#define CODA_REG_BIT_INT_CLEAR			0x00c
+#define		CODA_REG_BIT_INT_CLEAR_SET	0x1
+#define CODA_REG_BIT_INT_STATUS		0x010
+#define CODA_REG_BIT_CODE_RESET		0x014
+#define		CODA_REG_RESET_ENABLE		(1 << 0)
+#define CODA_REG_BIT_CUR_PC			0x018
+#define CODA9_REG_BIT_SW_RESET			0x024
+#define		CODA9_SW_RESET_BPU_CORE   0x008
+#define		CODA9_SW_RESET_BPU_BUS    0x010
+#define		CODA9_SW_RESET_VCE_CORE   0x020
+#define		CODA9_SW_RESET_VCE_BUS    0x040
+#define		CODA9_SW_RESET_GDI_CORE   0x080
+#define		CODA9_SW_RESET_GDI_BUS    0x100
+#define CODA9_REG_BIT_SW_RESET_STATUS		0x034
+
+/* Static SW registers */
+#define CODA_REG_BIT_CODE_BUF_ADDR		0x100
+#define CODA_REG_BIT_WORK_BUF_ADDR		0x104
+#define CODA_REG_BIT_PARA_BUF_ADDR		0x108
+#define CODA_REG_BIT_STREAM_CTRL		0x10c
+#define		CODA7_STREAM_BUF_PIC_RESET	(1 << 4)
+#define		CODADX6_STREAM_BUF_PIC_RESET	(1 << 3)
+#define		CODA7_STREAM_BUF_PIC_FLUSH	(1 << 3)
+#define		CODADX6_STREAM_BUF_PIC_FLUSH	(1 << 2)
+#define		CODA7_STREAM_BUF_DYNALLOC_EN	(1 << 5)
+#define		CODADX6_STREAM_BUF_DYNALLOC_EN	(1 << 4)
+#define		CODADX6_STREAM_CHKDIS_OFFSET	(1 << 1)
+#define		CODA7_STREAM_SEL_64BITS_ENDIAN	(1 << 1)
+#define		CODA_STREAM_ENDIAN_SELECT	(1 << 0)
+#define CODA_REG_BIT_FRAME_MEM_CTRL		0x110
+#define		CODA9_FRAME_TILED2LINEAR	(1 << 11)
+#define		CODA_FRAME_CHROMA_INTERLEAVE	(1 << 2)
+#define		CODA_IMAGE_ENDIAN_SELECT	(1 << 0)
+#define CODA_REG_BIT_BIT_STREAM_PARAM		0x114
+#define		CODA_BIT_STREAM_END_FLAG	(1 << 2)
+#define		CODA_BIT_DEC_SEQ_INIT_ESCAPE	(1 << 0)
+#define CODA_REG_BIT_TEMP_BUF_ADDR		0x118
+#define CODA_REG_BIT_RD_PTR(x)			(0x120 + 8 * (x))
+#define CODA_REG_BIT_WR_PTR(x)			(0x124 + 8 * (x))
+#define CODA_REG_BIT_FRM_DIS_FLG(x)		(0x150 + 4 * (x))
+#define CODADX6_REG_BIT_SEARCH_RAM_BASE_ADDR	0x140
+#define CODA7_REG_BIT_AXI_SRAM_USE		0x140
+#define		CODA9_USE_HOST_BTP_ENABLE	(1 << 13)
+#define		CODA9_USE_HOST_OVL_ENABLE	(1 << 12)
+#define		CODA7_USE_HOST_ME_ENABLE	(1 << 11)
+#define		CODA9_USE_HOST_DBK_ENABLE	(3 << 10)
+#define		CODA7_USE_HOST_OVL_ENABLE	(1 << 10)
+#define		CODA7_USE_HOST_DBK_ENABLE	(1 << 9)
+#define		CODA9_USE_HOST_IP_ENABLE	(1 << 9)
+#define		CODA7_USE_HOST_IP_ENABLE	(1 << 8)
+#define		CODA9_USE_HOST_BIT_ENABLE	(1 << 8)
+#define		CODA7_USE_HOST_BIT_ENABLE	(1 << 7)
+#define		CODA9_USE_BTP_ENABLE		(1 << 5)
+#define		CODA7_USE_ME_ENABLE		(1 << 4)
+#define		CODA9_USE_OVL_ENABLE		(1 << 4)
+#define		CODA7_USE_OVL_ENABLE		(1 << 3)
+#define		CODA9_USE_DBK_ENABLE		(3 << 2)
+#define		CODA7_USE_DBK_ENABLE		(1 << 2)
+#define		CODA7_USE_IP_ENABLE		(1 << 1)
+#define		CODA7_USE_BIT_ENABLE		(1 << 0)
+
+#define CODA_REG_BIT_BUSY			0x160
+#define		CODA_REG_BIT_BUSY_FLAG		1
+#define CODA_REG_BIT_RUN_COMMAND		0x164
+#define		CODA_COMMAND_SEQ_INIT		1
+#define		CODA_COMMAND_SEQ_END		2
+#define		CODA_COMMAND_PIC_RUN		3
+#define		CODA_COMMAND_SET_FRAME_BUF	4
+#define		CODA_COMMAND_ENCODE_HEADER	5
+#define		CODA_COMMAND_ENC_PARA_SET	6
+#define		CODA_COMMAND_DEC_PARA_SET	7
+#define		CODA_COMMAND_DEC_BUF_FLUSH	8
+#define		CODA_COMMAND_RC_CHANGE_PARAMETER 9
+#define		CODA_COMMAND_FIRMWARE_GET	0xf
+#define CODA_REG_BIT_RUN_INDEX			0x168
+#define		CODA_INDEX_SET(x)		((x) & 0x3)
+#define CODA_REG_BIT_RUN_COD_STD		0x16c
+#define		CODADX6_MODE_DECODE_MP4		0
+#define		CODADX6_MODE_ENCODE_MP4		1
+#define		CODADX6_MODE_DECODE_H264	2
+#define		CODADX6_MODE_ENCODE_H264	3
+#define		CODA7_MODE_DECODE_H264		0
+#define		CODA7_MODE_DECODE_VC1		1
+#define		CODA7_MODE_DECODE_MP2		2
+#define		CODA7_MODE_DECODE_MP4		3
+#define		CODA7_MODE_DECODE_DV3		3
+#define		CODA7_MODE_DECODE_RV		4
+#define		CODA7_MODE_DECODE_MJPG		5
+#define		CODA7_MODE_ENCODE_H264		8
+#define		CODA7_MODE_ENCODE_MP4		11
+#define		CODA7_MODE_ENCODE_MJPG		13
+#define		CODA9_MODE_DECODE_H264		0
+#define		CODA9_MODE_DECODE_VC1		1
+#define		CODA9_MODE_DECODE_MP2		2
+#define		CODA9_MODE_DECODE_MP4		3
+#define		CODA9_MODE_DECODE_DV3		3
+#define		CODA9_MODE_DECODE_RV		4
+#define		CODA9_MODE_DECODE_AVS		5
+#define		CODA9_MODE_DECODE_MJPG		6
+#define		CODA9_MODE_DECODE_VPX		7
+#define		CODA9_MODE_ENCODE_H264		8
+#define		CODA9_MODE_ENCODE_MP4		11
+#define		CODA9_MODE_ENCODE_MJPG		13
+#define 	CODA_MODE_INVALID		0xffff
+#define CODA_REG_BIT_INT_ENABLE		0x170
+#define		CODA_INT_INTERRUPT_ENABLE	(1 << 3)
+#define CODA_REG_BIT_INT_REASON			0x174
+#define CODA7_REG_BIT_RUN_AUX_STD		0x178
+#define		CODA_MP4_AUX_MPEG4		0
+#define		CODA_MP4_AUX_DIVX3		1
+#define		CODA_VPX_AUX_THO		0
+#define		CODA_VPX_AUX_VP6		1
+#define		CODA_VPX_AUX_VP8		2
+#define		CODA_H264_AUX_AVC		0
+#define		CODA_H264_AUX_MVC		1
+
+/*
+ * Commands' mailbox:
+ * registers with offsets in the range 0x180-0x1d0
+ * have different meaning depending on the command being
+ * issued.
+ */
+
+/* Decoder Sequence Initialization */
+#define CODA_CMD_DEC_SEQ_BB_START		0x180
+#define CODA_CMD_DEC_SEQ_BB_SIZE		0x184
+#define CODA_CMD_DEC_SEQ_OPTION			0x188
+#define		CODA_NO_INT_ENABLE			(1 << 10)
+#define		CODA_REORDER_ENABLE			(1 << 1)
+#define		CODADX6_QP_REPORT			(1 << 0)
+#define		CODA7_MP4_DEBLK_ENABLE			(1 << 0)
+#define CODA_CMD_DEC_SEQ_SRC_SIZE		0x18c
+#define CODA_CMD_DEC_SEQ_START_BYTE		0x190
+#define CODA_CMD_DEC_SEQ_PS_BB_START		0x194
+#define CODA_CMD_DEC_SEQ_PS_BB_SIZE		0x198
+#define CODA_CMD_DEC_SEQ_MP4_ASP_CLASS		0x19c
+#define CODA_CMD_DEC_SEQ_X264_MV_EN		0x19c
+#define CODA_CMD_DEC_SEQ_SPP_CHUNK_SIZE		0x1a0
+
+#define CODA7_RET_DEC_SEQ_ASPECT		0x1b0
+#define CODA9_RET_DEC_SEQ_BITRATE		0x1b4
+#define CODA_RET_DEC_SEQ_SUCCESS		0x1c0
+#define CODA_RET_DEC_SEQ_SRC_FMT		0x1c4 /* SRC_SIZE on CODA7 */
+#define CODA_RET_DEC_SEQ_SRC_SIZE		0x1c4
+#define CODA_RET_DEC_SEQ_SRC_F_RATE		0x1c8
+#define CODA9_RET_DEC_SEQ_ASPECT		0x1c8
+#define CODA_RET_DEC_SEQ_FRAME_NEED		0x1cc
+#define CODA_RET_DEC_SEQ_FRAME_DELAY		0x1d0
+#define CODA_RET_DEC_SEQ_INFO			0x1d4
+#define CODA_RET_DEC_SEQ_CROP_LEFT_RIGHT	0x1d8
+#define CODA_RET_DEC_SEQ_CROP_TOP_BOTTOM	0x1dc
+#define CODA_RET_DEC_SEQ_NEXT_FRAME_NUM		0x1e0
+#define CODA_RET_DEC_SEQ_ERR_REASON		0x1e0
+#define CODA_RET_DEC_SEQ_FRATE_NR		0x1e4
+#define CODA_RET_DEC_SEQ_FRATE_DR		0x1e8
+#define CODA_RET_DEC_SEQ_JPG_PARA		0x1e4
+#define CODA_RET_DEC_SEQ_JPG_THUMB_IND		0x1e8
+#define CODA9_RET_DEC_SEQ_HEADER_REPORT		0x1ec
+
+/* Decoder Picture Run */
+#define CODA_CMD_DEC_PIC_ROT_MODE		0x180
+#define CODA_CMD_DEC_PIC_ROT_ADDR_Y		0x184
+#define CODA9_CMD_DEC_PIC_ROT_INDEX		0x184
+#define CODA_CMD_DEC_PIC_ROT_ADDR_CB		0x188
+#define CODA9_CMD_DEC_PIC_ROT_ADDR_Y		0x188
+#define CODA_CMD_DEC_PIC_ROT_ADDR_CR		0x18c
+#define CODA9_CMD_DEC_PIC_ROT_ADDR_CB		0x18c
+#define CODA_CMD_DEC_PIC_ROT_STRIDE		0x190
+#define CODA9_CMD_DEC_PIC_ROT_ADDR_CR		0x190
+#define CODA9_CMD_DEC_PIC_ROT_STRIDE		0x1b8
+
+#define CODA_CMD_DEC_PIC_OPTION			0x194
+#define		CODA_PRE_SCAN_EN			(1 << 0)
+#define		CODA_PRE_SCAN_MODE_DECODE		(0 << 1)
+#define		CODA_PRE_SCAN_MODE_RETURN		(1 << 1)
+#define		CODA_IFRAME_SEARCH_EN			(1 << 2)
+#define		CODA_SKIP_FRAME_MODE			(0x3 << 3)
+#define CODA_CMD_DEC_PIC_SKIP_NUM		0x198
+#define CODA_CMD_DEC_PIC_CHUNK_SIZE		0x19c
+#define CODA_CMD_DEC_PIC_BB_START		0x1a0
+#define CODA_CMD_DEC_PIC_START_BYTE		0x1a4
+#define CODA_RET_DEC_PIC_SIZE			0x1bc
+#define CODA_RET_DEC_PIC_FRAME_NUM		0x1c0
+#define CODA_RET_DEC_PIC_FRAME_IDX		0x1c4
+#define CODA_RET_DEC_PIC_ERR_MB			0x1c8
+#define CODA_RET_DEC_PIC_TYPE			0x1cc
+#define		CODA_PIC_TYPE_MASK			0x7
+#define		CODA_PIC_TYPE_MASK_VC1			0x3f
+#define		CODA9_PIC_TYPE_FIRST_MASK		(0x7 << 3)
+#define		CODA9_PIC_TYPE_IDR_MASK			(0x3 << 6)
+#define		CODA7_PIC_TYPE_H264_NPF_MASK		(0x3 << 16)
+#define		CODA7_PIC_TYPE_INTERLACED		(1 << 18)
+#define CODA_RET_DEC_PIC_POST			0x1d0
+#define CODA_RET_DEC_PIC_MVC_REPORT		0x1d0
+#define CODA_RET_DEC_PIC_OPTION			0x1d4
+#define CODA_RET_DEC_PIC_SUCCESS		0x1d8
+#define CODA_RET_DEC_PIC_CUR_IDX		0x1dc
+#define CODA_RET_DEC_PIC_CROP_LEFT_RIGHT	0x1e0
+#define CODA_RET_DEC_PIC_CROP_TOP_BOTTOM	0x1e4
+#define CODA_RET_DEC_PIC_FRAME_NEED		0x1ec
+
+#define CODA9_RET_DEC_PIC_VP8_PIC_REPORT	0x1e8
+#define CODA9_RET_DEC_PIC_ASPECT		0x1f0
+#define CODA9_RET_DEC_PIC_VP8_SCALE_INFO	0x1f0
+#define CODA9_RET_DEC_PIC_FRATE_NR		0x1f4
+#define CODA9_RET_DEC_PIC_FRATE_DR		0x1f8
+
+/* Encoder Sequence Initialization */
+#define CODA_CMD_ENC_SEQ_BB_START				0x180
+#define CODA_CMD_ENC_SEQ_BB_SIZE				0x184
+#define CODA_CMD_ENC_SEQ_OPTION				0x188
+#define		CODA7_OPTION_AVCINTRA16X16ONLY_OFFSET		9
+#define		CODA9_OPTION_MVC_PREFIX_NAL_OFFSET		9
+#define		CODA7_OPTION_GAMMA_OFFSET			8
+#define		CODA9_OPTION_MVC_PARASET_REFRESH_OFFSET		8
+#define		CODA7_OPTION_RCQPMAX_OFFSET			7
+#define		CODA9_OPTION_GAMMA_OFFSET			7
+#define		CODADX6_OPTION_GAMMA_OFFSET			7
+#define		CODA7_OPTION_RCQPMIN_OFFSET			6
+#define		CODA9_OPTION_RCQPMAX_OFFSET			6
+#define		CODA_OPTION_LIMITQP_OFFSET			6
+#define		CODA_OPTION_RCINTRAQP_OFFSET			5
+#define		CODA_OPTION_FMO_OFFSET				4
+#define		CODA9_OPTION_MVC_INTERVIEW_OFFSET		4
+#define		CODA_OPTION_AVC_AUD_OFFSET			2
+#define		CODA_OPTION_SLICEREPORT_OFFSET			1
+#define CODA_CMD_ENC_SEQ_COD_STD				0x18c
+#define		CODA_STD_MPEG4					0
+#define		CODA9_STD_H264					0
+#define		CODA_STD_H263					1
+#define		CODA_STD_H264					2
+#define		CODA_STD_MJPG					3
+#define		CODA9_STD_MPEG4					3
+
+#define CODA_CMD_ENC_SEQ_SRC_SIZE				0x190
+#define		CODA7_PICWIDTH_OFFSET				16
+#define		CODA7_PICWIDTH_MASK				0xffff
+#define		CODADX6_PICWIDTH_OFFSET				10
+#define		CODADX6_PICWIDTH_MASK				0x3ff
+#define		CODA_PICHEIGHT_OFFSET				0
+#define		CODADX6_PICHEIGHT_MASK				0x3ff
+#define		CODA7_PICHEIGHT_MASK				0xffff
+#define CODA_CMD_ENC_SEQ_SRC_F_RATE				0x194
+#define		CODA_FRATE_RES_OFFSET				0
+#define		CODA_FRATE_RES_MASK				0xffff
+#define		CODA_FRATE_DIV_OFFSET				16
+#define		CODA_FRATE_DIV_MASK				0xffff
+#define CODA_CMD_ENC_SEQ_MP4_PARA				0x198
+#define		CODA_MP4PARAM_VERID_OFFSET			6
+#define		CODA_MP4PARAM_VERID_MASK			0x01
+#define		CODA_MP4PARAM_INTRADCVLCTHR_OFFSET		2
+#define		CODA_MP4PARAM_INTRADCVLCTHR_MASK		0x07
+#define		CODA_MP4PARAM_REVERSIBLEVLCENABLE_OFFSET	1
+#define		CODA_MP4PARAM_REVERSIBLEVLCENABLE_MASK		0x01
+#define		CODA_MP4PARAM_DATAPARTITIONENABLE_OFFSET	0
+#define		CODA_MP4PARAM_DATAPARTITIONENABLE_MASK		0x01
+#define CODA_CMD_ENC_SEQ_263_PARA				0x19c
+#define		CODA_263PARAM_ANNEXJENABLE_OFFSET		2
+#define		CODA_263PARAM_ANNEXJENABLE_MASK		0x01
+#define		CODA_263PARAM_ANNEXKENABLE_OFFSET		1
+#define		CODA_263PARAM_ANNEXKENABLE_MASK		0x01
+#define		CODA_263PARAM_ANNEXTENABLE_OFFSET		0
+#define		CODA_263PARAM_ANNEXTENABLE_MASK		0x01
+#define CODA_CMD_ENC_SEQ_264_PARA				0x1a0
+#define		CODA_264PARAM_DEBLKFILTEROFFSETBETA_OFFSET	12
+#define		CODA_264PARAM_DEBLKFILTEROFFSETBETA_MASK	0x0f
+#define		CODA_264PARAM_DEBLKFILTEROFFSETALPHA_OFFSET	8
+#define		CODA_264PARAM_DEBLKFILTEROFFSETALPHA_MASK	0x0f
+#define		CODA_264PARAM_DISABLEDEBLK_OFFSET		6
+#define		CODA_264PARAM_DISABLEDEBLK_MASK		0x01
+#define		CODA_264PARAM_CONSTRAINEDINTRAPREDFLAG_OFFSET	5
+#define		CODA_264PARAM_CONSTRAINEDINTRAPREDFLAG_MASK	0x01
+#define		CODA_264PARAM_CHROMAQPOFFSET_OFFSET		0
+#define		CODA_264PARAM_CHROMAQPOFFSET_MASK		0x1f
+#define CODA_CMD_ENC_SEQ_SLICE_MODE				0x1a4
+#define		CODA_SLICING_SIZE_OFFSET			2
+#define		CODA_SLICING_SIZE_MASK				0x3fffffff
+#define		CODA_SLICING_UNIT_OFFSET			1
+#define		CODA_SLICING_UNIT_MASK				0x01
+#define		CODA_SLICING_MODE_OFFSET			0
+#define		CODA_SLICING_MODE_MASK				0x01
+#define CODA_CMD_ENC_SEQ_GOP_SIZE				0x1a8
+#define		CODA_GOP_SIZE_OFFSET				0
+#define		CODA_GOP_SIZE_MASK				0x3f
+#define CODA_CMD_ENC_SEQ_RC_PARA				0x1ac
+#define		CODA_RATECONTROL_AUTOSKIP_OFFSET		31
+#define		CODA_RATECONTROL_AUTOSKIP_MASK			0x01
+#define		CODA_RATECONTROL_INITIALDELAY_OFFSET		16
+#define		CODA_RATECONTROL_INITIALDELAY_MASK		0x7fff
+#define		CODA_RATECONTROL_BITRATE_OFFSET		1
+#define		CODA_RATECONTROL_BITRATE_MASK			0x7fff
+#define		CODA_RATECONTROL_ENABLE_OFFSET			0
+#define		CODA_RATECONTROL_ENABLE_MASK			0x01
+#define CODA_CMD_ENC_SEQ_RC_BUF_SIZE				0x1b0
+#define CODA_CMD_ENC_SEQ_INTRA_REFRESH				0x1b4
+#define CODADX6_CMD_ENC_SEQ_FMO					0x1b8
+#define		CODA_FMOPARAM_TYPE_OFFSET			4
+#define		CODA_FMOPARAM_TYPE_MASK				1
+#define		CODA_FMOPARAM_SLICENUM_OFFSET			0
+#define		CODA_FMOPARAM_SLICENUM_MASK			0x0f
+#define CODADX6_CMD_ENC_SEQ_INTRA_QP				0x1bc
+#define CODA7_CMD_ENC_SEQ_SEARCH_BASE				0x1b8
+#define CODA7_CMD_ENC_SEQ_SEARCH_SIZE				0x1bc
+#define CODA7_CMD_ENC_SEQ_INTRA_QP				0x1c4
+#define CODA_CMD_ENC_SEQ_RC_QP_MIN_MAX				0x1c8
+#define		CODA_QPMIN_OFFSET				8
+#define		CODA_QPMIN_MASK					0x3f
+#define		CODA_QPMAX_OFFSET				0
+#define		CODA_QPMAX_MASK					0x3f
+#define CODA_CMD_ENC_SEQ_RC_GAMMA				0x1cc
+#define		CODA_GAMMA_OFFSET				0
+#define		CODA_GAMMA_MASK					0xffff
+#define CODA_CMD_ENC_SEQ_RC_INTERVAL_MODE			0x1d0
+#define CODA9_CMD_ENC_SEQ_INTRA_WEIGHT				0x1d4
+#define CODA9_CMD_ENC_SEQ_ME_OPTION				0x1d8
+#define CODA_RET_ENC_SEQ_SUCCESS				0x1c0
+
+#define CODA_CMD_ENC_SEQ_JPG_PARA				0x198
+#define CODA_CMD_ENC_SEQ_JPG_RST_INTERVAL			0x19C
+#define CODA_CMD_ENC_SEQ_JPG_THUMB_EN				0x1a0
+#define CODA_CMD_ENC_SEQ_JPG_THUMB_SIZE				0x1a4
+#define CODA_CMD_ENC_SEQ_JPG_THUMB_OFFSET			0x1a8
+
+/* Encoder Picture Run */
+#define CODA9_CMD_ENC_PIC_SRC_INDEX		0x180
+#define CODA9_CMD_ENC_PIC_SRC_STRIDE		0x184
+#define CODA9_CMD_ENC_PIC_SUB_FRAME_SYNC	0x1a4
+#define CODA9_CMD_ENC_PIC_SRC_ADDR_Y		0x1a8
+#define CODA9_CMD_ENC_PIC_SRC_ADDR_CB		0x1ac
+#define CODA9_CMD_ENC_PIC_SRC_ADDR_CR		0x1b0
+#define CODA_CMD_ENC_PIC_SRC_ADDR_Y	0x180
+#define CODA_CMD_ENC_PIC_SRC_ADDR_CB	0x184
+#define CODA_CMD_ENC_PIC_SRC_ADDR_CR	0x188
+#define CODA_CMD_ENC_PIC_QS		0x18c
+#define CODA_CMD_ENC_PIC_ROT_MODE	0x190
+#define		CODA_ROT_MIR_ENABLE				(1 << 4)
+#define		CODA_ROT_0					(0x0 << 0)
+#define		CODA_ROT_90					(0x1 << 0)
+#define		CODA_ROT_180					(0x2 << 0)
+#define		CODA_ROT_270					(0x3 << 0)
+#define		CODA_MIR_NONE					(0x0 << 2)
+#define		CODA_MIR_VER					(0x1 << 2)
+#define		CODA_MIR_HOR					(0x2 << 2)
+#define		CODA_MIR_VER_HOR				(0x3 << 2)
+#define CODA_CMD_ENC_PIC_OPTION		0x194
+#define		CODA_FORCE_IPICTURE				BIT(1)
+#define		CODA_REPORT_MB_INFO				BIT(3)
+#define		CODA_REPORT_MV_INFO				BIT(4)
+#define		CODA_REPORT_SLICE_INFO				BIT(5)
+#define CODA_CMD_ENC_PIC_BB_START	0x198
+#define CODA_CMD_ENC_PIC_BB_SIZE	0x19c
+#define CODA_RET_ENC_FRAME_NUM		0x1c0
+#define CODA_RET_ENC_PIC_TYPE		0x1c4
+#define CODA_RET_ENC_PIC_FRAME_IDX	0x1c8
+#define CODA_RET_ENC_PIC_SLICE_NUM	0x1cc
+#define CODA_RET_ENC_PIC_FLAG		0x1d0
+#define CODA_RET_ENC_PIC_SUCCESS	0x1d8
+
+/* Set Frame Buffer */
+#define CODA_CMD_SET_FRAME_BUF_NUM		0x180
+#define CODA_CMD_SET_FRAME_BUF_STRIDE		0x184
+#define CODA_CMD_SET_FRAME_SLICE_BB_START	0x188
+#define CODA_CMD_SET_FRAME_SLICE_BB_SIZE	0x18c
+#define CODA9_CMD_SET_FRAME_SUBSAMP_A		0x188
+#define CODA9_CMD_SET_FRAME_SUBSAMP_B		0x18c
+#define CODA7_CMD_SET_FRAME_AXI_BIT_ADDR	0x190
+#define CODA7_CMD_SET_FRAME_AXI_IPACDC_ADDR	0x194
+#define CODA7_CMD_SET_FRAME_AXI_DBKY_ADDR	0x198
+#define CODA7_CMD_SET_FRAME_AXI_DBKC_ADDR	0x19c
+#define CODA7_CMD_SET_FRAME_AXI_OVL_ADDR	0x1a0
+#define CODA7_CMD_SET_FRAME_MAX_DEC_SIZE	0x1a4
+#define CODA9_CMD_SET_FRAME_AXI_BTP_ADDR	0x1a4
+#define CODA7_CMD_SET_FRAME_SOURCE_BUF_STRIDE	0x1a8
+#define CODA9_CMD_SET_FRAME_CACHE_SIZE		0x1a8
+#define CODA9_CMD_SET_FRAME_CACHE_CONFIG	0x1ac
+#define		CODA9_CACHE_BYPASS_OFFSET		28
+#define		CODA9_CACHE_DUALCONF_OFFSET		26
+#define		CODA9_CACHE_PAGEMERGE_OFFSET		24
+#define		CODA9_CACHE_LUMA_BUFFER_SIZE_OFFSET	16
+#define		CODA9_CACHE_CB_BUFFER_SIZE_OFFSET	8
+#define		CODA9_CACHE_CR_BUFFER_SIZE_OFFSET	0
+#define CODA9_CMD_SET_FRAME_SUBSAMP_A_MVC	0x1b0
+#define CODA9_CMD_SET_FRAME_SUBSAMP_B_MVC	0x1b4
+#define CODA9_CMD_SET_FRAME_DP_BUF_BASE		0x1b0
+#define CODA9_CMD_SET_FRAME_DP_BUF_SIZE		0x1b4
+#define CODA9_CMD_SET_FRAME_MAX_DEC_SIZE	0x1b8
+#define CODA9_CMD_SET_FRAME_DELAY		0x1bc
+
+/* Encoder Header */
+#define CODA_CMD_ENC_HEADER_CODE	0x180
+#define		CODA_GAMMA_OFFSET	0
+#define		CODA_HEADER_H264_SPS	0
+#define		CODA_HEADER_H264_PPS	1
+#define		CODA_HEADER_MP4V_VOL	0
+#define		CODA_HEADER_MP4V_VOS	1
+#define		CODA_HEADER_MP4V_VIS	2
+#define		CODA9_HEADER_FRAME_CROP	(1 << 3)
+#define CODA_CMD_ENC_HEADER_BB_START	0x184
+#define CODA_CMD_ENC_HEADER_BB_SIZE	0x188
+#define CODA9_CMD_ENC_HEADER_FRAME_CROP_H	0x18c
+#define CODA9_CMD_ENC_HEADER_FRAME_CROP_V	0x190
+
+/* Get Version */
+#define CODA_CMD_FIRMWARE_VERNUM		0x1c0
+#define		CODA_FIRMWARE_PRODUCT(x)	(((x) >> 16) & 0xffff)
+#define		CODA_FIRMWARE_MAJOR(x)		(((x) >> 12) & 0x0f)
+#define		CODA_FIRMWARE_MINOR(x)		(((x) >> 8) & 0x0f)
+#define		CODA_FIRMWARE_RELEASE(x)	((x) & 0xff)
+#define		CODA_FIRMWARE_VERNUM(product, major, minor, release)	\
+			((product) << 16 | ((major) << 12) |		\
+			((minor) << 8) | (release))
+#define CODA9_CMD_FIRMWARE_CODE_REV		0x1c4
+
+#define CODA9_GDMA_BASE				0x1000
+#define CODA9_GDI_WPROT_ERR_CLR			(CODA9_GDMA_BASE + 0x0a0)
+#define CODA9_GDI_WPROT_RGN_EN			(CODA9_GDMA_BASE + 0x0ac)
+
+#define CODA9_GDI_BUS_CTRL			(CODA9_GDMA_BASE + 0x0f0)
+#define CODA9_GDI_BUS_STATUS			(CODA9_GDMA_BASE + 0x0f4)
+
+#define CODA9_GDI_XY2_CAS_0			(CODA9_GDMA_BASE + 0x800)
+#define CODA9_GDI_XY2_CAS_F			(CODA9_GDMA_BASE + 0x83c)
+
+#define CODA9_GDI_XY2_BA_0			(CODA9_GDMA_BASE + 0x840)
+#define CODA9_GDI_XY2_BA_1			(CODA9_GDMA_BASE + 0x844)
+#define CODA9_GDI_XY2_BA_2			(CODA9_GDMA_BASE + 0x848)
+#define CODA9_GDI_XY2_BA_3			(CODA9_GDMA_BASE + 0x84c)
+
+#define CODA9_GDI_XY2_RAS_0			(CODA9_GDMA_BASE + 0x850)
+#define CODA9_GDI_XY2_RAS_F			(CODA9_GDMA_BASE + 0x88c)
+
+#define CODA9_GDI_XY2_RBC_CONFIG		(CODA9_GDMA_BASE + 0x890)
+#define		CODA9_XY2RBC_SEPARATE_MAP		BIT(19)
+#define		CODA9_XY2RBC_TOP_BOT_SPLIT		BIT(18)
+#define		CODA9_XY2RBC_TILED_MAP			BIT(17)
+#define		CODA9_XY2RBC_CA_INC_HOR			BIT(16)
+#define CODA9_GDI_RBC2_AXI_0			(CODA9_GDMA_BASE + 0x8a0)
+#define CODA9_GDI_RBC2_AXI_1F			(CODA9_GDMA_BASE + 0x91c)
+#define	CODA9_GDI_TILEDBUF_BASE			(CODA9_GDMA_BASE + 0x920)
+
+#endif
diff --git a/drivers/media/platform/coda/trace.h b/drivers/media/platform/coda/trace.h
new file mode 100644
index 0000000..f20666a
--- /dev/null
+++ b/drivers/media/platform/coda/trace.h
@@ -0,0 +1,162 @@
+#undef TRACE_SYSTEM
+#define TRACE_SYSTEM coda
+
+#if !defined(__CODA_TRACE_H__) || defined(TRACE_HEADER_MULTI_READ)
+#define __CODA_TRACE_H__
+
+#include <linux/tracepoint.h>
+#include <media/videobuf2-v4l2.h>
+
+#include "coda.h"
+
+TRACE_EVENT(coda_bit_run,
+	TP_PROTO(struct coda_ctx *ctx, int cmd),
+
+	TP_ARGS(ctx, cmd),
+
+	TP_STRUCT__entry(
+		__field(int, minor)
+		__field(int, ctx)
+		__field(int, cmd)
+	),
+
+	TP_fast_assign(
+		__entry->minor = ctx->fh.vdev->minor;
+		__entry->ctx = ctx->idx;
+		__entry->cmd = cmd;
+	),
+
+	TP_printk("minor = %d, ctx = %d, cmd = %d",
+		  __entry->minor, __entry->ctx, __entry->cmd)
+);
+
+TRACE_EVENT(coda_bit_done,
+	TP_PROTO(struct coda_ctx *ctx),
+
+	TP_ARGS(ctx),
+
+	TP_STRUCT__entry(
+		__field(int, minor)
+		__field(int, ctx)
+	),
+
+	TP_fast_assign(
+		__entry->minor = ctx->fh.vdev->minor;
+		__entry->ctx = ctx->idx;
+	),
+
+	TP_printk("minor = %d, ctx = %d", __entry->minor, __entry->ctx)
+);
+
+DECLARE_EVENT_CLASS(coda_buf_class,
+	TP_PROTO(struct coda_ctx *ctx, struct vb2_v4l2_buffer *buf),
+
+	TP_ARGS(ctx, buf),
+
+	TP_STRUCT__entry(
+		__field(int, minor)
+		__field(int, index)
+		__field(int, ctx)
+	),
+
+	TP_fast_assign(
+		__entry->minor = ctx->fh.vdev->minor;
+		__entry->index = buf->vb2_buf.index;
+		__entry->ctx = ctx->idx;
+	),
+
+	TP_printk("minor = %d, index = %d, ctx = %d",
+		  __entry->minor, __entry->index, __entry->ctx)
+);
+
+DEFINE_EVENT(coda_buf_class, coda_enc_pic_run,
+	TP_PROTO(struct coda_ctx *ctx, struct vb2_v4l2_buffer *buf),
+	TP_ARGS(ctx, buf)
+);
+
+DEFINE_EVENT(coda_buf_class, coda_enc_pic_done,
+	TP_PROTO(struct coda_ctx *ctx, struct vb2_v4l2_buffer *buf),
+	TP_ARGS(ctx, buf)
+);
+
+DECLARE_EVENT_CLASS(coda_buf_meta_class,
+	TP_PROTO(struct coda_ctx *ctx, struct vb2_v4l2_buffer *buf,
+		 struct coda_buffer_meta *meta),
+
+	TP_ARGS(ctx, buf, meta),
+
+	TP_STRUCT__entry(
+		__field(int, minor)
+		__field(int, index)
+		__field(int, start)
+		__field(int, end)
+		__field(int, ctx)
+	),
+
+	TP_fast_assign(
+		__entry->minor = ctx->fh.vdev->minor;
+		__entry->index = buf->vb2_buf.index;
+		__entry->start = meta->start;
+		__entry->end = meta->end;
+		__entry->ctx = ctx->idx;
+	),
+
+	TP_printk("minor = %d, index = %d, start = 0x%x, end = 0x%x, ctx = %d",
+		  __entry->minor, __entry->index, __entry->start, __entry->end,
+		  __entry->ctx)
+);
+
+DEFINE_EVENT(coda_buf_meta_class, coda_bit_queue,
+	TP_PROTO(struct coda_ctx *ctx, struct vb2_v4l2_buffer *buf,
+		 struct coda_buffer_meta *meta),
+	TP_ARGS(ctx, buf, meta)
+);
+
+DECLARE_EVENT_CLASS(coda_meta_class,
+	TP_PROTO(struct coda_ctx *ctx, struct coda_buffer_meta *meta),
+
+	TP_ARGS(ctx, meta),
+
+	TP_STRUCT__entry(
+		__field(int, minor)
+		__field(int, start)
+		__field(int, end)
+		__field(int, ctx)
+	),
+
+	TP_fast_assign(
+		__entry->minor = ctx->fh.vdev->minor;
+		__entry->start = meta ? meta->start : 0;
+		__entry->end = meta ? meta->end : 0;
+		__entry->ctx = ctx->idx;
+	),
+
+	TP_printk("minor = %d, start = 0x%x, end = 0x%x, ctx = %d",
+		  __entry->minor, __entry->start, __entry->end, __entry->ctx)
+);
+
+DEFINE_EVENT(coda_meta_class, coda_dec_pic_run,
+	TP_PROTO(struct coda_ctx *ctx, struct coda_buffer_meta *meta),
+	TP_ARGS(ctx, meta)
+);
+
+DEFINE_EVENT(coda_meta_class, coda_dec_pic_done,
+	TP_PROTO(struct coda_ctx *ctx, struct coda_buffer_meta *meta),
+	TP_ARGS(ctx, meta)
+);
+
+DEFINE_EVENT(coda_buf_meta_class, coda_dec_rot_done,
+	TP_PROTO(struct coda_ctx *ctx, struct vb2_v4l2_buffer *buf,
+		 struct coda_buffer_meta *meta),
+	TP_ARGS(ctx, buf, meta)
+);
+
+#endif /* __CODA_TRACE_H__ */
+
+#undef TRACE_INCLUDE_PATH
+#define TRACE_INCLUDE_PATH .
+#undef TRACE_INCLUDE_FILE
+#define TRACE_INCLUDE_FILE trace
+
+/* This part must be outside protection */
+#include <trace/define_trace.h>
diff --git a/drivers/media/platform/davinci/Kconfig b/drivers/media/platform/davinci/Kconfig
new file mode 100644
index 0000000..469e9d2
--- /dev/null
+++ b/drivers/media/platform/davinci/Kconfig
@@ -0,0 +1,88 @@
+config VIDEO_DAVINCI_VPIF_DISPLAY
+	tristate "TI DaVinci VPIF V4L2-Display driver"
+	depends on VIDEO_V4L2
+	depends on ARCH_DAVINCI || COMPILE_TEST
+	depends on HAS_DMA
+	select VIDEOBUF2_DMA_CONTIG
+	select VIDEO_ADV7343 if MEDIA_SUBDRV_AUTOSELECT
+	select VIDEO_THS7303 if MEDIA_SUBDRV_AUTOSELECT
+	help
+	  Enables Davinci VPIF module used for display devices.
+	  This module is used for display on TI DM6467/DA850/OMAPL138
+	  SoCs.
+
+	  To compile this driver as a module, choose M here. There will
+	  be two modules called vpif.ko and vpif_display.ko
+
+config VIDEO_DAVINCI_VPIF_CAPTURE
+	tristate "TI DaVinci VPIF video capture driver"
+	depends on VIDEO_V4L2
+	depends on ARCH_DAVINCI || COMPILE_TEST
+	depends on HAS_DMA
+	select VIDEOBUF2_DMA_CONTIG
+	help
+	  Enables Davinci VPIF module used for capture devices.
+	  This module is used for capture on TI DM6467/DA850/OMAPL138
+	  SoCs.
+
+	  To compile this driver as a module, choose M here. There will
+	  be two modules called vpif.ko and vpif_capture.ko
+
+config VIDEO_DM6446_CCDC
+	tristate "TI DM6446 CCDC video capture driver"
+	depends on VIDEO_V4L2
+	depends on ARCH_DAVINCI || COMPILE_TEST
+	depends on HAS_DMA
+	select VIDEOBUF_DMA_CONTIG
+	help
+	   Enables DaVinci CCD hw module. DaVinci CCDC hw interfaces
+	   with decoder modules such as TVP5146 over BT656 or
+	   sensor module such as MT9T001 over a raw interface. This
+	   module configures the interface and CCDC/ISIF to do
+	   video frame capture from slave decoders.
+
+	   To compile this driver as a module, choose M here. There will
+	   be three modules called vpfe_capture.ko, vpss.ko and dm644x_ccdc.ko
+
+config VIDEO_DM355_CCDC
+	tristate "TI DM355 CCDC video capture driver"
+	depends on VIDEO_V4L2
+	depends on ARCH_DAVINCI || COMPILE_TEST
+	depends on HAS_DMA
+	select VIDEOBUF_DMA_CONTIG
+	help
+	   Enables DM355 CCD hw module. DM355 CCDC hw interfaces
+	   with decoder modules such as TVP5146 over BT656 or
+	   sensor module such as MT9T001 over a raw interface. This
+	   module configures the interface and CCDC/ISIF to do
+	   video frame capture from a slave decoders
+
+	   To compile this driver as a module, choose M here. There will
+	   be three modules called vpfe_capture.ko, vpss.ko and dm355_ccdc.ko
+
+config VIDEO_DM365_ISIF
+	tristate "TI DM365 ISIF video capture driver"
+	depends on VIDEO_V4L2 && ARCH_DAVINCI
+	depends on HAS_DMA
+	select VIDEOBUF_DMA_CONTIG
+	help
+	   Enables ISIF hw module. This is the hardware module for
+	   configuring ISIF in VPFE to capture Raw Bayer RGB data from
+	   a image sensor or YUV data from a YUV source.
+
+	   To compile this driver as a module, choose M here. There will
+	   be three modules called vpfe_capture.ko, vpss.ko and isif.ko
+
+config VIDEO_DAVINCI_VPBE_DISPLAY
+	tristate "TI DaVinci VPBE V4L2-Display driver"
+	depends on VIDEO_V4L2 && ARCH_DAVINCI
+	depends on HAS_DMA
+	select VIDEOBUF2_DMA_CONTIG
+	help
+	    Enables Davinci VPBE module used for display devices.
+	    This module is used for display on TI DM644x/DM365/DM355
+	    based display devices.
+
+	    To compile this driver as a module, choose M here. There will
+	    be five modules created called vpss.ko, vpbe.ko, vpbe_osd.ko,
+	    vpbe_venc.ko and vpbe_display.ko
diff --git a/drivers/media/platform/davinci/Makefile b/drivers/media/platform/davinci/Makefile
new file mode 100644
index 0000000..d74d9ee
--- /dev/null
+++ b/drivers/media/platform/davinci/Makefile
@@ -0,0 +1,15 @@
+#
+# Makefile for the davinci video device drivers.
+#
+
+#VPIF Display driver
+obj-$(CONFIG_VIDEO_DAVINCI_VPIF_DISPLAY) += vpif.o vpif_display.o
+#VPIF Capture driver
+obj-$(CONFIG_VIDEO_DAVINCI_VPIF_CAPTURE) += vpif.o vpif_capture.o
+
+# Capture: DM6446 and DM355
+obj-$(CONFIG_VIDEO_DM6446_CCDC) += vpfe_capture.o vpss.o dm644x_ccdc.o
+obj-$(CONFIG_VIDEO_DM355_CCDC) += vpfe_capture.o vpss.o dm355_ccdc.o
+obj-$(CONFIG_VIDEO_DM365_ISIF) += vpfe_capture.o vpss.o isif.o
+obj-$(CONFIG_VIDEO_DAVINCI_VPBE_DISPLAY) += vpss.o vpbe.o vpbe_osd.o \
+	vpbe_venc.o vpbe_display.o
diff --git a/drivers/media/platform/davinci/ccdc_hw_device.h b/drivers/media/platform/davinci/ccdc_hw_device.h
new file mode 100644
index 0000000..86b9b35
--- /dev/null
+++ b/drivers/media/platform/davinci/ccdc_hw_device.h
@@ -0,0 +1,110 @@
+/*
+ * Copyright (C) 2008-2009 Texas Instruments Inc
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ * ccdc device API
+ */
+#ifndef _CCDC_HW_DEVICE_H
+#define _CCDC_HW_DEVICE_H
+
+#ifdef __KERNEL__
+#include <linux/videodev2.h>
+#include <linux/device.h>
+#include <media/davinci/vpfe_types.h>
+#include <media/davinci/ccdc_types.h>
+
+/*
+ * ccdc hw operations
+ */
+struct ccdc_hw_ops {
+	/* Pointer to initialize function to initialize ccdc device */
+	int (*open) (struct device *dev);
+	/* Pointer to deinitialize function */
+	int (*close) (struct device *dev);
+	/* set ccdc base address */
+	void (*set_ccdc_base)(void *base, int size);
+	/* Pointer to function to enable or disable ccdc */
+	void (*enable) (int en);
+	/* reset sbl. only for 6446 */
+	void (*reset) (void);
+	/* enable output to sdram */
+	void (*enable_out_to_sdram) (int en);
+	/* Pointer to function to set hw parameters */
+	int (*set_hw_if_params) (struct vpfe_hw_if_param *param);
+	/* get interface parameters */
+	int (*get_hw_if_params) (struct vpfe_hw_if_param *param);
+	/*
+	 * Pointer to function to set parameters. Used
+	 * for implementing VPFE_S_CCDC_PARAMS
+	 */
+	int (*set_params) (void *params);
+	/*
+	 * Pointer to function to get parameter. Used
+	 * for implementing VPFE_G_CCDC_PARAMS
+	 */
+	int (*get_params) (void *params);
+	/* Pointer to function to configure ccdc */
+	int (*configure) (void);
+
+	/* Pointer to function to set buffer type */
+	int (*set_buftype) (enum ccdc_buftype buf_type);
+	/* Pointer to function to get buffer type */
+	enum ccdc_buftype (*get_buftype) (void);
+	/* Pointer to function to set frame format */
+	int (*set_frame_format) (enum ccdc_frmfmt frm_fmt);
+	/* Pointer to function to get frame format */
+	enum ccdc_frmfmt (*get_frame_format) (void);
+	/* enumerate hw pix formats */
+	int (*enum_pix)(u32 *hw_pix, int i);
+	/* Pointer to function to set buffer type */
+	u32 (*get_pixel_format) (void);
+	/* Pointer to function to get pixel format. */
+	int (*set_pixel_format) (u32 pixfmt);
+	/* Pointer to function to set image window */
+	int (*set_image_window) (struct v4l2_rect *win);
+	/* Pointer to function to set image window */
+	void (*get_image_window) (struct v4l2_rect *win);
+	/* Pointer to function to get line length */
+	unsigned int (*get_line_length) (void);
+
+	/* Query CCDC control IDs */
+	int (*queryctrl)(struct v4l2_queryctrl *qctrl);
+	/* Set CCDC control */
+	int (*set_control)(struct v4l2_control *ctrl);
+	/* Get CCDC control */
+	int (*get_control)(struct v4l2_control *ctrl);
+
+	/* Pointer to function to set frame buffer address */
+	void (*setfbaddr) (unsigned long addr);
+	/* Pointer to function to get field id */
+	int (*getfid) (void);
+};
+
+struct ccdc_hw_device {
+	/* ccdc device name */
+	char name[32];
+	/* module owner */
+	struct module *owner;
+	/* hw ops */
+	struct ccdc_hw_ops hw_ops;
+};
+
+/* Used by CCDC module to register & unregister with vpfe capture driver */
+int vpfe_register_ccdc_device(struct ccdc_hw_device *dev);
+void vpfe_unregister_ccdc_device(struct ccdc_hw_device *dev);
+
+#endif
+#endif
diff --git a/drivers/media/platform/davinci/dm355_ccdc.c b/drivers/media/platform/davinci/dm355_ccdc.c
new file mode 100644
index 0000000..c90b9a4
--- /dev/null
+++ b/drivers/media/platform/davinci/dm355_ccdc.c
@@ -0,0 +1,1038 @@
+/*
+ * Copyright (C) 2005-2009 Texas Instruments Inc
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ * CCDC hardware module for DM355
+ * ------------------------------
+ *
+ * This module is for configuring DM355 CCD controller of VPFE to capture
+ * Raw yuv or Bayer RGB data from a decoder. CCDC has several modules
+ * such as Defect Pixel Correction, Color Space Conversion etc to
+ * pre-process the Bayer RGB data, before writing it to SDRAM. This
+ * module also allows application to configure individual
+ * module parameters through VPFE_CMD_S_CCDC_RAW_PARAMS IOCTL.
+ * To do so, application include dm355_ccdc.h and vpfe_capture.h header
+ * files. The setparams() API is called by vpfe_capture driver
+ * to configure module parameters
+ *
+ * TODO: 1) Raw bayer parameter settings and bayer capture
+ * 	 2) Split module parameter structure to module specific ioctl structs
+ *	 3) add support for lense shading correction
+ *	 4) investigate if enum used for user space type definition
+ * 	    to be replaced by #defines or integer
+ */
+#include <linux/platform_device.h>
+#include <linux/uaccess.h>
+#include <linux/videodev2.h>
+#include <linux/err.h>
+#include <linux/module.h>
+
+#include <media/davinci/dm355_ccdc.h>
+#include <media/davinci/vpss.h>
+
+#include "dm355_ccdc_regs.h"
+#include "ccdc_hw_device.h"
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("CCDC Driver for DM355");
+MODULE_AUTHOR("Texas Instruments");
+
+static struct ccdc_oper_config {
+	struct device *dev;
+	/* CCDC interface type */
+	enum vpfe_hw_if_type if_type;
+	/* Raw Bayer configuration */
+	struct ccdc_params_raw bayer;
+	/* YCbCr configuration */
+	struct ccdc_params_ycbcr ycbcr;
+	/* ccdc base address */
+	void __iomem *base_addr;
+} ccdc_cfg = {
+	/* Raw configurations */
+	.bayer = {
+		.pix_fmt = CCDC_PIXFMT_RAW,
+		.frm_fmt = CCDC_FRMFMT_PROGRESSIVE,
+		.win = CCDC_WIN_VGA,
+		.fid_pol = VPFE_PINPOL_POSITIVE,
+		.vd_pol = VPFE_PINPOL_POSITIVE,
+		.hd_pol = VPFE_PINPOL_POSITIVE,
+		.gain = {
+			.r_ye = 256,
+			.gb_g = 256,
+			.gr_cy = 256,
+			.b_mg = 256
+		},
+		.config_params = {
+			.datasft = 2,
+			.mfilt1 = CCDC_NO_MEDIAN_FILTER1,
+			.mfilt2 = CCDC_NO_MEDIAN_FILTER2,
+			.alaw = {
+				.gamma_wd = 2,
+			},
+			.blk_clamp = {
+				.sample_pixel = 1,
+				.dc_sub = 25
+			},
+			.col_pat_field0 = {
+				.olop = CCDC_GREEN_BLUE,
+				.olep = CCDC_BLUE,
+				.elop = CCDC_RED,
+				.elep = CCDC_GREEN_RED
+			},
+			.col_pat_field1 = {
+				.olop = CCDC_GREEN_BLUE,
+				.olep = CCDC_BLUE,
+				.elop = CCDC_RED,
+				.elep = CCDC_GREEN_RED
+			},
+		},
+	},
+	/* YCbCr configuration */
+	.ycbcr = {
+		.win = CCDC_WIN_PAL,
+		.pix_fmt = CCDC_PIXFMT_YCBCR_8BIT,
+		.frm_fmt = CCDC_FRMFMT_INTERLACED,
+		.fid_pol = VPFE_PINPOL_POSITIVE,
+		.vd_pol = VPFE_PINPOL_POSITIVE,
+		.hd_pol = VPFE_PINPOL_POSITIVE,
+		.bt656_enable = 1,
+		.pix_order = CCDC_PIXORDER_CBYCRY,
+		.buf_type = CCDC_BUFTYPE_FLD_INTERLEAVED
+	},
+};
+
+
+/* Raw Bayer formats */
+static u32 ccdc_raw_bayer_pix_formats[] =
+		{V4L2_PIX_FMT_SBGGR8, V4L2_PIX_FMT_SBGGR16};
+
+/* Raw YUV formats */
+static u32 ccdc_raw_yuv_pix_formats[] =
+		{V4L2_PIX_FMT_UYVY, V4L2_PIX_FMT_YUYV};
+
+/* register access routines */
+static inline u32 regr(u32 offset)
+{
+	return __raw_readl(ccdc_cfg.base_addr + offset);
+}
+
+static inline void regw(u32 val, u32 offset)
+{
+	__raw_writel(val, ccdc_cfg.base_addr + offset);
+}
+
+static void ccdc_enable(int en)
+{
+	unsigned int temp;
+	temp = regr(SYNCEN);
+	temp &= (~CCDC_SYNCEN_VDHDEN_MASK);
+	temp |= (en & CCDC_SYNCEN_VDHDEN_MASK);
+	regw(temp, SYNCEN);
+}
+
+static void ccdc_enable_output_to_sdram(int en)
+{
+	unsigned int temp;
+	temp = regr(SYNCEN);
+	temp &= (~(CCDC_SYNCEN_WEN_MASK));
+	temp |= ((en << CCDC_SYNCEN_WEN_SHIFT) & CCDC_SYNCEN_WEN_MASK);
+	regw(temp, SYNCEN);
+}
+
+static void ccdc_config_gain_offset(void)
+{
+	/* configure gain */
+	regw(ccdc_cfg.bayer.gain.r_ye, RYEGAIN);
+	regw(ccdc_cfg.bayer.gain.gr_cy, GRCYGAIN);
+	regw(ccdc_cfg.bayer.gain.gb_g, GBGGAIN);
+	regw(ccdc_cfg.bayer.gain.b_mg, BMGGAIN);
+	/* configure offset */
+	regw(ccdc_cfg.bayer.ccdc_offset, OFFSET);
+}
+
+/*
+ * ccdc_restore_defaults()
+ * This function restore power on defaults in the ccdc registers
+ */
+static int ccdc_restore_defaults(void)
+{
+	int i;
+
+	dev_dbg(ccdc_cfg.dev, "\nstarting ccdc_restore_defaults...");
+	/* set all registers to zero */
+	for (i = 0; i <= CCDC_REG_LAST; i += 4)
+		regw(0, i);
+
+	/* now override the values with power on defaults in registers */
+	regw(MODESET_DEFAULT, MODESET);
+	/* no culling support */
+	regw(CULH_DEFAULT, CULH);
+	regw(CULV_DEFAULT, CULV);
+	/* Set default Gain and Offset */
+	ccdc_cfg.bayer.gain.r_ye = GAIN_DEFAULT;
+	ccdc_cfg.bayer.gain.gb_g = GAIN_DEFAULT;
+	ccdc_cfg.bayer.gain.gr_cy = GAIN_DEFAULT;
+	ccdc_cfg.bayer.gain.b_mg = GAIN_DEFAULT;
+	ccdc_config_gain_offset();
+	regw(OUTCLIP_DEFAULT, OUTCLIP);
+	regw(LSCCFG2_DEFAULT, LSCCFG2);
+	/* select ccdc input */
+	if (vpss_select_ccdc_source(VPSS_CCDCIN)) {
+		dev_dbg(ccdc_cfg.dev, "\ncouldn't select ccdc input source");
+		return -EFAULT;
+	}
+	/* select ccdc clock */
+	if (vpss_enable_clock(VPSS_CCDC_CLOCK, 1) < 0) {
+		dev_dbg(ccdc_cfg.dev, "\ncouldn't enable ccdc clock");
+		return -EFAULT;
+	}
+	dev_dbg(ccdc_cfg.dev, "\nEnd of ccdc_restore_defaults...");
+	return 0;
+}
+
+static int ccdc_open(struct device *device)
+{
+	return ccdc_restore_defaults();
+}
+
+static int ccdc_close(struct device *device)
+{
+	/* disable clock */
+	vpss_enable_clock(VPSS_CCDC_CLOCK, 0);
+	/* do nothing for now */
+	return 0;
+}
+/*
+ * ccdc_setwin()
+ * This function will configure the window size to
+ * be capture in CCDC reg.
+ */
+static void ccdc_setwin(struct v4l2_rect *image_win,
+			enum ccdc_frmfmt frm_fmt, int ppc)
+{
+	int horz_start, horz_nr_pixels;
+	int vert_start, vert_nr_lines;
+	int mid_img = 0;
+
+	dev_dbg(ccdc_cfg.dev, "\nStarting ccdc_setwin...");
+
+	/*
+	 * ppc - per pixel count. indicates how many pixels per cell
+	 * output to SDRAM. example, for ycbcr, it is one y and one c, so 2.
+	 * raw capture this is 1
+	 */
+	horz_start = image_win->left << (ppc - 1);
+	horz_nr_pixels = ((image_win->width) << (ppc - 1)) - 1;
+
+	/* Writing the horizontal info into the registers */
+	regw(horz_start, SPH);
+	regw(horz_nr_pixels, NPH);
+	vert_start = image_win->top;
+
+	if (frm_fmt == CCDC_FRMFMT_INTERLACED) {
+		vert_nr_lines = (image_win->height >> 1) - 1;
+		vert_start >>= 1;
+		/* Since first line doesn't have any data */
+		vert_start += 1;
+		/* configure VDINT0 and VDINT1 */
+		regw(vert_start, VDINT0);
+	} else {
+		/* Since first line doesn't have any data */
+		vert_start += 1;
+		vert_nr_lines = image_win->height - 1;
+		/* configure VDINT0 and VDINT1 */
+		mid_img = vert_start + (image_win->height / 2);
+		regw(vert_start, VDINT0);
+		regw(mid_img, VDINT1);
+	}
+	regw(vert_start & CCDC_START_VER_ONE_MASK, SLV0);
+	regw(vert_start & CCDC_START_VER_TWO_MASK, SLV1);
+	regw(vert_nr_lines & CCDC_NUM_LINES_VER, NLV);
+	dev_dbg(ccdc_cfg.dev, "\nEnd of ccdc_setwin...");
+}
+
+static int validate_ccdc_param(struct ccdc_config_params_raw *ccdcparam)
+{
+	if (ccdcparam->datasft < CCDC_DATA_NO_SHIFT ||
+	    ccdcparam->datasft > CCDC_DATA_SHIFT_6BIT) {
+		dev_dbg(ccdc_cfg.dev, "Invalid value of data shift\n");
+		return -EINVAL;
+	}
+
+	if (ccdcparam->mfilt1 < CCDC_NO_MEDIAN_FILTER1 ||
+	    ccdcparam->mfilt1 > CCDC_MEDIAN_FILTER1) {
+		dev_dbg(ccdc_cfg.dev, "Invalid value of median filter1\n");
+		return -EINVAL;
+	}
+
+	if (ccdcparam->mfilt2 < CCDC_NO_MEDIAN_FILTER2 ||
+	    ccdcparam->mfilt2 > CCDC_MEDIAN_FILTER2) {
+		dev_dbg(ccdc_cfg.dev, "Invalid value of median filter2\n");
+		return -EINVAL;
+	}
+
+	if ((ccdcparam->med_filt_thres < 0) ||
+	   (ccdcparam->med_filt_thres > CCDC_MED_FILT_THRESH)) {
+		dev_dbg(ccdc_cfg.dev,
+			"Invalid value of median filter threshold\n");
+		return -EINVAL;
+	}
+
+	if (ccdcparam->data_sz < CCDC_DATA_16BITS ||
+	    ccdcparam->data_sz > CCDC_DATA_8BITS) {
+		dev_dbg(ccdc_cfg.dev, "Invalid value of data size\n");
+		return -EINVAL;
+	}
+
+	if (ccdcparam->alaw.enable) {
+		if (ccdcparam->alaw.gamma_wd < CCDC_GAMMA_BITS_13_4 ||
+		    ccdcparam->alaw.gamma_wd > CCDC_GAMMA_BITS_09_0) {
+			dev_dbg(ccdc_cfg.dev, "Invalid value of ALAW\n");
+			return -EINVAL;
+		}
+	}
+
+	if (ccdcparam->blk_clamp.b_clamp_enable) {
+		if (ccdcparam->blk_clamp.sample_pixel < CCDC_SAMPLE_1PIXELS ||
+		    ccdcparam->blk_clamp.sample_pixel > CCDC_SAMPLE_16PIXELS) {
+			dev_dbg(ccdc_cfg.dev,
+				"Invalid value of sample pixel\n");
+			return -EINVAL;
+		}
+		if (ccdcparam->blk_clamp.sample_ln < CCDC_SAMPLE_1LINES ||
+		    ccdcparam->blk_clamp.sample_ln > CCDC_SAMPLE_16LINES) {
+			dev_dbg(ccdc_cfg.dev,
+				"Invalid value of sample lines\n");
+			return -EINVAL;
+		}
+	}
+	return 0;
+}
+
+/* Parameter operations */
+static int ccdc_set_params(void __user *params)
+{
+	struct ccdc_config_params_raw ccdc_raw_params;
+	int x;
+
+	/* only raw module parameters can be set through the IOCTL */
+	if (ccdc_cfg.if_type != VPFE_RAW_BAYER)
+		return -EINVAL;
+
+	x = copy_from_user(&ccdc_raw_params, params, sizeof(ccdc_raw_params));
+	if (x) {
+		dev_dbg(ccdc_cfg.dev, "ccdc_set_params: error in copying ccdc"
+			"params, %d\n", x);
+		return -EFAULT;
+	}
+
+	if (!validate_ccdc_param(&ccdc_raw_params)) {
+		memcpy(&ccdc_cfg.bayer.config_params,
+			&ccdc_raw_params,
+			sizeof(ccdc_raw_params));
+		return 0;
+	}
+	return -EINVAL;
+}
+
+/* This function will configure CCDC for YCbCr video capture */
+static void ccdc_config_ycbcr(void)
+{
+	struct ccdc_params_ycbcr *params = &ccdc_cfg.ycbcr;
+	u32 temp;
+
+	/* first set the CCDC power on defaults values in all registers */
+	dev_dbg(ccdc_cfg.dev, "\nStarting ccdc_config_ycbcr...");
+	ccdc_restore_defaults();
+
+	/* configure pixel format & video frame format */
+	temp = (((params->pix_fmt & CCDC_INPUT_MODE_MASK) <<
+		CCDC_INPUT_MODE_SHIFT) |
+		((params->frm_fmt & CCDC_FRM_FMT_MASK) <<
+		CCDC_FRM_FMT_SHIFT));
+
+	/* setup BT.656 sync mode */
+	if (params->bt656_enable) {
+		regw(CCDC_REC656IF_BT656_EN, REC656IF);
+		/*
+		 * configure the FID, VD, HD pin polarity fld,hd pol positive,
+		 * vd negative, 8-bit pack mode
+		 */
+		temp |= CCDC_VD_POL_NEGATIVE;
+	} else {		/* y/c external sync mode */
+		temp |= (((params->fid_pol & CCDC_FID_POL_MASK) <<
+			CCDC_FID_POL_SHIFT) |
+			((params->hd_pol & CCDC_HD_POL_MASK) <<
+			CCDC_HD_POL_SHIFT) |
+			((params->vd_pol & CCDC_VD_POL_MASK) <<
+			CCDC_VD_POL_SHIFT));
+	}
+
+	/* pack the data to 8-bit */
+	temp |= CCDC_DATA_PACK_ENABLE;
+
+	regw(temp, MODESET);
+
+	/* configure video window */
+	ccdc_setwin(&params->win, params->frm_fmt, 2);
+
+	/* configure the order of y cb cr in SD-RAM */
+	temp = (params->pix_order << CCDC_Y8POS_SHIFT);
+	temp |= CCDC_LATCH_ON_VSYNC_DISABLE | CCDC_CCDCFG_FIDMD_NO_LATCH_VSYNC;
+	regw(temp, CCDCFG);
+
+	/*
+	 * configure the horizontal line offset. This is done by rounding up
+	 * width to a multiple of 16 pixels and multiply by two to account for
+	 * y:cb:cr 4:2:2 data
+	 */
+	regw(((params->win.width * 2 + 31) >> 5), HSIZE);
+
+	/* configure the memory line offset */
+	if (params->buf_type == CCDC_BUFTYPE_FLD_INTERLEAVED) {
+		/* two fields are interleaved in memory */
+		regw(CCDC_SDOFST_FIELD_INTERLEAVED, SDOFST);
+	}
+
+	dev_dbg(ccdc_cfg.dev, "\nEnd of ccdc_config_ycbcr...\n");
+}
+
+/*
+ * ccdc_config_black_clamp()
+ * configure parameters for Optical Black Clamp
+ */
+static void ccdc_config_black_clamp(struct ccdc_black_clamp *bclamp)
+{
+	u32 val;
+
+	if (!bclamp->b_clamp_enable) {
+		/* configure DCSub */
+		regw(bclamp->dc_sub & CCDC_BLK_DC_SUB_MASK, DCSUB);
+		regw(0x0000, CLAMP);
+		return;
+	}
+	/* Enable the Black clamping, set sample lines and pixels */
+	val = (bclamp->start_pixel & CCDC_BLK_ST_PXL_MASK) |
+	      ((bclamp->sample_pixel & CCDC_BLK_SAMPLE_LN_MASK) <<
+		CCDC_BLK_SAMPLE_LN_SHIFT) | CCDC_BLK_CLAMP_ENABLE;
+	regw(val, CLAMP);
+
+	/* If Black clamping is enable then make dcsub 0 */
+	val = (bclamp->sample_ln & CCDC_NUM_LINE_CALC_MASK)
+			<< CCDC_NUM_LINE_CALC_SHIFT;
+	regw(val, DCSUB);
+}
+
+/*
+ * ccdc_config_black_compense()
+ * configure parameters for Black Compensation
+ */
+static void ccdc_config_black_compense(struct ccdc_black_compensation *bcomp)
+{
+	u32 val;
+
+	val = (bcomp->b & CCDC_BLK_COMP_MASK) |
+		((bcomp->gb & CCDC_BLK_COMP_MASK) <<
+		CCDC_BLK_COMP_GB_COMP_SHIFT);
+	regw(val, BLKCMP1);
+
+	val = ((bcomp->gr & CCDC_BLK_COMP_MASK) <<
+		CCDC_BLK_COMP_GR_COMP_SHIFT) |
+		((bcomp->r & CCDC_BLK_COMP_MASK) <<
+		CCDC_BLK_COMP_R_COMP_SHIFT);
+	regw(val, BLKCMP0);
+}
+
+/*
+ * ccdc_write_dfc_entry()
+ * write an entry in the dfc table.
+ */
+static int ccdc_write_dfc_entry(int index, struct ccdc_vertical_dft *dfc)
+{
+/* TODO This is to be re-visited and adjusted */
+#define DFC_WRITE_WAIT_COUNT	1000
+	u32 val, count = DFC_WRITE_WAIT_COUNT;
+
+	regw(dfc->dft_corr_vert[index], DFCMEM0);
+	regw(dfc->dft_corr_horz[index], DFCMEM1);
+	regw(dfc->dft_corr_sub1[index], DFCMEM2);
+	regw(dfc->dft_corr_sub2[index], DFCMEM3);
+	regw(dfc->dft_corr_sub3[index], DFCMEM4);
+	/* set WR bit to write */
+	val = regr(DFCMEMCTL) | CCDC_DFCMEMCTL_DFCMWR_MASK;
+	regw(val, DFCMEMCTL);
+
+	/*
+	 * Assume, it is very short. If we get an error, we need to
+	 * adjust this value
+	 */
+	while (regr(DFCMEMCTL) & CCDC_DFCMEMCTL_DFCMWR_MASK)
+		count--;
+	/*
+	 * TODO We expect the count to be non-zero to be successful. Adjust
+	 * the count if write requires more time
+	 */
+
+	if (count) {
+		dev_err(ccdc_cfg.dev, "defect table write timeout !!!\n");
+		return -1;
+	}
+	return 0;
+}
+
+/*
+ * ccdc_config_vdfc()
+ * configure parameters for Vertical Defect Correction
+ */
+static int ccdc_config_vdfc(struct ccdc_vertical_dft *dfc)
+{
+	u32 val;
+	int i;
+
+	/* Configure General Defect Correction. The table used is from IPIPE */
+	val = dfc->gen_dft_en & CCDC_DFCCTL_GDFCEN_MASK;
+
+	/* Configure Vertical Defect Correction if needed */
+	if (!dfc->ver_dft_en) {
+		/* Enable only General Defect Correction */
+		regw(val, DFCCTL);
+		return 0;
+	}
+
+	if (dfc->table_size > CCDC_DFT_TABLE_SIZE)
+		return -EINVAL;
+
+	val |= CCDC_DFCCTL_VDFC_DISABLE;
+	val |= (dfc->dft_corr_ctl.vdfcsl & CCDC_DFCCTL_VDFCSL_MASK) <<
+		CCDC_DFCCTL_VDFCSL_SHIFT;
+	val |= (dfc->dft_corr_ctl.vdfcuda & CCDC_DFCCTL_VDFCUDA_MASK) <<
+		CCDC_DFCCTL_VDFCUDA_SHIFT;
+	val |= (dfc->dft_corr_ctl.vdflsft & CCDC_DFCCTL_VDFLSFT_MASK) <<
+		CCDC_DFCCTL_VDFLSFT_SHIFT;
+	regw(val , DFCCTL);
+
+	/* clear address ptr to offset 0 */
+	val = CCDC_DFCMEMCTL_DFCMARST_MASK << CCDC_DFCMEMCTL_DFCMARST_SHIFT;
+
+	/* write defect table entries */
+	for (i = 0; i < dfc->table_size; i++) {
+		/* increment address for non zero index */
+		if (i != 0)
+			val = CCDC_DFCMEMCTL_INC_ADDR;
+		regw(val, DFCMEMCTL);
+		if (ccdc_write_dfc_entry(i, dfc) < 0)
+			return -EFAULT;
+	}
+
+	/* update saturation level and enable dfc */
+	regw(dfc->saturation_ctl & CCDC_VDC_DFCVSAT_MASK, DFCVSAT);
+	val = regr(DFCCTL) | (CCDC_DFCCTL_VDFCEN_MASK <<
+			CCDC_DFCCTL_VDFCEN_SHIFT);
+	regw(val, DFCCTL);
+	return 0;
+}
+
+/*
+ * ccdc_config_csc()
+ * configure parameters for color space conversion
+ * Each register CSCM0-7 has two values in S8Q5 format.
+ */
+static void ccdc_config_csc(struct ccdc_csc *csc)
+{
+	u32 val1 = 0, val2;
+	int i;
+
+	if (!csc->enable)
+		return;
+
+	/* Enable the CSC sub-module */
+	regw(CCDC_CSC_ENABLE, CSCCTL);
+
+	/* Converting the co-eff as per the format of the register */
+	for (i = 0; i < CCDC_CSC_COEFF_TABLE_SIZE; i++) {
+		if ((i % 2) == 0) {
+			/* CSCM - LSB */
+			val1 = (csc->coeff[i].integer &
+				CCDC_CSC_COEF_INTEG_MASK)
+				<< CCDC_CSC_COEF_INTEG_SHIFT;
+			/*
+			 * convert decimal part to binary. Use 2 decimal
+			 * precision, user values range from .00 - 0.99
+			 */
+			val1 |= (((csc->coeff[i].decimal &
+				CCDC_CSC_COEF_DECIMAL_MASK) *
+				CCDC_CSC_DEC_MAX) / 100);
+		} else {
+
+			/* CSCM - MSB */
+			val2 = (csc->coeff[i].integer &
+				CCDC_CSC_COEF_INTEG_MASK)
+				<< CCDC_CSC_COEF_INTEG_SHIFT;
+			val2 |= (((csc->coeff[i].decimal &
+				 CCDC_CSC_COEF_DECIMAL_MASK) *
+				 CCDC_CSC_DEC_MAX) / 100);
+			val2 <<= CCDC_CSCM_MSB_SHIFT;
+			val2 |= val1;
+			regw(val2, (CSCM0 + ((i - 1) << 1)));
+		}
+	}
+}
+
+/*
+ * ccdc_config_color_patterns()
+ * configure parameters for color patterns
+ */
+static void ccdc_config_color_patterns(struct ccdc_col_pat *pat0,
+				       struct ccdc_col_pat *pat1)
+{
+	u32 val;
+
+	val = (pat0->olop | (pat0->olep << 2) | (pat0->elop << 4) |
+		(pat0->elep << 6) | (pat1->olop << 8) | (pat1->olep << 10) |
+		(pat1->elop << 12) | (pat1->elep << 14));
+	regw(val, COLPTN);
+}
+
+/* This function will configure CCDC for Raw mode image capture */
+static int ccdc_config_raw(void)
+{
+	struct ccdc_params_raw *params = &ccdc_cfg.bayer;
+	struct ccdc_config_params_raw *config_params =
+					&ccdc_cfg.bayer.config_params;
+	unsigned int val;
+
+	dev_dbg(ccdc_cfg.dev, "\nStarting ccdc_config_raw...");
+
+	/* restore power on defaults to register */
+	ccdc_restore_defaults();
+
+	/* CCDCFG register:
+	 * set CCD Not to swap input since input is RAW data
+	 * set FID detection function to Latch at V-Sync
+	 * set WENLOG - ccdc valid area to AND
+	 * set TRGSEL to WENBIT
+	 * set EXTRG to DISABLE
+	 * disable latching function on VSYNC - shadowed registers
+	 */
+	regw(CCDC_YCINSWP_RAW | CCDC_CCDCFG_FIDMD_LATCH_VSYNC |
+	     CCDC_CCDCFG_WENLOG_AND | CCDC_CCDCFG_TRGSEL_WEN |
+	     CCDC_CCDCFG_EXTRG_DISABLE | CCDC_LATCH_ON_VSYNC_DISABLE, CCDCFG);
+
+	/*
+	 * Set VDHD direction to input,  input type to raw input
+	 * normal data polarity, do not use external WEN
+	 */
+	val = (CCDC_VDHDOUT_INPUT | CCDC_RAW_IP_MODE | CCDC_DATAPOL_NORMAL |
+		CCDC_EXWEN_DISABLE);
+
+	/*
+	 * Configure the vertical sync polarity (MODESET.VDPOL), horizontal
+	 * sync polarity (MODESET.HDPOL), field id polarity (MODESET.FLDPOL),
+	 * frame format(progressive or interlace), & pixel format (Input mode)
+	 */
+	val |= (((params->vd_pol & CCDC_VD_POL_MASK) << CCDC_VD_POL_SHIFT) |
+		((params->hd_pol & CCDC_HD_POL_MASK) << CCDC_HD_POL_SHIFT) |
+		((params->fid_pol & CCDC_FID_POL_MASK) << CCDC_FID_POL_SHIFT) |
+		((params->frm_fmt & CCDC_FRM_FMT_MASK) << CCDC_FRM_FMT_SHIFT) |
+		((params->pix_fmt & CCDC_PIX_FMT_MASK) << CCDC_PIX_FMT_SHIFT));
+
+	/* set pack for alaw compression */
+	if ((config_params->data_sz == CCDC_DATA_8BITS) ||
+	     config_params->alaw.enable)
+		val |= CCDC_DATA_PACK_ENABLE;
+
+	/* Configure for LPF */
+	if (config_params->lpf_enable)
+		val |= (config_params->lpf_enable & CCDC_LPF_MASK) <<
+			CCDC_LPF_SHIFT;
+
+	/* Configure the data shift */
+	val |= (config_params->datasft & CCDC_DATASFT_MASK) <<
+		CCDC_DATASFT_SHIFT;
+	regw(val , MODESET);
+	dev_dbg(ccdc_cfg.dev, "\nWriting 0x%x to MODESET...\n", val);
+
+	/* Configure the Median Filter threshold */
+	regw((config_params->med_filt_thres) & CCDC_MED_FILT_THRESH, MEDFILT);
+
+	/* Configure GAMMAWD register. defaur 11-2, and Mosaic cfa pattern */
+	val = CCDC_GAMMA_BITS_11_2 << CCDC_GAMMAWD_INPUT_SHIFT |
+		CCDC_CFA_MOSAIC;
+
+	/* Enable and configure aLaw register if needed */
+	if (config_params->alaw.enable) {
+		val |= (CCDC_ALAW_ENABLE |
+			((config_params->alaw.gamma_wd &
+			CCDC_ALAW_GAMMA_WD_MASK) <<
+			CCDC_GAMMAWD_INPUT_SHIFT));
+	}
+
+	/* Configure Median filter1 & filter2 */
+	val |= ((config_params->mfilt1 << CCDC_MFILT1_SHIFT) |
+		(config_params->mfilt2 << CCDC_MFILT2_SHIFT));
+
+	regw(val, GAMMAWD);
+	dev_dbg(ccdc_cfg.dev, "\nWriting 0x%x to GAMMAWD...\n", val);
+
+	/* configure video window */
+	ccdc_setwin(&params->win, params->frm_fmt, 1);
+
+	/* Optical Clamp Averaging */
+	ccdc_config_black_clamp(&config_params->blk_clamp);
+
+	/* Black level compensation */
+	ccdc_config_black_compense(&config_params->blk_comp);
+
+	/* Vertical Defect Correction if needed */
+	if (ccdc_config_vdfc(&config_params->vertical_dft) < 0)
+		return -EFAULT;
+
+	/* color space conversion */
+	ccdc_config_csc(&config_params->csc);
+
+	/* color pattern */
+	ccdc_config_color_patterns(&config_params->col_pat_field0,
+				   &config_params->col_pat_field1);
+
+	/* Configure the Gain  & offset control */
+	ccdc_config_gain_offset();
+
+	dev_dbg(ccdc_cfg.dev, "\nWriting %x to COLPTN...\n", val);
+
+	/* Configure DATAOFST  register */
+	val = (config_params->data_offset.horz_offset & CCDC_DATAOFST_MASK) <<
+		CCDC_DATAOFST_H_SHIFT;
+	val |= (config_params->data_offset.vert_offset & CCDC_DATAOFST_MASK) <<
+		CCDC_DATAOFST_V_SHIFT;
+	regw(val, DATAOFST);
+
+	/* configuring HSIZE register */
+	val = (params->horz_flip_enable & CCDC_HSIZE_FLIP_MASK) <<
+		CCDC_HSIZE_FLIP_SHIFT;
+
+	/* If pack 8 is enable then 1 pixel will take 1 byte */
+	if ((config_params->data_sz == CCDC_DATA_8BITS) ||
+	     config_params->alaw.enable) {
+		val |= (((params->win.width) + 31) >> 5) &
+			CCDC_HSIZE_VAL_MASK;
+
+		/* adjust to multiple of 32 */
+		dev_dbg(ccdc_cfg.dev, "\nWriting 0x%x to HSIZE...\n",
+		       (((params->win.width) + 31) >> 5) &
+			CCDC_HSIZE_VAL_MASK);
+	} else {
+		/* else one pixel will take 2 byte */
+		val |= (((params->win.width * 2) + 31) >> 5) &
+			CCDC_HSIZE_VAL_MASK;
+
+		dev_dbg(ccdc_cfg.dev, "\nWriting 0x%x to HSIZE...\n",
+		       (((params->win.width * 2) + 31) >> 5) &
+			CCDC_HSIZE_VAL_MASK);
+	}
+	regw(val, HSIZE);
+
+	/* Configure SDOFST register */
+	if (params->frm_fmt == CCDC_FRMFMT_INTERLACED) {
+		if (params->image_invert_enable) {
+			/* For interlace inverse mode */
+			regw(CCDC_SDOFST_INTERLACE_INVERSE, SDOFST);
+			dev_dbg(ccdc_cfg.dev, "\nWriting %x to SDOFST...\n",
+				CCDC_SDOFST_INTERLACE_INVERSE);
+		} else {
+			/* For interlace non inverse mode */
+			regw(CCDC_SDOFST_INTERLACE_NORMAL, SDOFST);
+			dev_dbg(ccdc_cfg.dev, "\nWriting %x to SDOFST...\n",
+				CCDC_SDOFST_INTERLACE_NORMAL);
+		}
+	} else if (params->frm_fmt == CCDC_FRMFMT_PROGRESSIVE) {
+		if (params->image_invert_enable) {
+			/* For progessive inverse mode */
+			regw(CCDC_SDOFST_PROGRESSIVE_INVERSE, SDOFST);
+			dev_dbg(ccdc_cfg.dev, "\nWriting %x to SDOFST...\n",
+				CCDC_SDOFST_PROGRESSIVE_INVERSE);
+		} else {
+			/* For progessive non inverse mode */
+			regw(CCDC_SDOFST_PROGRESSIVE_NORMAL, SDOFST);
+			dev_dbg(ccdc_cfg.dev, "\nWriting %x to SDOFST...\n",
+				CCDC_SDOFST_PROGRESSIVE_NORMAL);
+		}
+	}
+	dev_dbg(ccdc_cfg.dev, "\nend of ccdc_config_raw...");
+	return 0;
+}
+
+static int ccdc_configure(void)
+{
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		return ccdc_config_raw();
+	else
+		ccdc_config_ycbcr();
+	return 0;
+}
+
+static int ccdc_set_buftype(enum ccdc_buftype buf_type)
+{
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		ccdc_cfg.bayer.buf_type = buf_type;
+	else
+		ccdc_cfg.ycbcr.buf_type = buf_type;
+	return 0;
+}
+static enum ccdc_buftype ccdc_get_buftype(void)
+{
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		return ccdc_cfg.bayer.buf_type;
+	return ccdc_cfg.ycbcr.buf_type;
+}
+
+static int ccdc_enum_pix(u32 *pix, int i)
+{
+	int ret = -EINVAL;
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER) {
+		if (i < ARRAY_SIZE(ccdc_raw_bayer_pix_formats)) {
+			*pix = ccdc_raw_bayer_pix_formats[i];
+			ret = 0;
+		}
+	} else {
+		if (i < ARRAY_SIZE(ccdc_raw_yuv_pix_formats)) {
+			*pix = ccdc_raw_yuv_pix_formats[i];
+			ret = 0;
+		}
+	}
+	return ret;
+}
+
+static int ccdc_set_pixel_format(u32 pixfmt)
+{
+	struct ccdc_a_law *alaw = &ccdc_cfg.bayer.config_params.alaw;
+
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER) {
+		ccdc_cfg.bayer.pix_fmt = CCDC_PIXFMT_RAW;
+		if (pixfmt == V4L2_PIX_FMT_SBGGR8)
+			alaw->enable = 1;
+		else if (pixfmt != V4L2_PIX_FMT_SBGGR16)
+			return -EINVAL;
+	} else {
+		if (pixfmt == V4L2_PIX_FMT_YUYV)
+			ccdc_cfg.ycbcr.pix_order = CCDC_PIXORDER_YCBYCR;
+		else if (pixfmt == V4L2_PIX_FMT_UYVY)
+			ccdc_cfg.ycbcr.pix_order = CCDC_PIXORDER_CBYCRY;
+		else
+			return -EINVAL;
+	}
+	return 0;
+}
+static u32 ccdc_get_pixel_format(void)
+{
+	struct ccdc_a_law *alaw = &ccdc_cfg.bayer.config_params.alaw;
+	u32 pixfmt;
+
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		if (alaw->enable)
+			pixfmt = V4L2_PIX_FMT_SBGGR8;
+		else
+			pixfmt = V4L2_PIX_FMT_SBGGR16;
+	else {
+		if (ccdc_cfg.ycbcr.pix_order == CCDC_PIXORDER_YCBYCR)
+			pixfmt = V4L2_PIX_FMT_YUYV;
+		else
+			pixfmt = V4L2_PIX_FMT_UYVY;
+	}
+	return pixfmt;
+}
+static int ccdc_set_image_window(struct v4l2_rect *win)
+{
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		ccdc_cfg.bayer.win = *win;
+	else
+		ccdc_cfg.ycbcr.win = *win;
+	return 0;
+}
+
+static void ccdc_get_image_window(struct v4l2_rect *win)
+{
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		*win = ccdc_cfg.bayer.win;
+	else
+		*win = ccdc_cfg.ycbcr.win;
+}
+
+static unsigned int ccdc_get_line_length(void)
+{
+	struct ccdc_config_params_raw *config_params =
+				&ccdc_cfg.bayer.config_params;
+	unsigned int len;
+
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER) {
+		if ((config_params->alaw.enable) ||
+		    (config_params->data_sz == CCDC_DATA_8BITS))
+			len = ccdc_cfg.bayer.win.width;
+		else
+			len = ccdc_cfg.bayer.win.width * 2;
+	} else
+		len = ccdc_cfg.ycbcr.win.width * 2;
+	return ALIGN(len, 32);
+}
+
+static int ccdc_set_frame_format(enum ccdc_frmfmt frm_fmt)
+{
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		ccdc_cfg.bayer.frm_fmt = frm_fmt;
+	else
+		ccdc_cfg.ycbcr.frm_fmt = frm_fmt;
+	return 0;
+}
+
+static enum ccdc_frmfmt ccdc_get_frame_format(void)
+{
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		return ccdc_cfg.bayer.frm_fmt;
+	else
+		return ccdc_cfg.ycbcr.frm_fmt;
+}
+
+static int ccdc_getfid(void)
+{
+	return  (regr(MODESET) >> 15) & 1;
+}
+
+/* misc operations */
+static inline void ccdc_setfbaddr(unsigned long addr)
+{
+	regw((addr >> 21) & 0x007f, STADRH);
+	regw((addr >> 5) & 0x0ffff, STADRL);
+}
+
+static int ccdc_set_hw_if_params(struct vpfe_hw_if_param *params)
+{
+	ccdc_cfg.if_type = params->if_type;
+
+	switch (params->if_type) {
+	case VPFE_BT656:
+	case VPFE_YCBCR_SYNC_16:
+	case VPFE_YCBCR_SYNC_8:
+		ccdc_cfg.ycbcr.vd_pol = params->vdpol;
+		ccdc_cfg.ycbcr.hd_pol = params->hdpol;
+		break;
+	default:
+		/* TODO add support for raw bayer here */
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static struct ccdc_hw_device ccdc_hw_dev = {
+	.name = "DM355 CCDC",
+	.owner = THIS_MODULE,
+	.hw_ops = {
+		.open = ccdc_open,
+		.close = ccdc_close,
+		.enable = ccdc_enable,
+		.enable_out_to_sdram = ccdc_enable_output_to_sdram,
+		.set_hw_if_params = ccdc_set_hw_if_params,
+		.set_params = ccdc_set_params,
+		.configure = ccdc_configure,
+		.set_buftype = ccdc_set_buftype,
+		.get_buftype = ccdc_get_buftype,
+		.enum_pix = ccdc_enum_pix,
+		.set_pixel_format = ccdc_set_pixel_format,
+		.get_pixel_format = ccdc_get_pixel_format,
+		.set_frame_format = ccdc_set_frame_format,
+		.get_frame_format = ccdc_get_frame_format,
+		.set_image_window = ccdc_set_image_window,
+		.get_image_window = ccdc_get_image_window,
+		.get_line_length = ccdc_get_line_length,
+		.setfbaddr = ccdc_setfbaddr,
+		.getfid = ccdc_getfid,
+	},
+};
+
+static int dm355_ccdc_probe(struct platform_device *pdev)
+{
+	void (*setup_pinmux)(void);
+	struct resource	*res;
+	int status = 0;
+
+	/*
+	 * first try to register with vpfe. If not correct platform, then we
+	 * don't have to iomap
+	 */
+	status = vpfe_register_ccdc_device(&ccdc_hw_dev);
+	if (status < 0)
+		return status;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		status = -ENODEV;
+		goto fail_nores;
+	}
+
+	res = request_mem_region(res->start, resource_size(res), res->name);
+	if (!res) {
+		status = -EBUSY;
+		goto fail_nores;
+	}
+
+	ccdc_cfg.base_addr = ioremap_nocache(res->start, resource_size(res));
+	if (!ccdc_cfg.base_addr) {
+		status = -ENOMEM;
+		goto fail_nomem;
+	}
+
+	/* Platform data holds setup_pinmux function ptr */
+	if (NULL == pdev->dev.platform_data) {
+		status = -ENODEV;
+		goto fail_nomap;
+	}
+	setup_pinmux = pdev->dev.platform_data;
+	/*
+	 * setup Mux configuration for ccdc which may be different for
+	 * different SoCs using this CCDC
+	 */
+	setup_pinmux();
+	ccdc_cfg.dev = &pdev->dev;
+	printk(KERN_NOTICE "%s is registered with vpfe.\n", ccdc_hw_dev.name);
+	return 0;
+fail_nomap:
+	iounmap(ccdc_cfg.base_addr);
+fail_nomem:
+	release_mem_region(res->start, resource_size(res));
+fail_nores:
+	vpfe_unregister_ccdc_device(&ccdc_hw_dev);
+	return status;
+}
+
+static int dm355_ccdc_remove(struct platform_device *pdev)
+{
+	struct resource	*res;
+
+	iounmap(ccdc_cfg.base_addr);
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res)
+		release_mem_region(res->start, resource_size(res));
+	vpfe_unregister_ccdc_device(&ccdc_hw_dev);
+	return 0;
+}
+
+static struct platform_driver dm355_ccdc_driver = {
+	.driver = {
+		.name	= "dm355_ccdc",
+	},
+	.remove = dm355_ccdc_remove,
+	.probe = dm355_ccdc_probe,
+};
+
+module_platform_driver(dm355_ccdc_driver);
diff --git a/drivers/media/platform/davinci/dm355_ccdc_regs.h b/drivers/media/platform/davinci/dm355_ccdc_regs.h
new file mode 100644
index 0000000..2e1946e
--- /dev/null
+++ b/drivers/media/platform/davinci/dm355_ccdc_regs.h
@@ -0,0 +1,310 @@
+/*
+ * Copyright (C) 2005-2009 Texas Instruments Inc
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+#ifndef _DM355_CCDC_REGS_H
+#define _DM355_CCDC_REGS_H
+
+/**************************************************************************\
+* Register OFFSET Definitions
+\**************************************************************************/
+#define SYNCEN				0x00
+#define MODESET				0x04
+#define HDWIDTH				0x08
+#define VDWIDTH				0x0c
+#define PPLN				0x10
+#define LPFR				0x14
+#define SPH				0x18
+#define NPH				0x1c
+#define SLV0				0x20
+#define SLV1				0x24
+#define NLV				0x28
+#define CULH				0x2c
+#define CULV				0x30
+#define HSIZE				0x34
+#define SDOFST				0x38
+#define STADRH				0x3c
+#define STADRL				0x40
+#define CLAMP				0x44
+#define DCSUB				0x48
+#define COLPTN				0x4c
+#define BLKCMP0				0x50
+#define BLKCMP1				0x54
+#define MEDFILT				0x58
+#define RYEGAIN				0x5c
+#define GRCYGAIN			0x60
+#define GBGGAIN				0x64
+#define BMGGAIN				0x68
+#define OFFSET				0x6c
+#define OUTCLIP				0x70
+#define VDINT0				0x74
+#define VDINT1				0x78
+#define RSV0				0x7c
+#define GAMMAWD				0x80
+#define REC656IF			0x84
+#define CCDCFG				0x88
+#define FMTCFG				0x8c
+#define FMTPLEN				0x90
+#define FMTSPH				0x94
+#define FMTLNH				0x98
+#define FMTSLV				0x9c
+#define FMTLNV				0xa0
+#define FMTRLEN				0xa4
+#define FMTHCNT				0xa8
+#define FMT_ADDR_PTR_B			0xac
+#define FMT_ADDR_PTR(i)			(FMT_ADDR_PTR_B + (i * 4))
+#define FMTPGM_VF0			0xcc
+#define FMTPGM_VF1			0xd0
+#define FMTPGM_AP0			0xd4
+#define FMTPGM_AP1			0xd8
+#define FMTPGM_AP2			0xdc
+#define FMTPGM_AP3                      0xe0
+#define FMTPGM_AP4                      0xe4
+#define FMTPGM_AP5                      0xe8
+#define FMTPGM_AP6                      0xec
+#define FMTPGM_AP7                      0xf0
+#define LSCCFG1                         0xf4
+#define LSCCFG2                         0xf8
+#define LSCH0                           0xfc
+#define LSCV0                           0x100
+#define LSCKH                           0x104
+#define LSCKV                           0x108
+#define LSCMEMCTL                       0x10c
+#define LSCMEMD                         0x110
+#define LSCMEMQ                         0x114
+#define DFCCTL                          0x118
+#define DFCVSAT                         0x11c
+#define DFCMEMCTL                       0x120
+#define DFCMEM0                         0x124
+#define DFCMEM1                         0x128
+#define DFCMEM2                         0x12c
+#define DFCMEM3                         0x130
+#define DFCMEM4                         0x134
+#define CSCCTL                          0x138
+#define CSCM0                           0x13c
+#define CSCM1                           0x140
+#define CSCM2                           0x144
+#define CSCM3                           0x148
+#define CSCM4                           0x14c
+#define CSCM5                           0x150
+#define CSCM6                           0x154
+#define CSCM7                           0x158
+#define DATAOFST			0x15c
+#define CCDC_REG_LAST			DATAOFST
+/**************************************************************
+*	Define for various register bit mask and shifts for CCDC
+*
+**************************************************************/
+#define CCDC_RAW_IP_MODE			0
+#define CCDC_VDHDOUT_INPUT			0
+#define CCDC_YCINSWP_RAW			(0 << 4)
+#define CCDC_EXWEN_DISABLE 			0
+#define CCDC_DATAPOL_NORMAL			0
+#define CCDC_CCDCFG_FIDMD_LATCH_VSYNC		0
+#define CCDC_CCDCFG_FIDMD_NO_LATCH_VSYNC	(1 << 6)
+#define CCDC_CCDCFG_WENLOG_AND			0
+#define CCDC_CCDCFG_TRGSEL_WEN			0
+#define CCDC_CCDCFG_EXTRG_DISABLE		0
+#define CCDC_CFA_MOSAIC				0
+#define CCDC_Y8POS_SHIFT			11
+
+#define CCDC_VDC_DFCVSAT_MASK			0x3fff
+#define CCDC_DATAOFST_MASK			0x0ff
+#define CCDC_DATAOFST_H_SHIFT			0
+#define CCDC_DATAOFST_V_SHIFT			8
+#define CCDC_GAMMAWD_CFA_MASK			1
+#define CCDC_GAMMAWD_CFA_SHIFT			5
+#define CCDC_GAMMAWD_INPUT_SHIFT		2
+#define CCDC_FID_POL_MASK			1
+#define CCDC_FID_POL_SHIFT			4
+#define CCDC_HD_POL_MASK			1
+#define CCDC_HD_POL_SHIFT			3
+#define CCDC_VD_POL_MASK			1
+#define CCDC_VD_POL_SHIFT			2
+#define CCDC_VD_POL_NEGATIVE			(1 << 2)
+#define CCDC_FRM_FMT_MASK			1
+#define CCDC_FRM_FMT_SHIFT			7
+#define CCDC_DATA_SZ_MASK			7
+#define CCDC_DATA_SZ_SHIFT			8
+#define CCDC_VDHDOUT_MASK			1
+#define CCDC_VDHDOUT_SHIFT			0
+#define CCDC_EXWEN_MASK				1
+#define CCDC_EXWEN_SHIFT			5
+#define CCDC_INPUT_MODE_MASK			3
+#define CCDC_INPUT_MODE_SHIFT			12
+#define CCDC_PIX_FMT_MASK			3
+#define CCDC_PIX_FMT_SHIFT			12
+#define CCDC_DATAPOL_MASK			1
+#define CCDC_DATAPOL_SHIFT			6
+#define CCDC_WEN_ENABLE				(1 << 1)
+#define CCDC_VDHDEN_ENABLE			(1 << 16)
+#define CCDC_LPF_ENABLE				(1 << 14)
+#define CCDC_ALAW_ENABLE			1
+#define CCDC_ALAW_GAMMA_WD_MASK			7
+#define CCDC_REC656IF_BT656_EN			3
+
+#define CCDC_FMTCFG_FMTMODE_MASK 		3
+#define CCDC_FMTCFG_FMTMODE_SHIFT		1
+#define CCDC_FMTCFG_LNUM_MASK			3
+#define CCDC_FMTCFG_LNUM_SHIFT			4
+#define CCDC_FMTCFG_ADDRINC_MASK		7
+#define CCDC_FMTCFG_ADDRINC_SHIFT		8
+
+#define CCDC_CCDCFG_FIDMD_SHIFT			6
+#define	CCDC_CCDCFG_WENLOG_SHIFT		8
+#define CCDC_CCDCFG_TRGSEL_SHIFT		9
+#define CCDC_CCDCFG_EXTRG_SHIFT			10
+#define CCDC_CCDCFG_MSBINVI_SHIFT		13
+
+#define CCDC_HSIZE_FLIP_SHIFT			12
+#define CCDC_HSIZE_FLIP_MASK			1
+#define CCDC_HSIZE_VAL_MASK			0xFFF
+#define CCDC_SDOFST_FIELD_INTERLEAVED		0x249
+#define CCDC_SDOFST_INTERLACE_INVERSE		0x4B6D
+#define CCDC_SDOFST_INTERLACE_NORMAL		0x0B6D
+#define CCDC_SDOFST_PROGRESSIVE_INVERSE		0x4000
+#define CCDC_SDOFST_PROGRESSIVE_NORMAL		0
+#define CCDC_START_PX_HOR_MASK			0x7FFF
+#define CCDC_NUM_PX_HOR_MASK			0x7FFF
+#define CCDC_START_VER_ONE_MASK			0x7FFF
+#define CCDC_START_VER_TWO_MASK			0x7FFF
+#define CCDC_NUM_LINES_VER			0x7FFF
+
+#define CCDC_BLK_CLAMP_ENABLE			(1 << 15)
+#define CCDC_BLK_SGAIN_MASK			0x1F
+#define CCDC_BLK_ST_PXL_MASK			0x1FFF
+#define CCDC_BLK_SAMPLE_LN_MASK			3
+#define CCDC_BLK_SAMPLE_LN_SHIFT		13
+
+#define CCDC_NUM_LINE_CALC_MASK			3
+#define CCDC_NUM_LINE_CALC_SHIFT		14
+
+#define CCDC_BLK_DC_SUB_MASK			0x3FFF
+#define CCDC_BLK_COMP_MASK			0xFF
+#define CCDC_BLK_COMP_GB_COMP_SHIFT		8
+#define CCDC_BLK_COMP_GR_COMP_SHIFT		0
+#define CCDC_BLK_COMP_R_COMP_SHIFT		8
+#define CCDC_LATCH_ON_VSYNC_DISABLE		(1 << 15)
+#define CCDC_LATCH_ON_VSYNC_ENABLE		(0 << 15)
+#define CCDC_FPC_ENABLE				(1 << 15)
+#define CCDC_FPC_FPC_NUM_MASK 			0x7FFF
+#define CCDC_DATA_PACK_ENABLE			(1 << 11)
+#define CCDC_FMT_HORZ_FMTLNH_MASK		0x1FFF
+#define CCDC_FMT_HORZ_FMTSPH_MASK		0x1FFF
+#define CCDC_FMT_HORZ_FMTSPH_SHIFT		16
+#define CCDC_FMT_VERT_FMTLNV_MASK		0x1FFF
+#define CCDC_FMT_VERT_FMTSLV_MASK		0x1FFF
+#define CCDC_FMT_VERT_FMTSLV_SHIFT		16
+#define CCDC_VP_OUT_VERT_NUM_MASK		0x3FFF
+#define CCDC_VP_OUT_VERT_NUM_SHIFT		17
+#define CCDC_VP_OUT_HORZ_NUM_MASK		0x1FFF
+#define CCDC_VP_OUT_HORZ_NUM_SHIFT		4
+#define CCDC_VP_OUT_HORZ_ST_MASK		0xF
+
+#define CCDC_CSC_COEF_INTEG_MASK		7
+#define CCDC_CSC_COEF_DECIMAL_MASK		0x1f
+#define CCDC_CSC_COEF_INTEG_SHIFT		5
+#define CCDC_CSCM_MSB_SHIFT			8
+#define CCDC_CSC_ENABLE				1
+#define CCDC_CSC_DEC_MAX			32
+
+#define CCDC_MFILT1_SHIFT			10
+#define CCDC_MFILT2_SHIFT			8
+#define CCDC_MED_FILT_THRESH			0x3FFF
+#define CCDC_LPF_MASK				1
+#define CCDC_LPF_SHIFT				14
+#define CCDC_OFFSET_MASK			0x3FF
+#define CCDC_DATASFT_MASK			7
+#define CCDC_DATASFT_SHIFT			8
+
+#define CCDC_DF_ENABLE				1
+
+#define CCDC_FMTPLEN_P0_MASK			0xF
+#define CCDC_FMTPLEN_P1_MASK			0xF
+#define CCDC_FMTPLEN_P2_MASK			7
+#define CCDC_FMTPLEN_P3_MASK			7
+#define CCDC_FMTPLEN_P0_SHIFT			0
+#define CCDC_FMTPLEN_P1_SHIFT			4
+#define CCDC_FMTPLEN_P2_SHIFT			8
+#define CCDC_FMTPLEN_P3_SHIFT			12
+
+#define CCDC_FMTSPH_MASK			0x1FFF
+#define CCDC_FMTLNH_MASK			0x1FFF
+#define CCDC_FMTSLV_MASK			0x1FFF
+#define CCDC_FMTLNV_MASK			0x7FFF
+#define CCDC_FMTRLEN_MASK			0x1FFF
+#define CCDC_FMTHCNT_MASK			0x1FFF
+
+#define CCDC_ADP_INIT_MASK			0x1FFF
+#define CCDC_ADP_LINE_SHIFT			13
+#define CCDC_ADP_LINE_MASK			3
+#define CCDC_FMTPGN_APTR_MASK			7
+
+#define CCDC_DFCCTL_GDFCEN_MASK			1
+#define CCDC_DFCCTL_VDFCEN_MASK			1
+#define CCDC_DFCCTL_VDFC_DISABLE		(0 << 4)
+#define CCDC_DFCCTL_VDFCEN_SHIFT		4
+#define CCDC_DFCCTL_VDFCSL_MASK			3
+#define CCDC_DFCCTL_VDFCSL_SHIFT		5
+#define CCDC_DFCCTL_VDFCUDA_MASK		1
+#define CCDC_DFCCTL_VDFCUDA_SHIFT		7
+#define CCDC_DFCCTL_VDFLSFT_MASK		3
+#define CCDC_DFCCTL_VDFLSFT_SHIFT		8
+#define CCDC_DFCMEMCTL_DFCMARST_MASK		1
+#define CCDC_DFCMEMCTL_DFCMARST_SHIFT		2
+#define CCDC_DFCMEMCTL_DFCMWR_MASK		1
+#define CCDC_DFCMEMCTL_DFCMWR_SHIFT		0
+#define CCDC_DFCMEMCTL_INC_ADDR			(0 << 2)
+
+#define CCDC_LSCCFG_GFTSF_MASK			7
+#define CCDC_LSCCFG_GFTSF_SHIFT			1
+#define CCDC_LSCCFG_GFTINV_MASK			0xf
+#define CCDC_LSCCFG_GFTINV_SHIFT		4
+#define CCDC_LSC_GFTABLE_SEL_MASK		3
+#define CCDC_LSC_GFTABLE_EPEL_SHIFT		8
+#define CCDC_LSC_GFTABLE_OPEL_SHIFT		10
+#define CCDC_LSC_GFTABLE_EPOL_SHIFT		12
+#define CCDC_LSC_GFTABLE_OPOL_SHIFT		14
+#define CCDC_LSC_GFMODE_MASK			3
+#define CCDC_LSC_GFMODE_SHIFT			4
+#define CCDC_LSC_DISABLE			0
+#define CCDC_LSC_ENABLE				1
+#define CCDC_LSC_TABLE1_SLC			0
+#define CCDC_LSC_TABLE2_SLC			1
+#define CCDC_LSC_TABLE3_SLC			2
+#define CCDC_LSC_MEMADDR_RESET			(1 << 2)
+#define CCDC_LSC_MEMADDR_INCR			(0 << 2)
+#define CCDC_LSC_FRAC_MASK_T1			0xFF
+#define CCDC_LSC_INT_MASK			3
+#define CCDC_LSC_FRAC_MASK			0x3FFF
+#define CCDC_LSC_CENTRE_MASK			0x3FFF
+#define CCDC_LSC_COEF_MASK			0xff
+#define CCDC_LSC_COEFL_SHIFT			0
+#define CCDC_LSC_COEFU_SHIFT			8
+#define CCDC_GAIN_MASK				0x7FF
+#define CCDC_SYNCEN_VDHDEN_MASK			(1 << 0)
+#define CCDC_SYNCEN_WEN_MASK			(1 << 1)
+#define CCDC_SYNCEN_WEN_SHIFT			1
+
+/* Power on Defaults in hardware */
+#define MODESET_DEFAULT				0x200
+#define CULH_DEFAULT				0xFFFF
+#define CULV_DEFAULT				0xFF
+#define GAIN_DEFAULT				256
+#define OUTCLIP_DEFAULT				0x3FFF
+#define LSCCFG2_DEFAULT				0xE
+
+#endif
diff --git a/drivers/media/platform/davinci/dm644x_ccdc.c b/drivers/media/platform/davinci/dm644x_ccdc.c
new file mode 100644
index 0000000..ffbefdf
--- /dev/null
+++ b/drivers/media/platform/davinci/dm644x_ccdc.c
@@ -0,0 +1,1038 @@
+/*
+ * Copyright (C) 2006-2009 Texas Instruments Inc
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ * CCDC hardware module for DM6446
+ * ------------------------------
+ *
+ * This module is for configuring CCD controller of DM6446 VPFE to capture
+ * Raw yuv or Bayer RGB data from a decoder. CCDC has several modules
+ * such as Defect Pixel Correction, Color Space Conversion etc to
+ * pre-process the Raw Bayer RGB data, before writing it to SDRAM. This
+ * module also allows application to configure individual
+ * module parameters through VPFE_CMD_S_CCDC_RAW_PARAMS IOCTL.
+ * To do so, application includes dm644x_ccdc.h and vpfe_capture.h header
+ * files.  The setparams() API is called by vpfe_capture driver
+ * to configure module parameters. This file is named DM644x so that other
+ * variants such DM6443 may be supported using the same module.
+ *
+ * TODO: Test Raw bayer parameter settings and bayer capture
+ * 	 Split module parameter structure to module specific ioctl structs
+ * 	 investigate if enum used for user space type definition
+ * 	 to be replaced by #defines or integer
+ */
+#include <linux/platform_device.h>
+#include <linux/uaccess.h>
+#include <linux/videodev2.h>
+#include <linux/gfp.h>
+#include <linux/err.h>
+#include <linux/module.h>
+
+#include <media/davinci/dm644x_ccdc.h>
+#include <media/davinci/vpss.h>
+
+#include "dm644x_ccdc_regs.h"
+#include "ccdc_hw_device.h"
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("CCDC Driver for DM6446");
+MODULE_AUTHOR("Texas Instruments");
+
+static struct ccdc_oper_config {
+	struct device *dev;
+	/* CCDC interface type */
+	enum vpfe_hw_if_type if_type;
+	/* Raw Bayer configuration */
+	struct ccdc_params_raw bayer;
+	/* YCbCr configuration */
+	struct ccdc_params_ycbcr ycbcr;
+	/* ccdc base address */
+	void __iomem *base_addr;
+} ccdc_cfg = {
+	/* Raw configurations */
+	.bayer = {
+		.pix_fmt = CCDC_PIXFMT_RAW,
+		.frm_fmt = CCDC_FRMFMT_PROGRESSIVE,
+		.win = CCDC_WIN_VGA,
+		.fid_pol = VPFE_PINPOL_POSITIVE,
+		.vd_pol = VPFE_PINPOL_POSITIVE,
+		.hd_pol = VPFE_PINPOL_POSITIVE,
+		.config_params = {
+			.data_sz = CCDC_DATA_10BITS,
+		},
+	},
+	.ycbcr = {
+		.pix_fmt = CCDC_PIXFMT_YCBCR_8BIT,
+		.frm_fmt = CCDC_FRMFMT_INTERLACED,
+		.win = CCDC_WIN_PAL,
+		.fid_pol = VPFE_PINPOL_POSITIVE,
+		.vd_pol = VPFE_PINPOL_POSITIVE,
+		.hd_pol = VPFE_PINPOL_POSITIVE,
+		.bt656_enable = 1,
+		.pix_order = CCDC_PIXORDER_CBYCRY,
+		.buf_type = CCDC_BUFTYPE_FLD_INTERLEAVED
+	},
+};
+
+#define CCDC_MAX_RAW_YUV_FORMATS	2
+
+/* Raw Bayer formats */
+static u32 ccdc_raw_bayer_pix_formats[] =
+	{V4L2_PIX_FMT_SBGGR8, V4L2_PIX_FMT_SBGGR16};
+
+/* Raw YUV formats */
+static u32 ccdc_raw_yuv_pix_formats[] =
+	{V4L2_PIX_FMT_UYVY, V4L2_PIX_FMT_YUYV};
+
+/* CCDC Save/Restore context */
+static u32 ccdc_ctx[CCDC_REG_END / sizeof(u32)];
+
+/* register access routines */
+static inline u32 regr(u32 offset)
+{
+	return __raw_readl(ccdc_cfg.base_addr + offset);
+}
+
+static inline void regw(u32 val, u32 offset)
+{
+	__raw_writel(val, ccdc_cfg.base_addr + offset);
+}
+
+static void ccdc_enable(int flag)
+{
+	regw(flag, CCDC_PCR);
+}
+
+static void ccdc_enable_vport(int flag)
+{
+	if (flag)
+		/* enable video port */
+		regw(CCDC_ENABLE_VIDEO_PORT, CCDC_FMTCFG);
+	else
+		regw(CCDC_DISABLE_VIDEO_PORT, CCDC_FMTCFG);
+}
+
+/*
+ * ccdc_setwin()
+ * This function will configure the window size
+ * to be capture in CCDC reg
+ */
+static void ccdc_setwin(struct v4l2_rect *image_win,
+			enum ccdc_frmfmt frm_fmt,
+			int ppc)
+{
+	int horz_start, horz_nr_pixels;
+	int vert_start, vert_nr_lines;
+	int val = 0, mid_img = 0;
+
+	dev_dbg(ccdc_cfg.dev, "\nStarting ccdc_setwin...");
+	/*
+	 * ppc - per pixel count. indicates how many pixels per cell
+	 * output to SDRAM. example, for ycbcr, it is one y and one c, so 2.
+	 * raw capture this is 1
+	 */
+	horz_start = image_win->left << (ppc - 1);
+	horz_nr_pixels = (image_win->width << (ppc - 1)) - 1;
+	regw((horz_start << CCDC_HORZ_INFO_SPH_SHIFT) | horz_nr_pixels,
+	     CCDC_HORZ_INFO);
+
+	vert_start = image_win->top;
+
+	if (frm_fmt == CCDC_FRMFMT_INTERLACED) {
+		vert_nr_lines = (image_win->height >> 1) - 1;
+		vert_start >>= 1;
+		/* Since first line doesn't have any data */
+		vert_start += 1;
+		/* configure VDINT0 */
+		val = (vert_start << CCDC_VDINT_VDINT0_SHIFT);
+		regw(val, CCDC_VDINT);
+
+	} else {
+		/* Since first line doesn't have any data */
+		vert_start += 1;
+		vert_nr_lines = image_win->height - 1;
+		/*
+		 * configure VDINT0 and VDINT1. VDINT1 will be at half
+		 * of image height
+		 */
+		mid_img = vert_start + (image_win->height / 2);
+		val = (vert_start << CCDC_VDINT_VDINT0_SHIFT) |
+		    (mid_img & CCDC_VDINT_VDINT1_MASK);
+		regw(val, CCDC_VDINT);
+
+	}
+	regw((vert_start << CCDC_VERT_START_SLV0_SHIFT) | vert_start,
+	     CCDC_VERT_START);
+	regw(vert_nr_lines, CCDC_VERT_LINES);
+	dev_dbg(ccdc_cfg.dev, "\nEnd of ccdc_setwin...");
+}
+
+static void ccdc_readregs(void)
+{
+	unsigned int val = 0;
+
+	val = regr(CCDC_ALAW);
+	dev_notice(ccdc_cfg.dev, "\nReading 0x%x to ALAW...\n", val);
+	val = regr(CCDC_CLAMP);
+	dev_notice(ccdc_cfg.dev, "\nReading 0x%x to CLAMP...\n", val);
+	val = regr(CCDC_DCSUB);
+	dev_notice(ccdc_cfg.dev, "\nReading 0x%x to DCSUB...\n", val);
+	val = regr(CCDC_BLKCMP);
+	dev_notice(ccdc_cfg.dev, "\nReading 0x%x to BLKCMP...\n", val);
+	val = regr(CCDC_FPC_ADDR);
+	dev_notice(ccdc_cfg.dev, "\nReading 0x%x to FPC_ADDR...\n", val);
+	val = regr(CCDC_FPC);
+	dev_notice(ccdc_cfg.dev, "\nReading 0x%x to FPC...\n", val);
+	val = regr(CCDC_FMTCFG);
+	dev_notice(ccdc_cfg.dev, "\nReading 0x%x to FMTCFG...\n", val);
+	val = regr(CCDC_COLPTN);
+	dev_notice(ccdc_cfg.dev, "\nReading 0x%x to COLPTN...\n", val);
+	val = regr(CCDC_FMT_HORZ);
+	dev_notice(ccdc_cfg.dev, "\nReading 0x%x to FMT_HORZ...\n", val);
+	val = regr(CCDC_FMT_VERT);
+	dev_notice(ccdc_cfg.dev, "\nReading 0x%x to FMT_VERT...\n", val);
+	val = regr(CCDC_HSIZE_OFF);
+	dev_notice(ccdc_cfg.dev, "\nReading 0x%x to HSIZE_OFF...\n", val);
+	val = regr(CCDC_SDOFST);
+	dev_notice(ccdc_cfg.dev, "\nReading 0x%x to SDOFST...\n", val);
+	val = regr(CCDC_VP_OUT);
+	dev_notice(ccdc_cfg.dev, "\nReading 0x%x to VP_OUT...\n", val);
+	val = regr(CCDC_SYN_MODE);
+	dev_notice(ccdc_cfg.dev, "\nReading 0x%x to SYN_MODE...\n", val);
+	val = regr(CCDC_HORZ_INFO);
+	dev_notice(ccdc_cfg.dev, "\nReading 0x%x to HORZ_INFO...\n", val);
+	val = regr(CCDC_VERT_START);
+	dev_notice(ccdc_cfg.dev, "\nReading 0x%x to VERT_START...\n", val);
+	val = regr(CCDC_VERT_LINES);
+	dev_notice(ccdc_cfg.dev, "\nReading 0x%x to VERT_LINES...\n", val);
+}
+
+static int validate_ccdc_param(struct ccdc_config_params_raw *ccdcparam)
+{
+	if (ccdcparam->alaw.enable) {
+		u8 max_gamma = ccdc_gamma_width_max_bit(ccdcparam->alaw.gamma_wd);
+		u8 max_data = ccdc_data_size_max_bit(ccdcparam->data_sz);
+
+		if ((ccdcparam->alaw.gamma_wd > CCDC_GAMMA_BITS_09_0) ||
+		    (ccdcparam->alaw.gamma_wd < CCDC_GAMMA_BITS_15_6) ||
+		    (max_gamma > max_data)) {
+			dev_dbg(ccdc_cfg.dev, "\nInvalid data line select");
+			return -1;
+		}
+	}
+	return 0;
+}
+
+static int ccdc_update_raw_params(struct ccdc_config_params_raw *raw_params)
+{
+	struct ccdc_config_params_raw *config_params =
+				&ccdc_cfg.bayer.config_params;
+	unsigned int *fpc_virtaddr = NULL;
+	unsigned int *fpc_physaddr = NULL;
+
+	memcpy(config_params, raw_params, sizeof(*raw_params));
+	/*
+	 * allocate memory for fault pixel table and copy the user
+	 * values to the table
+	 */
+	if (!config_params->fault_pxl.enable)
+		return 0;
+
+	fpc_physaddr = (unsigned int *)config_params->fault_pxl.fpc_table_addr;
+	fpc_virtaddr = (unsigned int *)phys_to_virt(
+				(unsigned long)fpc_physaddr);
+	/*
+	 * Allocate memory for FPC table if current
+	 * FPC table buffer is not big enough to
+	 * accommodate FPC Number requested
+	 */
+	if (raw_params->fault_pxl.fp_num != config_params->fault_pxl.fp_num) {
+		if (fpc_physaddr != NULL) {
+			free_pages((unsigned long)fpc_physaddr,
+				   get_order
+				   (config_params->fault_pxl.fp_num *
+				   FP_NUM_BYTES));
+		}
+
+		/* Allocate memory for FPC table */
+		fpc_virtaddr =
+			(unsigned int *)__get_free_pages(GFP_KERNEL | GFP_DMA,
+							 get_order(raw_params->
+							 fault_pxl.fp_num *
+							 FP_NUM_BYTES));
+
+		if (fpc_virtaddr == NULL) {
+			dev_dbg(ccdc_cfg.dev,
+				"\nUnable to allocate memory for FPC");
+			return -EFAULT;
+		}
+		fpc_physaddr =
+		    (unsigned int *)virt_to_phys((void *)fpc_virtaddr);
+	}
+
+	/* Copy number of fault pixels and FPC table */
+	config_params->fault_pxl.fp_num = raw_params->fault_pxl.fp_num;
+	if (copy_from_user(fpc_virtaddr,
+			(void __user *)raw_params->fault_pxl.fpc_table_addr,
+			config_params->fault_pxl.fp_num * FP_NUM_BYTES)) {
+		dev_dbg(ccdc_cfg.dev, "\n copy_from_user failed");
+		return -EFAULT;
+	}
+	config_params->fault_pxl.fpc_table_addr = (unsigned long)fpc_physaddr;
+	return 0;
+}
+
+static int ccdc_close(struct device *dev)
+{
+	struct ccdc_config_params_raw *config_params =
+				&ccdc_cfg.bayer.config_params;
+	unsigned int *fpc_physaddr = NULL, *fpc_virtaddr = NULL;
+
+	fpc_physaddr = (unsigned int *)config_params->fault_pxl.fpc_table_addr;
+
+	if (fpc_physaddr != NULL) {
+		fpc_virtaddr = (unsigned int *)
+		    phys_to_virt((unsigned long)fpc_physaddr);
+		free_pages((unsigned long)fpc_virtaddr,
+			   get_order(config_params->fault_pxl.fp_num *
+			   FP_NUM_BYTES));
+	}
+	return 0;
+}
+
+/*
+ * ccdc_restore_defaults()
+ * This function will write defaults to all CCDC registers
+ */
+static void ccdc_restore_defaults(void)
+{
+	int i;
+
+	/* disable CCDC */
+	ccdc_enable(0);
+	/* set all registers to default value */
+	for (i = 4; i <= 0x94; i += 4)
+		regw(0,  i);
+	regw(CCDC_NO_CULLING, CCDC_CULLING);
+	regw(CCDC_GAMMA_BITS_11_2, CCDC_ALAW);
+}
+
+static int ccdc_open(struct device *device)
+{
+	ccdc_restore_defaults();
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		ccdc_enable_vport(1);
+	return 0;
+}
+
+static void ccdc_sbl_reset(void)
+{
+	vpss_clear_wbl_overflow(VPSS_PCR_CCDC_WBL_O);
+}
+
+/* Parameter operations */
+static int ccdc_set_params(void __user *params)
+{
+	struct ccdc_config_params_raw ccdc_raw_params;
+	int x;
+
+	if (ccdc_cfg.if_type != VPFE_RAW_BAYER)
+		return -EINVAL;
+
+	x = copy_from_user(&ccdc_raw_params, params, sizeof(ccdc_raw_params));
+	if (x) {
+		dev_dbg(ccdc_cfg.dev, "ccdc_set_params: error in copying"
+			   "ccdc params, %d\n", x);
+		return -EFAULT;
+	}
+
+	if (!validate_ccdc_param(&ccdc_raw_params)) {
+		if (!ccdc_update_raw_params(&ccdc_raw_params))
+			return 0;
+	}
+	return -EINVAL;
+}
+
+/*
+ * ccdc_config_ycbcr()
+ * This function will configure CCDC for YCbCr video capture
+ */
+static void ccdc_config_ycbcr(void)
+{
+	struct ccdc_params_ycbcr *params = &ccdc_cfg.ycbcr;
+	u32 syn_mode;
+
+	dev_dbg(ccdc_cfg.dev, "\nStarting ccdc_config_ycbcr...");
+	/*
+	 * first restore the CCDC registers to default values
+	 * This is important since we assume default values to be set in
+	 * a lot of registers that we didn't touch
+	 */
+	ccdc_restore_defaults();
+
+	/*
+	 * configure pixel format, frame format, configure video frame
+	 * format, enable output to SDRAM, enable internal timing generator
+	 * and 8bit pack mode
+	 */
+	syn_mode = (((params->pix_fmt & CCDC_SYN_MODE_INPMOD_MASK) <<
+		    CCDC_SYN_MODE_INPMOD_SHIFT) |
+		    ((params->frm_fmt & CCDC_SYN_FLDMODE_MASK) <<
+		    CCDC_SYN_FLDMODE_SHIFT) | CCDC_VDHDEN_ENABLE |
+		    CCDC_WEN_ENABLE | CCDC_DATA_PACK_ENABLE);
+
+	/* setup BT.656 sync mode */
+	if (params->bt656_enable) {
+		regw(CCDC_REC656IF_BT656_EN, CCDC_REC656IF);
+
+		/*
+		 * configure the FID, VD, HD pin polarity,
+		 * fld,hd pol positive, vd negative, 8-bit data
+		 */
+		syn_mode |= CCDC_SYN_MODE_VD_POL_NEGATIVE;
+		if (ccdc_cfg.if_type == VPFE_BT656_10BIT)
+			syn_mode |= CCDC_SYN_MODE_10BITS;
+		else
+			syn_mode |= CCDC_SYN_MODE_8BITS;
+	} else {
+		/* y/c external sync mode */
+		syn_mode |= (((params->fid_pol & CCDC_FID_POL_MASK) <<
+			     CCDC_FID_POL_SHIFT) |
+			     ((params->hd_pol & CCDC_HD_POL_MASK) <<
+			     CCDC_HD_POL_SHIFT) |
+			     ((params->vd_pol & CCDC_VD_POL_MASK) <<
+			     CCDC_VD_POL_SHIFT));
+	}
+	regw(syn_mode, CCDC_SYN_MODE);
+
+	/* configure video window */
+	ccdc_setwin(&params->win, params->frm_fmt, 2);
+
+	/*
+	 * configure the order of y cb cr in SDRAM, and disable latch
+	 * internal register on vsync
+	 */
+	if (ccdc_cfg.if_type == VPFE_BT656_10BIT)
+		regw((params->pix_order << CCDC_CCDCFG_Y8POS_SHIFT) |
+			CCDC_LATCH_ON_VSYNC_DISABLE | CCDC_CCDCFG_BW656_10BIT,
+			CCDC_CCDCFG);
+	else
+		regw((params->pix_order << CCDC_CCDCFG_Y8POS_SHIFT) |
+			CCDC_LATCH_ON_VSYNC_DISABLE, CCDC_CCDCFG);
+
+	/*
+	 * configure the horizontal line offset. This should be a
+	 * on 32 byte boundary. So clear LSB 5 bits
+	 */
+	regw(((params->win.width * 2  + 31) & ~0x1f), CCDC_HSIZE_OFF);
+
+	/* configure the memory line offset */
+	if (params->buf_type == CCDC_BUFTYPE_FLD_INTERLEAVED)
+		/* two fields are interleaved in memory */
+		regw(CCDC_SDOFST_FIELD_INTERLEAVED, CCDC_SDOFST);
+
+	ccdc_sbl_reset();
+	dev_dbg(ccdc_cfg.dev, "\nEnd of ccdc_config_ycbcr...\n");
+}
+
+static void ccdc_config_black_clamp(struct ccdc_black_clamp *bclamp)
+{
+	u32 val;
+
+	if (!bclamp->enable) {
+		/* configure DCSub */
+		val = (bclamp->dc_sub) & CCDC_BLK_DC_SUB_MASK;
+		regw(val, CCDC_DCSUB);
+		dev_dbg(ccdc_cfg.dev, "\nWriting 0x%x to DCSUB...\n", val);
+		regw(CCDC_CLAMP_DEFAULT_VAL, CCDC_CLAMP);
+		dev_dbg(ccdc_cfg.dev, "\nWriting 0x0000 to CLAMP...\n");
+		return;
+	}
+	/*
+	 * Configure gain,  Start pixel, No of line to be avg,
+	 * No of pixel/line to be avg, & Enable the Black clamping
+	 */
+	val = ((bclamp->sgain & CCDC_BLK_SGAIN_MASK) |
+	       ((bclamp->start_pixel & CCDC_BLK_ST_PXL_MASK) <<
+		CCDC_BLK_ST_PXL_SHIFT) |
+	       ((bclamp->sample_ln & CCDC_BLK_SAMPLE_LINE_MASK) <<
+		CCDC_BLK_SAMPLE_LINE_SHIFT) |
+	       ((bclamp->sample_pixel & CCDC_BLK_SAMPLE_LN_MASK) <<
+		CCDC_BLK_SAMPLE_LN_SHIFT) | CCDC_BLK_CLAMP_ENABLE);
+	regw(val, CCDC_CLAMP);
+	dev_dbg(ccdc_cfg.dev, "\nWriting 0x%x to CLAMP...\n", val);
+	/* If Black clamping is enable then make dcsub 0 */
+	regw(CCDC_DCSUB_DEFAULT_VAL, CCDC_DCSUB);
+	dev_dbg(ccdc_cfg.dev, "\nWriting 0x00000000 to DCSUB...\n");
+}
+
+static void ccdc_config_black_compense(struct ccdc_black_compensation *bcomp)
+{
+	u32 val;
+
+	val = ((bcomp->b & CCDC_BLK_COMP_MASK) |
+	      ((bcomp->gb & CCDC_BLK_COMP_MASK) <<
+	       CCDC_BLK_COMP_GB_COMP_SHIFT) |
+	      ((bcomp->gr & CCDC_BLK_COMP_MASK) <<
+	       CCDC_BLK_COMP_GR_COMP_SHIFT) |
+	      ((bcomp->r & CCDC_BLK_COMP_MASK) <<
+	       CCDC_BLK_COMP_R_COMP_SHIFT));
+	regw(val, CCDC_BLKCMP);
+}
+
+static void ccdc_config_fpc(struct ccdc_fault_pixel *fpc)
+{
+	u32 val;
+
+	/* Initially disable FPC */
+	val = CCDC_FPC_DISABLE;
+	regw(val, CCDC_FPC);
+
+	if (!fpc->enable)
+		return;
+
+	/* Configure Fault pixel if needed */
+	regw(fpc->fpc_table_addr, CCDC_FPC_ADDR);
+	dev_dbg(ccdc_cfg.dev, "\nWriting 0x%lx to FPC_ADDR...\n",
+		       (fpc->fpc_table_addr));
+	/* Write the FPC params with FPC disable */
+	val = fpc->fp_num & CCDC_FPC_FPC_NUM_MASK;
+	regw(val, CCDC_FPC);
+
+	dev_dbg(ccdc_cfg.dev, "\nWriting 0x%x to FPC...\n", val);
+	/* read the FPC register */
+	val = regr(CCDC_FPC) | CCDC_FPC_ENABLE;
+	regw(val, CCDC_FPC);
+	dev_dbg(ccdc_cfg.dev, "\nWriting 0x%x to FPC...\n", val);
+}
+
+/*
+ * ccdc_config_raw()
+ * This function will configure CCDC for Raw capture mode
+ */
+static void ccdc_config_raw(void)
+{
+	struct ccdc_params_raw *params = &ccdc_cfg.bayer;
+	struct ccdc_config_params_raw *config_params =
+				&ccdc_cfg.bayer.config_params;
+	unsigned int syn_mode = 0;
+	unsigned int val;
+
+	dev_dbg(ccdc_cfg.dev, "\nStarting ccdc_config_raw...");
+
+	/*      Reset CCDC */
+	ccdc_restore_defaults();
+
+	/* Disable latching function registers on VSYNC  */
+	regw(CCDC_LATCH_ON_VSYNC_DISABLE, CCDC_CCDCFG);
+
+	/*
+	 * Configure the vertical sync polarity(SYN_MODE.VDPOL),
+	 * horizontal sync polarity (SYN_MODE.HDPOL), frame id polarity
+	 * (SYN_MODE.FLDPOL), frame format(progressive or interlace),
+	 * data size(SYNMODE.DATSIZ), &pixel format (Input mode), output
+	 * SDRAM, enable internal timing generator
+	 */
+	syn_mode =
+		(((params->vd_pol & CCDC_VD_POL_MASK) << CCDC_VD_POL_SHIFT) |
+		((params->hd_pol & CCDC_HD_POL_MASK) << CCDC_HD_POL_SHIFT) |
+		((params->fid_pol & CCDC_FID_POL_MASK) << CCDC_FID_POL_SHIFT) |
+		((params->frm_fmt & CCDC_FRM_FMT_MASK) << CCDC_FRM_FMT_SHIFT) |
+		((config_params->data_sz & CCDC_DATA_SZ_MASK) <<
+		CCDC_DATA_SZ_SHIFT) |
+		((params->pix_fmt & CCDC_PIX_FMT_MASK) << CCDC_PIX_FMT_SHIFT) |
+		CCDC_WEN_ENABLE | CCDC_VDHDEN_ENABLE);
+
+	/* Enable and configure aLaw register if needed */
+	if (config_params->alaw.enable) {
+		val = ((config_params->alaw.gamma_wd &
+		      CCDC_ALAW_GAMMA_WD_MASK) | CCDC_ALAW_ENABLE);
+		regw(val, CCDC_ALAW);
+		dev_dbg(ccdc_cfg.dev, "\nWriting 0x%x to ALAW...\n", val);
+	}
+
+	/* Configure video window */
+	ccdc_setwin(&params->win, params->frm_fmt, CCDC_PPC_RAW);
+
+	/* Configure Black Clamp */
+	ccdc_config_black_clamp(&config_params->blk_clamp);
+
+	/* Configure Black level compensation */
+	ccdc_config_black_compense(&config_params->blk_comp);
+
+	/* Configure Fault Pixel Correction */
+	ccdc_config_fpc(&config_params->fault_pxl);
+
+	/* If data size is 8 bit then pack the data */
+	if ((config_params->data_sz == CCDC_DATA_8BITS) ||
+	     config_params->alaw.enable)
+		syn_mode |= CCDC_DATA_PACK_ENABLE;
+
+	/* disable video port */
+	val = CCDC_DISABLE_VIDEO_PORT;
+
+	if (config_params->data_sz == CCDC_DATA_8BITS)
+		val |= (CCDC_DATA_10BITS & CCDC_FMTCFG_VPIN_MASK)
+		    << CCDC_FMTCFG_VPIN_SHIFT;
+	else
+		val |= (config_params->data_sz & CCDC_FMTCFG_VPIN_MASK)
+		    << CCDC_FMTCFG_VPIN_SHIFT;
+	/* Write value in FMTCFG */
+	regw(val, CCDC_FMTCFG);
+
+	dev_dbg(ccdc_cfg.dev, "\nWriting 0x%x to FMTCFG...\n", val);
+	/* Configure the color pattern according to mt9t001 sensor */
+	regw(CCDC_COLPTN_VAL, CCDC_COLPTN);
+
+	dev_dbg(ccdc_cfg.dev, "\nWriting 0xBB11BB11 to COLPTN...\n");
+	/*
+	 * Configure Data formatter(Video port) pixel selection
+	 * (FMT_HORZ, FMT_VERT)
+	 */
+	val = ((params->win.left & CCDC_FMT_HORZ_FMTSPH_MASK) <<
+	      CCDC_FMT_HORZ_FMTSPH_SHIFT) |
+	      (params->win.width & CCDC_FMT_HORZ_FMTLNH_MASK);
+	regw(val, CCDC_FMT_HORZ);
+
+	dev_dbg(ccdc_cfg.dev, "\nWriting 0x%x to FMT_HORZ...\n", val);
+	val = (params->win.top & CCDC_FMT_VERT_FMTSLV_MASK)
+	    << CCDC_FMT_VERT_FMTSLV_SHIFT;
+	if (params->frm_fmt == CCDC_FRMFMT_PROGRESSIVE)
+		val |= (params->win.height) & CCDC_FMT_VERT_FMTLNV_MASK;
+	else
+		val |= (params->win.height >> 1) & CCDC_FMT_VERT_FMTLNV_MASK;
+
+	dev_dbg(ccdc_cfg.dev, "\nparams->win.height  0x%x ...\n",
+	       params->win.height);
+	regw(val, CCDC_FMT_VERT);
+
+	dev_dbg(ccdc_cfg.dev, "\nWriting 0x%x to FMT_VERT...\n", val);
+
+	dev_dbg(ccdc_cfg.dev, "\nbelow regw(val, FMT_VERT)...");
+
+	/*
+	 * Configure Horizontal offset register. If pack 8 is enabled then
+	 * 1 pixel will take 1 byte
+	 */
+	if ((config_params->data_sz == CCDC_DATA_8BITS) ||
+	    config_params->alaw.enable)
+		regw((params->win.width + CCDC_32BYTE_ALIGN_VAL) &
+		    CCDC_HSIZE_OFF_MASK, CCDC_HSIZE_OFF);
+	else
+		/* else one pixel will take 2 byte */
+		regw(((params->win.width * CCDC_TWO_BYTES_PER_PIXEL) +
+		    CCDC_32BYTE_ALIGN_VAL) & CCDC_HSIZE_OFF_MASK,
+		    CCDC_HSIZE_OFF);
+
+	/* Set value for SDOFST */
+	if (params->frm_fmt == CCDC_FRMFMT_INTERLACED) {
+		if (params->image_invert_enable) {
+			/* For intelace inverse mode */
+			regw(CCDC_INTERLACED_IMAGE_INVERT, CCDC_SDOFST);
+			dev_dbg(ccdc_cfg.dev, "\nWriting 0x4B6D to SDOFST..\n");
+		}
+
+		else {
+			/* For intelace non inverse mode */
+			regw(CCDC_INTERLACED_NO_IMAGE_INVERT, CCDC_SDOFST);
+			dev_dbg(ccdc_cfg.dev, "\nWriting 0x0249 to SDOFST..\n");
+		}
+	} else if (params->frm_fmt == CCDC_FRMFMT_PROGRESSIVE) {
+		regw(CCDC_PROGRESSIVE_NO_IMAGE_INVERT, CCDC_SDOFST);
+		dev_dbg(ccdc_cfg.dev, "\nWriting 0x0000 to SDOFST...\n");
+	}
+
+	/*
+	 * Configure video port pixel selection (VPOUT)
+	 * Here -1 is to make the height value less than FMT_VERT.FMTLNV
+	 */
+	if (params->frm_fmt == CCDC_FRMFMT_PROGRESSIVE)
+		val = (((params->win.height - 1) & CCDC_VP_OUT_VERT_NUM_MASK))
+		    << CCDC_VP_OUT_VERT_NUM_SHIFT;
+	else
+		val =
+		    ((((params->win.height >> CCDC_INTERLACED_HEIGHT_SHIFT) -
+		     1) & CCDC_VP_OUT_VERT_NUM_MASK)) <<
+		    CCDC_VP_OUT_VERT_NUM_SHIFT;
+
+	val |= ((((params->win.width))) & CCDC_VP_OUT_HORZ_NUM_MASK)
+	    << CCDC_VP_OUT_HORZ_NUM_SHIFT;
+	val |= (params->win.left) & CCDC_VP_OUT_HORZ_ST_MASK;
+	regw(val, CCDC_VP_OUT);
+
+	dev_dbg(ccdc_cfg.dev, "\nWriting 0x%x to VP_OUT...\n", val);
+	regw(syn_mode, CCDC_SYN_MODE);
+	dev_dbg(ccdc_cfg.dev, "\nWriting 0x%x to SYN_MODE...\n", syn_mode);
+
+	ccdc_sbl_reset();
+	dev_dbg(ccdc_cfg.dev, "\nend of ccdc_config_raw...");
+	ccdc_readregs();
+}
+
+static int ccdc_configure(void)
+{
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		ccdc_config_raw();
+	else
+		ccdc_config_ycbcr();
+	return 0;
+}
+
+static int ccdc_set_buftype(enum ccdc_buftype buf_type)
+{
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		ccdc_cfg.bayer.buf_type = buf_type;
+	else
+		ccdc_cfg.ycbcr.buf_type = buf_type;
+	return 0;
+}
+
+static enum ccdc_buftype ccdc_get_buftype(void)
+{
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		return ccdc_cfg.bayer.buf_type;
+	return ccdc_cfg.ycbcr.buf_type;
+}
+
+static int ccdc_enum_pix(u32 *pix, int i)
+{
+	int ret = -EINVAL;
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER) {
+		if (i < ARRAY_SIZE(ccdc_raw_bayer_pix_formats)) {
+			*pix = ccdc_raw_bayer_pix_formats[i];
+			ret = 0;
+		}
+	} else {
+		if (i < ARRAY_SIZE(ccdc_raw_yuv_pix_formats)) {
+			*pix = ccdc_raw_yuv_pix_formats[i];
+			ret = 0;
+		}
+	}
+	return ret;
+}
+
+static int ccdc_set_pixel_format(u32 pixfmt)
+{
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER) {
+		ccdc_cfg.bayer.pix_fmt = CCDC_PIXFMT_RAW;
+		if (pixfmt == V4L2_PIX_FMT_SBGGR8)
+			ccdc_cfg.bayer.config_params.alaw.enable = 1;
+		else if (pixfmt != V4L2_PIX_FMT_SBGGR16)
+			return -EINVAL;
+	} else {
+		if (pixfmt == V4L2_PIX_FMT_YUYV)
+			ccdc_cfg.ycbcr.pix_order = CCDC_PIXORDER_YCBYCR;
+		else if (pixfmt == V4L2_PIX_FMT_UYVY)
+			ccdc_cfg.ycbcr.pix_order = CCDC_PIXORDER_CBYCRY;
+		else
+			return -EINVAL;
+	}
+	return 0;
+}
+
+static u32 ccdc_get_pixel_format(void)
+{
+	struct ccdc_a_law *alaw = &ccdc_cfg.bayer.config_params.alaw;
+	u32 pixfmt;
+
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		if (alaw->enable)
+			pixfmt = V4L2_PIX_FMT_SBGGR8;
+		else
+			pixfmt = V4L2_PIX_FMT_SBGGR16;
+	else {
+		if (ccdc_cfg.ycbcr.pix_order == CCDC_PIXORDER_YCBYCR)
+			pixfmt = V4L2_PIX_FMT_YUYV;
+		else
+			pixfmt = V4L2_PIX_FMT_UYVY;
+	}
+	return pixfmt;
+}
+
+static int ccdc_set_image_window(struct v4l2_rect *win)
+{
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		ccdc_cfg.bayer.win = *win;
+	else
+		ccdc_cfg.ycbcr.win = *win;
+	return 0;
+}
+
+static void ccdc_get_image_window(struct v4l2_rect *win)
+{
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		*win = ccdc_cfg.bayer.win;
+	else
+		*win = ccdc_cfg.ycbcr.win;
+}
+
+static unsigned int ccdc_get_line_length(void)
+{
+	struct ccdc_config_params_raw *config_params =
+				&ccdc_cfg.bayer.config_params;
+	unsigned int len;
+
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER) {
+		if ((config_params->alaw.enable) ||
+		    (config_params->data_sz == CCDC_DATA_8BITS))
+			len = ccdc_cfg.bayer.win.width;
+		else
+			len = ccdc_cfg.bayer.win.width * 2;
+	} else
+		len = ccdc_cfg.ycbcr.win.width * 2;
+	return ALIGN(len, 32);
+}
+
+static int ccdc_set_frame_format(enum ccdc_frmfmt frm_fmt)
+{
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		ccdc_cfg.bayer.frm_fmt = frm_fmt;
+	else
+		ccdc_cfg.ycbcr.frm_fmt = frm_fmt;
+	return 0;
+}
+
+static enum ccdc_frmfmt ccdc_get_frame_format(void)
+{
+	if (ccdc_cfg.if_type == VPFE_RAW_BAYER)
+		return ccdc_cfg.bayer.frm_fmt;
+	else
+		return ccdc_cfg.ycbcr.frm_fmt;
+}
+
+static int ccdc_getfid(void)
+{
+	return (regr(CCDC_SYN_MODE) >> 15) & 1;
+}
+
+/* misc operations */
+static inline void ccdc_setfbaddr(unsigned long addr)
+{
+	regw(addr & 0xffffffe0, CCDC_SDR_ADDR);
+}
+
+static int ccdc_set_hw_if_params(struct vpfe_hw_if_param *params)
+{
+	ccdc_cfg.if_type = params->if_type;
+
+	switch (params->if_type) {
+	case VPFE_BT656:
+	case VPFE_YCBCR_SYNC_16:
+	case VPFE_YCBCR_SYNC_8:
+	case VPFE_BT656_10BIT:
+		ccdc_cfg.ycbcr.vd_pol = params->vdpol;
+		ccdc_cfg.ycbcr.hd_pol = params->hdpol;
+		break;
+	default:
+		/* TODO add support for raw bayer here */
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static void ccdc_save_context(void)
+{
+	ccdc_ctx[CCDC_PCR >> 2] = regr(CCDC_PCR);
+	ccdc_ctx[CCDC_SYN_MODE >> 2] = regr(CCDC_SYN_MODE);
+	ccdc_ctx[CCDC_HD_VD_WID >> 2] = regr(CCDC_HD_VD_WID);
+	ccdc_ctx[CCDC_PIX_LINES >> 2] = regr(CCDC_PIX_LINES);
+	ccdc_ctx[CCDC_HORZ_INFO >> 2] = regr(CCDC_HORZ_INFO);
+	ccdc_ctx[CCDC_VERT_START >> 2] = regr(CCDC_VERT_START);
+	ccdc_ctx[CCDC_VERT_LINES >> 2] = regr(CCDC_VERT_LINES);
+	ccdc_ctx[CCDC_CULLING >> 2] = regr(CCDC_CULLING);
+	ccdc_ctx[CCDC_HSIZE_OFF >> 2] = regr(CCDC_HSIZE_OFF);
+	ccdc_ctx[CCDC_SDOFST >> 2] = regr(CCDC_SDOFST);
+	ccdc_ctx[CCDC_SDR_ADDR >> 2] = regr(CCDC_SDR_ADDR);
+	ccdc_ctx[CCDC_CLAMP >> 2] = regr(CCDC_CLAMP);
+	ccdc_ctx[CCDC_DCSUB >> 2] = regr(CCDC_DCSUB);
+	ccdc_ctx[CCDC_COLPTN >> 2] = regr(CCDC_COLPTN);
+	ccdc_ctx[CCDC_BLKCMP >> 2] = regr(CCDC_BLKCMP);
+	ccdc_ctx[CCDC_FPC >> 2] = regr(CCDC_FPC);
+	ccdc_ctx[CCDC_FPC_ADDR >> 2] = regr(CCDC_FPC_ADDR);
+	ccdc_ctx[CCDC_VDINT >> 2] = regr(CCDC_VDINT);
+	ccdc_ctx[CCDC_ALAW >> 2] = regr(CCDC_ALAW);
+	ccdc_ctx[CCDC_REC656IF >> 2] = regr(CCDC_REC656IF);
+	ccdc_ctx[CCDC_CCDCFG >> 2] = regr(CCDC_CCDCFG);
+	ccdc_ctx[CCDC_FMTCFG >> 2] = regr(CCDC_FMTCFG);
+	ccdc_ctx[CCDC_FMT_HORZ >> 2] = regr(CCDC_FMT_HORZ);
+	ccdc_ctx[CCDC_FMT_VERT >> 2] = regr(CCDC_FMT_VERT);
+	ccdc_ctx[CCDC_FMT_ADDR0 >> 2] = regr(CCDC_FMT_ADDR0);
+	ccdc_ctx[CCDC_FMT_ADDR1 >> 2] = regr(CCDC_FMT_ADDR1);
+	ccdc_ctx[CCDC_FMT_ADDR2 >> 2] = regr(CCDC_FMT_ADDR2);
+	ccdc_ctx[CCDC_FMT_ADDR3 >> 2] = regr(CCDC_FMT_ADDR3);
+	ccdc_ctx[CCDC_FMT_ADDR4 >> 2] = regr(CCDC_FMT_ADDR4);
+	ccdc_ctx[CCDC_FMT_ADDR5 >> 2] = regr(CCDC_FMT_ADDR5);
+	ccdc_ctx[CCDC_FMT_ADDR6 >> 2] = regr(CCDC_FMT_ADDR6);
+	ccdc_ctx[CCDC_FMT_ADDR7 >> 2] = regr(CCDC_FMT_ADDR7);
+	ccdc_ctx[CCDC_PRGEVEN_0 >> 2] = regr(CCDC_PRGEVEN_0);
+	ccdc_ctx[CCDC_PRGEVEN_1 >> 2] = regr(CCDC_PRGEVEN_1);
+	ccdc_ctx[CCDC_PRGODD_0 >> 2] = regr(CCDC_PRGODD_0);
+	ccdc_ctx[CCDC_PRGODD_1 >> 2] = regr(CCDC_PRGODD_1);
+	ccdc_ctx[CCDC_VP_OUT >> 2] = regr(CCDC_VP_OUT);
+}
+
+static void ccdc_restore_context(void)
+{
+	regw(ccdc_ctx[CCDC_SYN_MODE >> 2], CCDC_SYN_MODE);
+	regw(ccdc_ctx[CCDC_HD_VD_WID >> 2], CCDC_HD_VD_WID);
+	regw(ccdc_ctx[CCDC_PIX_LINES >> 2], CCDC_PIX_LINES);
+	regw(ccdc_ctx[CCDC_HORZ_INFO >> 2], CCDC_HORZ_INFO);
+	regw(ccdc_ctx[CCDC_VERT_START >> 2], CCDC_VERT_START);
+	regw(ccdc_ctx[CCDC_VERT_LINES >> 2], CCDC_VERT_LINES);
+	regw(ccdc_ctx[CCDC_CULLING >> 2], CCDC_CULLING);
+	regw(ccdc_ctx[CCDC_HSIZE_OFF >> 2], CCDC_HSIZE_OFF);
+	regw(ccdc_ctx[CCDC_SDOFST >> 2], CCDC_SDOFST);
+	regw(ccdc_ctx[CCDC_SDR_ADDR >> 2], CCDC_SDR_ADDR);
+	regw(ccdc_ctx[CCDC_CLAMP >> 2], CCDC_CLAMP);
+	regw(ccdc_ctx[CCDC_DCSUB >> 2], CCDC_DCSUB);
+	regw(ccdc_ctx[CCDC_COLPTN >> 2], CCDC_COLPTN);
+	regw(ccdc_ctx[CCDC_BLKCMP >> 2], CCDC_BLKCMP);
+	regw(ccdc_ctx[CCDC_FPC >> 2], CCDC_FPC);
+	regw(ccdc_ctx[CCDC_FPC_ADDR >> 2], CCDC_FPC_ADDR);
+	regw(ccdc_ctx[CCDC_VDINT >> 2], CCDC_VDINT);
+	regw(ccdc_ctx[CCDC_ALAW >> 2], CCDC_ALAW);
+	regw(ccdc_ctx[CCDC_REC656IF >> 2], CCDC_REC656IF);
+	regw(ccdc_ctx[CCDC_CCDCFG >> 2], CCDC_CCDCFG);
+	regw(ccdc_ctx[CCDC_FMTCFG >> 2], CCDC_FMTCFG);
+	regw(ccdc_ctx[CCDC_FMT_HORZ >> 2], CCDC_FMT_HORZ);
+	regw(ccdc_ctx[CCDC_FMT_VERT >> 2], CCDC_FMT_VERT);
+	regw(ccdc_ctx[CCDC_FMT_ADDR0 >> 2], CCDC_FMT_ADDR0);
+	regw(ccdc_ctx[CCDC_FMT_ADDR1 >> 2], CCDC_FMT_ADDR1);
+	regw(ccdc_ctx[CCDC_FMT_ADDR2 >> 2], CCDC_FMT_ADDR2);
+	regw(ccdc_ctx[CCDC_FMT_ADDR3 >> 2], CCDC_FMT_ADDR3);
+	regw(ccdc_ctx[CCDC_FMT_ADDR4 >> 2], CCDC_FMT_ADDR4);
+	regw(ccdc_ctx[CCDC_FMT_ADDR5 >> 2], CCDC_FMT_ADDR5);
+	regw(ccdc_ctx[CCDC_FMT_ADDR6 >> 2], CCDC_FMT_ADDR6);
+	regw(ccdc_ctx[CCDC_FMT_ADDR7 >> 2], CCDC_FMT_ADDR7);
+	regw(ccdc_ctx[CCDC_PRGEVEN_0 >> 2], CCDC_PRGEVEN_0);
+	regw(ccdc_ctx[CCDC_PRGEVEN_1 >> 2], CCDC_PRGEVEN_1);
+	regw(ccdc_ctx[CCDC_PRGODD_0 >> 2], CCDC_PRGODD_0);
+	regw(ccdc_ctx[CCDC_PRGODD_1 >> 2], CCDC_PRGODD_1);
+	regw(ccdc_ctx[CCDC_VP_OUT >> 2], CCDC_VP_OUT);
+	regw(ccdc_ctx[CCDC_PCR >> 2], CCDC_PCR);
+}
+static struct ccdc_hw_device ccdc_hw_dev = {
+	.name = "DM6446 CCDC",
+	.owner = THIS_MODULE,
+	.hw_ops = {
+		.open = ccdc_open,
+		.close = ccdc_close,
+		.reset = ccdc_sbl_reset,
+		.enable = ccdc_enable,
+		.set_hw_if_params = ccdc_set_hw_if_params,
+		.set_params = ccdc_set_params,
+		.configure = ccdc_configure,
+		.set_buftype = ccdc_set_buftype,
+		.get_buftype = ccdc_get_buftype,
+		.enum_pix = ccdc_enum_pix,
+		.set_pixel_format = ccdc_set_pixel_format,
+		.get_pixel_format = ccdc_get_pixel_format,
+		.set_frame_format = ccdc_set_frame_format,
+		.get_frame_format = ccdc_get_frame_format,
+		.set_image_window = ccdc_set_image_window,
+		.get_image_window = ccdc_get_image_window,
+		.get_line_length = ccdc_get_line_length,
+		.setfbaddr = ccdc_setfbaddr,
+		.getfid = ccdc_getfid,
+	},
+};
+
+static int dm644x_ccdc_probe(struct platform_device *pdev)
+{
+	struct resource	*res;
+	int status = 0;
+
+	/*
+	 * first try to register with vpfe. If not correct platform, then we
+	 * don't have to iomap
+	 */
+	status = vpfe_register_ccdc_device(&ccdc_hw_dev);
+	if (status < 0)
+		return status;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		status = -ENODEV;
+		goto fail_nores;
+	}
+
+	res = request_mem_region(res->start, resource_size(res), res->name);
+	if (!res) {
+		status = -EBUSY;
+		goto fail_nores;
+	}
+
+	ccdc_cfg.base_addr = ioremap_nocache(res->start, resource_size(res));
+	if (!ccdc_cfg.base_addr) {
+		status = -ENOMEM;
+		goto fail_nomem;
+	}
+
+	ccdc_cfg.dev = &pdev->dev;
+	printk(KERN_NOTICE "%s is registered with vpfe.\n", ccdc_hw_dev.name);
+	return 0;
+fail_nomem:
+	release_mem_region(res->start, resource_size(res));
+fail_nores:
+	vpfe_unregister_ccdc_device(&ccdc_hw_dev);
+	return status;
+}
+
+static int dm644x_ccdc_remove(struct platform_device *pdev)
+{
+	struct resource	*res;
+
+	iounmap(ccdc_cfg.base_addr);
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res)
+		release_mem_region(res->start, resource_size(res));
+	vpfe_unregister_ccdc_device(&ccdc_hw_dev);
+	return 0;
+}
+
+static int dm644x_ccdc_suspend(struct device *dev)
+{
+	/* Save CCDC context */
+	ccdc_save_context();
+	/* Disable CCDC */
+	ccdc_enable(0);
+
+	return 0;
+}
+
+static int dm644x_ccdc_resume(struct device *dev)
+{
+	/* Restore CCDC context */
+	ccdc_restore_context();
+
+	return 0;
+}
+
+static const struct dev_pm_ops dm644x_ccdc_pm_ops = {
+	.suspend = dm644x_ccdc_suspend,
+	.resume = dm644x_ccdc_resume,
+};
+
+static struct platform_driver dm644x_ccdc_driver = {
+	.driver = {
+		.name	= "dm644x_ccdc",
+		.pm = &dm644x_ccdc_pm_ops,
+	},
+	.remove = dm644x_ccdc_remove,
+	.probe = dm644x_ccdc_probe,
+};
+
+module_platform_driver(dm644x_ccdc_driver);
diff --git a/drivers/media/platform/davinci/dm644x_ccdc_regs.h b/drivers/media/platform/davinci/dm644x_ccdc_regs.h
new file mode 100644
index 0000000..2b0aca5
--- /dev/null
+++ b/drivers/media/platform/davinci/dm644x_ccdc_regs.h
@@ -0,0 +1,153 @@
+/*
+ * Copyright (C) 2006-2009 Texas Instruments Inc
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+#ifndef _DM644X_CCDC_REGS_H
+#define _DM644X_CCDC_REGS_H
+
+/**************************************************************************\
+* Register OFFSET Definitions
+\**************************************************************************/
+#define CCDC_PID				0x0
+#define CCDC_PCR				0x4
+#define CCDC_SYN_MODE				0x8
+#define CCDC_HD_VD_WID				0xc
+#define CCDC_PIX_LINES				0x10
+#define CCDC_HORZ_INFO				0x14
+#define CCDC_VERT_START				0x18
+#define CCDC_VERT_LINES				0x1c
+#define CCDC_CULLING				0x20
+#define CCDC_HSIZE_OFF				0x24
+#define CCDC_SDOFST				0x28
+#define CCDC_SDR_ADDR				0x2c
+#define CCDC_CLAMP				0x30
+#define CCDC_DCSUB				0x34
+#define CCDC_COLPTN				0x38
+#define CCDC_BLKCMP				0x3c
+#define CCDC_FPC				0x40
+#define CCDC_FPC_ADDR				0x44
+#define CCDC_VDINT				0x48
+#define CCDC_ALAW				0x4c
+#define CCDC_REC656IF				0x50
+#define CCDC_CCDCFG				0x54
+#define CCDC_FMTCFG				0x58
+#define CCDC_FMT_HORZ				0x5c
+#define CCDC_FMT_VERT				0x60
+#define CCDC_FMT_ADDR0				0x64
+#define CCDC_FMT_ADDR1				0x68
+#define CCDC_FMT_ADDR2				0x6c
+#define CCDC_FMT_ADDR3				0x70
+#define CCDC_FMT_ADDR4				0x74
+#define CCDC_FMT_ADDR5				0x78
+#define CCDC_FMT_ADDR6				0x7c
+#define CCDC_FMT_ADDR7				0x80
+#define CCDC_PRGEVEN_0				0x84
+#define CCDC_PRGEVEN_1				0x88
+#define CCDC_PRGODD_0				0x8c
+#define CCDC_PRGODD_1				0x90
+#define CCDC_VP_OUT				0x94
+#define CCDC_REG_END				0x98
+
+/***************************************************************
+*	Define for various register bit mask and shifts for CCDC
+****************************************************************/
+#define CCDC_FID_POL_MASK			1
+#define CCDC_FID_POL_SHIFT			4
+#define CCDC_HD_POL_MASK			1
+#define CCDC_HD_POL_SHIFT			3
+#define CCDC_VD_POL_MASK			1
+#define CCDC_VD_POL_SHIFT			2
+#define CCDC_HSIZE_OFF_MASK			0xffffffe0
+#define CCDC_32BYTE_ALIGN_VAL			31
+#define CCDC_FRM_FMT_MASK			0x1
+#define CCDC_FRM_FMT_SHIFT			7
+#define CCDC_DATA_SZ_MASK			7
+#define CCDC_DATA_SZ_SHIFT			8
+#define CCDC_PIX_FMT_MASK			3
+#define CCDC_PIX_FMT_SHIFT			12
+#define CCDC_VP2SDR_DISABLE			0xFFFBFFFF
+#define CCDC_WEN_ENABLE				(1 << 17)
+#define CCDC_SDR2RSZ_DISABLE			0xFFF7FFFF
+#define CCDC_VDHDEN_ENABLE			(1 << 16)
+#define CCDC_LPF_ENABLE				(1 << 14)
+#define CCDC_ALAW_ENABLE			(1 << 3)
+#define CCDC_ALAW_GAMMA_WD_MASK			7
+#define CCDC_BLK_CLAMP_ENABLE			(1 << 31)
+#define CCDC_BLK_SGAIN_MASK			0x1F
+#define CCDC_BLK_ST_PXL_MASK			0x7FFF
+#define CCDC_BLK_ST_PXL_SHIFT			10
+#define CCDC_BLK_SAMPLE_LN_MASK			7
+#define CCDC_BLK_SAMPLE_LN_SHIFT		28
+#define CCDC_BLK_SAMPLE_LINE_MASK		7
+#define CCDC_BLK_SAMPLE_LINE_SHIFT		25
+#define CCDC_BLK_DC_SUB_MASK			0x03FFF
+#define CCDC_BLK_COMP_MASK			0xFF
+#define CCDC_BLK_COMP_GB_COMP_SHIFT		8
+#define CCDC_BLK_COMP_GR_COMP_SHIFT		16
+#define CCDC_BLK_COMP_R_COMP_SHIFT		24
+#define CCDC_LATCH_ON_VSYNC_DISABLE		(1 << 15)
+#define CCDC_FPC_ENABLE				(1 << 15)
+#define CCDC_FPC_DISABLE			0
+#define CCDC_FPC_FPC_NUM_MASK 			0x7FFF
+#define CCDC_DATA_PACK_ENABLE			(1 << 11)
+#define CCDC_FMTCFG_VPIN_MASK			7
+#define CCDC_FMTCFG_VPIN_SHIFT			12
+#define CCDC_FMT_HORZ_FMTLNH_MASK		0x1FFF
+#define CCDC_FMT_HORZ_FMTSPH_MASK		0x1FFF
+#define CCDC_FMT_HORZ_FMTSPH_SHIFT		16
+#define CCDC_FMT_VERT_FMTLNV_MASK		0x1FFF
+#define CCDC_FMT_VERT_FMTSLV_MASK		0x1FFF
+#define CCDC_FMT_VERT_FMTSLV_SHIFT		16
+#define CCDC_VP_OUT_VERT_NUM_MASK		0x3FFF
+#define CCDC_VP_OUT_VERT_NUM_SHIFT		17
+#define CCDC_VP_OUT_HORZ_NUM_MASK		0x1FFF
+#define CCDC_VP_OUT_HORZ_NUM_SHIFT		4
+#define CCDC_VP_OUT_HORZ_ST_MASK		0xF
+#define CCDC_HORZ_INFO_SPH_SHIFT		16
+#define CCDC_VERT_START_SLV0_SHIFT		16
+#define CCDC_VDINT_VDINT0_SHIFT			16
+#define CCDC_VDINT_VDINT1_MASK			0xFFFF
+#define CCDC_PPC_RAW				1
+#define CCDC_DCSUB_DEFAULT_VAL			0
+#define CCDC_CLAMP_DEFAULT_VAL			0
+#define CCDC_ENABLE_VIDEO_PORT			0x8000
+#define CCDC_DISABLE_VIDEO_PORT			0
+#define CCDC_COLPTN_VAL				0xBB11BB11
+#define CCDC_TWO_BYTES_PER_PIXEL		2
+#define CCDC_INTERLACED_IMAGE_INVERT		0x4B6D
+#define CCDC_INTERLACED_NO_IMAGE_INVERT		0x0249
+#define CCDC_PROGRESSIVE_IMAGE_INVERT		0x4000
+#define CCDC_PROGRESSIVE_NO_IMAGE_INVERT	0
+#define CCDC_INTERLACED_HEIGHT_SHIFT		1
+#define CCDC_SYN_MODE_INPMOD_SHIFT		12
+#define CCDC_SYN_MODE_INPMOD_MASK		3
+#define CCDC_SYN_MODE_8BITS			(7 << 8)
+#define CCDC_SYN_MODE_10BITS			(6 << 8)
+#define CCDC_SYN_MODE_11BITS			(5 << 8)
+#define CCDC_SYN_MODE_12BITS			(4 << 8)
+#define CCDC_SYN_MODE_13BITS			(3 << 8)
+#define CCDC_SYN_MODE_14BITS			(2 << 8)
+#define CCDC_SYN_MODE_15BITS			(1 << 8)
+#define CCDC_SYN_MODE_16BITS			(0 << 8)
+#define CCDC_SYN_FLDMODE_MASK			1
+#define CCDC_SYN_FLDMODE_SHIFT			7
+#define CCDC_REC656IF_BT656_EN			3
+#define CCDC_SYN_MODE_VD_POL_NEGATIVE		(1 << 2)
+#define CCDC_CCDCFG_Y8POS_SHIFT			11
+#define CCDC_CCDCFG_BW656_10BIT 		(1 << 5)
+#define CCDC_SDOFST_FIELD_INTERLEAVED		0x249
+#define CCDC_NO_CULLING				0xffff00ff
+#endif
diff --git a/drivers/media/platform/davinci/isif.c b/drivers/media/platform/davinci/isif.c
new file mode 100644
index 0000000..99faea2
--- /dev/null
+++ b/drivers/media/platform/davinci/isif.c
@@ -0,0 +1,1144 @@
+/*
+ * Copyright (C) 2008-2009 Texas Instruments Inc
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ * Image Sensor Interface (ISIF) driver
+ *
+ * This driver is for configuring the ISIF IP available on DM365 or any other
+ * TI SoCs. This is used for capturing yuv or bayer video or image data
+ * from a decoder or sensor. This IP is similar to the CCDC IP on DM355
+ * and DM6446, but with enhanced or additional ip blocks. The driver
+ * configures the ISIF upon commands from the vpfe bridge driver through
+ * ccdc_hw_device interface.
+ *
+ * TODO: 1) Raw bayer parameter settings and bayer capture
+ *	 2) Add support for control ioctl
+ */
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/uaccess.h>
+#include <linux/io.h>
+#include <linux/videodev2.h>
+#include <linux/err.h>
+#include <linux/module.h>
+
+#include <mach/mux.h>
+
+#include <media/davinci/isif.h>
+#include <media/davinci/vpss.h>
+
+#include "isif_regs.h"
+#include "ccdc_hw_device.h"
+
+/* Defaults for module configuration parameters */
+static struct isif_config_params_raw isif_config_defaults = {
+	.linearize = {
+		.en = 0,
+		.corr_shft = ISIF_NO_SHIFT,
+		.scale_fact = {1, 0},
+	},
+	.df_csc = {
+		.df_or_csc = 0,
+		.csc = {
+			.en = 0,
+		},
+	},
+	.dfc = {
+		.en = 0,
+	},
+	.bclamp = {
+		.en = 0,
+	},
+	.gain_offset = {
+		.gain = {
+			.r_ye = {1, 0},
+			.gr_cy = {1, 0},
+			.gb_g = {1, 0},
+			.b_mg = {1, 0},
+		},
+	},
+	.culling = {
+		.hcpat_odd = 0xff,
+		.hcpat_even = 0xff,
+		.vcpat = 0xff,
+	},
+	.compress = {
+		.alg = ISIF_ALAW,
+	},
+};
+
+/* ISIF operation configuration */
+static struct isif_oper_config {
+	struct device *dev;
+	enum vpfe_hw_if_type if_type;
+	struct isif_ycbcr_config ycbcr;
+	struct isif_params_raw bayer;
+	enum isif_data_pack data_pack;
+	/* ISIF base address */
+	void __iomem *base_addr;
+	/* ISIF Linear Table 0 */
+	void __iomem *linear_tbl0_addr;
+	/* ISIF Linear Table 1 */
+	void __iomem *linear_tbl1_addr;
+} isif_cfg = {
+	.ycbcr = {
+		.pix_fmt = CCDC_PIXFMT_YCBCR_8BIT,
+		.frm_fmt = CCDC_FRMFMT_INTERLACED,
+		.win = ISIF_WIN_NTSC,
+		.fid_pol = VPFE_PINPOL_POSITIVE,
+		.vd_pol = VPFE_PINPOL_POSITIVE,
+		.hd_pol = VPFE_PINPOL_POSITIVE,
+		.pix_order = CCDC_PIXORDER_CBYCRY,
+		.buf_type = CCDC_BUFTYPE_FLD_INTERLEAVED,
+	},
+	.bayer = {
+		.pix_fmt = CCDC_PIXFMT_RAW,
+		.frm_fmt = CCDC_FRMFMT_PROGRESSIVE,
+		.win = ISIF_WIN_VGA,
+		.fid_pol = VPFE_PINPOL_POSITIVE,
+		.vd_pol = VPFE_PINPOL_POSITIVE,
+		.hd_pol = VPFE_PINPOL_POSITIVE,
+		.gain = {
+			.r_ye = {1, 0},
+			.gr_cy = {1, 0},
+			.gb_g = {1, 0},
+			.b_mg = {1, 0},
+		},
+		.cfa_pat = ISIF_CFA_PAT_MOSAIC,
+		.data_msb = ISIF_BIT_MSB_11,
+		.config_params = {
+			.data_shift = ISIF_NO_SHIFT,
+			.col_pat_field0 = {
+				.olop = ISIF_GREEN_BLUE,
+				.olep = ISIF_BLUE,
+				.elop = ISIF_RED,
+				.elep = ISIF_GREEN_RED,
+			},
+			.col_pat_field1 = {
+				.olop = ISIF_GREEN_BLUE,
+				.olep = ISIF_BLUE,
+				.elop = ISIF_RED,
+				.elep = ISIF_GREEN_RED,
+			},
+			.test_pat_gen = 0,
+		},
+	},
+	.data_pack = ISIF_DATA_PACK8,
+};
+
+/* Raw Bayer formats */
+static const u32 isif_raw_bayer_pix_formats[] = {
+	V4L2_PIX_FMT_SBGGR8, V4L2_PIX_FMT_SBGGR16};
+
+/* Raw YUV formats */
+static const u32 isif_raw_yuv_pix_formats[] = {
+	V4L2_PIX_FMT_UYVY, V4L2_PIX_FMT_YUYV};
+
+/* register access routines */
+static inline u32 regr(u32 offset)
+{
+	return __raw_readl(isif_cfg.base_addr + offset);
+}
+
+static inline void regw(u32 val, u32 offset)
+{
+	__raw_writel(val, isif_cfg.base_addr + offset);
+}
+
+/* reg_modify() - read, modify and write register */
+static inline u32 reg_modify(u32 mask, u32 val, u32 offset)
+{
+	u32 new_val = (regr(offset) & ~mask) | (val & mask);
+
+	regw(new_val, offset);
+	return new_val;
+}
+
+static inline void regw_lin_tbl(u32 val, u32 offset, int i)
+{
+	if (!i)
+		__raw_writel(val, isif_cfg.linear_tbl0_addr + offset);
+	else
+		__raw_writel(val, isif_cfg.linear_tbl1_addr + offset);
+}
+
+static void isif_disable_all_modules(void)
+{
+	/* disable BC */
+	regw(0, CLAMPCFG);
+	/* disable vdfc */
+	regw(0, DFCCTL);
+	/* disable CSC */
+	regw(0, CSCCTL);
+	/* disable linearization */
+	regw(0, LINCFG0);
+	/* disable other modules here as they are supported */
+}
+
+static void isif_enable(int en)
+{
+	if (!en) {
+		/* Before disable isif, disable all ISIF modules */
+		isif_disable_all_modules();
+		/*
+		 * wait for next VD. Assume lowest scan rate is 12 Hz. So
+		 * 100 msec delay is good enough
+		 */
+		msleep(100);
+	}
+	reg_modify(ISIF_SYNCEN_VDHDEN_MASK, en, SYNCEN);
+}
+
+static void isif_enable_output_to_sdram(int en)
+{
+	reg_modify(ISIF_SYNCEN_WEN_MASK, en << ISIF_SYNCEN_WEN_SHIFT, SYNCEN);
+}
+
+static void isif_config_culling(struct isif_cul *cul)
+{
+	u32 val;
+
+	/* Horizontal pattern */
+	val = (cul->hcpat_even << CULL_PAT_EVEN_LINE_SHIFT) | cul->hcpat_odd;
+	regw(val, CULH);
+
+	/* vertical pattern */
+	regw(cul->vcpat, CULV);
+
+	/* LPF */
+	reg_modify(ISIF_LPF_MASK << ISIF_LPF_SHIFT,
+		  cul->en_lpf << ISIF_LPF_SHIFT, MODESET);
+}
+
+static void isif_config_gain_offset(void)
+{
+	struct isif_gain_offsets_adj *gain_off_p =
+		&isif_cfg.bayer.config_params.gain_offset;
+	u32 val;
+
+	val = (!!gain_off_p->gain_sdram_en << GAIN_SDRAM_EN_SHIFT) |
+	      (!!gain_off_p->gain_ipipe_en << GAIN_IPIPE_EN_SHIFT) |
+	      (!!gain_off_p->gain_h3a_en << GAIN_H3A_EN_SHIFT) |
+	      (!!gain_off_p->offset_sdram_en << OFST_SDRAM_EN_SHIFT) |
+	      (!!gain_off_p->offset_ipipe_en << OFST_IPIPE_EN_SHIFT) |
+	      (!!gain_off_p->offset_h3a_en << OFST_H3A_EN_SHIFT);
+
+	reg_modify(GAIN_OFFSET_EN_MASK, val, CGAMMAWD);
+
+	val = (gain_off_p->gain.r_ye.integer << GAIN_INTEGER_SHIFT) |
+	       gain_off_p->gain.r_ye.decimal;
+	regw(val, CRGAIN);
+
+	val = (gain_off_p->gain.gr_cy.integer << GAIN_INTEGER_SHIFT) |
+	       gain_off_p->gain.gr_cy.decimal;
+	regw(val, CGRGAIN);
+
+	val = (gain_off_p->gain.gb_g.integer << GAIN_INTEGER_SHIFT) |
+	       gain_off_p->gain.gb_g.decimal;
+	regw(val, CGBGAIN);
+
+	val = (gain_off_p->gain.b_mg.integer << GAIN_INTEGER_SHIFT) |
+	       gain_off_p->gain.b_mg.decimal;
+	regw(val, CBGAIN);
+
+	regw(gain_off_p->offset, COFSTA);
+}
+
+static void isif_restore_defaults(void)
+{
+	enum vpss_ccdc_source_sel source = VPSS_CCDCIN;
+
+	dev_dbg(isif_cfg.dev, "\nstarting isif_restore_defaults...");
+	isif_cfg.bayer.config_params = isif_config_defaults;
+	/* Enable clock to ISIF, IPIPEIF and BL */
+	vpss_enable_clock(VPSS_CCDC_CLOCK, 1);
+	vpss_enable_clock(VPSS_IPIPEIF_CLOCK, 1);
+	vpss_enable_clock(VPSS_BL_CLOCK, 1);
+	/* Set default offset and gain */
+	isif_config_gain_offset();
+	vpss_select_ccdc_source(source);
+	dev_dbg(isif_cfg.dev, "\nEnd of isif_restore_defaults...");
+}
+
+static int isif_open(struct device *device)
+{
+	isif_restore_defaults();
+	return 0;
+}
+
+/* This function will configure the window size to be capture in ISIF reg */
+static void isif_setwin(struct v4l2_rect *image_win,
+			enum ccdc_frmfmt frm_fmt, int ppc)
+{
+	int horz_start, horz_nr_pixels;
+	int vert_start, vert_nr_lines;
+	int mid_img = 0;
+
+	dev_dbg(isif_cfg.dev, "\nStarting isif_setwin...");
+	/*
+	 * ppc - per pixel count. indicates how many pixels per cell
+	 * output to SDRAM. example, for ycbcr, it is one y and one c, so 2.
+	 * raw capture this is 1
+	 */
+	horz_start = image_win->left << (ppc - 1);
+	horz_nr_pixels = ((image_win->width) << (ppc - 1)) - 1;
+
+	/* Writing the horizontal info into the registers */
+	regw(horz_start & START_PX_HOR_MASK, SPH);
+	regw(horz_nr_pixels & NUM_PX_HOR_MASK, LNH);
+	vert_start = image_win->top;
+
+	if (frm_fmt == CCDC_FRMFMT_INTERLACED) {
+		vert_nr_lines = (image_win->height >> 1) - 1;
+		vert_start >>= 1;
+		/* To account for VD since line 0 doesn't have any data */
+		vert_start += 1;
+	} else {
+		/* To account for VD since line 0 doesn't have any data */
+		vert_start += 1;
+		vert_nr_lines = image_win->height - 1;
+		/* configure VDINT0 and VDINT1 */
+		mid_img = vert_start + (image_win->height / 2);
+		regw(mid_img, VDINT1);
+	}
+
+	regw(0, VDINT0);
+	regw(vert_start & START_VER_ONE_MASK, SLV0);
+	regw(vert_start & START_VER_TWO_MASK, SLV1);
+	regw(vert_nr_lines & NUM_LINES_VER, LNV);
+}
+
+static void isif_config_bclamp(struct isif_black_clamp *bc)
+{
+	u32 val;
+
+	/*
+	 * DC Offset is always added to image data irrespective of bc enable
+	 * status
+	 */
+	regw(bc->dc_offset, CLDCOFST);
+
+	if (bc->en) {
+		val = bc->bc_mode_color << ISIF_BC_MODE_COLOR_SHIFT;
+
+		/* Enable BC and horizontal clamp caculation paramaters */
+		val = val | 1 | (bc->horz.mode << ISIF_HORZ_BC_MODE_SHIFT);
+
+		regw(val, CLAMPCFG);
+
+		if (bc->horz.mode != ISIF_HORZ_BC_DISABLE) {
+			/*
+			 * Window count for calculation
+			 * Base window selection
+			 * pixel limit
+			 * Horizontal size of window
+			 * vertical size of the window
+			 * Horizontal start position of the window
+			 * Vertical start position of the window
+			 */
+			val = bc->horz.win_count_calc |
+			      ((!!bc->horz.base_win_sel_calc) <<
+				ISIF_HORZ_BC_WIN_SEL_SHIFT) |
+			      ((!!bc->horz.clamp_pix_limit) <<
+				ISIF_HORZ_BC_PIX_LIMIT_SHIFT) |
+			      (bc->horz.win_h_sz_calc <<
+				ISIF_HORZ_BC_WIN_H_SIZE_SHIFT) |
+			      (bc->horz.win_v_sz_calc <<
+				ISIF_HORZ_BC_WIN_V_SIZE_SHIFT);
+			regw(val, CLHWIN0);
+
+			regw(bc->horz.win_start_h_calc, CLHWIN1);
+			regw(bc->horz.win_start_v_calc, CLHWIN2);
+		}
+
+		/* vertical clamp caculation paramaters */
+
+		/* Reset clamp value sel for previous line */
+		val |=
+		(bc->vert.reset_val_sel << ISIF_VERT_BC_RST_VAL_SEL_SHIFT) |
+		(bc->vert.line_ave_coef << ISIF_VERT_BC_LINE_AVE_COEF_SHIFT);
+		regw(val, CLVWIN0);
+
+		/* Optical Black horizontal start position */
+		regw(bc->vert.ob_start_h, CLVWIN1);
+		/* Optical Black vertical start position */
+		regw(bc->vert.ob_start_v, CLVWIN2);
+		/* Optical Black vertical size for calculation */
+		regw(bc->vert.ob_v_sz_calc, CLVWIN3);
+		/* Vertical start position for BC subtraction */
+		regw(bc->vert_start_sub, CLSV);
+	}
+}
+
+static void isif_config_linearization(struct isif_linearize *linearize)
+{
+	u32 val, i;
+
+	if (!linearize->en) {
+		regw(0, LINCFG0);
+		return;
+	}
+
+	/* shift value for correction & enable linearization (set lsb) */
+	val = (linearize->corr_shft << ISIF_LIN_CORRSFT_SHIFT) | 1;
+	regw(val, LINCFG0);
+
+	/* Scale factor */
+	val = ((!!linearize->scale_fact.integer) <<
+	       ISIF_LIN_SCALE_FACT_INTEG_SHIFT) |
+	       linearize->scale_fact.decimal;
+	regw(val, LINCFG1);
+
+	for (i = 0; i < ISIF_LINEAR_TAB_SIZE; i++) {
+		if (i % 2)
+			regw_lin_tbl(linearize->table[i], ((i >> 1) << 2), 1);
+		else
+			regw_lin_tbl(linearize->table[i], ((i >> 1) << 2), 0);
+	}
+}
+
+static int isif_config_dfc(struct isif_dfc *vdfc)
+{
+	/* initialize retries to loop for max ~ 250 usec */
+	u32 val, count, retries = loops_per_jiffy / (4000/HZ);
+	int i;
+
+	if (!vdfc->en)
+		return 0;
+
+	/* Correction mode */
+	val = (vdfc->corr_mode << ISIF_VDFC_CORR_MOD_SHIFT);
+
+	/* Correct whole line or partial */
+	if (vdfc->corr_whole_line)
+		val |= 1 << ISIF_VDFC_CORR_WHOLE_LN_SHIFT;
+
+	/* level shift value */
+	val |= vdfc->def_level_shift << ISIF_VDFC_LEVEL_SHFT_SHIFT;
+
+	regw(val, DFCCTL);
+
+	/* Defect saturation level */
+	regw(vdfc->def_sat_level, VDFSATLV);
+
+	regw(vdfc->table[0].pos_vert, DFCMEM0);
+	regw(vdfc->table[0].pos_horz, DFCMEM1);
+	if (vdfc->corr_mode == ISIF_VDFC_NORMAL ||
+	    vdfc->corr_mode == ISIF_VDFC_HORZ_INTERPOL_IF_SAT) {
+		regw(vdfc->table[0].level_at_pos, DFCMEM2);
+		regw(vdfc->table[0].level_up_pixels, DFCMEM3);
+		regw(vdfc->table[0].level_low_pixels, DFCMEM4);
+	}
+
+	/* set DFCMARST and set DFCMWR */
+	val = regr(DFCMEMCTL) | (1 << ISIF_DFCMEMCTL_DFCMARST_SHIFT) | 1;
+	regw(val, DFCMEMCTL);
+
+	count = retries;
+	while (count && (regr(DFCMEMCTL) & 0x1))
+		count--;
+
+	if (!count) {
+		dev_dbg(isif_cfg.dev, "defect table write timeout !!!\n");
+		return -1;
+	}
+
+	for (i = 1; i < vdfc->num_vdefects; i++) {
+		regw(vdfc->table[i].pos_vert, DFCMEM0);
+		regw(vdfc->table[i].pos_horz, DFCMEM1);
+		if (vdfc->corr_mode == ISIF_VDFC_NORMAL ||
+		    vdfc->corr_mode == ISIF_VDFC_HORZ_INTERPOL_IF_SAT) {
+			regw(vdfc->table[i].level_at_pos, DFCMEM2);
+			regw(vdfc->table[i].level_up_pixels, DFCMEM3);
+			regw(vdfc->table[i].level_low_pixels, DFCMEM4);
+		}
+		val = regr(DFCMEMCTL);
+		/* clear DFCMARST and set DFCMWR */
+		val &= ~BIT(ISIF_DFCMEMCTL_DFCMARST_SHIFT);
+		val |= 1;
+		regw(val, DFCMEMCTL);
+
+		count = retries;
+		while (count && (regr(DFCMEMCTL) & 0x1))
+			count--;
+
+		if (!count) {
+			dev_err(isif_cfg.dev,
+				"defect table write timeout !!!\n");
+			return -1;
+		}
+	}
+	if (vdfc->num_vdefects < ISIF_VDFC_TABLE_SIZE) {
+		/* Extra cycle needed */
+		regw(0, DFCMEM0);
+		regw(0x1FFF, DFCMEM1);
+		regw(1, DFCMEMCTL);
+	}
+
+	/* enable VDFC */
+	reg_modify((1 << ISIF_VDFC_EN_SHIFT), (1 << ISIF_VDFC_EN_SHIFT),
+		   DFCCTL);
+	return 0;
+}
+
+static void isif_config_csc(struct isif_df_csc *df_csc)
+{
+	u32 val1 = 0, val2 = 0, i;
+
+	if (!df_csc->csc.en) {
+		regw(0, CSCCTL);
+		return;
+	}
+	for (i = 0; i < ISIF_CSC_NUM_COEFF; i++) {
+		if ((i % 2) == 0) {
+			/* CSCM - LSB */
+			val1 = (df_csc->csc.coeff[i].integer <<
+				ISIF_CSC_COEF_INTEG_SHIFT) |
+				df_csc->csc.coeff[i].decimal;
+		} else {
+
+			/* CSCM - MSB */
+			val2 = (df_csc->csc.coeff[i].integer <<
+				ISIF_CSC_COEF_INTEG_SHIFT) |
+				df_csc->csc.coeff[i].decimal;
+			val2 <<= ISIF_CSCM_MSB_SHIFT;
+			val2 |= val1;
+			regw(val2, (CSCM0 + ((i - 1) << 1)));
+		}
+	}
+
+	/* program the active area */
+	regw(df_csc->start_pix, FMTSPH);
+	/*
+	 * one extra pixel as required for CSC. Actually number of
+	 * pixel - 1 should be configured in this register. So we
+	 * need to subtract 1 before writing to FMTSPH, but we will
+	 * not do this since csc requires one extra pixel
+	 */
+	regw(df_csc->num_pixels, FMTLNH);
+	regw(df_csc->start_line, FMTSLV);
+	/*
+	 * one extra line as required for CSC. See reason documented for
+	 * num_pixels
+	 */
+	regw(df_csc->num_lines, FMTLNV);
+
+	/* Enable CSC */
+	regw(1, CSCCTL);
+}
+
+static int isif_config_raw(void)
+{
+	struct isif_params_raw *params = &isif_cfg.bayer;
+	struct isif_config_params_raw *module_params =
+		&isif_cfg.bayer.config_params;
+	struct vpss_pg_frame_size frame_size;
+	struct vpss_sync_pol sync;
+	u32 val;
+
+	dev_dbg(isif_cfg.dev, "\nStarting isif_config_raw..\n");
+
+	/*
+	 * Configure CCDCFG register:-
+	 * Set CCD Not to swap input since input is RAW data
+	 * Set FID detection function to Latch at V-Sync
+	 * Set WENLOG - isif valid area
+	 * Set TRGSEL
+	 * Set EXTRG
+	 * Packed to 8 or 16 bits
+	 */
+
+	val = ISIF_YCINSWP_RAW | ISIF_CCDCFG_FIDMD_LATCH_VSYNC |
+		ISIF_CCDCFG_WENLOG_AND | ISIF_CCDCFG_TRGSEL_WEN |
+		ISIF_CCDCFG_EXTRG_DISABLE | isif_cfg.data_pack;
+
+	dev_dbg(isif_cfg.dev, "Writing 0x%x to ...CCDCFG \n", val);
+	regw(val, CCDCFG);
+
+	/*
+	 * Configure the vertical sync polarity(MODESET.VDPOL)
+	 * Configure the horizontal sync polarity (MODESET.HDPOL)
+	 * Configure frame id polarity (MODESET.FLDPOL)
+	 * Configure data polarity
+	 * Configure External WEN Selection
+	 * Configure frame format(progressive or interlace)
+	 * Configure pixel format (Input mode)
+	 * Configure the data shift
+	 */
+
+	val = ISIF_VDHDOUT_INPUT | (params->vd_pol << ISIF_VD_POL_SHIFT) |
+		(params->hd_pol << ISIF_HD_POL_SHIFT) |
+		(params->fid_pol << ISIF_FID_POL_SHIFT) |
+		(ISIF_DATAPOL_NORMAL << ISIF_DATAPOL_SHIFT) |
+		(ISIF_EXWEN_DISABLE << ISIF_EXWEN_SHIFT) |
+		(params->frm_fmt << ISIF_FRM_FMT_SHIFT) |
+		(params->pix_fmt << ISIF_INPUT_SHIFT) |
+		(params->config_params.data_shift << ISIF_DATASFT_SHIFT);
+
+	regw(val, MODESET);
+	dev_dbg(isif_cfg.dev, "Writing 0x%x to MODESET...\n", val);
+
+	/*
+	 * Configure GAMMAWD register
+	 * CFA pattern setting
+	 */
+	val = params->cfa_pat << ISIF_GAMMAWD_CFA_SHIFT;
+
+	/* Gamma msb */
+	if (module_params->compress.alg == ISIF_ALAW)
+		val |= ISIF_ALAW_ENABLE;
+
+	val |= (params->data_msb << ISIF_ALAW_GAMMA_WD_SHIFT);
+	regw(val, CGAMMAWD);
+
+	/* Configure DPCM compression settings */
+	if (module_params->compress.alg == ISIF_DPCM) {
+		val =  BIT(ISIF_DPCM_EN_SHIFT) |
+		       (module_params->compress.pred <<
+		       ISIF_DPCM_PREDICTOR_SHIFT);
+	}
+
+	regw(val, MISC);
+
+	/* Configure Gain & Offset */
+	isif_config_gain_offset();
+
+	/* Configure Color pattern */
+	val = (params->config_params.col_pat_field0.olop) |
+	      (params->config_params.col_pat_field0.olep << 2) |
+	      (params->config_params.col_pat_field0.elop << 4) |
+	      (params->config_params.col_pat_field0.elep << 6) |
+	      (params->config_params.col_pat_field1.olop << 8) |
+	      (params->config_params.col_pat_field1.olep << 10) |
+	      (params->config_params.col_pat_field1.elop << 12) |
+	      (params->config_params.col_pat_field1.elep << 14);
+	regw(val, CCOLP);
+	dev_dbg(isif_cfg.dev, "Writing %x to CCOLP ...\n", val);
+
+	/* Configure HSIZE register  */
+	val = (!!params->horz_flip_en) << ISIF_HSIZE_FLIP_SHIFT;
+
+	/* calculate line offset in 32 bytes based on pack value */
+	if (isif_cfg.data_pack == ISIF_PACK_8BIT)
+		val |= ((params->win.width + 31) >> 5);
+	else if (isif_cfg.data_pack == ISIF_PACK_12BIT)
+		val |= (((params->win.width +
+		       (params->win.width >> 2)) + 31) >> 5);
+	else
+		val |= (((params->win.width * 2) + 31) >> 5);
+	regw(val, HSIZE);
+
+	/* Configure SDOFST register  */
+	if (params->frm_fmt == CCDC_FRMFMT_INTERLACED) {
+		if (params->image_invert_en) {
+			/* For interlace inverse mode */
+			regw(0x4B6D, SDOFST);
+			dev_dbg(isif_cfg.dev, "Writing 0x4B6D to SDOFST...\n");
+		} else {
+			/* For interlace non inverse mode */
+			regw(0x0B6D, SDOFST);
+			dev_dbg(isif_cfg.dev, "Writing 0x0B6D to SDOFST...\n");
+		}
+	} else if (params->frm_fmt == CCDC_FRMFMT_PROGRESSIVE) {
+		if (params->image_invert_en) {
+			/* For progressive inverse mode */
+			regw(0x4000, SDOFST);
+			dev_dbg(isif_cfg.dev, "Writing 0x4000 to SDOFST...\n");
+		} else {
+			/* For progressive non inverse mode */
+			regw(0x0000, SDOFST);
+			dev_dbg(isif_cfg.dev, "Writing 0x0000 to SDOFST...\n");
+		}
+	}
+
+	/* Configure video window */
+	isif_setwin(&params->win, params->frm_fmt, 1);
+
+	/* Configure Black Clamp */
+	isif_config_bclamp(&module_params->bclamp);
+
+	/* Configure Vertical Defection Pixel Correction */
+	if (isif_config_dfc(&module_params->dfc) < 0)
+		return -EFAULT;
+
+	if (!module_params->df_csc.df_or_csc)
+		/* Configure Color Space Conversion */
+		isif_config_csc(&module_params->df_csc);
+
+	isif_config_linearization(&module_params->linearize);
+
+	/* Configure Culling */
+	isif_config_culling(&module_params->culling);
+
+	/* Configure horizontal and vertical offsets(DFC,LSC,Gain) */
+	regw(module_params->horz_offset, DATAHOFST);
+	regw(module_params->vert_offset, DATAVOFST);
+
+	/* Setup test pattern if enabled */
+	if (params->config_params.test_pat_gen) {
+		/* Use the HD/VD pol settings from user */
+		sync.ccdpg_hdpol = params->hd_pol;
+		sync.ccdpg_vdpol = params->vd_pol;
+		dm365_vpss_set_sync_pol(sync);
+		frame_size.hlpfr = isif_cfg.bayer.win.width;
+		frame_size.pplen = isif_cfg.bayer.win.height;
+		dm365_vpss_set_pg_frame_size(frame_size);
+		vpss_select_ccdc_source(VPSS_PGLPBK);
+	}
+
+	dev_dbg(isif_cfg.dev, "\nEnd of isif_config_ycbcr...\n");
+	return 0;
+}
+
+static int isif_set_buftype(enum ccdc_buftype buf_type)
+{
+	if (isif_cfg.if_type == VPFE_RAW_BAYER)
+		isif_cfg.bayer.buf_type = buf_type;
+	else
+		isif_cfg.ycbcr.buf_type = buf_type;
+
+	return 0;
+
+}
+static enum ccdc_buftype isif_get_buftype(void)
+{
+	if (isif_cfg.if_type == VPFE_RAW_BAYER)
+		return isif_cfg.bayer.buf_type;
+
+	return isif_cfg.ycbcr.buf_type;
+}
+
+static int isif_enum_pix(u32 *pix, int i)
+{
+	int ret = -EINVAL;
+
+	if (isif_cfg.if_type == VPFE_RAW_BAYER) {
+		if (i < ARRAY_SIZE(isif_raw_bayer_pix_formats)) {
+			*pix = isif_raw_bayer_pix_formats[i];
+			ret = 0;
+		}
+	} else {
+		if (i < ARRAY_SIZE(isif_raw_yuv_pix_formats)) {
+			*pix = isif_raw_yuv_pix_formats[i];
+			ret = 0;
+		}
+	}
+
+	return ret;
+}
+
+static int isif_set_pixel_format(unsigned int pixfmt)
+{
+	if (isif_cfg.if_type == VPFE_RAW_BAYER) {
+		if (pixfmt == V4L2_PIX_FMT_SBGGR8) {
+			if ((isif_cfg.bayer.config_params.compress.alg !=
+			     ISIF_ALAW) &&
+			    (isif_cfg.bayer.config_params.compress.alg !=
+			     ISIF_DPCM)) {
+				dev_dbg(isif_cfg.dev,
+					"Either configure A-Law or DPCM\n");
+				return -EINVAL;
+			}
+			isif_cfg.data_pack = ISIF_PACK_8BIT;
+		} else if (pixfmt == V4L2_PIX_FMT_SBGGR16) {
+			isif_cfg.bayer.config_params.compress.alg =
+					ISIF_NO_COMPRESSION;
+			isif_cfg.data_pack = ISIF_PACK_16BIT;
+		} else
+			return -EINVAL;
+		isif_cfg.bayer.pix_fmt = CCDC_PIXFMT_RAW;
+	} else {
+		if (pixfmt == V4L2_PIX_FMT_YUYV)
+			isif_cfg.ycbcr.pix_order = CCDC_PIXORDER_YCBYCR;
+		else if (pixfmt == V4L2_PIX_FMT_UYVY)
+			isif_cfg.ycbcr.pix_order = CCDC_PIXORDER_CBYCRY;
+		else
+			return -EINVAL;
+		isif_cfg.data_pack = ISIF_PACK_8BIT;
+	}
+	return 0;
+}
+
+static u32 isif_get_pixel_format(void)
+{
+	u32 pixfmt;
+
+	if (isif_cfg.if_type == VPFE_RAW_BAYER)
+		if (isif_cfg.bayer.config_params.compress.alg == ISIF_ALAW ||
+		    isif_cfg.bayer.config_params.compress.alg == ISIF_DPCM)
+			pixfmt = V4L2_PIX_FMT_SBGGR8;
+		else
+			pixfmt = V4L2_PIX_FMT_SBGGR16;
+	else {
+		if (isif_cfg.ycbcr.pix_order == CCDC_PIXORDER_YCBYCR)
+			pixfmt = V4L2_PIX_FMT_YUYV;
+		else
+			pixfmt = V4L2_PIX_FMT_UYVY;
+	}
+	return pixfmt;
+}
+
+static int isif_set_image_window(struct v4l2_rect *win)
+{
+	if (isif_cfg.if_type == VPFE_RAW_BAYER) {
+		isif_cfg.bayer.win.top = win->top;
+		isif_cfg.bayer.win.left = win->left;
+		isif_cfg.bayer.win.width = win->width;
+		isif_cfg.bayer.win.height = win->height;
+	} else {
+		isif_cfg.ycbcr.win.top = win->top;
+		isif_cfg.ycbcr.win.left = win->left;
+		isif_cfg.ycbcr.win.width = win->width;
+		isif_cfg.ycbcr.win.height = win->height;
+	}
+	return 0;
+}
+
+static void isif_get_image_window(struct v4l2_rect *win)
+{
+	if (isif_cfg.if_type == VPFE_RAW_BAYER)
+		*win = isif_cfg.bayer.win;
+	else
+		*win = isif_cfg.ycbcr.win;
+}
+
+static unsigned int isif_get_line_length(void)
+{
+	unsigned int len;
+
+	if (isif_cfg.if_type == VPFE_RAW_BAYER) {
+		if (isif_cfg.data_pack == ISIF_PACK_8BIT)
+			len = ((isif_cfg.bayer.win.width));
+		else if (isif_cfg.data_pack == ISIF_PACK_12BIT)
+			len = (((isif_cfg.bayer.win.width * 2) +
+				 (isif_cfg.bayer.win.width >> 2)));
+		else
+			len = (((isif_cfg.bayer.win.width * 2)));
+	} else
+		len = (((isif_cfg.ycbcr.win.width * 2)));
+	return ALIGN(len, 32);
+}
+
+static int isif_set_frame_format(enum ccdc_frmfmt frm_fmt)
+{
+	if (isif_cfg.if_type == VPFE_RAW_BAYER)
+		isif_cfg.bayer.frm_fmt = frm_fmt;
+	else
+		isif_cfg.ycbcr.frm_fmt = frm_fmt;
+	return 0;
+}
+static enum ccdc_frmfmt isif_get_frame_format(void)
+{
+	if (isif_cfg.if_type == VPFE_RAW_BAYER)
+		return isif_cfg.bayer.frm_fmt;
+	return isif_cfg.ycbcr.frm_fmt;
+}
+
+static int isif_getfid(void)
+{
+	return (regr(MODESET) >> 15) & 0x1;
+}
+
+/* misc operations */
+static void isif_setfbaddr(unsigned long addr)
+{
+	regw((addr >> 21) & 0x07ff, CADU);
+	regw((addr >> 5) & 0x0ffff, CADL);
+}
+
+static int isif_set_hw_if_params(struct vpfe_hw_if_param *params)
+{
+	isif_cfg.if_type = params->if_type;
+
+	switch (params->if_type) {
+	case VPFE_BT656:
+	case VPFE_BT656_10BIT:
+	case VPFE_YCBCR_SYNC_8:
+		isif_cfg.ycbcr.pix_fmt = CCDC_PIXFMT_YCBCR_8BIT;
+		isif_cfg.ycbcr.pix_order = CCDC_PIXORDER_CBYCRY;
+		break;
+	case VPFE_BT1120:
+	case VPFE_YCBCR_SYNC_16:
+		isif_cfg.ycbcr.pix_fmt = CCDC_PIXFMT_YCBCR_16BIT;
+		isif_cfg.ycbcr.pix_order = CCDC_PIXORDER_CBYCRY;
+		break;
+	case VPFE_RAW_BAYER:
+		isif_cfg.bayer.pix_fmt = CCDC_PIXFMT_RAW;
+		break;
+	default:
+		dev_dbg(isif_cfg.dev, "Invalid interface type\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* This function will configure ISIF for YCbCr parameters. */
+static int isif_config_ycbcr(void)
+{
+	struct isif_ycbcr_config *params = &isif_cfg.ycbcr;
+	struct vpss_pg_frame_size frame_size;
+	u32 modeset = 0, ccdcfg = 0;
+	struct vpss_sync_pol sync;
+
+	dev_dbg(isif_cfg.dev, "\nStarting isif_config_ycbcr...");
+
+	/* configure pixel format or input mode */
+	modeset = modeset | (params->pix_fmt << ISIF_INPUT_SHIFT) |
+		  (params->frm_fmt << ISIF_FRM_FMT_SHIFT) |
+		  (params->fid_pol << ISIF_FID_POL_SHIFT) |
+		  (params->hd_pol << ISIF_HD_POL_SHIFT) |
+		  (params->vd_pol << ISIF_VD_POL_SHIFT);
+
+	/* pack the data to 8-bit ISIFCFG */
+	switch (isif_cfg.if_type) {
+	case VPFE_BT656:
+		if (params->pix_fmt != CCDC_PIXFMT_YCBCR_8BIT) {
+			dev_dbg(isif_cfg.dev, "Invalid pix_fmt(input mode)\n");
+			return -EINVAL;
+		}
+		modeset |= (VPFE_PINPOL_NEGATIVE << ISIF_VD_POL_SHIFT);
+		regw(3, REC656IF);
+		ccdcfg = ccdcfg | ISIF_DATA_PACK8 | ISIF_YCINSWP_YCBCR;
+		break;
+	case VPFE_BT656_10BIT:
+		if (params->pix_fmt != CCDC_PIXFMT_YCBCR_8BIT) {
+			dev_dbg(isif_cfg.dev, "Invalid pix_fmt(input mode)\n");
+			return -EINVAL;
+		}
+		/* setup BT.656, embedded sync  */
+		regw(3, REC656IF);
+		/* enable 10 bit mode in ccdcfg */
+		ccdcfg = ccdcfg | ISIF_DATA_PACK8 | ISIF_YCINSWP_YCBCR |
+			ISIF_BW656_ENABLE;
+		break;
+	case VPFE_BT1120:
+		if (params->pix_fmt != CCDC_PIXFMT_YCBCR_16BIT) {
+			dev_dbg(isif_cfg.dev, "Invalid pix_fmt(input mode)\n");
+			return -EINVAL;
+		}
+		regw(3, REC656IF);
+		break;
+
+	case VPFE_YCBCR_SYNC_8:
+		ccdcfg |= ISIF_DATA_PACK8;
+		ccdcfg |= ISIF_YCINSWP_YCBCR;
+		if (params->pix_fmt != CCDC_PIXFMT_YCBCR_8BIT) {
+			dev_dbg(isif_cfg.dev, "Invalid pix_fmt(input mode)\n");
+			return -EINVAL;
+		}
+		break;
+	case VPFE_YCBCR_SYNC_16:
+		if (params->pix_fmt != CCDC_PIXFMT_YCBCR_16BIT) {
+			dev_dbg(isif_cfg.dev, "Invalid pix_fmt(input mode)\n");
+			return -EINVAL;
+		}
+		break;
+	default:
+		/* should never come here */
+		dev_dbg(isif_cfg.dev, "Invalid interface type\n");
+		return -EINVAL;
+	}
+
+	regw(modeset, MODESET);
+
+	/* Set up pix order */
+	ccdcfg |= params->pix_order << ISIF_PIX_ORDER_SHIFT;
+
+	regw(ccdcfg, CCDCFG);
+
+	/* configure video window */
+	if ((isif_cfg.if_type == VPFE_BT1120) ||
+	    (isif_cfg.if_type == VPFE_YCBCR_SYNC_16))
+		isif_setwin(&params->win, params->frm_fmt, 1);
+	else
+		isif_setwin(&params->win, params->frm_fmt, 2);
+
+	/*
+	 * configure the horizontal line offset
+	 * this is done by rounding up width to a multiple of 16 pixels
+	 * and multiply by two to account for y:cb:cr 4:2:2 data
+	 */
+	regw(((((params->win.width * 2) + 31) & 0xffffffe0) >> 5), HSIZE);
+
+	/* configure the memory line offset */
+	if ((params->frm_fmt == CCDC_FRMFMT_INTERLACED) &&
+	    (params->buf_type == CCDC_BUFTYPE_FLD_INTERLEAVED))
+		/* two fields are interleaved in memory */
+		regw(0x00000249, SDOFST);
+
+	/* Setup test pattern if enabled */
+	if (isif_cfg.bayer.config_params.test_pat_gen) {
+		sync.ccdpg_hdpol = params->hd_pol;
+		sync.ccdpg_vdpol = params->vd_pol;
+		dm365_vpss_set_sync_pol(sync);
+		dm365_vpss_set_pg_frame_size(frame_size);
+	}
+	return 0;
+}
+
+static int isif_configure(void)
+{
+	if (isif_cfg.if_type == VPFE_RAW_BAYER)
+		return isif_config_raw();
+	return isif_config_ycbcr();
+}
+
+static int isif_close(struct device *device)
+{
+	/* copy defaults to module params */
+	isif_cfg.bayer.config_params = isif_config_defaults;
+	return 0;
+}
+
+static struct ccdc_hw_device isif_hw_dev = {
+	.name = "ISIF",
+	.owner = THIS_MODULE,
+	.hw_ops = {
+		.open = isif_open,
+		.close = isif_close,
+		.enable = isif_enable,
+		.enable_out_to_sdram = isif_enable_output_to_sdram,
+		.set_hw_if_params = isif_set_hw_if_params,
+		.configure = isif_configure,
+		.set_buftype = isif_set_buftype,
+		.get_buftype = isif_get_buftype,
+		.enum_pix = isif_enum_pix,
+		.set_pixel_format = isif_set_pixel_format,
+		.get_pixel_format = isif_get_pixel_format,
+		.set_frame_format = isif_set_frame_format,
+		.get_frame_format = isif_get_frame_format,
+		.set_image_window = isif_set_image_window,
+		.get_image_window = isif_get_image_window,
+		.get_line_length = isif_get_line_length,
+		.setfbaddr = isif_setfbaddr,
+		.getfid = isif_getfid,
+	},
+};
+
+static int isif_probe(struct platform_device *pdev)
+{
+	void (*setup_pinmux)(void);
+	struct resource	*res;
+	void *__iomem addr;
+	int status = 0, i;
+
+	/* Platform data holds setup_pinmux function ptr */
+	if (!pdev->dev.platform_data)
+		return -ENODEV;
+
+	/*
+	 * first try to register with vpfe. If not correct platform, then we
+	 * don't have to iomap
+	 */
+	status = vpfe_register_ccdc_device(&isif_hw_dev);
+	if (status < 0)
+		return status;
+
+	setup_pinmux = pdev->dev.platform_data;
+	/*
+	 * setup Mux configuration for ccdc which may be different for
+	 * different SoCs using this CCDC
+	 */
+	setup_pinmux();
+
+	i = 0;
+	/* Get the ISIF base address, linearization table0 and table1 addr. */
+	while (i < 3) {
+		res = platform_get_resource(pdev, IORESOURCE_MEM, i);
+		if (!res) {
+			status = -ENODEV;
+			goto fail_nobase_res;
+		}
+		res = request_mem_region(res->start, resource_size(res),
+					 res->name);
+		if (!res) {
+			status = -EBUSY;
+			goto fail_nobase_res;
+		}
+		addr = ioremap_nocache(res->start, resource_size(res));
+		if (!addr) {
+			status = -ENOMEM;
+			goto fail_base_iomap;
+		}
+		switch (i) {
+		case 0:
+			/* ISIF base address */
+			isif_cfg.base_addr = addr;
+			break;
+		case 1:
+			/* ISIF linear tbl0 address */
+			isif_cfg.linear_tbl0_addr = addr;
+			break;
+		default:
+			/* ISIF linear tbl0 address */
+			isif_cfg.linear_tbl1_addr = addr;
+			break;
+		}
+		i++;
+	}
+	isif_cfg.dev = &pdev->dev;
+
+	printk(KERN_NOTICE "%s is registered with vpfe.\n",
+		isif_hw_dev.name);
+	return 0;
+fail_base_iomap:
+	release_mem_region(res->start, resource_size(res));
+	i--;
+fail_nobase_res:
+	if (isif_cfg.base_addr)
+		iounmap(isif_cfg.base_addr);
+	if (isif_cfg.linear_tbl0_addr)
+		iounmap(isif_cfg.linear_tbl0_addr);
+
+	while (i >= 0) {
+		res = platform_get_resource(pdev, IORESOURCE_MEM, i);
+		release_mem_region(res->start, resource_size(res));
+		i--;
+	}
+	vpfe_unregister_ccdc_device(&isif_hw_dev);
+	return status;
+}
+
+static int isif_remove(struct platform_device *pdev)
+{
+	struct resource	*res;
+	int i = 0;
+
+	iounmap(isif_cfg.base_addr);
+	iounmap(isif_cfg.linear_tbl0_addr);
+	iounmap(isif_cfg.linear_tbl1_addr);
+	while (i < 3) {
+		res = platform_get_resource(pdev, IORESOURCE_MEM, i);
+		if (res)
+			release_mem_region(res->start, resource_size(res));
+		i++;
+	}
+	vpfe_unregister_ccdc_device(&isif_hw_dev);
+	return 0;
+}
+
+static struct platform_driver isif_driver = {
+	.driver = {
+		.name	= "isif",
+	},
+	.remove = isif_remove,
+	.probe = isif_probe,
+};
+
+module_platform_driver(isif_driver);
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/davinci/isif_regs.h b/drivers/media/platform/davinci/isif_regs.h
new file mode 100644
index 0000000..3993aec
--- /dev/null
+++ b/drivers/media/platform/davinci/isif_regs.h
@@ -0,0 +1,269 @@
+/*
+ * Copyright (C) 2008-2009 Texas Instruments Inc
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+#ifndef _ISIF_REGS_H
+#define _ISIF_REGS_H
+
+/* ISIF registers relative offsets */
+#define SYNCEN					0x00
+#define MODESET					0x04
+#define HDW					0x08
+#define VDW					0x0c
+#define PPLN					0x10
+#define LPFR					0x14
+#define SPH					0x18
+#define LNH					0x1c
+#define SLV0					0x20
+#define SLV1					0x24
+#define LNV					0x28
+#define CULH					0x2c
+#define CULV					0x30
+#define HSIZE					0x34
+#define SDOFST					0x38
+#define CADU					0x3c
+#define CADL					0x40
+#define LINCFG0					0x44
+#define LINCFG1					0x48
+#define CCOLP					0x4c
+#define CRGAIN 					0x50
+#define CGRGAIN					0x54
+#define CGBGAIN					0x58
+#define CBGAIN					0x5c
+#define COFSTA					0x60
+#define FLSHCFG0				0x64
+#define FLSHCFG1				0x68
+#define FLSHCFG2				0x6c
+#define VDINT0					0x70
+#define VDINT1					0x74
+#define VDINT2					0x78
+#define MISC 					0x7c
+#define CGAMMAWD				0x80
+#define REC656IF				0x84
+#define CCDCFG					0x88
+/*****************************************************
+* Defect Correction registers
+*****************************************************/
+#define DFCCTL					0x8c
+#define VDFSATLV				0x90
+#define DFCMEMCTL				0x94
+#define DFCMEM0					0x98
+#define DFCMEM1					0x9c
+#define DFCMEM2					0xa0
+#define DFCMEM3					0xa4
+#define DFCMEM4					0xa8
+/****************************************************
+* Black Clamp registers
+****************************************************/
+#define CLAMPCFG				0xac
+#define CLDCOFST				0xb0
+#define CLSV					0xb4
+#define CLHWIN0					0xb8
+#define CLHWIN1					0xbc
+#define CLHWIN2					0xc0
+#define CLVRV					0xc4
+#define CLVWIN0					0xc8
+#define CLVWIN1					0xcc
+#define CLVWIN2					0xd0
+#define CLVWIN3					0xd4
+/****************************************************
+* Lense Shading Correction
+****************************************************/
+#define DATAHOFST				0xd8
+#define DATAVOFST				0xdc
+#define LSCHVAL					0xe0
+#define LSCVVAL					0xe4
+#define TWODLSCCFG				0xe8
+#define TWODLSCOFST				0xec
+#define TWODLSCINI				0xf0
+#define TWODLSCGRBU				0xf4
+#define TWODLSCGRBL				0xf8
+#define TWODLSCGROF				0xfc
+#define TWODLSCORBU				0x100
+#define TWODLSCORBL				0x104
+#define TWODLSCOROF				0x108
+#define TWODLSCIRQEN				0x10c
+#define TWODLSCIRQST				0x110
+/****************************************************
+* Data formatter
+****************************************************/
+#define FMTCFG					0x114
+#define FMTPLEN					0x118
+#define FMTSPH					0x11c
+#define FMTLNH					0x120
+#define FMTSLV					0x124
+#define FMTLNV					0x128
+#define FMTRLEN					0x12c
+#define FMTHCNT					0x130
+#define FMTAPTR_BASE				0x134
+/* Below macro for addresses FMTAPTR0 - FMTAPTR15 */
+#define FMTAPTR(i)			(FMTAPTR_BASE + (i * 4))
+#define FMTPGMVF0				0x174
+#define FMTPGMVF1				0x178
+#define FMTPGMAPU0				0x17c
+#define FMTPGMAPU1				0x180
+#define FMTPGMAPS0				0x184
+#define FMTPGMAPS1				0x188
+#define FMTPGMAPS2				0x18c
+#define FMTPGMAPS3				0x190
+#define FMTPGMAPS4				0x194
+#define FMTPGMAPS5				0x198
+#define FMTPGMAPS6				0x19c
+#define FMTPGMAPS7				0x1a0
+/************************************************
+* Color Space Converter
+************************************************/
+#define CSCCTL					0x1a4
+#define CSCM0					0x1a8
+#define CSCM1					0x1ac
+#define CSCM2					0x1b0
+#define CSCM3					0x1b4
+#define CSCM4					0x1b8
+#define CSCM5					0x1bc
+#define CSCM6					0x1c0
+#define CSCM7					0x1c4
+#define OBWIN0					0x1c8
+#define OBWIN1					0x1cc
+#define OBWIN2					0x1d0
+#define OBWIN3					0x1d4
+#define OBVAL0					0x1d8
+#define OBVAL1					0x1dc
+#define OBVAL2					0x1e0
+#define OBVAL3					0x1e4
+#define OBVAL4					0x1e8
+#define OBVAL5					0x1ec
+#define OBVAL6					0x1f0
+#define OBVAL7					0x1f4
+#define CLKCTL					0x1f8
+
+/* Masks & Shifts below */
+#define START_PX_HOR_MASK			0x7FFF
+#define NUM_PX_HOR_MASK				0x7FFF
+#define START_VER_ONE_MASK			0x7FFF
+#define START_VER_TWO_MASK			0x7FFF
+#define NUM_LINES_VER				0x7FFF
+
+/* gain - offset masks */
+#define GAIN_INTEGER_SHIFT			9
+#define OFFSET_MASK				0xFFF
+#define GAIN_SDRAM_EN_SHIFT			12
+#define GAIN_IPIPE_EN_SHIFT			13
+#define GAIN_H3A_EN_SHIFT			14
+#define OFST_SDRAM_EN_SHIFT			8
+#define OFST_IPIPE_EN_SHIFT			9
+#define OFST_H3A_EN_SHIFT			10
+#define GAIN_OFFSET_EN_MASK			0x7700
+
+/* Culling */
+#define CULL_PAT_EVEN_LINE_SHIFT		8
+
+/* CCDCFG register */
+#define ISIF_YCINSWP_RAW			(0x00 << 4)
+#define ISIF_YCINSWP_YCBCR			(0x01 << 4)
+#define ISIF_CCDCFG_FIDMD_LATCH_VSYNC		(0x00 << 6)
+#define ISIF_CCDCFG_WENLOG_AND			(0x00 << 8)
+#define ISIF_CCDCFG_TRGSEL_WEN			(0x00 << 9)
+#define ISIF_CCDCFG_EXTRG_DISABLE		(0x00 << 10)
+#define ISIF_LATCH_ON_VSYNC_DISABLE		(0x01 << 15)
+#define ISIF_LATCH_ON_VSYNC_ENABLE		(0x00 << 15)
+#define ISIF_DATA_PACK_MASK			3
+#define ISIF_DATA_PACK16			0
+#define ISIF_DATA_PACK12			1
+#define ISIF_DATA_PACK8				2
+#define ISIF_PIX_ORDER_SHIFT			11
+#define ISIF_BW656_ENABLE			(0x01 << 5)
+
+/* MODESET registers */
+#define ISIF_VDHDOUT_INPUT			(0x00 << 0)
+#define ISIF_INPUT_SHIFT			12
+#define ISIF_RAW_INPUT_MODE			0
+#define ISIF_FID_POL_SHIFT			4
+#define ISIF_HD_POL_SHIFT			3
+#define ISIF_VD_POL_SHIFT			2
+#define ISIF_DATAPOL_NORMAL			0
+#define ISIF_DATAPOL_SHIFT			6
+#define ISIF_EXWEN_DISABLE 			0
+#define ISIF_EXWEN_SHIFT			5
+#define ISIF_FRM_FMT_SHIFT			7
+#define ISIF_DATASFT_SHIFT			8
+#define ISIF_LPF_SHIFT				14
+#define ISIF_LPF_MASK				1
+
+/* GAMMAWD registers */
+#define ISIF_ALAW_GAMMA_WD_MASK			0xF
+#define ISIF_ALAW_GAMMA_WD_SHIFT		1
+#define ISIF_ALAW_ENABLE			1
+#define ISIF_GAMMAWD_CFA_SHIFT			5
+
+/* HSIZE registers */
+#define ISIF_HSIZE_FLIP_MASK			1
+#define ISIF_HSIZE_FLIP_SHIFT			12
+
+/* MISC registers */
+#define ISIF_DPCM_EN_SHIFT			12
+#define ISIF_DPCM_PREDICTOR_SHIFT		13
+
+/* Black clamp related */
+#define ISIF_BC_MODE_COLOR_SHIFT		4
+#define ISIF_HORZ_BC_MODE_SHIFT			1
+#define ISIF_HORZ_BC_WIN_SEL_SHIFT		5
+#define ISIF_HORZ_BC_PIX_LIMIT_SHIFT		6
+#define ISIF_HORZ_BC_WIN_H_SIZE_SHIFT		8
+#define ISIF_HORZ_BC_WIN_V_SIZE_SHIFT		12
+#define	ISIF_VERT_BC_RST_VAL_SEL_SHIFT		4
+#define ISIF_VERT_BC_LINE_AVE_COEF_SHIFT	8
+
+/* VDFC registers */
+#define ISIF_VDFC_EN_SHIFT			4
+#define ISIF_VDFC_CORR_MOD_SHIFT		5
+#define ISIF_VDFC_CORR_WHOLE_LN_SHIFT		7
+#define ISIF_VDFC_LEVEL_SHFT_SHIFT		8
+#define ISIF_VDFC_POS_MASK			0x1FFF
+#define ISIF_DFCMEMCTL_DFCMARST_SHIFT		2
+
+/* CSC registers */
+#define ISIF_CSC_COEF_INTEG_MASK		7
+#define ISIF_CSC_COEF_DECIMAL_MASK		0x1f
+#define ISIF_CSC_COEF_INTEG_SHIFT		5
+#define ISIF_CSCM_MSB_SHIFT			8
+#define ISIF_DF_CSC_SPH_MASK			0x1FFF
+#define ISIF_DF_CSC_LNH_MASK			0x1FFF
+#define ISIF_DF_CSC_SLV_MASK			0x1FFF
+#define ISIF_DF_CSC_LNV_MASK			0x1FFF
+#define ISIF_DF_NUMLINES			0x7FFF
+#define ISIF_DF_NUMPIX				0x1FFF
+
+/* Offsets for LSC/DFC/Gain */
+#define ISIF_DATA_H_OFFSET_MASK			0x1FFF
+#define ISIF_DATA_V_OFFSET_MASK			0x1FFF
+
+/* Linearization */
+#define ISIF_LIN_CORRSFT_SHIFT			4
+#define ISIF_LIN_SCALE_FACT_INTEG_SHIFT		10
+
+
+/* Pattern registers */
+#define ISIF_PG_EN				(1 << 3)
+#define ISIF_SEL_PG_SRC				(3 << 4)
+#define ISIF_PG_VD_POL_SHIFT			0
+#define ISIF_PG_HD_POL_SHIFT			1
+
+/*random other junk*/
+#define ISIF_SYNCEN_VDHDEN_MASK			(1 << 0)
+#define ISIF_SYNCEN_WEN_MASK			(1 << 1)
+#define ISIF_SYNCEN_WEN_SHIFT			1
+
+#endif
diff --git a/drivers/media/platform/davinci/vpbe.c b/drivers/media/platform/davinci/vpbe.c
new file mode 100644
index 0000000..9a6c2cc
--- /dev/null
+++ b/drivers/media/platform/davinci/vpbe.c
@@ -0,0 +1,884 @@
+/*
+ * Copyright (C) 2010 Texas Instruments Inc
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation version 2.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/string.h>
+#include <linux/wait.h>
+#include <linux/time.h>
+#include <linux/platform_device.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+
+#include <media/v4l2-device.h>
+#include <media/davinci/vpbe_types.h>
+#include <media/davinci/vpbe.h>
+#include <media/davinci/vpss.h>
+#include <media/davinci/vpbe_venc.h>
+
+#define VPBE_DEFAULT_OUTPUT	"Composite"
+#define VPBE_DEFAULT_MODE	"ntsc"
+
+static char *def_output = VPBE_DEFAULT_OUTPUT;
+static char *def_mode = VPBE_DEFAULT_MODE;
+static int debug;
+
+module_param(def_output, charp, S_IRUGO);
+module_param(def_mode, charp, S_IRUGO);
+module_param(debug, int, 0644);
+
+MODULE_PARM_DESC(def_output, "vpbe output name (default:Composite)");
+MODULE_PARM_DESC(def_mode, "vpbe output mode name (default:ntsc");
+MODULE_PARM_DESC(debug, "Debug level 0-1");
+
+MODULE_DESCRIPTION("TI DMXXX VPBE Display controller");
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Texas Instruments");
+
+/**
+ * vpbe_current_encoder_info - Get config info for current encoder
+ * @vpbe_dev - vpbe device ptr
+ *
+ * Return ptr to current encoder config info
+ */
+static struct encoder_config_info*
+vpbe_current_encoder_info(struct vpbe_device *vpbe_dev)
+{
+	struct vpbe_config *cfg = vpbe_dev->cfg;
+	int index = vpbe_dev->current_sd_index;
+
+	return ((index == 0) ? &cfg->venc :
+				&cfg->ext_encoders[index-1]);
+}
+
+/**
+ * vpbe_find_encoder_sd_index - Given a name find encoder sd index
+ *
+ * @vpbe_config - ptr to vpbe cfg
+ * @output_index - index used by application
+ *
+ * Return sd index of the encoder
+ */
+static int vpbe_find_encoder_sd_index(struct vpbe_config *cfg,
+			     int index)
+{
+	char *encoder_name = cfg->outputs[index].subdev_name;
+	int i;
+
+	/* Venc is always first	*/
+	if (!strcmp(encoder_name, cfg->venc.module_name))
+		return 0;
+
+	for (i = 0; i < cfg->num_ext_encoders; i++) {
+		if (!strcmp(encoder_name,
+		     cfg->ext_encoders[i].module_name))
+			return i+1;
+	}
+
+	return -EINVAL;
+}
+
+/**
+ * vpbe_g_cropcap - Get crop capabilities of the display
+ * @vpbe_dev - vpbe device ptr
+ * @cropcap - cropcap is a ptr to struct v4l2_cropcap
+ *
+ * Update the crop capabilities in crop cap for current
+ * mode
+ */
+static int vpbe_g_cropcap(struct vpbe_device *vpbe_dev,
+			  struct v4l2_cropcap *cropcap)
+{
+	if (NULL == cropcap)
+		return -EINVAL;
+	cropcap->bounds.left = 0;
+	cropcap->bounds.top = 0;
+	cropcap->bounds.width = vpbe_dev->current_timings.xres;
+	cropcap->bounds.height = vpbe_dev->current_timings.yres;
+	cropcap->defrect = cropcap->bounds;
+
+	return 0;
+}
+
+/**
+ * vpbe_enum_outputs - enumerate outputs
+ * @vpbe_dev - vpbe device ptr
+ * @output - ptr to v4l2_output structure
+ *
+ * Enumerates the outputs available at the vpbe display
+ * returns the status, -EINVAL if end of output list
+ */
+static int vpbe_enum_outputs(struct vpbe_device *vpbe_dev,
+			     struct v4l2_output *output)
+{
+	struct vpbe_config *cfg = vpbe_dev->cfg;
+	int temp_index = output->index;
+
+	if (temp_index >= cfg->num_outputs)
+		return -EINVAL;
+
+	*output = cfg->outputs[temp_index].output;
+	output->index = temp_index;
+
+	return 0;
+}
+
+static int vpbe_get_mode_info(struct vpbe_device *vpbe_dev, char *mode,
+			      int output_index)
+{
+	struct vpbe_config *cfg = vpbe_dev->cfg;
+	struct vpbe_enc_mode_info var;
+	int curr_output = output_index;
+	int i;
+
+	if (NULL == mode)
+		return -EINVAL;
+
+	for (i = 0; i < cfg->outputs[curr_output].num_modes; i++) {
+		var = cfg->outputs[curr_output].modes[i];
+		if (!strcmp(mode, var.name)) {
+			vpbe_dev->current_timings = var;
+			return 0;
+		}
+	}
+
+	return -EINVAL;
+}
+
+static int vpbe_get_current_mode_info(struct vpbe_device *vpbe_dev,
+				      struct vpbe_enc_mode_info *mode_info)
+{
+	if (NULL == mode_info)
+		return -EINVAL;
+
+	*mode_info = vpbe_dev->current_timings;
+
+	return 0;
+}
+
+/* Get std by std id */
+static int vpbe_get_std_info(struct vpbe_device *vpbe_dev,
+			     v4l2_std_id std_id)
+{
+	struct vpbe_config *cfg = vpbe_dev->cfg;
+	struct vpbe_enc_mode_info var;
+	int curr_output = vpbe_dev->current_out_index;
+	int i;
+
+	for (i = 0; i < vpbe_dev->cfg->outputs[curr_output].num_modes; i++) {
+		var = cfg->outputs[curr_output].modes[i];
+		if ((var.timings_type & VPBE_ENC_STD) &&
+		  (var.std_id & std_id)) {
+			vpbe_dev->current_timings = var;
+			return 0;
+		}
+	}
+
+	return -EINVAL;
+}
+
+static int vpbe_get_std_info_by_name(struct vpbe_device *vpbe_dev,
+				char *std_name)
+{
+	struct vpbe_config *cfg = vpbe_dev->cfg;
+	struct vpbe_enc_mode_info var;
+	int curr_output = vpbe_dev->current_out_index;
+	int i;
+
+	for (i = 0; i < vpbe_dev->cfg->outputs[curr_output].num_modes; i++) {
+		var = cfg->outputs[curr_output].modes[i];
+		if (!strcmp(var.name, std_name)) {
+			vpbe_dev->current_timings = var;
+			return 0;
+		}
+	}
+
+	return -EINVAL;
+}
+
+/**
+ * vpbe_set_output - Set output
+ * @vpbe_dev - vpbe device ptr
+ * @index - index of output
+ *
+ * Set vpbe output to the output specified by the index
+ */
+static int vpbe_set_output(struct vpbe_device *vpbe_dev, int index)
+{
+	struct encoder_config_info *curr_enc_info =
+			vpbe_current_encoder_info(vpbe_dev);
+	struct vpbe_config *cfg = vpbe_dev->cfg;
+	struct venc_platform_data *venc_device = vpbe_dev->venc_device;
+	u32 if_params;
+	int enc_out_index;
+	int sd_index;
+	int ret = 0;
+
+	if (index >= cfg->num_outputs)
+		return -EINVAL;
+
+	mutex_lock(&vpbe_dev->lock);
+
+	sd_index = vpbe_dev->current_sd_index;
+	enc_out_index = cfg->outputs[index].output.index;
+	/*
+	 * Currently we switch the encoder based on output selected
+	 * by the application. If media controller is implemented later
+	 * there is will be an API added to setup_link between venc
+	 * and external encoder. So in that case below comparison always
+	 * match and encoder will not be switched. But if application
+	 * chose not to use media controller, then this provides current
+	 * way of switching encoder at the venc output.
+	 */
+	if (strcmp(curr_enc_info->module_name,
+		   cfg->outputs[index].subdev_name)) {
+		/* Need to switch the encoder at the output */
+		sd_index = vpbe_find_encoder_sd_index(cfg, index);
+		if (sd_index < 0) {
+			ret = -EINVAL;
+			goto out;
+		}
+
+		if_params = cfg->outputs[index].if_params;
+		venc_device->setup_if_config(if_params);
+		if (ret)
+			goto out;
+	}
+
+	/* Set output at the encoder */
+	ret = v4l2_subdev_call(vpbe_dev->encoders[sd_index], video,
+				       s_routing, 0, enc_out_index, 0);
+	if (ret)
+		goto out;
+
+	/*
+	 * It is assumed that venc or extenal encoder will set a default
+	 * mode in the sub device. For external encoder or LCD pannel output,
+	 * we also need to set up the lcd port for the required mode. So setup
+	 * the lcd port for the default mode that is configured in the board
+	 * arch/arm/mach-davinci/board-dm355-evm.setup file for the external
+	 * encoder.
+	 */
+	ret = vpbe_get_mode_info(vpbe_dev,
+				 cfg->outputs[index].default_mode, index);
+	if (!ret) {
+		struct osd_state *osd_device = vpbe_dev->osd_device;
+
+		osd_device->ops.set_left_margin(osd_device,
+			vpbe_dev->current_timings.left_margin);
+		osd_device->ops.set_top_margin(osd_device,
+		vpbe_dev->current_timings.upper_margin);
+		vpbe_dev->current_sd_index = sd_index;
+		vpbe_dev->current_out_index = index;
+	}
+out:
+	mutex_unlock(&vpbe_dev->lock);
+	return ret;
+}
+
+static int vpbe_set_default_output(struct vpbe_device *vpbe_dev)
+{
+	struct vpbe_config *cfg = vpbe_dev->cfg;
+	int ret = 0;
+	int i;
+
+	for (i = 0; i < cfg->num_outputs; i++) {
+		if (!strcmp(def_output,
+			    cfg->outputs[i].output.name)) {
+			ret = vpbe_set_output(vpbe_dev, i);
+			if (!ret)
+				vpbe_dev->current_out_index = i;
+			return ret;
+		}
+	}
+	return ret;
+}
+
+/**
+ * vpbe_get_output - Get output
+ * @vpbe_dev - vpbe device ptr
+ *
+ * return current vpbe output to the the index
+ */
+static unsigned int vpbe_get_output(struct vpbe_device *vpbe_dev)
+{
+	return vpbe_dev->current_out_index;
+}
+
+/**
+ * vpbe_s_dv_timings - Set the given preset timings in the encoder
+ *
+ * Sets the timings if supported by the current encoder. Return the status.
+ * 0 - success & -EINVAL on error
+ */
+static int vpbe_s_dv_timings(struct vpbe_device *vpbe_dev,
+		    struct v4l2_dv_timings *dv_timings)
+{
+	struct vpbe_config *cfg = vpbe_dev->cfg;
+	int out_index = vpbe_dev->current_out_index;
+	struct vpbe_output *output = &cfg->outputs[out_index];
+	int sd_index = vpbe_dev->current_sd_index;
+	int ret, i;
+
+
+	if (!(cfg->outputs[out_index].output.capabilities &
+	    V4L2_OUT_CAP_DV_TIMINGS))
+		return -ENODATA;
+
+	for (i = 0; i < output->num_modes; i++) {
+		if (output->modes[i].timings_type == VPBE_ENC_DV_TIMINGS &&
+		    !memcmp(&output->modes[i].dv_timings,
+				dv_timings, sizeof(*dv_timings)))
+			break;
+	}
+	if (i >= output->num_modes)
+		return -EINVAL;
+	vpbe_dev->current_timings = output->modes[i];
+	mutex_lock(&vpbe_dev->lock);
+
+	ret = v4l2_subdev_call(vpbe_dev->encoders[sd_index], video,
+					s_dv_timings, dv_timings);
+	if (!ret && (vpbe_dev->amp != NULL)) {
+		/* Call amplifier subdevice */
+		ret = v4l2_subdev_call(vpbe_dev->amp, video,
+				s_dv_timings, dv_timings);
+	}
+	/* set the lcd controller output for the given mode */
+	if (!ret) {
+		struct osd_state *osd_device = vpbe_dev->osd_device;
+
+		osd_device->ops.set_left_margin(osd_device,
+		vpbe_dev->current_timings.left_margin);
+		osd_device->ops.set_top_margin(osd_device,
+		vpbe_dev->current_timings.upper_margin);
+	}
+	mutex_unlock(&vpbe_dev->lock);
+
+	return ret;
+}
+
+/**
+ * vpbe_g_dv_timings - Get the timings in the current encoder
+ *
+ * Get the timings in the current encoder. Return the status. 0 - success
+ * -EINVAL on error
+ */
+static int vpbe_g_dv_timings(struct vpbe_device *vpbe_dev,
+		     struct v4l2_dv_timings *dv_timings)
+{
+	struct vpbe_config *cfg = vpbe_dev->cfg;
+	int out_index = vpbe_dev->current_out_index;
+
+	if (!(cfg->outputs[out_index].output.capabilities &
+		V4L2_OUT_CAP_DV_TIMINGS))
+		return -ENODATA;
+
+	if (vpbe_dev->current_timings.timings_type &
+	  VPBE_ENC_DV_TIMINGS) {
+		*dv_timings = vpbe_dev->current_timings.dv_timings;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+/**
+ * vpbe_enum_dv_timings - Enumerate the dv timings in the current encoder
+ *
+ * Get the timings in the current encoder. Return the status. 0 - success
+ * -EINVAL on error
+ */
+static int vpbe_enum_dv_timings(struct vpbe_device *vpbe_dev,
+			 struct v4l2_enum_dv_timings *timings)
+{
+	struct vpbe_config *cfg = vpbe_dev->cfg;
+	int out_index = vpbe_dev->current_out_index;
+	struct vpbe_output *output = &cfg->outputs[out_index];
+	int j = 0;
+	int i;
+
+	if (!(output->output.capabilities & V4L2_OUT_CAP_DV_TIMINGS))
+		return -ENODATA;
+
+	for (i = 0; i < output->num_modes; i++) {
+		if (output->modes[i].timings_type == VPBE_ENC_DV_TIMINGS) {
+			if (j == timings->index)
+				break;
+			j++;
+		}
+	}
+
+	if (i == output->num_modes)
+		return -EINVAL;
+	timings->timings = output->modes[i].dv_timings;
+	return 0;
+}
+
+/**
+ * vpbe_s_std - Set the given standard in the encoder
+ *
+ * Sets the standard if supported by the current encoder. Return the status.
+ * 0 - success & -EINVAL on error
+ */
+static int vpbe_s_std(struct vpbe_device *vpbe_dev, v4l2_std_id std_id)
+{
+	struct vpbe_config *cfg = vpbe_dev->cfg;
+	int out_index = vpbe_dev->current_out_index;
+	int sd_index = vpbe_dev->current_sd_index;
+	int ret;
+
+	if (!(cfg->outputs[out_index].output.capabilities &
+		V4L2_OUT_CAP_STD))
+		return -ENODATA;
+
+	ret = vpbe_get_std_info(vpbe_dev, std_id);
+	if (ret)
+		return ret;
+
+	mutex_lock(&vpbe_dev->lock);
+
+	ret = v4l2_subdev_call(vpbe_dev->encoders[sd_index], video,
+			       s_std_output, std_id);
+	/* set the lcd controller output for the given mode */
+	if (!ret) {
+		struct osd_state *osd_device = vpbe_dev->osd_device;
+
+		osd_device->ops.set_left_margin(osd_device,
+		vpbe_dev->current_timings.left_margin);
+		osd_device->ops.set_top_margin(osd_device,
+		vpbe_dev->current_timings.upper_margin);
+	}
+	mutex_unlock(&vpbe_dev->lock);
+
+	return ret;
+}
+
+/**
+ * vpbe_g_std - Get the standard in the current encoder
+ *
+ * Get the standard in the current encoder. Return the status. 0 - success
+ * -EINVAL on error
+ */
+static int vpbe_g_std(struct vpbe_device *vpbe_dev, v4l2_std_id *std_id)
+{
+	struct vpbe_enc_mode_info *cur_timings = &vpbe_dev->current_timings;
+	struct vpbe_config *cfg = vpbe_dev->cfg;
+	int out_index = vpbe_dev->current_out_index;
+
+	if (!(cfg->outputs[out_index].output.capabilities & V4L2_OUT_CAP_STD))
+		return -ENODATA;
+
+	if (cur_timings->timings_type & VPBE_ENC_STD) {
+		*std_id = cur_timings->std_id;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+/**
+ * vpbe_set_mode - Set mode in the current encoder using mode info
+ *
+ * Use the mode string to decide what timings to set in the encoder
+ * This is typically useful when fbset command is used to change the current
+ * timings by specifying a string to indicate the timings.
+ */
+static int vpbe_set_mode(struct vpbe_device *vpbe_dev,
+			 struct vpbe_enc_mode_info *mode_info)
+{
+	struct vpbe_enc_mode_info *preset_mode = NULL;
+	struct vpbe_config *cfg = vpbe_dev->cfg;
+	struct v4l2_dv_timings dv_timings;
+	struct osd_state *osd_device;
+	int out_index = vpbe_dev->current_out_index;
+	int ret = 0;
+	int i;
+
+	if ((NULL == mode_info) || (NULL == mode_info->name))
+		return -EINVAL;
+
+	for (i = 0; i < cfg->outputs[out_index].num_modes; i++) {
+		if (!strcmp(mode_info->name,
+		     cfg->outputs[out_index].modes[i].name)) {
+			preset_mode = &cfg->outputs[out_index].modes[i];
+			/*
+			 * it may be one of the 3 timings type. Check and
+			 * invoke right API
+			 */
+			if (preset_mode->timings_type & VPBE_ENC_STD)
+				return vpbe_s_std(vpbe_dev,
+						 preset_mode->std_id);
+			if (preset_mode->timings_type &
+						VPBE_ENC_DV_TIMINGS) {
+				dv_timings =
+					preset_mode->dv_timings;
+				return vpbe_s_dv_timings(vpbe_dev, &dv_timings);
+			}
+		}
+	}
+
+	/* Only custom timing should reach here */
+	if (preset_mode == NULL)
+		return -EINVAL;
+
+	mutex_lock(&vpbe_dev->lock);
+
+	osd_device = vpbe_dev->osd_device;
+	vpbe_dev->current_timings = *preset_mode;
+	osd_device->ops.set_left_margin(osd_device,
+		vpbe_dev->current_timings.left_margin);
+	osd_device->ops.set_top_margin(osd_device,
+		vpbe_dev->current_timings.upper_margin);
+
+	mutex_unlock(&vpbe_dev->lock);
+
+	return ret;
+}
+
+static int vpbe_set_default_mode(struct vpbe_device *vpbe_dev)
+{
+	int ret;
+
+	ret = vpbe_get_std_info_by_name(vpbe_dev, def_mode);
+	if (ret)
+		return ret;
+
+	/* set the default mode in the encoder */
+	return vpbe_set_mode(vpbe_dev, &vpbe_dev->current_timings);
+}
+
+static int platform_device_get(struct device *dev, void *data)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct vpbe_device *vpbe_dev = data;
+
+	if (strstr(pdev->name, "vpbe-osd") != NULL)
+		vpbe_dev->osd_device = platform_get_drvdata(pdev);
+	if (strstr(pdev->name, "vpbe-venc") != NULL)
+		vpbe_dev->venc_device = dev_get_platdata(&pdev->dev);
+
+	return 0;
+}
+
+/**
+ * vpbe_initialize() - Initialize the vpbe display controller
+ * @vpbe_dev - vpbe device ptr
+ *
+ * Master frame buffer device drivers calls this to initialize vpbe
+ * display controller. This will then registers v4l2 device and the sub
+ * devices and sets a current encoder sub device for display. v4l2 display
+ * device driver is the master and frame buffer display device driver is
+ * the slave. Frame buffer display driver checks the initialized during
+ * probe and exit if not initialized. Returns status.
+ */
+static int vpbe_initialize(struct device *dev, struct vpbe_device *vpbe_dev)
+{
+	struct encoder_config_info *enc_info;
+	struct amp_config_info *amp_info;
+	struct v4l2_subdev **enc_subdev;
+	struct osd_state *osd_device;
+	struct i2c_adapter *i2c_adap;
+	int num_encoders;
+	int ret = 0;
+	int err;
+	int i;
+
+	/*
+	 * v4l2 abd FBDev frame buffer devices will get the vpbe_dev pointer
+	 * from the platform device by iteration of platform drivers and
+	 * matching with device name
+	 */
+	if (NULL == vpbe_dev || NULL == dev) {
+		printk(KERN_ERR "Null device pointers.\n");
+		return -ENODEV;
+	}
+
+	if (vpbe_dev->initialized)
+		return 0;
+
+	mutex_lock(&vpbe_dev->lock);
+
+	if (strcmp(vpbe_dev->cfg->module_name, "dm644x-vpbe-display") != 0) {
+		/* We have dac clock available for platform */
+		vpbe_dev->dac_clk = clk_get(vpbe_dev->pdev, "vpss_dac");
+		if (IS_ERR(vpbe_dev->dac_clk)) {
+			ret =  PTR_ERR(vpbe_dev->dac_clk);
+			goto fail_mutex_unlock;
+		}
+		if (clk_prepare_enable(vpbe_dev->dac_clk)) {
+			ret =  -ENODEV;
+			clk_put(vpbe_dev->dac_clk);
+			goto fail_mutex_unlock;
+		}
+	}
+
+	/* first enable vpss clocks */
+	vpss_enable_clock(VPSS_VPBE_CLOCK, 1);
+
+	/* First register a v4l2 device */
+	ret = v4l2_device_register(dev, &vpbe_dev->v4l2_dev);
+	if (ret) {
+		v4l2_err(dev->driver,
+			"Unable to register v4l2 device.\n");
+		goto fail_clk_put;
+	}
+	v4l2_info(&vpbe_dev->v4l2_dev, "vpbe v4l2 device registered\n");
+
+	err = bus_for_each_dev(&platform_bus_type, NULL, vpbe_dev,
+			       platform_device_get);
+	if (err < 0) {
+		ret = err;
+		goto fail_dev_unregister;
+	}
+
+	vpbe_dev->venc = venc_sub_dev_init(&vpbe_dev->v4l2_dev,
+					   vpbe_dev->cfg->venc.module_name);
+	/* register venc sub device */
+	if (vpbe_dev->venc == NULL) {
+		v4l2_err(&vpbe_dev->v4l2_dev,
+			"vpbe unable to init venc sub device\n");
+		ret = -ENODEV;
+		goto fail_dev_unregister;
+	}
+	/* initialize osd device */
+	osd_device = vpbe_dev->osd_device;
+
+	if (NULL != osd_device->ops.initialize) {
+		err = osd_device->ops.initialize(osd_device);
+		if (err) {
+			v4l2_err(&vpbe_dev->v4l2_dev,
+				 "unable to initialize the OSD device");
+			err = -ENOMEM;
+			goto fail_dev_unregister;
+		}
+	}
+
+	/*
+	 * Register any external encoders that are configured. At index 0 we
+	 * store venc sd index.
+	 */
+	num_encoders = vpbe_dev->cfg->num_ext_encoders + 1;
+	vpbe_dev->encoders = kmalloc(
+				sizeof(struct v4l2_subdev *)*num_encoders,
+				GFP_KERNEL);
+	if (NULL == vpbe_dev->encoders) {
+		v4l2_err(&vpbe_dev->v4l2_dev,
+			"unable to allocate memory for encoders sub devices");
+		ret = -ENOMEM;
+		goto fail_dev_unregister;
+	}
+
+	i2c_adap = i2c_get_adapter(vpbe_dev->cfg->i2c_adapter_id);
+	for (i = 0; i < (vpbe_dev->cfg->num_ext_encoders + 1); i++) {
+		if (i == 0) {
+			/* venc is at index 0 */
+			enc_subdev = &vpbe_dev->encoders[i];
+			*enc_subdev = vpbe_dev->venc;
+			continue;
+		}
+		enc_info = &vpbe_dev->cfg->ext_encoders[i];
+		if (enc_info->is_i2c) {
+			enc_subdev = &vpbe_dev->encoders[i];
+			*enc_subdev = v4l2_i2c_new_subdev_board(
+						&vpbe_dev->v4l2_dev, i2c_adap,
+						&enc_info->board_info, NULL);
+			if (*enc_subdev)
+				v4l2_info(&vpbe_dev->v4l2_dev,
+					  "v4l2 sub device %s registered\n",
+					  enc_info->module_name);
+			else {
+				v4l2_err(&vpbe_dev->v4l2_dev, "encoder %s"
+					 " failed to register",
+					 enc_info->module_name);
+				ret = -ENODEV;
+				goto fail_kfree_encoders;
+			}
+		} else
+			v4l2_warn(&vpbe_dev->v4l2_dev, "non-i2c encoders"
+				 " currently not supported");
+	}
+	/* Add amplifier subdevice for dm365 */
+	if ((strcmp(vpbe_dev->cfg->module_name, "dm365-vpbe-display") == 0) &&
+			vpbe_dev->cfg->amp != NULL) {
+		amp_info = vpbe_dev->cfg->amp;
+		if (amp_info->is_i2c) {
+			vpbe_dev->amp = v4l2_i2c_new_subdev_board(
+			&vpbe_dev->v4l2_dev, i2c_adap,
+			&amp_info->board_info, NULL);
+			if (!vpbe_dev->amp) {
+				v4l2_err(&vpbe_dev->v4l2_dev,
+					 "amplifier %s failed to register",
+					 amp_info->module_name);
+				ret = -ENODEV;
+				goto fail_kfree_encoders;
+			}
+			v4l2_info(&vpbe_dev->v4l2_dev,
+					  "v4l2 sub device %s registered\n",
+					  amp_info->module_name);
+		} else {
+			    vpbe_dev->amp = NULL;
+			    v4l2_warn(&vpbe_dev->v4l2_dev, "non-i2c amplifiers"
+			    " currently not supported");
+		}
+	} else {
+	    vpbe_dev->amp = NULL;
+	}
+
+	/* set the current encoder and output to that of venc by default */
+	vpbe_dev->current_sd_index = 0;
+	vpbe_dev->current_out_index = 0;
+
+	mutex_unlock(&vpbe_dev->lock);
+
+	printk(KERN_NOTICE "Setting default output to %s\n", def_output);
+	ret = vpbe_set_default_output(vpbe_dev);
+	if (ret) {
+		v4l2_err(&vpbe_dev->v4l2_dev, "Failed to set default output %s",
+			 def_output);
+		return ret;
+	}
+
+	printk(KERN_NOTICE "Setting default mode to %s\n", def_mode);
+	ret = vpbe_set_default_mode(vpbe_dev);
+	if (ret) {
+		v4l2_err(&vpbe_dev->v4l2_dev, "Failed to set default mode %s",
+			 def_mode);
+		return ret;
+	}
+	vpbe_dev->initialized = 1;
+	/* TBD handling of bootargs for default output and mode */
+	return 0;
+
+fail_kfree_encoders:
+	kfree(vpbe_dev->encoders);
+fail_dev_unregister:
+	v4l2_device_unregister(&vpbe_dev->v4l2_dev);
+fail_clk_put:
+	if (strcmp(vpbe_dev->cfg->module_name, "dm644x-vpbe-display") != 0) {
+		clk_disable_unprepare(vpbe_dev->dac_clk);
+		clk_put(vpbe_dev->dac_clk);
+	}
+fail_mutex_unlock:
+	mutex_unlock(&vpbe_dev->lock);
+	return ret;
+}
+
+/**
+ * vpbe_deinitialize() - de-initialize the vpbe display controller
+ * @dev - Master and slave device ptr
+ *
+ * vpbe_master and slave frame buffer devices calls this to de-initialize
+ * the display controller. It is called when master and slave device
+ * driver modules are removed and no longer requires the display controller.
+ */
+static void vpbe_deinitialize(struct device *dev, struct vpbe_device *vpbe_dev)
+{
+	v4l2_device_unregister(&vpbe_dev->v4l2_dev);
+	if (strcmp(vpbe_dev->cfg->module_name, "dm644x-vpbe-display") != 0) {
+		clk_disable_unprepare(vpbe_dev->dac_clk);
+		clk_put(vpbe_dev->dac_clk);
+	}
+
+	kfree(vpbe_dev->amp);
+	kfree(vpbe_dev->encoders);
+	vpbe_dev->initialized = 0;
+	/* disable vpss clocks */
+	vpss_enable_clock(VPSS_VPBE_CLOCK, 0);
+}
+
+static struct vpbe_device_ops vpbe_dev_ops = {
+	.g_cropcap = vpbe_g_cropcap,
+	.enum_outputs = vpbe_enum_outputs,
+	.set_output = vpbe_set_output,
+	.get_output = vpbe_get_output,
+	.s_dv_timings = vpbe_s_dv_timings,
+	.g_dv_timings = vpbe_g_dv_timings,
+	.enum_dv_timings = vpbe_enum_dv_timings,
+	.s_std = vpbe_s_std,
+	.g_std = vpbe_g_std,
+	.initialize = vpbe_initialize,
+	.deinitialize = vpbe_deinitialize,
+	.get_mode_info = vpbe_get_current_mode_info,
+	.set_mode = vpbe_set_mode,
+};
+
+static int vpbe_probe(struct platform_device *pdev)
+{
+	struct vpbe_device *vpbe_dev;
+	struct vpbe_config *cfg;
+	int ret = -EINVAL;
+
+	if (pdev->dev.platform_data == NULL) {
+		v4l2_err(pdev->dev.driver, "No platform data\n");
+		return -ENODEV;
+	}
+	cfg = pdev->dev.platform_data;
+
+	if (!cfg->module_name[0] ||
+	    !cfg->osd.module_name[0] ||
+	    !cfg->venc.module_name[0]) {
+		v4l2_err(pdev->dev.driver, "vpbe display module names not"
+			 " defined\n");
+		return ret;
+	}
+
+	vpbe_dev = kzalloc(sizeof(*vpbe_dev), GFP_KERNEL);
+	if (vpbe_dev == NULL) {
+		v4l2_err(pdev->dev.driver, "Unable to allocate memory"
+			 " for vpbe_device\n");
+		return -ENOMEM;
+	}
+	vpbe_dev->cfg = cfg;
+	vpbe_dev->ops = vpbe_dev_ops;
+	vpbe_dev->pdev = &pdev->dev;
+
+	if (cfg->outputs->num_modes > 0)
+		vpbe_dev->current_timings = vpbe_dev->cfg->outputs[0].modes[0];
+	else {
+		kfree(vpbe_dev);
+		return -ENODEV;
+	}
+
+	/* set the driver data in platform device */
+	platform_set_drvdata(pdev, vpbe_dev);
+	mutex_init(&vpbe_dev->lock);
+
+	return 0;
+}
+
+static int vpbe_remove(struct platform_device *device)
+{
+	struct vpbe_device *vpbe_dev = platform_get_drvdata(device);
+
+	kfree(vpbe_dev);
+
+	return 0;
+}
+
+static struct platform_driver vpbe_driver = {
+	.driver	= {
+		.name	= "vpbe_controller",
+	},
+	.probe = vpbe_probe,
+	.remove = vpbe_remove,
+};
+
+module_platform_driver(vpbe_driver);
diff --git a/drivers/media/platform/davinci/vpbe_display.c b/drivers/media/platform/davinci/vpbe_display.c
new file mode 100644
index 0000000..6d91422
--- /dev/null
+++ b/drivers/media/platform/davinci/vpbe_display.c
@@ -0,0 +1,1540 @@
+/*
+ * Copyright (C) 2010 Texas Instruments Incorporated - http://www.ti.com/
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/interrupt.h>
+#include <linux/string.h>
+#include <linux/wait.h>
+#include <linux/time.h>
+#include <linux/platform_device.h>
+#include <linux/irq.h>
+#include <linux/mm.h>
+#include <linux/mutex.h>
+#include <linux/videodev2.h>
+#include <linux/slab.h>
+
+#include <asm/pgtable.h>
+#include <mach/cputype.h>
+
+#include <media/v4l2-dev.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-device.h>
+#include <media/davinci/vpbe_display.h>
+#include <media/davinci/vpbe_types.h>
+#include <media/davinci/vpbe.h>
+#include <media/davinci/vpbe_venc.h>
+#include <media/davinci/vpbe_osd.h>
+#include "vpbe_venc_regs.h"
+
+#define VPBE_DISPLAY_DRIVER "vpbe-v4l2"
+
+static int debug;
+
+#define VPBE_DEFAULT_NUM_BUFS 3
+
+module_param(debug, int, 0644);
+
+static int vpbe_set_osd_display_params(struct vpbe_display *disp_dev,
+			struct vpbe_layer *layer);
+
+static int venc_is_second_field(struct vpbe_display *disp_dev)
+{
+	struct vpbe_device *vpbe_dev = disp_dev->vpbe_dev;
+	int ret;
+	int val;
+
+	ret = v4l2_subdev_call(vpbe_dev->venc,
+			       core,
+			       ioctl,
+			       VENC_GET_FLD,
+			       &val);
+	if (ret < 0) {
+		v4l2_err(&vpbe_dev->v4l2_dev,
+			 "Error in getting Field ID 0\n");
+	}
+	return val;
+}
+
+static void vpbe_isr_even_field(struct vpbe_display *disp_obj,
+				struct vpbe_layer *layer)
+{
+	if (layer->cur_frm == layer->next_frm)
+		return;
+
+	v4l2_get_timestamp(&layer->cur_frm->vb.timestamp);
+	vb2_buffer_done(&layer->cur_frm->vb.vb2_buf, VB2_BUF_STATE_DONE);
+	/* Make cur_frm pointing to next_frm */
+	layer->cur_frm = layer->next_frm;
+}
+
+static void vpbe_isr_odd_field(struct vpbe_display *disp_obj,
+				struct vpbe_layer *layer)
+{
+	struct osd_state *osd_device = disp_obj->osd_device;
+	unsigned long addr;
+
+	spin_lock(&disp_obj->dma_queue_lock);
+	if (list_empty(&layer->dma_queue) ||
+		(layer->cur_frm != layer->next_frm)) {
+		spin_unlock(&disp_obj->dma_queue_lock);
+		return;
+	}
+	/*
+	 * one field is displayed configure
+	 * the next frame if it is available
+	 * otherwise hold on current frame
+	 * Get next from the buffer queue
+	 */
+	layer->next_frm = list_entry(layer->dma_queue.next,
+			  struct  vpbe_disp_buffer, list);
+	/* Remove that from the buffer queue */
+	list_del(&layer->next_frm->list);
+	spin_unlock(&disp_obj->dma_queue_lock);
+	/* Mark state of the frame to active */
+	layer->next_frm->vb.vb2_buf.state = VB2_BUF_STATE_ACTIVE;
+	addr = vb2_dma_contig_plane_dma_addr(&layer->next_frm->vb.vb2_buf, 0);
+	osd_device->ops.start_layer(osd_device,
+			layer->layer_info.id,
+			addr,
+			disp_obj->cbcr_ofst);
+}
+
+/* interrupt service routine */
+static irqreturn_t venc_isr(int irq, void *arg)
+{
+	struct vpbe_display *disp_dev = (struct vpbe_display *)arg;
+	struct vpbe_layer *layer;
+	static unsigned last_event;
+	unsigned event = 0;
+	int fid;
+	int i;
+
+	if ((NULL == arg) || (NULL == disp_dev->dev[0]))
+		return IRQ_HANDLED;
+
+	if (venc_is_second_field(disp_dev))
+		event |= VENC_SECOND_FIELD;
+	else
+		event |= VENC_FIRST_FIELD;
+
+	if (event == (last_event & ~VENC_END_OF_FRAME)) {
+		/*
+		* If the display is non-interlaced, then we need to flag the
+		* end-of-frame event at every interrupt regardless of the
+		* value of the FIDST bit.  We can conclude that the display is
+		* non-interlaced if the value of the FIDST bit is unchanged
+		* from the previous interrupt.
+		*/
+		event |= VENC_END_OF_FRAME;
+	} else if (event == VENC_SECOND_FIELD) {
+		/* end-of-frame for interlaced display */
+		event |= VENC_END_OF_FRAME;
+	}
+	last_event = event;
+
+	for (i = 0; i < VPBE_DISPLAY_MAX_DEVICES; i++) {
+		layer = disp_dev->dev[i];
+
+		if (!vb2_start_streaming_called(&layer->buffer_queue))
+			continue;
+
+		if (layer->layer_first_int) {
+			layer->layer_first_int = 0;
+			continue;
+		}
+		/* Check the field format */
+		if ((V4L2_FIELD_NONE == layer->pix_fmt.field) &&
+			(event & VENC_END_OF_FRAME)) {
+			/* Progressive mode */
+
+			vpbe_isr_even_field(disp_dev, layer);
+			vpbe_isr_odd_field(disp_dev, layer);
+		} else {
+		/* Interlaced mode */
+
+			layer->field_id ^= 1;
+			if (event & VENC_FIRST_FIELD)
+				fid = 0;
+			else
+				fid = 1;
+
+			/*
+			* If field id does not match with store
+			* field id
+			*/
+			if (fid != layer->field_id) {
+				/* Make them in sync */
+				layer->field_id = fid;
+				continue;
+			}
+			/*
+			* device field id and local field id are
+			* in sync. If this is even field
+			*/
+			if (0 == fid)
+				vpbe_isr_even_field(disp_dev, layer);
+			else  /* odd field */
+				vpbe_isr_odd_field(disp_dev, layer);
+		}
+	}
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * vpbe_buffer_prepare()
+ * This is the callback function called from vb2_qbuf() function
+ * the buffer is prepared and user space virtual address is converted into
+ * physical address
+ */
+static int vpbe_buffer_prepare(struct vb2_buffer *vb)
+{
+	struct vb2_queue *q = vb->vb2_queue;
+	struct vpbe_layer *layer = vb2_get_drv_priv(q);
+	struct vpbe_device *vpbe_dev = layer->disp_dev->vpbe_dev;
+	unsigned long addr;
+
+	v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,
+				"vpbe_buffer_prepare\n");
+
+	vb2_set_plane_payload(vb, 0, layer->pix_fmt.sizeimage);
+	if (vb2_get_plane_payload(vb, 0) > vb2_plane_size(vb, 0))
+		return -EINVAL;
+
+	addr = vb2_dma_contig_plane_dma_addr(vb, 0);
+	if (!IS_ALIGNED(addr, 8)) {
+		v4l2_err(&vpbe_dev->v4l2_dev,
+			 "buffer_prepare:offset is not aligned to 32 bytes\n");
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/*
+ * vpbe_buffer_setup()
+ * This function allocates memory for the buffers
+ */
+static int
+vpbe_buffer_queue_setup(struct vb2_queue *vq, const void *parg,
+			unsigned int *nbuffers, unsigned int *nplanes,
+			unsigned int sizes[], void *alloc_ctxs[])
+
+{
+	const struct v4l2_format *fmt = parg;
+	/* Get the file handle object and layer object */
+	struct vpbe_layer *layer = vb2_get_drv_priv(vq);
+	struct vpbe_device *vpbe_dev = layer->disp_dev->vpbe_dev;
+
+	v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "vpbe_buffer_setup\n");
+
+	if (fmt && fmt->fmt.pix.sizeimage < layer->pix_fmt.sizeimage)
+		return -EINVAL;
+
+	/* Store number of buffers allocated in numbuffer member */
+	if (vq->num_buffers + *nbuffers < VPBE_DEFAULT_NUM_BUFS)
+		*nbuffers = VPBE_DEFAULT_NUM_BUFS - vq->num_buffers;
+
+	*nplanes = 1;
+	sizes[0] = fmt ? fmt->fmt.pix.sizeimage : layer->pix_fmt.sizeimage;
+	alloc_ctxs[0] = layer->alloc_ctx;
+
+	return 0;
+}
+
+/*
+ * vpbe_buffer_queue()
+ * This function adds the buffer to DMA queue
+ */
+static void vpbe_buffer_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	/* Get the file handle object and layer object */
+	struct vpbe_disp_buffer *buf = container_of(vbuf,
+				struct vpbe_disp_buffer, vb);
+	struct vpbe_layer *layer = vb2_get_drv_priv(vb->vb2_queue);
+	struct vpbe_display *disp = layer->disp_dev;
+	struct vpbe_device *vpbe_dev = layer->disp_dev->vpbe_dev;
+	unsigned long flags;
+
+	v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,
+			"vpbe_buffer_queue\n");
+
+	/* add the buffer to the DMA queue */
+	spin_lock_irqsave(&disp->dma_queue_lock, flags);
+	list_add_tail(&buf->list, &layer->dma_queue);
+	spin_unlock_irqrestore(&disp->dma_queue_lock, flags);
+}
+
+static int vpbe_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct vpbe_layer *layer = vb2_get_drv_priv(vq);
+	struct osd_state *osd_device = layer->disp_dev->osd_device;
+	int ret;
+
+	 osd_device->ops.disable_layer(osd_device, layer->layer_info.id);
+
+	/* Get the next frame from the buffer queue */
+	layer->next_frm = layer->cur_frm = list_entry(layer->dma_queue.next,
+				struct vpbe_disp_buffer, list);
+	/* Remove buffer from the buffer queue */
+	list_del(&layer->cur_frm->list);
+	/* Mark state of the current frame to active */
+	layer->cur_frm->vb.vb2_buf.state = VB2_BUF_STATE_ACTIVE;
+	/* Initialize field_id and started member */
+	layer->field_id = 0;
+
+	/* Set parameters in OSD and VENC */
+	ret = vpbe_set_osd_display_params(layer->disp_dev, layer);
+	if (ret < 0) {
+		struct vpbe_disp_buffer *buf, *tmp;
+
+		vb2_buffer_done(&layer->cur_frm->vb.vb2_buf,
+				VB2_BUF_STATE_QUEUED);
+		list_for_each_entry_safe(buf, tmp, &layer->dma_queue, list) {
+			list_del(&buf->list);
+			vb2_buffer_done(&buf->vb.vb2_buf,
+					VB2_BUF_STATE_QUEUED);
+		}
+
+		return ret;
+	}
+
+	/*
+	 * if request format is yuv420 semiplanar, need to
+	 * enable both video windows
+	 */
+	layer->layer_first_int = 1;
+
+	return ret;
+}
+
+static void vpbe_stop_streaming(struct vb2_queue *vq)
+{
+	struct vpbe_layer *layer = vb2_get_drv_priv(vq);
+	struct osd_state *osd_device = layer->disp_dev->osd_device;
+	struct vpbe_display *disp = layer->disp_dev;
+	unsigned long flags;
+
+	if (!vb2_is_streaming(vq))
+		return;
+
+	osd_device->ops.disable_layer(osd_device, layer->layer_info.id);
+
+	/* release all active buffers */
+	spin_lock_irqsave(&disp->dma_queue_lock, flags);
+	if (layer->cur_frm == layer->next_frm) {
+		vb2_buffer_done(&layer->cur_frm->vb.vb2_buf,
+				VB2_BUF_STATE_ERROR);
+	} else {
+		if (layer->cur_frm != NULL)
+			vb2_buffer_done(&layer->cur_frm->vb.vb2_buf,
+					VB2_BUF_STATE_ERROR);
+		if (layer->next_frm != NULL)
+			vb2_buffer_done(&layer->next_frm->vb.vb2_buf,
+					VB2_BUF_STATE_ERROR);
+	}
+
+	while (!list_empty(&layer->dma_queue)) {
+		layer->next_frm = list_entry(layer->dma_queue.next,
+						struct vpbe_disp_buffer, list);
+		list_del(&layer->next_frm->list);
+		vb2_buffer_done(&layer->next_frm->vb.vb2_buf,
+				VB2_BUF_STATE_ERROR);
+	}
+	spin_unlock_irqrestore(&disp->dma_queue_lock, flags);
+}
+
+static struct vb2_ops video_qops = {
+	.queue_setup = vpbe_buffer_queue_setup,
+	.wait_prepare = vb2_ops_wait_prepare,
+	.wait_finish = vb2_ops_wait_finish,
+	.buf_prepare = vpbe_buffer_prepare,
+	.start_streaming = vpbe_start_streaming,
+	.stop_streaming = vpbe_stop_streaming,
+	.buf_queue = vpbe_buffer_queue,
+};
+
+static
+struct vpbe_layer*
+_vpbe_display_get_other_win_layer(struct vpbe_display *disp_dev,
+			struct vpbe_layer *layer)
+{
+	enum vpbe_display_device_id thiswin, otherwin;
+	thiswin = layer->device_id;
+
+	otherwin = (thiswin == VPBE_DISPLAY_DEVICE_0) ?
+	VPBE_DISPLAY_DEVICE_1 : VPBE_DISPLAY_DEVICE_0;
+	return disp_dev->dev[otherwin];
+}
+
+static int vpbe_set_osd_display_params(struct vpbe_display *disp_dev,
+			struct vpbe_layer *layer)
+{
+	struct osd_layer_config *cfg  = &layer->layer_info.config;
+	struct osd_state *osd_device = disp_dev->osd_device;
+	struct vpbe_device *vpbe_dev = disp_dev->vpbe_dev;
+	unsigned long addr;
+	int ret;
+
+	addr = vb2_dma_contig_plane_dma_addr(&layer->cur_frm->vb.vb2_buf, 0);
+	/* Set address in the display registers */
+	osd_device->ops.start_layer(osd_device,
+				    layer->layer_info.id,
+				    addr,
+				    disp_dev->cbcr_ofst);
+
+	ret = osd_device->ops.enable_layer(osd_device,
+				layer->layer_info.id, 0);
+	if (ret < 0) {
+		v4l2_err(&vpbe_dev->v4l2_dev,
+			"Error in enabling osd window layer 0\n");
+		return -1;
+	}
+
+	/* Enable the window */
+	layer->layer_info.enable = 1;
+	if (cfg->pixfmt == PIXFMT_NV12) {
+		struct vpbe_layer *otherlayer =
+			_vpbe_display_get_other_win_layer(disp_dev, layer);
+
+		ret = osd_device->ops.enable_layer(osd_device,
+				otherlayer->layer_info.id, 1);
+		if (ret < 0) {
+			v4l2_err(&vpbe_dev->v4l2_dev,
+				"Error in enabling osd window layer 1\n");
+			return -1;
+		}
+		otherlayer->layer_info.enable = 1;
+	}
+	return 0;
+}
+
+static void
+vpbe_disp_calculate_scale_factor(struct vpbe_display *disp_dev,
+			struct vpbe_layer *layer,
+			int expected_xsize, int expected_ysize)
+{
+	struct display_layer_info *layer_info = &layer->layer_info;
+	struct v4l2_pix_format *pixfmt = &layer->pix_fmt;
+	struct osd_layer_config *cfg  = &layer->layer_info.config;
+	struct vpbe_device *vpbe_dev = disp_dev->vpbe_dev;
+	int calculated_xsize;
+	int h_exp = 0;
+	int v_exp = 0;
+	int h_scale;
+	int v_scale;
+
+	v4l2_std_id standard_id = vpbe_dev->current_timings.std_id;
+
+	/*
+	 * Application initially set the image format. Current display
+	 * size is obtained from the vpbe display controller. expected_xsize
+	 * and expected_ysize are set through S_CROP ioctl. Based on this,
+	 * driver will calculate the scale factors for vertical and
+	 * horizontal direction so that the image is displayed scaled
+	 * and expanded. Application uses expansion to display the image
+	 * in a square pixel. Otherwise it is displayed using displays
+	 * pixel aspect ratio.It is expected that application chooses
+	 * the crop coordinates for cropped or scaled display. if crop
+	 * size is less than the image size, it is displayed cropped or
+	 * it is displayed scaled and/or expanded.
+	 *
+	 * to begin with, set the crop window same as expected. Later we
+	 * will override with scaled window size
+	 */
+
+	cfg->xsize = pixfmt->width;
+	cfg->ysize = pixfmt->height;
+	layer_info->h_zoom = ZOOM_X1;	/* no horizontal zoom */
+	layer_info->v_zoom = ZOOM_X1;	/* no horizontal zoom */
+	layer_info->h_exp = H_EXP_OFF;	/* no horizontal zoom */
+	layer_info->v_exp = V_EXP_OFF;	/* no horizontal zoom */
+
+	if (pixfmt->width < expected_xsize) {
+		h_scale = vpbe_dev->current_timings.xres / pixfmt->width;
+		if (h_scale < 2)
+			h_scale = 1;
+		else if (h_scale >= 4)
+			h_scale = 4;
+		else
+			h_scale = 2;
+		cfg->xsize *= h_scale;
+		if (cfg->xsize < expected_xsize) {
+			if ((standard_id & V4L2_STD_525_60) ||
+			(standard_id & V4L2_STD_625_50)) {
+				calculated_xsize = (cfg->xsize *
+					VPBE_DISPLAY_H_EXP_RATIO_N) /
+					VPBE_DISPLAY_H_EXP_RATIO_D;
+				if (calculated_xsize <= expected_xsize) {
+					h_exp = 1;
+					cfg->xsize = calculated_xsize;
+				}
+			}
+		}
+		if (h_scale == 2)
+			layer_info->h_zoom = ZOOM_X2;
+		else if (h_scale == 4)
+			layer_info->h_zoom = ZOOM_X4;
+		if (h_exp)
+			layer_info->h_exp = H_EXP_9_OVER_8;
+	} else {
+		/* no scaling, only cropping. Set display area to crop area */
+		cfg->xsize = expected_xsize;
+	}
+
+	if (pixfmt->height < expected_ysize) {
+		v_scale = expected_ysize / pixfmt->height;
+		if (v_scale < 2)
+			v_scale = 1;
+		else if (v_scale >= 4)
+			v_scale = 4;
+		else
+			v_scale = 2;
+		cfg->ysize *= v_scale;
+		if (cfg->ysize < expected_ysize) {
+			if ((standard_id & V4L2_STD_625_50)) {
+				calculated_xsize = (cfg->ysize *
+					VPBE_DISPLAY_V_EXP_RATIO_N) /
+					VPBE_DISPLAY_V_EXP_RATIO_D;
+				if (calculated_xsize <= expected_ysize) {
+					v_exp = 1;
+					cfg->ysize = calculated_xsize;
+				}
+			}
+		}
+		if (v_scale == 2)
+			layer_info->v_zoom = ZOOM_X2;
+		else if (v_scale == 4)
+			layer_info->v_zoom = ZOOM_X4;
+		if (v_exp)
+			layer_info->h_exp = V_EXP_6_OVER_5;
+	} else {
+		/* no scaling, only cropping. Set display area to crop area */
+		cfg->ysize = expected_ysize;
+	}
+	v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,
+		"crop display xsize = %d, ysize = %d\n",
+		cfg->xsize, cfg->ysize);
+}
+
+static void vpbe_disp_adj_position(struct vpbe_display *disp_dev,
+			struct vpbe_layer *layer,
+			int top, int left)
+{
+	struct osd_layer_config *cfg = &layer->layer_info.config;
+	struct vpbe_device *vpbe_dev = disp_dev->vpbe_dev;
+
+	cfg->xpos = min((unsigned int)left,
+			vpbe_dev->current_timings.xres - cfg->xsize);
+	cfg->ypos = min((unsigned int)top,
+			vpbe_dev->current_timings.yres - cfg->ysize);
+
+	v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,
+		"new xpos = %d, ypos = %d\n",
+		cfg->xpos, cfg->ypos);
+}
+
+static void vpbe_disp_check_window_params(struct vpbe_display *disp_dev,
+			struct v4l2_rect *c)
+{
+	struct vpbe_device *vpbe_dev = disp_dev->vpbe_dev;
+
+	if ((c->width == 0) ||
+	  ((c->width + c->left) > vpbe_dev->current_timings.xres))
+		c->width = vpbe_dev->current_timings.xres - c->left;
+
+	if ((c->height == 0) || ((c->height + c->top) >
+	  vpbe_dev->current_timings.yres))
+		c->height = vpbe_dev->current_timings.yres - c->top;
+
+	/* window height must be even for interlaced display */
+	if (vpbe_dev->current_timings.interlaced)
+		c->height &= (~0x01);
+
+}
+
+/**
+ * vpbe_try_format()
+ * If user application provides width and height, and have bytesperline set
+ * to zero, driver calculates bytesperline and sizeimage based on hardware
+ * limits.
+ */
+static int vpbe_try_format(struct vpbe_display *disp_dev,
+			struct v4l2_pix_format *pixfmt, int check)
+{
+	struct vpbe_device *vpbe_dev = disp_dev->vpbe_dev;
+	int min_height = 1;
+	int min_width = 32;
+	int max_height;
+	int max_width;
+	int bpp;
+
+	if ((pixfmt->pixelformat != V4L2_PIX_FMT_UYVY) &&
+	    (pixfmt->pixelformat != V4L2_PIX_FMT_NV12))
+		/* choose default as V4L2_PIX_FMT_UYVY */
+		pixfmt->pixelformat = V4L2_PIX_FMT_UYVY;
+
+	/* Check the field format */
+	if ((pixfmt->field != V4L2_FIELD_INTERLACED) &&
+		(pixfmt->field != V4L2_FIELD_NONE)) {
+		if (vpbe_dev->current_timings.interlaced)
+			pixfmt->field = V4L2_FIELD_INTERLACED;
+		else
+			pixfmt->field = V4L2_FIELD_NONE;
+	}
+
+	if (pixfmt->field == V4L2_FIELD_INTERLACED)
+		min_height = 2;
+
+	if (pixfmt->pixelformat == V4L2_PIX_FMT_NV12)
+		bpp = 1;
+	else
+		bpp = 2;
+
+	max_width = vpbe_dev->current_timings.xres;
+	max_height = vpbe_dev->current_timings.yres;
+
+	min_width /= bpp;
+
+	if (!pixfmt->width || (pixfmt->width < min_width) ||
+		(pixfmt->width > max_width)) {
+		pixfmt->width = vpbe_dev->current_timings.xres;
+	}
+
+	if (!pixfmt->height || (pixfmt->height  < min_height) ||
+		(pixfmt->height  > max_height)) {
+		pixfmt->height = vpbe_dev->current_timings.yres;
+	}
+
+	if (pixfmt->bytesperline < (pixfmt->width * bpp))
+		pixfmt->bytesperline = pixfmt->width * bpp;
+
+	/* Make the bytesperline 32 byte aligned */
+	pixfmt->bytesperline = ((pixfmt->width * bpp + 31) & ~31);
+
+	if (pixfmt->pixelformat == V4L2_PIX_FMT_NV12)
+		pixfmt->sizeimage = pixfmt->bytesperline * pixfmt->height +
+				(pixfmt->bytesperline * pixfmt->height >> 1);
+	else
+		pixfmt->sizeimage = pixfmt->bytesperline * pixfmt->height;
+
+	return 0;
+}
+
+static int vpbe_display_querycap(struct file *file, void  *priv,
+			       struct v4l2_capability *cap)
+{
+	struct vpbe_layer *layer = video_drvdata(file);
+	struct vpbe_device *vpbe_dev = layer->disp_dev->vpbe_dev;
+
+	cap->device_caps = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	snprintf(cap->driver, sizeof(cap->driver), "%s",
+		dev_name(vpbe_dev->pdev));
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
+		 dev_name(vpbe_dev->pdev));
+	strlcpy(cap->card, vpbe_dev->cfg->module_name, sizeof(cap->card));
+
+	return 0;
+}
+
+static int vpbe_display_s_crop(struct file *file, void *priv,
+			     const struct v4l2_crop *crop)
+{
+	struct vpbe_layer *layer = video_drvdata(file);
+	struct vpbe_display *disp_dev = layer->disp_dev;
+	struct vpbe_device *vpbe_dev = disp_dev->vpbe_dev;
+	struct osd_layer_config *cfg = &layer->layer_info.config;
+	struct osd_state *osd_device = disp_dev->osd_device;
+	struct v4l2_rect rect = crop->c;
+	int ret;
+
+	v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,
+		"VIDIOC_S_CROP, layer id = %d\n", layer->device_id);
+
+	if (crop->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+		v4l2_err(&vpbe_dev->v4l2_dev, "Invalid buf type\n");
+		return -EINVAL;
+	}
+
+	if (rect.top < 0)
+		rect.top = 0;
+	if (rect.left < 0)
+		rect.left = 0;
+
+	vpbe_disp_check_window_params(disp_dev, &rect);
+
+	osd_device->ops.get_layer_config(osd_device,
+			layer->layer_info.id, cfg);
+
+	vpbe_disp_calculate_scale_factor(disp_dev, layer,
+					rect.width,
+					rect.height);
+	vpbe_disp_adj_position(disp_dev, layer, rect.top,
+					rect.left);
+	ret = osd_device->ops.set_layer_config(osd_device,
+				layer->layer_info.id, cfg);
+	if (ret < 0) {
+		v4l2_err(&vpbe_dev->v4l2_dev,
+			"Error in set layer config:\n");
+		return -EINVAL;
+	}
+
+	/* apply zooming and h or v expansion */
+	osd_device->ops.set_zoom(osd_device,
+			layer->layer_info.id,
+			layer->layer_info.h_zoom,
+			layer->layer_info.v_zoom);
+	ret = osd_device->ops.set_vid_expansion(osd_device,
+			layer->layer_info.h_exp,
+			layer->layer_info.v_exp);
+	if (ret < 0) {
+		v4l2_err(&vpbe_dev->v4l2_dev,
+		"Error in set vid expansion:\n");
+		return -EINVAL;
+	}
+
+	if ((layer->layer_info.h_zoom != ZOOM_X1) ||
+		(layer->layer_info.v_zoom != ZOOM_X1) ||
+		(layer->layer_info.h_exp != H_EXP_OFF) ||
+		(layer->layer_info.v_exp != V_EXP_OFF))
+		/* Enable expansion filter */
+		osd_device->ops.set_interpolation_filter(osd_device, 1);
+	else
+		osd_device->ops.set_interpolation_filter(osd_device, 0);
+
+	return 0;
+}
+
+static int vpbe_display_g_crop(struct file *file, void *priv,
+			     struct v4l2_crop *crop)
+{
+	struct vpbe_layer *layer = video_drvdata(file);
+	struct osd_layer_config *cfg = &layer->layer_info.config;
+	struct vpbe_device *vpbe_dev = layer->disp_dev->vpbe_dev;
+	struct osd_state *osd_device = layer->disp_dev->osd_device;
+	struct v4l2_rect *rect = &crop->c;
+
+	v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,
+			"VIDIOC_G_CROP, layer id = %d\n",
+			layer->device_id);
+
+	if (crop->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+		v4l2_err(&vpbe_dev->v4l2_dev, "Invalid buf type\n");
+		return -EINVAL;
+	}
+	osd_device->ops.get_layer_config(osd_device,
+				layer->layer_info.id, cfg);
+	rect->top = cfg->ypos;
+	rect->left = cfg->xpos;
+	rect->width = cfg->xsize;
+	rect->height = cfg->ysize;
+
+	return 0;
+}
+
+static int vpbe_display_cropcap(struct file *file, void *priv,
+			      struct v4l2_cropcap *cropcap)
+{
+	struct vpbe_layer *layer = video_drvdata(file);
+	struct vpbe_device *vpbe_dev = layer->disp_dev->vpbe_dev;
+
+	v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_CROPCAP ioctl\n");
+
+	cropcap->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	cropcap->bounds.left = 0;
+	cropcap->bounds.top = 0;
+	cropcap->bounds.width = vpbe_dev->current_timings.xres;
+	cropcap->bounds.height = vpbe_dev->current_timings.yres;
+	cropcap->pixelaspect = vpbe_dev->current_timings.aspect;
+	cropcap->defrect = cropcap->bounds;
+	return 0;
+}
+
+static int vpbe_display_g_fmt(struct file *file, void *priv,
+				struct v4l2_format *fmt)
+{
+	struct vpbe_layer *layer = video_drvdata(file);
+	struct vpbe_device *vpbe_dev = layer->disp_dev->vpbe_dev;
+
+	v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,
+			"VIDIOC_G_FMT, layer id = %d\n",
+			layer->device_id);
+
+	/* If buffer type is video output */
+	if (V4L2_BUF_TYPE_VIDEO_OUTPUT != fmt->type) {
+		v4l2_err(&vpbe_dev->v4l2_dev, "invalid type\n");
+		return -EINVAL;
+	}
+	/* Fill in the information about format */
+	fmt->fmt.pix = layer->pix_fmt;
+
+	return 0;
+}
+
+static int vpbe_display_enum_fmt(struct file *file, void  *priv,
+				   struct v4l2_fmtdesc *fmt)
+{
+	struct vpbe_layer *layer = video_drvdata(file);
+	struct vpbe_device *vpbe_dev = layer->disp_dev->vpbe_dev;
+	unsigned int index = 0;
+
+	v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,
+				"VIDIOC_ENUM_FMT, layer id = %d\n",
+				layer->device_id);
+	if (fmt->index > 1) {
+		v4l2_err(&vpbe_dev->v4l2_dev, "Invalid format index\n");
+		return -EINVAL;
+	}
+
+	/* Fill in the information about format */
+	index = fmt->index;
+	memset(fmt, 0, sizeof(*fmt));
+	fmt->index = index;
+	fmt->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	if (index == 0) {
+		strcpy(fmt->description, "YUV 4:2:2 - UYVY");
+		fmt->pixelformat = V4L2_PIX_FMT_UYVY;
+	} else {
+		strcpy(fmt->description, "Y/CbCr 4:2:0");
+		fmt->pixelformat = V4L2_PIX_FMT_NV12;
+	}
+
+	return 0;
+}
+
+static int vpbe_display_s_fmt(struct file *file, void *priv,
+				struct v4l2_format *fmt)
+{
+	struct vpbe_layer *layer = video_drvdata(file);
+	struct vpbe_display *disp_dev = layer->disp_dev;
+	struct vpbe_device *vpbe_dev = disp_dev->vpbe_dev;
+	struct osd_layer_config *cfg  = &layer->layer_info.config;
+	struct v4l2_pix_format *pixfmt = &fmt->fmt.pix;
+	struct osd_state *osd_device = disp_dev->osd_device;
+	int ret;
+
+	v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,
+			"VIDIOC_S_FMT, layer id = %d\n",
+			layer->device_id);
+
+	if (vb2_is_busy(&layer->buffer_queue))
+		return -EBUSY;
+
+	if (V4L2_BUF_TYPE_VIDEO_OUTPUT != fmt->type) {
+		v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "invalid type\n");
+		return -EINVAL;
+	}
+	/* Check for valid pixel format */
+	ret = vpbe_try_format(disp_dev, pixfmt, 1);
+	if (ret)
+		return ret;
+
+	/* YUV420 is requested, check availability of the
+	other video window */
+
+	layer->pix_fmt = *pixfmt;
+	if (pixfmt->pixelformat == V4L2_PIX_FMT_NV12) {
+		struct vpbe_layer *otherlayer;
+
+		otherlayer = _vpbe_display_get_other_win_layer(disp_dev, layer);
+		/* if other layer is available, only
+		 * claim it, do not configure it
+		 */
+		ret = osd_device->ops.request_layer(osd_device,
+						    otherlayer->layer_info.id);
+		if (ret < 0) {
+			v4l2_err(&vpbe_dev->v4l2_dev,
+				 "Display Manager failed to allocate layer\n");
+			return -EBUSY;
+		}
+	}
+
+	/* Get osd layer config */
+	osd_device->ops.get_layer_config(osd_device,
+			layer->layer_info.id, cfg);
+	/* Store the pixel format in the layer object */
+	cfg->xsize = pixfmt->width;
+	cfg->ysize = pixfmt->height;
+	cfg->line_length = pixfmt->bytesperline;
+	cfg->ypos = 0;
+	cfg->xpos = 0;
+	cfg->interlaced = vpbe_dev->current_timings.interlaced;
+
+	if (V4L2_PIX_FMT_UYVY == pixfmt->pixelformat)
+		cfg->pixfmt = PIXFMT_YCBCRI;
+
+	/* Change of the default pixel format for both video windows */
+	if (V4L2_PIX_FMT_NV12 == pixfmt->pixelformat) {
+		struct vpbe_layer *otherlayer;
+		cfg->pixfmt = PIXFMT_NV12;
+		otherlayer = _vpbe_display_get_other_win_layer(disp_dev,
+								layer);
+		otherlayer->layer_info.config.pixfmt = PIXFMT_NV12;
+	}
+
+	/* Set the layer config in the osd window */
+	ret = osd_device->ops.set_layer_config(osd_device,
+				layer->layer_info.id, cfg);
+	if (ret < 0) {
+		v4l2_err(&vpbe_dev->v4l2_dev,
+				"Error in S_FMT params:\n");
+		return -EINVAL;
+	}
+
+	/* Readback and fill the local copy of current pix format */
+	osd_device->ops.get_layer_config(osd_device,
+			layer->layer_info.id, cfg);
+
+	return 0;
+}
+
+static int vpbe_display_try_fmt(struct file *file, void *priv,
+				  struct v4l2_format *fmt)
+{
+	struct vpbe_layer *layer = video_drvdata(file);
+	struct vpbe_display *disp_dev = layer->disp_dev;
+	struct vpbe_device *vpbe_dev = layer->disp_dev->vpbe_dev;
+	struct v4l2_pix_format *pixfmt = &fmt->fmt.pix;
+
+	v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_TRY_FMT\n");
+
+	if (V4L2_BUF_TYPE_VIDEO_OUTPUT != fmt->type) {
+		v4l2_err(&vpbe_dev->v4l2_dev, "invalid type\n");
+		return -EINVAL;
+	}
+
+	/* Check for valid field format */
+	return  vpbe_try_format(disp_dev, pixfmt, 0);
+
+}
+
+/**
+ * vpbe_display_s_std - Set the given standard in the encoder
+ *
+ * Sets the standard if supported by the current encoder. Return the status.
+ * 0 - success & -EINVAL on error
+ */
+static int vpbe_display_s_std(struct file *file, void *priv,
+				v4l2_std_id std_id)
+{
+	struct vpbe_layer *layer = video_drvdata(file);
+	struct vpbe_device *vpbe_dev = layer->disp_dev->vpbe_dev;
+	int ret;
+
+	v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_S_STD\n");
+
+	if (vb2_is_busy(&layer->buffer_queue))
+		return -EBUSY;
+
+	if (NULL != vpbe_dev->ops.s_std) {
+		ret = vpbe_dev->ops.s_std(vpbe_dev, std_id);
+		if (ret) {
+			v4l2_err(&vpbe_dev->v4l2_dev,
+			"Failed to set standard for sub devices\n");
+			return -EINVAL;
+		}
+	} else {
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/**
+ * vpbe_display_g_std - Get the standard in the current encoder
+ *
+ * Get the standard in the current encoder. Return the status. 0 - success
+ * -EINVAL on error
+ */
+static int vpbe_display_g_std(struct file *file, void *priv,
+				v4l2_std_id *std_id)
+{
+	struct vpbe_layer *layer = video_drvdata(file);
+	struct vpbe_device *vpbe_dev = layer->disp_dev->vpbe_dev;
+
+	v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,	"VIDIOC_G_STD\n");
+
+	/* Get the standard from the current encoder */
+	if (vpbe_dev->current_timings.timings_type & VPBE_ENC_STD) {
+		*std_id = vpbe_dev->current_timings.std_id;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+/**
+ * vpbe_display_enum_output - enumerate outputs
+ *
+ * Enumerates the outputs available at the vpbe display
+ * returns the status, -EINVAL if end of output list
+ */
+static int vpbe_display_enum_output(struct file *file, void *priv,
+				    struct v4l2_output *output)
+{
+	struct vpbe_layer *layer = video_drvdata(file);
+	struct vpbe_device *vpbe_dev = layer->disp_dev->vpbe_dev;
+	int ret;
+
+	v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,	"VIDIOC_ENUM_OUTPUT\n");
+
+	/* Enumerate outputs */
+
+	if (NULL == vpbe_dev->ops.enum_outputs)
+		return -EINVAL;
+
+	ret = vpbe_dev->ops.enum_outputs(vpbe_dev, output);
+	if (ret) {
+		v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,
+			"Failed to enumerate outputs\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/**
+ * vpbe_display_s_output - Set output to
+ * the output specified by the index
+ */
+static int vpbe_display_s_output(struct file *file, void *priv,
+				unsigned int i)
+{
+	struct vpbe_layer *layer = video_drvdata(file);
+	struct vpbe_device *vpbe_dev = layer->disp_dev->vpbe_dev;
+	int ret;
+
+	v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,	"VIDIOC_S_OUTPUT\n");
+
+	if (vb2_is_busy(&layer->buffer_queue))
+		return -EBUSY;
+
+	if (NULL == vpbe_dev->ops.set_output)
+		return -EINVAL;
+
+	ret = vpbe_dev->ops.set_output(vpbe_dev, i);
+	if (ret) {
+		v4l2_err(&vpbe_dev->v4l2_dev,
+			"Failed to set output for sub devices\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/**
+ * vpbe_display_g_output - Get output from subdevice
+ * for a given by the index
+ */
+static int vpbe_display_g_output(struct file *file, void *priv,
+				unsigned int *i)
+{
+	struct vpbe_layer *layer = video_drvdata(file);
+	struct vpbe_device *vpbe_dev = layer->disp_dev->vpbe_dev;
+
+	v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_G_OUTPUT\n");
+	/* Get the standard from the current encoder */
+	*i = vpbe_dev->current_out_index;
+
+	return 0;
+}
+
+/**
+ * vpbe_display_enum_dv_timings - Enumerate the dv timings
+ *
+ * enum the timings in the current encoder. Return the status. 0 - success
+ * -EINVAL on error
+ */
+static int
+vpbe_display_enum_dv_timings(struct file *file, void *priv,
+			struct v4l2_enum_dv_timings *timings)
+{
+	struct vpbe_layer *layer = video_drvdata(file);
+	struct vpbe_device *vpbe_dev = layer->disp_dev->vpbe_dev;
+	int ret;
+
+	v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_ENUM_DV_TIMINGS\n");
+
+	/* Enumerate outputs */
+	if (NULL == vpbe_dev->ops.enum_dv_timings)
+		return -EINVAL;
+
+	ret = vpbe_dev->ops.enum_dv_timings(vpbe_dev, timings);
+	if (ret) {
+		v4l2_err(&vpbe_dev->v4l2_dev,
+			"Failed to enumerate dv timings info\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/**
+ * vpbe_display_s_dv_timings - Set the dv timings
+ *
+ * Set the timings in the current encoder. Return the status. 0 - success
+ * -EINVAL on error
+ */
+static int
+vpbe_display_s_dv_timings(struct file *file, void *priv,
+				struct v4l2_dv_timings *timings)
+{
+	struct vpbe_layer *layer = video_drvdata(file);
+	struct vpbe_device *vpbe_dev = layer->disp_dev->vpbe_dev;
+	int ret;
+
+	v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_S_DV_TIMINGS\n");
+
+	if (vb2_is_busy(&layer->buffer_queue))
+		return -EBUSY;
+
+	/* Set the given standard in the encoder */
+	if (!vpbe_dev->ops.s_dv_timings)
+		return -EINVAL;
+
+	ret = vpbe_dev->ops.s_dv_timings(vpbe_dev, timings);
+	if (ret) {
+		v4l2_err(&vpbe_dev->v4l2_dev,
+			"Failed to set the dv timings info\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/**
+ * vpbe_display_g_dv_timings - Set the dv timings
+ *
+ * Get the timings in the current encoder. Return the status. 0 - success
+ * -EINVAL on error
+ */
+static int
+vpbe_display_g_dv_timings(struct file *file, void *priv,
+				struct v4l2_dv_timings *dv_timings)
+{
+	struct vpbe_layer *layer = video_drvdata(file);
+	struct vpbe_device *vpbe_dev = layer->disp_dev->vpbe_dev;
+
+	v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "VIDIOC_G_DV_TIMINGS\n");
+
+	/* Get the given standard in the encoder */
+
+	if (vpbe_dev->current_timings.timings_type &
+				VPBE_ENC_DV_TIMINGS) {
+		*dv_timings = vpbe_dev->current_timings.dv_timings;
+	} else {
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/*
+ * vpbe_display_open()
+ * It creates object of file handle structure and stores it in private_data
+ * member of filepointer
+ */
+static int vpbe_display_open(struct file *file)
+{
+	struct vpbe_layer *layer = video_drvdata(file);
+	struct vpbe_display *disp_dev = layer->disp_dev;
+	struct vpbe_device *vpbe_dev = disp_dev->vpbe_dev;
+	struct osd_state *osd_device = disp_dev->osd_device;
+	int err;
+
+	/* creating context for file descriptor */
+	err = v4l2_fh_open(file);
+	if (err) {
+		v4l2_err(&vpbe_dev->v4l2_dev, "v4l2_fh_open failed\n");
+		return err;
+	}
+
+	/* leaving if layer is already initialized */
+	if (!v4l2_fh_is_singular_file(file))
+		return err;
+
+	if (!layer->usrs) {
+		if (mutex_lock_interruptible(&layer->opslock))
+			return -ERESTARTSYS;
+		/* First claim the layer for this device */
+		err = osd_device->ops.request_layer(osd_device,
+						layer->layer_info.id);
+		mutex_unlock(&layer->opslock);
+		if (err < 0) {
+			/* Couldn't get layer */
+			v4l2_err(&vpbe_dev->v4l2_dev,
+				"Display Manager failed to allocate layer\n");
+			v4l2_fh_release(file);
+			return -EINVAL;
+		}
+	}
+	/* Increment layer usrs counter */
+	layer->usrs++;
+	v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev,
+			"vpbe display device opened successfully\n");
+	return 0;
+}
+
+/*
+ * vpbe_display_release()
+ * This function deletes buffer queue, frees the buffers and the davinci
+ * display file * handle
+ */
+static int vpbe_display_release(struct file *file)
+{
+	struct vpbe_layer *layer = video_drvdata(file);
+	struct osd_layer_config *cfg  = &layer->layer_info.config;
+	struct vpbe_display *disp_dev = layer->disp_dev;
+	struct vpbe_device *vpbe_dev = disp_dev->vpbe_dev;
+	struct osd_state *osd_device = disp_dev->osd_device;
+
+	v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "vpbe_display_release\n");
+
+	mutex_lock(&layer->opslock);
+
+	osd_device->ops.disable_layer(osd_device,
+			layer->layer_info.id);
+	/* Decrement layer usrs counter */
+	layer->usrs--;
+	/* If this file handle has initialize encoder device, reset it */
+	if (!layer->usrs) {
+		if (cfg->pixfmt == PIXFMT_NV12) {
+			struct vpbe_layer *otherlayer;
+			otherlayer =
+			_vpbe_display_get_other_win_layer(disp_dev, layer);
+			osd_device->ops.disable_layer(osd_device,
+					otherlayer->layer_info.id);
+			osd_device->ops.release_layer(osd_device,
+					otherlayer->layer_info.id);
+		}
+		osd_device->ops.disable_layer(osd_device,
+				layer->layer_info.id);
+		osd_device->ops.release_layer(osd_device,
+				layer->layer_info.id);
+	}
+
+	_vb2_fop_release(file, NULL);
+	mutex_unlock(&layer->opslock);
+
+	disp_dev->cbcr_ofst = 0;
+
+	return 0;
+}
+
+/* vpbe capture ioctl operations */
+static const struct v4l2_ioctl_ops vpbe_ioctl_ops = {
+	.vidioc_querycap	 = vpbe_display_querycap,
+	.vidioc_g_fmt_vid_out    = vpbe_display_g_fmt,
+	.vidioc_enum_fmt_vid_out = vpbe_display_enum_fmt,
+	.vidioc_s_fmt_vid_out    = vpbe_display_s_fmt,
+	.vidioc_try_fmt_vid_out  = vpbe_display_try_fmt,
+
+	.vidioc_reqbufs		 = vb2_ioctl_reqbufs,
+	.vidioc_create_bufs	 = vb2_ioctl_create_bufs,
+	.vidioc_querybuf	 = vb2_ioctl_querybuf,
+	.vidioc_qbuf		 = vb2_ioctl_qbuf,
+	.vidioc_dqbuf		 = vb2_ioctl_dqbuf,
+	.vidioc_streamon	 = vb2_ioctl_streamon,
+	.vidioc_streamoff	 = vb2_ioctl_streamoff,
+	.vidioc_expbuf		 = vb2_ioctl_expbuf,
+
+	.vidioc_cropcap		 = vpbe_display_cropcap,
+	.vidioc_g_crop		 = vpbe_display_g_crop,
+	.vidioc_s_crop		 = vpbe_display_s_crop,
+
+	.vidioc_s_std		 = vpbe_display_s_std,
+	.vidioc_g_std		 = vpbe_display_g_std,
+
+	.vidioc_enum_output	 = vpbe_display_enum_output,
+	.vidioc_s_output	 = vpbe_display_s_output,
+	.vidioc_g_output	 = vpbe_display_g_output,
+
+	.vidioc_s_dv_timings	 = vpbe_display_s_dv_timings,
+	.vidioc_g_dv_timings	 = vpbe_display_g_dv_timings,
+	.vidioc_enum_dv_timings	 = vpbe_display_enum_dv_timings,
+};
+
+static struct v4l2_file_operations vpbe_fops = {
+	.owner = THIS_MODULE,
+	.open = vpbe_display_open,
+	.release = vpbe_display_release,
+	.unlocked_ioctl = video_ioctl2,
+	.mmap = vb2_fop_mmap,
+	.poll =  vb2_fop_poll,
+};
+
+static int vpbe_device_get(struct device *dev, void *data)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct vpbe_display *vpbe_disp  = data;
+
+	if (strcmp("vpbe_controller", pdev->name) == 0)
+		vpbe_disp->vpbe_dev = platform_get_drvdata(pdev);
+
+	if (strstr(pdev->name, "vpbe-osd") != NULL)
+		vpbe_disp->osd_device = platform_get_drvdata(pdev);
+
+	return 0;
+}
+
+static int init_vpbe_layer(int i, struct vpbe_display *disp_dev,
+			   struct platform_device *pdev)
+{
+	struct vpbe_layer *vpbe_display_layer = NULL;
+	struct video_device *vbd = NULL;
+
+	/* Allocate memory for four plane display objects */
+
+	disp_dev->dev[i] =
+		kzalloc(sizeof(struct vpbe_layer), GFP_KERNEL);
+
+	/* If memory allocation fails, return error */
+	if (!disp_dev->dev[i]) {
+		printk(KERN_ERR "ran out of memory\n");
+		return  -ENOMEM;
+	}
+	spin_lock_init(&disp_dev->dev[i]->irqlock);
+	mutex_init(&disp_dev->dev[i]->opslock);
+
+	/* Get the pointer to the layer object */
+	vpbe_display_layer = disp_dev->dev[i];
+	vbd = &vpbe_display_layer->video_dev;
+	/* Initialize field of video device */
+	vbd->release	= video_device_release_empty;
+	vbd->fops	= &vpbe_fops;
+	vbd->ioctl_ops	= &vpbe_ioctl_ops;
+	vbd->minor	= -1;
+	vbd->v4l2_dev   = &disp_dev->vpbe_dev->v4l2_dev;
+	vbd->lock	= &vpbe_display_layer->opslock;
+	vbd->vfl_dir	= VFL_DIR_TX;
+
+	if (disp_dev->vpbe_dev->current_timings.timings_type &
+			VPBE_ENC_STD)
+		vbd->tvnorms = (V4L2_STD_525_60 | V4L2_STD_625_50);
+
+	snprintf(vbd->name, sizeof(vbd->name),
+			"DaVinci_VPBE Display_DRIVER_V%d.%d.%d",
+			(VPBE_DISPLAY_VERSION_CODE >> 16) & 0xff,
+			(VPBE_DISPLAY_VERSION_CODE >> 8) & 0xff,
+			(VPBE_DISPLAY_VERSION_CODE) & 0xff);
+
+	vpbe_display_layer->device_id = i;
+
+	vpbe_display_layer->layer_info.id =
+		((i == VPBE_DISPLAY_DEVICE_0) ? WIN_VID0 : WIN_VID1);
+
+
+	return 0;
+}
+
+static int register_device(struct vpbe_layer *vpbe_display_layer,
+			   struct vpbe_display *disp_dev,
+			   struct platform_device *pdev)
+{
+	int err;
+
+	v4l2_info(&disp_dev->vpbe_dev->v4l2_dev,
+		  "Trying to register VPBE display device.\n");
+	v4l2_info(&disp_dev->vpbe_dev->v4l2_dev,
+		  "layer=%x,layer->video_dev=%x\n",
+		  (int)vpbe_display_layer,
+		  (int)&vpbe_display_layer->video_dev);
+
+	vpbe_display_layer->video_dev.queue = &vpbe_display_layer->buffer_queue;
+	err = video_register_device(&vpbe_display_layer->video_dev,
+				    VFL_TYPE_GRABBER,
+				    -1);
+	if (err)
+		return -ENODEV;
+
+	vpbe_display_layer->disp_dev = disp_dev;
+	/* set the driver data in platform device */
+	platform_set_drvdata(pdev, disp_dev);
+	video_set_drvdata(&vpbe_display_layer->video_dev,
+			  vpbe_display_layer);
+
+	return 0;
+}
+
+
+
+/*
+ * vpbe_display_probe()
+ * This function creates device entries by register itself to the V4L2 driver
+ * and initializes fields of each layer objects
+ */
+static int vpbe_display_probe(struct platform_device *pdev)
+{
+	struct vpbe_display *disp_dev;
+	struct v4l2_device *v4l2_dev;
+	struct resource *res = NULL;
+	struct vb2_queue *q;
+	int k;
+	int i;
+	int err;
+	int irq;
+
+	printk(KERN_DEBUG "vpbe_display_probe\n");
+	/* Allocate memory for vpbe_display */
+	disp_dev = devm_kzalloc(&pdev->dev, sizeof(struct vpbe_display),
+				GFP_KERNEL);
+	if (!disp_dev)
+		return -ENOMEM;
+
+	spin_lock_init(&disp_dev->dma_queue_lock);
+	/*
+	 * Scan all the platform devices to find the vpbe
+	 * controller device and get the vpbe_dev object
+	 */
+	err = bus_for_each_dev(&platform_bus_type, NULL, disp_dev,
+			vpbe_device_get);
+	if (err < 0)
+		return err;
+
+	v4l2_dev = &disp_dev->vpbe_dev->v4l2_dev;
+	/* Initialize the vpbe display controller */
+	if (NULL != disp_dev->vpbe_dev->ops.initialize) {
+		err = disp_dev->vpbe_dev->ops.initialize(&pdev->dev,
+							 disp_dev->vpbe_dev);
+		if (err) {
+			v4l2_err(v4l2_dev, "Error initing vpbe\n");
+			err = -ENOMEM;
+			goto probe_out;
+		}
+	}
+
+	for (i = 0; i < VPBE_DISPLAY_MAX_DEVICES; i++) {
+		if (init_vpbe_layer(i, disp_dev, pdev)) {
+			err = -ENODEV;
+			goto probe_out;
+		}
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res) {
+		v4l2_err(v4l2_dev, "Unable to get VENC interrupt resource\n");
+		err = -ENODEV;
+		goto probe_out;
+	}
+
+	irq = res->start;
+	err = devm_request_irq(&pdev->dev, irq, venc_isr, 0,
+			       VPBE_DISPLAY_DRIVER, disp_dev);
+	if (err) {
+		v4l2_err(v4l2_dev, "VPBE IRQ request failed\n");
+		goto probe_out;
+	}
+
+	for (i = 0; i < VPBE_DISPLAY_MAX_DEVICES; i++) {
+		/* initialize vb2 queue */
+		q = &disp_dev->dev[i]->buffer_queue;
+		memset(q, 0, sizeof(*q));
+		q->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+		q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
+		q->drv_priv = disp_dev->dev[i];
+		q->ops = &video_qops;
+		q->mem_ops = &vb2_dma_contig_memops;
+		q->buf_struct_size = sizeof(struct vpbe_disp_buffer);
+		q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+		q->min_buffers_needed = 1;
+		q->lock = &disp_dev->dev[i]->opslock;
+		err = vb2_queue_init(q);
+		if (err) {
+			v4l2_err(v4l2_dev, "vb2_queue_init() failed\n");
+			goto probe_out;
+		}
+
+		disp_dev->dev[i]->alloc_ctx =
+			vb2_dma_contig_init_ctx(disp_dev->vpbe_dev->pdev);
+		if (IS_ERR(disp_dev->dev[i]->alloc_ctx)) {
+			v4l2_err(v4l2_dev, "Failed to get the context\n");
+			err = PTR_ERR(disp_dev->dev[i]->alloc_ctx);
+			goto probe_out;
+		}
+
+		INIT_LIST_HEAD(&disp_dev->dev[i]->dma_queue);
+
+		if (register_device(disp_dev->dev[i], disp_dev, pdev)) {
+			err = -ENODEV;
+			goto probe_out;
+		}
+	}
+
+	v4l2_dbg(1, debug, v4l2_dev,
+		 "Successfully completed the probing of vpbe v4l2 device\n");
+
+	return 0;
+
+probe_out:
+	for (k = 0; k < VPBE_DISPLAY_MAX_DEVICES; k++) {
+		/* Unregister video device */
+		if (disp_dev->dev[k] != NULL) {
+			vb2_dma_contig_cleanup_ctx(disp_dev->dev[k]->alloc_ctx);
+			video_unregister_device(&disp_dev->dev[k]->video_dev);
+			kfree(disp_dev->dev[k]);
+		}
+	}
+	return err;
+}
+
+/*
+ * vpbe_display_remove()
+ * It un-register hardware layer from V4L2 driver
+ */
+static int vpbe_display_remove(struct platform_device *pdev)
+{
+	struct vpbe_layer *vpbe_display_layer;
+	struct vpbe_display *disp_dev = platform_get_drvdata(pdev);
+	struct vpbe_device *vpbe_dev = disp_dev->vpbe_dev;
+	int i;
+
+	v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "vpbe_display_remove\n");
+
+	/* deinitialize the vpbe display controller */
+	if (NULL != vpbe_dev->ops.deinitialize)
+		vpbe_dev->ops.deinitialize(&pdev->dev, vpbe_dev);
+	/* un-register device */
+	for (i = 0; i < VPBE_DISPLAY_MAX_DEVICES; i++) {
+		/* Get the pointer to the layer object */
+		vpbe_display_layer = disp_dev->dev[i];
+		vb2_dma_contig_cleanup_ctx(vpbe_display_layer->alloc_ctx);
+		/* Unregister video device */
+		video_unregister_device(&vpbe_display_layer->video_dev);
+
+	}
+	for (i = 0; i < VPBE_DISPLAY_MAX_DEVICES; i++) {
+		kfree(disp_dev->dev[i]);
+		disp_dev->dev[i] = NULL;
+	}
+
+	return 0;
+}
+
+static struct platform_driver vpbe_display_driver = {
+	.driver = {
+		.name = VPBE_DISPLAY_DRIVER,
+		.bus = &platform_bus_type,
+	},
+	.probe = vpbe_display_probe,
+	.remove = vpbe_display_remove,
+};
+
+module_platform_driver(vpbe_display_driver);
+
+MODULE_DESCRIPTION("TI DM644x/DM355/DM365 VPBE Display controller");
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Texas Instruments");
diff --git a/drivers/media/platform/davinci/vpbe_osd.c b/drivers/media/platform/davinci/vpbe_osd.c
new file mode 100644
index 0000000..7d96a4b
--- /dev/null
+++ b/drivers/media/platform/davinci/vpbe_osd.c
@@ -0,0 +1,1596 @@
+/*
+ * Copyright (C) 2007-2010 Texas Instruments Inc
+ * Copyright (C) 2007 MontaVista Software, Inc.
+ *
+ * Andy Lowe (alowe@mvista.com), MontaVista Software
+ * - Initial version
+ * Murali Karicheri (mkaricheri@gmail.com), Texas Instruments Ltd.
+ * - ported to sub device interface
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation version 2.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/slab.h>
+
+#include <mach/cputype.h>
+#include <mach/hardware.h>
+
+#include <media/davinci/vpss.h>
+#include <media/v4l2-device.h>
+#include <media/davinci/vpbe_types.h>
+#include <media/davinci/vpbe_osd.h>
+
+#include <linux/io.h>
+#include "vpbe_osd_regs.h"
+
+#define MODULE_NAME	"davinci-vpbe-osd"
+
+static struct platform_device_id vpbe_osd_devtype[] = {
+	{
+		.name = DM644X_VPBE_OSD_SUBDEV_NAME,
+		.driver_data = VPBE_VERSION_1,
+	}, {
+		.name = DM365_VPBE_OSD_SUBDEV_NAME,
+		.driver_data = VPBE_VERSION_2,
+	}, {
+		.name = DM355_VPBE_OSD_SUBDEV_NAME,
+		.driver_data = VPBE_VERSION_3,
+	},
+	{
+		/* sentinel */
+	}
+};
+
+MODULE_DEVICE_TABLE(platform, vpbe_osd_devtype);
+
+/* register access routines */
+static inline u32 osd_read(struct osd_state *sd, u32 offset)
+{
+	struct osd_state *osd = sd;
+
+	return readl(osd->osd_base + offset);
+}
+
+static inline u32 osd_write(struct osd_state *sd, u32 val, u32 offset)
+{
+	struct osd_state *osd = sd;
+
+	writel(val, osd->osd_base + offset);
+
+	return val;
+}
+
+static inline u32 osd_set(struct osd_state *sd, u32 mask, u32 offset)
+{
+	struct osd_state *osd = sd;
+
+	void __iomem *addr = osd->osd_base + offset;
+	u32 val = readl(addr) | mask;
+
+	writel(val, addr);
+
+	return val;
+}
+
+static inline u32 osd_clear(struct osd_state *sd, u32 mask, u32 offset)
+{
+	struct osd_state *osd = sd;
+
+	void __iomem *addr = osd->osd_base + offset;
+	u32 val = readl(addr) & ~mask;
+
+	writel(val, addr);
+
+	return val;
+}
+
+static inline u32 osd_modify(struct osd_state *sd, u32 mask, u32 val,
+				 u32 offset)
+{
+	struct osd_state *osd = sd;
+
+	void __iomem *addr = osd->osd_base + offset;
+	u32 new_val = (readl(addr) & ~mask) | (val & mask);
+
+	writel(new_val, addr);
+
+	return new_val;
+}
+
+/* define some macros for layer and pixfmt classification */
+#define is_osd_win(layer) (((layer) == WIN_OSD0) || ((layer) == WIN_OSD1))
+#define is_vid_win(layer) (((layer) == WIN_VID0) || ((layer) == WIN_VID1))
+#define is_rgb_pixfmt(pixfmt) \
+	(((pixfmt) == PIXFMT_RGB565) || ((pixfmt) == PIXFMT_RGB888))
+#define is_yc_pixfmt(pixfmt) \
+	(((pixfmt) == PIXFMT_YCBCRI) || ((pixfmt) == PIXFMT_YCRCBI) || \
+	((pixfmt) == PIXFMT_NV12))
+#define MAX_WIN_SIZE OSD_VIDWIN0XP_V0X
+#define MAX_LINE_LENGTH (OSD_VIDWIN0OFST_V0LO << 5)
+
+/**
+ * _osd_dm6446_vid0_pingpong() - field inversion fix for DM6446
+ * @sd - ptr to struct osd_state
+ * @field_inversion - inversion flag
+ * @fb_base_phys - frame buffer address
+ * @lconfig - ptr to layer config
+ *
+ * This routine implements a workaround for the field signal inversion silicon
+ * erratum described in Advisory 1.3.8 for the DM6446.  The fb_base_phys and
+ * lconfig parameters apply to the vid0 window.  This routine should be called
+ * whenever the vid0 layer configuration or start address is modified, or when
+ * the OSD field inversion setting is modified.
+ * Returns: 1 if the ping-pong buffers need to be toggled in the vsync isr, or
+ *          0 otherwise
+ */
+static int _osd_dm6446_vid0_pingpong(struct osd_state *sd,
+				     int field_inversion,
+				     unsigned long fb_base_phys,
+				     const struct osd_layer_config *lconfig)
+{
+	struct osd_platform_data *pdata;
+
+	pdata = (struct osd_platform_data *)sd->dev->platform_data;
+	if (pdata != NULL && pdata->field_inv_wa_enable) {
+
+		if (!field_inversion || !lconfig->interlaced) {
+			osd_write(sd, fb_base_phys & ~0x1F, OSD_VIDWIN0ADR);
+			osd_write(sd, fb_base_phys & ~0x1F, OSD_PPVWIN0ADR);
+			osd_modify(sd, OSD_MISCCTL_PPSW | OSD_MISCCTL_PPRV, 0,
+				   OSD_MISCCTL);
+			return 0;
+		} else {
+			unsigned miscctl = OSD_MISCCTL_PPRV;
+
+			osd_write(sd,
+				(fb_base_phys & ~0x1F) - lconfig->line_length,
+				OSD_VIDWIN0ADR);
+			osd_write(sd,
+				(fb_base_phys & ~0x1F) + lconfig->line_length,
+				OSD_PPVWIN0ADR);
+			osd_modify(sd,
+				OSD_MISCCTL_PPSW | OSD_MISCCTL_PPRV, miscctl,
+				OSD_MISCCTL);
+
+			return 1;
+		}
+	}
+
+	return 0;
+}
+
+static void _osd_set_field_inversion(struct osd_state *sd, int enable)
+{
+	unsigned fsinv = 0;
+
+	if (enable)
+		fsinv = OSD_MODE_FSINV;
+
+	osd_modify(sd, OSD_MODE_FSINV, fsinv, OSD_MODE);
+}
+
+static void _osd_set_blink_attribute(struct osd_state *sd, int enable,
+				     enum osd_blink_interval blink)
+{
+	u32 osdatrmd = 0;
+
+	if (enable) {
+		osdatrmd |= OSD_OSDATRMD_BLNK;
+		osdatrmd |= blink << OSD_OSDATRMD_BLNKINT_SHIFT;
+	}
+	/* caller must ensure that OSD1 is configured in attribute mode */
+	osd_modify(sd, OSD_OSDATRMD_BLNKINT | OSD_OSDATRMD_BLNK, osdatrmd,
+		  OSD_OSDATRMD);
+}
+
+static void _osd_set_rom_clut(struct osd_state *sd,
+			      enum osd_rom_clut rom_clut)
+{
+	if (rom_clut == ROM_CLUT0)
+		osd_clear(sd, OSD_MISCCTL_RSEL, OSD_MISCCTL);
+	else
+		osd_set(sd, OSD_MISCCTL_RSEL, OSD_MISCCTL);
+}
+
+static void _osd_set_palette_map(struct osd_state *sd,
+				 enum osd_win_layer osdwin,
+				 unsigned char pixel_value,
+				 unsigned char clut_index,
+				 enum osd_pix_format pixfmt)
+{
+	static const int map_2bpp[] = { 0, 5, 10, 15 };
+	static const int map_1bpp[] = { 0, 15 };
+	int bmp_offset;
+	int bmp_shift;
+	int bmp_mask;
+	int bmp_reg;
+
+	switch (pixfmt) {
+	case PIXFMT_1BPP:
+		bmp_reg = map_1bpp[pixel_value & 0x1];
+		break;
+	case PIXFMT_2BPP:
+		bmp_reg = map_2bpp[pixel_value & 0x3];
+		break;
+	case PIXFMT_4BPP:
+		bmp_reg = pixel_value & 0xf;
+		break;
+	default:
+		return;
+	}
+
+	switch (osdwin) {
+	case OSDWIN_OSD0:
+		bmp_offset = OSD_W0BMP01 + (bmp_reg >> 1) * sizeof(u32);
+		break;
+	case OSDWIN_OSD1:
+		bmp_offset = OSD_W1BMP01 + (bmp_reg >> 1) * sizeof(u32);
+		break;
+	default:
+		return;
+	}
+
+	if (bmp_reg & 1) {
+		bmp_shift = 8;
+		bmp_mask = 0xff << 8;
+	} else {
+		bmp_shift = 0;
+		bmp_mask = 0xff;
+	}
+
+	osd_modify(sd, bmp_mask, clut_index << bmp_shift, bmp_offset);
+}
+
+static void _osd_set_rec601_attenuation(struct osd_state *sd,
+					enum osd_win_layer osdwin, int enable)
+{
+	switch (osdwin) {
+	case OSDWIN_OSD0:
+		osd_modify(sd, OSD_OSDWIN0MD_ATN0E,
+			  enable ? OSD_OSDWIN0MD_ATN0E : 0,
+			  OSD_OSDWIN0MD);
+		if (sd->vpbe_type == VPBE_VERSION_1)
+			osd_modify(sd, OSD_OSDWIN0MD_ATN0E,
+				  enable ? OSD_OSDWIN0MD_ATN0E : 0,
+				  OSD_OSDWIN0MD);
+		else if ((sd->vpbe_type == VPBE_VERSION_3) ||
+			   (sd->vpbe_type == VPBE_VERSION_2))
+			osd_modify(sd, OSD_EXTMODE_ATNOSD0EN,
+				  enable ? OSD_EXTMODE_ATNOSD0EN : 0,
+				  OSD_EXTMODE);
+		break;
+	case OSDWIN_OSD1:
+		osd_modify(sd, OSD_OSDWIN1MD_ATN1E,
+			  enable ? OSD_OSDWIN1MD_ATN1E : 0,
+			  OSD_OSDWIN1MD);
+		if (sd->vpbe_type == VPBE_VERSION_1)
+			osd_modify(sd, OSD_OSDWIN1MD_ATN1E,
+				  enable ? OSD_OSDWIN1MD_ATN1E : 0,
+				  OSD_OSDWIN1MD);
+		else if ((sd->vpbe_type == VPBE_VERSION_3) ||
+			   (sd->vpbe_type == VPBE_VERSION_2))
+			osd_modify(sd, OSD_EXTMODE_ATNOSD1EN,
+				  enable ? OSD_EXTMODE_ATNOSD1EN : 0,
+				  OSD_EXTMODE);
+		break;
+	}
+}
+
+static void _osd_set_blending_factor(struct osd_state *sd,
+				     enum osd_win_layer osdwin,
+				     enum osd_blending_factor blend)
+{
+	switch (osdwin) {
+	case OSDWIN_OSD0:
+		osd_modify(sd, OSD_OSDWIN0MD_BLND0,
+			  blend << OSD_OSDWIN0MD_BLND0_SHIFT, OSD_OSDWIN0MD);
+		break;
+	case OSDWIN_OSD1:
+		osd_modify(sd, OSD_OSDWIN1MD_BLND1,
+			  blend << OSD_OSDWIN1MD_BLND1_SHIFT, OSD_OSDWIN1MD);
+		break;
+	}
+}
+
+static void _osd_enable_rgb888_pixblend(struct osd_state *sd,
+					enum osd_win_layer osdwin)
+{
+
+	osd_modify(sd, OSD_MISCCTL_BLDSEL, 0, OSD_MISCCTL);
+	switch (osdwin) {
+	case OSDWIN_OSD0:
+		osd_modify(sd, OSD_EXTMODE_OSD0BLDCHR,
+			  OSD_EXTMODE_OSD0BLDCHR, OSD_EXTMODE);
+		break;
+	case OSDWIN_OSD1:
+		osd_modify(sd, OSD_EXTMODE_OSD1BLDCHR,
+			  OSD_EXTMODE_OSD1BLDCHR, OSD_EXTMODE);
+		break;
+	}
+}
+
+static void _osd_enable_color_key(struct osd_state *sd,
+				  enum osd_win_layer osdwin,
+				  unsigned colorkey,
+				  enum osd_pix_format pixfmt)
+{
+	switch (pixfmt) {
+	case PIXFMT_1BPP:
+	case PIXFMT_2BPP:
+	case PIXFMT_4BPP:
+	case PIXFMT_8BPP:
+		if (sd->vpbe_type == VPBE_VERSION_3) {
+			switch (osdwin) {
+			case OSDWIN_OSD0:
+				osd_modify(sd, OSD_TRANSPBMPIDX_BMP0,
+					  colorkey <<
+					  OSD_TRANSPBMPIDX_BMP0_SHIFT,
+					  OSD_TRANSPBMPIDX);
+				break;
+			case OSDWIN_OSD1:
+				osd_modify(sd, OSD_TRANSPBMPIDX_BMP1,
+					  colorkey <<
+					  OSD_TRANSPBMPIDX_BMP1_SHIFT,
+					  OSD_TRANSPBMPIDX);
+				break;
+			}
+		}
+		break;
+	case PIXFMT_RGB565:
+		if (sd->vpbe_type == VPBE_VERSION_1)
+			osd_write(sd, colorkey & OSD_TRANSPVAL_RGBTRANS,
+				  OSD_TRANSPVAL);
+		else if (sd->vpbe_type == VPBE_VERSION_3)
+			osd_write(sd, colorkey & OSD_TRANSPVALL_RGBL,
+				  OSD_TRANSPVALL);
+		break;
+	case PIXFMT_YCBCRI:
+	case PIXFMT_YCRCBI:
+		if (sd->vpbe_type == VPBE_VERSION_3)
+			osd_modify(sd, OSD_TRANSPVALU_Y, colorkey,
+				   OSD_TRANSPVALU);
+		break;
+	case PIXFMT_RGB888:
+		if (sd->vpbe_type == VPBE_VERSION_3) {
+			osd_write(sd, colorkey & OSD_TRANSPVALL_RGBL,
+				  OSD_TRANSPVALL);
+			osd_modify(sd, OSD_TRANSPVALU_RGBU, colorkey >> 16,
+				  OSD_TRANSPVALU);
+		}
+		break;
+	default:
+		break;
+	}
+
+	switch (osdwin) {
+	case OSDWIN_OSD0:
+		osd_set(sd, OSD_OSDWIN0MD_TE0, OSD_OSDWIN0MD);
+		break;
+	case OSDWIN_OSD1:
+		osd_set(sd, OSD_OSDWIN1MD_TE1, OSD_OSDWIN1MD);
+		break;
+	}
+}
+
+static void _osd_disable_color_key(struct osd_state *sd,
+				   enum osd_win_layer osdwin)
+{
+	switch (osdwin) {
+	case OSDWIN_OSD0:
+		osd_clear(sd, OSD_OSDWIN0MD_TE0, OSD_OSDWIN0MD);
+		break;
+	case OSDWIN_OSD1:
+		osd_clear(sd, OSD_OSDWIN1MD_TE1, OSD_OSDWIN1MD);
+		break;
+	}
+}
+
+static void _osd_set_osd_clut(struct osd_state *sd,
+			      enum osd_win_layer osdwin,
+			      enum osd_clut clut)
+{
+	u32 winmd = 0;
+
+	switch (osdwin) {
+	case OSDWIN_OSD0:
+		if (clut == RAM_CLUT)
+			winmd |= OSD_OSDWIN0MD_CLUTS0;
+		osd_modify(sd, OSD_OSDWIN0MD_CLUTS0, winmd, OSD_OSDWIN0MD);
+		break;
+	case OSDWIN_OSD1:
+		if (clut == RAM_CLUT)
+			winmd |= OSD_OSDWIN1MD_CLUTS1;
+		osd_modify(sd, OSD_OSDWIN1MD_CLUTS1, winmd, OSD_OSDWIN1MD);
+		break;
+	}
+}
+
+static void _osd_set_zoom(struct osd_state *sd, enum osd_layer layer,
+			  enum osd_zoom_factor h_zoom,
+			  enum osd_zoom_factor v_zoom)
+{
+	u32 winmd = 0;
+
+	switch (layer) {
+	case WIN_OSD0:
+		winmd |= (h_zoom << OSD_OSDWIN0MD_OHZ0_SHIFT);
+		winmd |= (v_zoom << OSD_OSDWIN0MD_OVZ0_SHIFT);
+		osd_modify(sd, OSD_OSDWIN0MD_OHZ0 | OSD_OSDWIN0MD_OVZ0, winmd,
+			  OSD_OSDWIN0MD);
+		break;
+	case WIN_VID0:
+		winmd |= (h_zoom << OSD_VIDWINMD_VHZ0_SHIFT);
+		winmd |= (v_zoom << OSD_VIDWINMD_VVZ0_SHIFT);
+		osd_modify(sd, OSD_VIDWINMD_VHZ0 | OSD_VIDWINMD_VVZ0, winmd,
+			  OSD_VIDWINMD);
+		break;
+	case WIN_OSD1:
+		winmd |= (h_zoom << OSD_OSDWIN1MD_OHZ1_SHIFT);
+		winmd |= (v_zoom << OSD_OSDWIN1MD_OVZ1_SHIFT);
+		osd_modify(sd, OSD_OSDWIN1MD_OHZ1 | OSD_OSDWIN1MD_OVZ1, winmd,
+			  OSD_OSDWIN1MD);
+		break;
+	case WIN_VID1:
+		winmd |= (h_zoom << OSD_VIDWINMD_VHZ1_SHIFT);
+		winmd |= (v_zoom << OSD_VIDWINMD_VVZ1_SHIFT);
+		osd_modify(sd, OSD_VIDWINMD_VHZ1 | OSD_VIDWINMD_VVZ1, winmd,
+			  OSD_VIDWINMD);
+		break;
+	}
+}
+
+static void _osd_disable_layer(struct osd_state *sd, enum osd_layer layer)
+{
+	switch (layer) {
+	case WIN_OSD0:
+		osd_clear(sd, OSD_OSDWIN0MD_OACT0, OSD_OSDWIN0MD);
+		break;
+	case WIN_VID0:
+		osd_clear(sd, OSD_VIDWINMD_ACT0, OSD_VIDWINMD);
+		break;
+	case WIN_OSD1:
+		/* disable attribute mode as well as disabling the window */
+		osd_clear(sd, OSD_OSDWIN1MD_OASW | OSD_OSDWIN1MD_OACT1,
+			  OSD_OSDWIN1MD);
+		break;
+	case WIN_VID1:
+		osd_clear(sd, OSD_VIDWINMD_ACT1, OSD_VIDWINMD);
+		break;
+	}
+}
+
+static void osd_disable_layer(struct osd_state *sd, enum osd_layer layer)
+{
+	struct osd_state *osd = sd;
+	struct osd_window_state *win = &osd->win[layer];
+	unsigned long flags;
+
+	spin_lock_irqsave(&osd->lock, flags);
+
+	if (!win->is_enabled) {
+		spin_unlock_irqrestore(&osd->lock, flags);
+		return;
+	}
+	win->is_enabled = 0;
+
+	_osd_disable_layer(sd, layer);
+
+	spin_unlock_irqrestore(&osd->lock, flags);
+}
+
+static void _osd_enable_attribute_mode(struct osd_state *sd)
+{
+	/* enable attribute mode for OSD1 */
+	osd_set(sd, OSD_OSDWIN1MD_OASW, OSD_OSDWIN1MD);
+}
+
+static void _osd_enable_layer(struct osd_state *sd, enum osd_layer layer)
+{
+	switch (layer) {
+	case WIN_OSD0:
+		osd_set(sd, OSD_OSDWIN0MD_OACT0, OSD_OSDWIN0MD);
+		break;
+	case WIN_VID0:
+		osd_set(sd, OSD_VIDWINMD_ACT0, OSD_VIDWINMD);
+		break;
+	case WIN_OSD1:
+		/* enable OSD1 and disable attribute mode */
+		osd_modify(sd, OSD_OSDWIN1MD_OASW | OSD_OSDWIN1MD_OACT1,
+			  OSD_OSDWIN1MD_OACT1, OSD_OSDWIN1MD);
+		break;
+	case WIN_VID1:
+		osd_set(sd, OSD_VIDWINMD_ACT1, OSD_VIDWINMD);
+		break;
+	}
+}
+
+static int osd_enable_layer(struct osd_state *sd, enum osd_layer layer,
+			    int otherwin)
+{
+	struct osd_state *osd = sd;
+	struct osd_window_state *win = &osd->win[layer];
+	struct osd_layer_config *cfg = &win->lconfig;
+	unsigned long flags;
+
+	spin_lock_irqsave(&osd->lock, flags);
+
+	/*
+	 * use otherwin flag to know this is the other vid window
+	 * in YUV420 mode, if is, skip this check
+	 */
+	if (!otherwin && (!win->is_allocated ||
+			!win->fb_base_phys ||
+			!cfg->line_length ||
+			!cfg->xsize ||
+			!cfg->ysize)) {
+		spin_unlock_irqrestore(&osd->lock, flags);
+		return -1;
+	}
+
+	if (win->is_enabled) {
+		spin_unlock_irqrestore(&osd->lock, flags);
+		return 0;
+	}
+	win->is_enabled = 1;
+
+	if (cfg->pixfmt != PIXFMT_OSD_ATTR)
+		_osd_enable_layer(sd, layer);
+	else {
+		_osd_enable_attribute_mode(sd);
+		_osd_set_blink_attribute(sd, osd->is_blinking, osd->blink);
+	}
+
+	spin_unlock_irqrestore(&osd->lock, flags);
+
+	return 0;
+}
+
+#define OSD_SRC_ADDR_HIGH4	0x7800000
+#define OSD_SRC_ADDR_HIGH7	0x7F0000
+#define OSD_SRCADD_OFSET_SFT	23
+#define OSD_SRCADD_ADD_SFT	16
+#define OSD_WINADL_MASK		0xFFFF
+#define OSD_WINOFST_MASK	0x1000
+#define VPBE_REG_BASE		0x80000000
+
+static void _osd_start_layer(struct osd_state *sd, enum osd_layer layer,
+			     unsigned long fb_base_phys,
+			     unsigned long cbcr_ofst)
+{
+
+	if (sd->vpbe_type == VPBE_VERSION_1) {
+		switch (layer) {
+		case WIN_OSD0:
+			osd_write(sd, fb_base_phys & ~0x1F, OSD_OSDWIN0ADR);
+			break;
+		case WIN_VID0:
+			osd_write(sd, fb_base_phys & ~0x1F, OSD_VIDWIN0ADR);
+			break;
+		case WIN_OSD1:
+			osd_write(sd, fb_base_phys & ~0x1F, OSD_OSDWIN1ADR);
+			break;
+		case WIN_VID1:
+			osd_write(sd, fb_base_phys & ~0x1F, OSD_VIDWIN1ADR);
+			break;
+	      }
+	} else if (sd->vpbe_type == VPBE_VERSION_3) {
+		unsigned long fb_offset_32 =
+		    (fb_base_phys - VPBE_REG_BASE) >> 5;
+
+		switch (layer) {
+		case WIN_OSD0:
+			osd_modify(sd, OSD_OSDWINADH_O0AH,
+				  fb_offset_32 >> (OSD_SRCADD_ADD_SFT -
+						   OSD_OSDWINADH_O0AH_SHIFT),
+				  OSD_OSDWINADH);
+			osd_write(sd, fb_offset_32 & OSD_OSDWIN0ADL_O0AL,
+				  OSD_OSDWIN0ADL);
+			break;
+		case WIN_VID0:
+			osd_modify(sd, OSD_VIDWINADH_V0AH,
+				  fb_offset_32 >> (OSD_SRCADD_ADD_SFT -
+						   OSD_VIDWINADH_V0AH_SHIFT),
+				  OSD_VIDWINADH);
+			osd_write(sd, fb_offset_32 & OSD_VIDWIN0ADL_V0AL,
+				  OSD_VIDWIN0ADL);
+			break;
+		case WIN_OSD1:
+			osd_modify(sd, OSD_OSDWINADH_O1AH,
+				  fb_offset_32 >> (OSD_SRCADD_ADD_SFT -
+						   OSD_OSDWINADH_O1AH_SHIFT),
+				  OSD_OSDWINADH);
+			osd_write(sd, fb_offset_32 & OSD_OSDWIN1ADL_O1AL,
+				  OSD_OSDWIN1ADL);
+			break;
+		case WIN_VID1:
+			osd_modify(sd, OSD_VIDWINADH_V1AH,
+				  fb_offset_32 >> (OSD_SRCADD_ADD_SFT -
+						   OSD_VIDWINADH_V1AH_SHIFT),
+				  OSD_VIDWINADH);
+			osd_write(sd, fb_offset_32 & OSD_VIDWIN1ADL_V1AL,
+				  OSD_VIDWIN1ADL);
+			break;
+		}
+	} else if (sd->vpbe_type == VPBE_VERSION_2) {
+		struct osd_window_state *win = &sd->win[layer];
+		unsigned long fb_offset_32, cbcr_offset_32;
+
+		fb_offset_32 = fb_base_phys - VPBE_REG_BASE;
+		if (cbcr_ofst)
+			cbcr_offset_32 = cbcr_ofst;
+		else
+			cbcr_offset_32 = win->lconfig.line_length *
+					 win->lconfig.ysize;
+		cbcr_offset_32 += fb_offset_32;
+		fb_offset_32 = fb_offset_32 >> 5;
+		cbcr_offset_32 = cbcr_offset_32 >> 5;
+		/*
+		 * DM365: start address is 27-bit long address b26 - b23 are
+		 * in offset register b12 - b9, and * bit 26 has to be '1'
+		 */
+		if (win->lconfig.pixfmt == PIXFMT_NV12) {
+			switch (layer) {
+			case WIN_VID0:
+			case WIN_VID1:
+				/* Y is in VID0 */
+				osd_modify(sd, OSD_VIDWIN0OFST_V0AH,
+					 ((fb_offset_32 & OSD_SRC_ADDR_HIGH4) >>
+					 (OSD_SRCADD_OFSET_SFT -
+					 OSD_WINOFST_AH_SHIFT)) |
+					 OSD_WINOFST_MASK, OSD_VIDWIN0OFST);
+				osd_modify(sd, OSD_VIDWINADH_V0AH,
+					  (fb_offset_32 & OSD_SRC_ADDR_HIGH7) >>
+					  (OSD_SRCADD_ADD_SFT -
+					  OSD_VIDWINADH_V0AH_SHIFT),
+					   OSD_VIDWINADH);
+				osd_write(sd, fb_offset_32 & OSD_WINADL_MASK,
+					  OSD_VIDWIN0ADL);
+				/* CbCr is in VID1 */
+				osd_modify(sd, OSD_VIDWIN1OFST_V1AH,
+					 ((cbcr_offset_32 &
+					 OSD_SRC_ADDR_HIGH4) >>
+					 (OSD_SRCADD_OFSET_SFT -
+					 OSD_WINOFST_AH_SHIFT)) |
+					 OSD_WINOFST_MASK, OSD_VIDWIN1OFST);
+				osd_modify(sd, OSD_VIDWINADH_V1AH,
+					  (cbcr_offset_32 &
+					  OSD_SRC_ADDR_HIGH7) >>
+					  (OSD_SRCADD_ADD_SFT -
+					  OSD_VIDWINADH_V1AH_SHIFT),
+					  OSD_VIDWINADH);
+				osd_write(sd, cbcr_offset_32 & OSD_WINADL_MASK,
+					  OSD_VIDWIN1ADL);
+				break;
+			default:
+				break;
+			}
+		}
+
+		switch (layer) {
+		case WIN_OSD0:
+			osd_modify(sd, OSD_OSDWIN0OFST_O0AH,
+				 ((fb_offset_32 & OSD_SRC_ADDR_HIGH4) >>
+				 (OSD_SRCADD_OFSET_SFT -
+				 OSD_WINOFST_AH_SHIFT)) | OSD_WINOFST_MASK,
+				  OSD_OSDWIN0OFST);
+			osd_modify(sd, OSD_OSDWINADH_O0AH,
+				 (fb_offset_32 & OSD_SRC_ADDR_HIGH7) >>
+				 (OSD_SRCADD_ADD_SFT -
+				 OSD_OSDWINADH_O0AH_SHIFT), OSD_OSDWINADH);
+			osd_write(sd, fb_offset_32 & OSD_WINADL_MASK,
+					OSD_OSDWIN0ADL);
+			break;
+		case WIN_VID0:
+			if (win->lconfig.pixfmt != PIXFMT_NV12) {
+				osd_modify(sd, OSD_VIDWIN0OFST_V0AH,
+					 ((fb_offset_32 & OSD_SRC_ADDR_HIGH4) >>
+					 (OSD_SRCADD_OFSET_SFT -
+					 OSD_WINOFST_AH_SHIFT)) |
+					 OSD_WINOFST_MASK, OSD_VIDWIN0OFST);
+				osd_modify(sd, OSD_VIDWINADH_V0AH,
+					  (fb_offset_32 & OSD_SRC_ADDR_HIGH7) >>
+					  (OSD_SRCADD_ADD_SFT -
+					  OSD_VIDWINADH_V0AH_SHIFT),
+					  OSD_VIDWINADH);
+				osd_write(sd, fb_offset_32 & OSD_WINADL_MASK,
+					  OSD_VIDWIN0ADL);
+			}
+			break;
+		case WIN_OSD1:
+			osd_modify(sd, OSD_OSDWIN1OFST_O1AH,
+				 ((fb_offset_32 & OSD_SRC_ADDR_HIGH4) >>
+				 (OSD_SRCADD_OFSET_SFT -
+				 OSD_WINOFST_AH_SHIFT)) | OSD_WINOFST_MASK,
+				  OSD_OSDWIN1OFST);
+			osd_modify(sd, OSD_OSDWINADH_O1AH,
+				  (fb_offset_32 & OSD_SRC_ADDR_HIGH7) >>
+				  (OSD_SRCADD_ADD_SFT -
+				  OSD_OSDWINADH_O1AH_SHIFT),
+				  OSD_OSDWINADH);
+			osd_write(sd, fb_offset_32 & OSD_WINADL_MASK,
+					OSD_OSDWIN1ADL);
+			break;
+		case WIN_VID1:
+			if (win->lconfig.pixfmt != PIXFMT_NV12) {
+				osd_modify(sd, OSD_VIDWIN1OFST_V1AH,
+					 ((fb_offset_32 & OSD_SRC_ADDR_HIGH4) >>
+					 (OSD_SRCADD_OFSET_SFT -
+					 OSD_WINOFST_AH_SHIFT)) |
+					 OSD_WINOFST_MASK, OSD_VIDWIN1OFST);
+				osd_modify(sd, OSD_VIDWINADH_V1AH,
+					  (fb_offset_32 & OSD_SRC_ADDR_HIGH7) >>
+					  (OSD_SRCADD_ADD_SFT -
+					  OSD_VIDWINADH_V1AH_SHIFT),
+					  OSD_VIDWINADH);
+				osd_write(sd, fb_offset_32 & OSD_WINADL_MASK,
+					  OSD_VIDWIN1ADL);
+			}
+			break;
+		}
+	}
+}
+
+static void osd_start_layer(struct osd_state *sd, enum osd_layer layer,
+			    unsigned long fb_base_phys,
+			    unsigned long cbcr_ofst)
+{
+	struct osd_state *osd = sd;
+	struct osd_window_state *win = &osd->win[layer];
+	struct osd_layer_config *cfg = &win->lconfig;
+	unsigned long flags;
+
+	spin_lock_irqsave(&osd->lock, flags);
+
+	win->fb_base_phys = fb_base_phys & ~0x1F;
+	_osd_start_layer(sd, layer, fb_base_phys, cbcr_ofst);
+
+	if (layer == WIN_VID0) {
+		osd->pingpong =
+		    _osd_dm6446_vid0_pingpong(sd, osd->field_inversion,
+						       win->fb_base_phys,
+						       cfg);
+	}
+
+	spin_unlock_irqrestore(&osd->lock, flags);
+}
+
+static void osd_get_layer_config(struct osd_state *sd, enum osd_layer layer,
+				 struct osd_layer_config *lconfig)
+{
+	struct osd_state *osd = sd;
+	struct osd_window_state *win = &osd->win[layer];
+	unsigned long flags;
+
+	spin_lock_irqsave(&osd->lock, flags);
+
+	*lconfig = win->lconfig;
+
+	spin_unlock_irqrestore(&osd->lock, flags);
+}
+
+/**
+ * try_layer_config() - Try a specific configuration for the layer
+ * @sd  - ptr to struct osd_state
+ * @layer - layer to configure
+ * @lconfig - layer configuration to try
+ *
+ * If the requested lconfig is completely rejected and the value of lconfig on
+ * exit is the current lconfig, then try_layer_config() returns 1.  Otherwise,
+ * try_layer_config() returns 0.  A return value of 0 does not necessarily mean
+ * that the value of lconfig on exit is identical to the value of lconfig on
+ * entry, but merely that it represents a change from the current lconfig.
+ */
+static int try_layer_config(struct osd_state *sd, enum osd_layer layer,
+			    struct osd_layer_config *lconfig)
+{
+	struct osd_state *osd = sd;
+	struct osd_window_state *win = &osd->win[layer];
+	int bad_config = 0;
+
+	/* verify that the pixel format is compatible with the layer */
+	switch (lconfig->pixfmt) {
+	case PIXFMT_1BPP:
+	case PIXFMT_2BPP:
+	case PIXFMT_4BPP:
+	case PIXFMT_8BPP:
+	case PIXFMT_RGB565:
+		if (osd->vpbe_type == VPBE_VERSION_1)
+			bad_config = !is_vid_win(layer);
+		break;
+	case PIXFMT_YCBCRI:
+	case PIXFMT_YCRCBI:
+		bad_config = !is_vid_win(layer);
+		break;
+	case PIXFMT_RGB888:
+		if (osd->vpbe_type == VPBE_VERSION_1)
+			bad_config = !is_vid_win(layer);
+		else if ((osd->vpbe_type == VPBE_VERSION_3) ||
+			 (osd->vpbe_type == VPBE_VERSION_2))
+			bad_config = !is_osd_win(layer);
+		break;
+	case PIXFMT_NV12:
+		if (osd->vpbe_type != VPBE_VERSION_2)
+			bad_config = 1;
+		else
+			bad_config = is_osd_win(layer);
+		break;
+	case PIXFMT_OSD_ATTR:
+		bad_config = (layer != WIN_OSD1);
+		break;
+	default:
+		bad_config = 1;
+		break;
+	}
+	if (bad_config) {
+		/*
+		 * The requested pixel format is incompatible with the layer,
+		 * so keep the current layer configuration.
+		 */
+		*lconfig = win->lconfig;
+		return bad_config;
+	}
+
+	/* DM6446: */
+	/* only one OSD window at a time can use RGB pixel formats */
+	  if ((osd->vpbe_type == VPBE_VERSION_1) &&
+		  is_osd_win(layer) && is_rgb_pixfmt(lconfig->pixfmt)) {
+		enum osd_pix_format pixfmt;
+		if (layer == WIN_OSD0)
+			pixfmt = osd->win[WIN_OSD1].lconfig.pixfmt;
+		else
+			pixfmt = osd->win[WIN_OSD0].lconfig.pixfmt;
+
+		if (is_rgb_pixfmt(pixfmt)) {
+			/*
+			 * The other OSD window is already configured for an
+			 * RGB, so keep the current layer configuration.
+			 */
+			*lconfig = win->lconfig;
+			return 1;
+		}
+	}
+
+	/* DM6446: only one video window at a time can use RGB888 */
+	if ((osd->vpbe_type == VPBE_VERSION_1) && is_vid_win(layer) &&
+		lconfig->pixfmt == PIXFMT_RGB888) {
+		enum osd_pix_format pixfmt;
+
+		if (layer == WIN_VID0)
+			pixfmt = osd->win[WIN_VID1].lconfig.pixfmt;
+		else
+			pixfmt = osd->win[WIN_VID0].lconfig.pixfmt;
+
+		if (pixfmt == PIXFMT_RGB888) {
+			/*
+			 * The other video window is already configured for
+			 * RGB888, so keep the current layer configuration.
+			 */
+			*lconfig = win->lconfig;
+			return 1;
+		}
+	}
+
+	/* window dimensions must be non-zero */
+	if (!lconfig->line_length || !lconfig->xsize || !lconfig->ysize) {
+		*lconfig = win->lconfig;
+		return 1;
+	}
+
+	/* round line_length up to a multiple of 32 */
+	lconfig->line_length = ((lconfig->line_length + 31) / 32) * 32;
+	lconfig->line_length =
+	    min(lconfig->line_length, (unsigned)MAX_LINE_LENGTH);
+	lconfig->xsize = min(lconfig->xsize, (unsigned)MAX_WIN_SIZE);
+	lconfig->ysize = min(lconfig->ysize, (unsigned)MAX_WIN_SIZE);
+	lconfig->xpos = min(lconfig->xpos, (unsigned)MAX_WIN_SIZE);
+	lconfig->ypos = min(lconfig->ypos, (unsigned)MAX_WIN_SIZE);
+	lconfig->interlaced = (lconfig->interlaced != 0);
+	if (lconfig->interlaced) {
+		/* ysize and ypos must be even for interlaced displays */
+		lconfig->ysize &= ~1;
+		lconfig->ypos &= ~1;
+	}
+
+	return 0;
+}
+
+static void _osd_disable_vid_rgb888(struct osd_state *sd)
+{
+	/*
+	 * The DM6446 supports RGB888 pixel format in a single video window.
+	 * This routine disables RGB888 pixel format for both video windows.
+	 * The caller must ensure that neither video window is currently
+	 * configured for RGB888 pixel format.
+	 */
+	if (sd->vpbe_type == VPBE_VERSION_1)
+		osd_clear(sd, OSD_MISCCTL_RGBEN, OSD_MISCCTL);
+}
+
+static void _osd_enable_vid_rgb888(struct osd_state *sd,
+				   enum osd_layer layer)
+{
+	/*
+	 * The DM6446 supports RGB888 pixel format in a single video window.
+	 * This routine enables RGB888 pixel format for the specified video
+	 * window.  The caller must ensure that the other video window is not
+	 * currently configured for RGB888 pixel format, as this routine will
+	 * disable RGB888 pixel format for the other window.
+	 */
+	if (sd->vpbe_type == VPBE_VERSION_1) {
+		if (layer == WIN_VID0)
+			osd_modify(sd, OSD_MISCCTL_RGBEN | OSD_MISCCTL_RGBWIN,
+				  OSD_MISCCTL_RGBEN, OSD_MISCCTL);
+		else if (layer == WIN_VID1)
+			osd_modify(sd, OSD_MISCCTL_RGBEN | OSD_MISCCTL_RGBWIN,
+				  OSD_MISCCTL_RGBEN | OSD_MISCCTL_RGBWIN,
+				  OSD_MISCCTL);
+	}
+}
+
+static void _osd_set_cbcr_order(struct osd_state *sd,
+				enum osd_pix_format pixfmt)
+{
+	/*
+	 * The caller must ensure that all windows using YC pixfmt use the same
+	 * Cb/Cr order.
+	 */
+	if (pixfmt == PIXFMT_YCBCRI)
+		osd_clear(sd, OSD_MODE_CS, OSD_MODE);
+	else if (pixfmt == PIXFMT_YCRCBI)
+		osd_set(sd, OSD_MODE_CS, OSD_MODE);
+}
+
+static void _osd_set_layer_config(struct osd_state *sd, enum osd_layer layer,
+				  const struct osd_layer_config *lconfig)
+{
+	u32 winmd = 0, winmd_mask = 0, bmw = 0;
+
+	_osd_set_cbcr_order(sd, lconfig->pixfmt);
+
+	switch (layer) {
+	case WIN_OSD0:
+		if (sd->vpbe_type == VPBE_VERSION_1) {
+			winmd_mask |= OSD_OSDWIN0MD_RGB0E;
+			if (lconfig->pixfmt == PIXFMT_RGB565)
+				winmd |= OSD_OSDWIN0MD_RGB0E;
+		} else if ((sd->vpbe_type == VPBE_VERSION_3) ||
+		  (sd->vpbe_type == VPBE_VERSION_2)) {
+			winmd_mask |= OSD_OSDWIN0MD_BMP0MD;
+			switch (lconfig->pixfmt) {
+			case PIXFMT_RGB565:
+					winmd |= (1 <<
+					OSD_OSDWIN0MD_BMP0MD_SHIFT);
+					break;
+			case PIXFMT_RGB888:
+				winmd |= (2 << OSD_OSDWIN0MD_BMP0MD_SHIFT);
+				_osd_enable_rgb888_pixblend(sd, OSDWIN_OSD0);
+				break;
+			case PIXFMT_YCBCRI:
+			case PIXFMT_YCRCBI:
+				winmd |= (3 << OSD_OSDWIN0MD_BMP0MD_SHIFT);
+				break;
+			default:
+				break;
+			}
+		}
+
+		winmd_mask |= OSD_OSDWIN0MD_BMW0 | OSD_OSDWIN0MD_OFF0;
+
+		switch (lconfig->pixfmt) {
+		case PIXFMT_1BPP:
+			bmw = 0;
+			break;
+		case PIXFMT_2BPP:
+			bmw = 1;
+			break;
+		case PIXFMT_4BPP:
+			bmw = 2;
+			break;
+		case PIXFMT_8BPP:
+			bmw = 3;
+			break;
+		default:
+			break;
+		}
+		winmd |= (bmw << OSD_OSDWIN0MD_BMW0_SHIFT);
+
+		if (lconfig->interlaced)
+			winmd |= OSD_OSDWIN0MD_OFF0;
+
+		osd_modify(sd, winmd_mask, winmd, OSD_OSDWIN0MD);
+		osd_write(sd, lconfig->line_length >> 5, OSD_OSDWIN0OFST);
+		osd_write(sd, lconfig->xpos, OSD_OSDWIN0XP);
+		osd_write(sd, lconfig->xsize, OSD_OSDWIN0XL);
+		if (lconfig->interlaced) {
+			osd_write(sd, lconfig->ypos >> 1, OSD_OSDWIN0YP);
+			osd_write(sd, lconfig->ysize >> 1, OSD_OSDWIN0YL);
+		} else {
+			osd_write(sd, lconfig->ypos, OSD_OSDWIN0YP);
+			osd_write(sd, lconfig->ysize, OSD_OSDWIN0YL);
+		}
+		break;
+	case WIN_VID0:
+		winmd_mask |= OSD_VIDWINMD_VFF0;
+		if (lconfig->interlaced)
+			winmd |= OSD_VIDWINMD_VFF0;
+
+		osd_modify(sd, winmd_mask, winmd, OSD_VIDWINMD);
+		osd_write(sd, lconfig->line_length >> 5, OSD_VIDWIN0OFST);
+		osd_write(sd, lconfig->xpos, OSD_VIDWIN0XP);
+		osd_write(sd, lconfig->xsize, OSD_VIDWIN0XL);
+		/*
+		 * For YUV420P format the register contents are
+		 * duplicated in both VID registers
+		 */
+		if ((sd->vpbe_type == VPBE_VERSION_2) &&
+				(lconfig->pixfmt == PIXFMT_NV12)) {
+			/* other window also */
+			if (lconfig->interlaced) {
+				winmd_mask |= OSD_VIDWINMD_VFF1;
+				winmd |= OSD_VIDWINMD_VFF1;
+				osd_modify(sd, winmd_mask, winmd,
+					  OSD_VIDWINMD);
+			}
+
+			osd_modify(sd, OSD_MISCCTL_S420D,
+				    OSD_MISCCTL_S420D, OSD_MISCCTL);
+			osd_write(sd, lconfig->line_length >> 5,
+				  OSD_VIDWIN1OFST);
+			osd_write(sd, lconfig->xpos, OSD_VIDWIN1XP);
+			osd_write(sd, lconfig->xsize, OSD_VIDWIN1XL);
+			/*
+			  * if NV21 pixfmt and line length not 32B
+			  * aligned (e.g. NTSC), Need to set window
+			  * X pixel size to be 32B aligned as well
+			  */
+			if (lconfig->xsize % 32) {
+				osd_write(sd,
+					  ((lconfig->xsize + 31) & ~31),
+					  OSD_VIDWIN1XL);
+				osd_write(sd,
+					  ((lconfig->xsize + 31) & ~31),
+					  OSD_VIDWIN0XL);
+			}
+		} else if ((sd->vpbe_type == VPBE_VERSION_2) &&
+				(lconfig->pixfmt != PIXFMT_NV12)) {
+			osd_modify(sd, OSD_MISCCTL_S420D, ~OSD_MISCCTL_S420D,
+						OSD_MISCCTL);
+		}
+
+		if (lconfig->interlaced) {
+			osd_write(sd, lconfig->ypos >> 1, OSD_VIDWIN0YP);
+			osd_write(sd, lconfig->ysize >> 1, OSD_VIDWIN0YL);
+			if ((sd->vpbe_type == VPBE_VERSION_2) &&
+				lconfig->pixfmt == PIXFMT_NV12) {
+				osd_write(sd, lconfig->ypos >> 1,
+					  OSD_VIDWIN1YP);
+				osd_write(sd, lconfig->ysize >> 1,
+					  OSD_VIDWIN1YL);
+			}
+		} else {
+			osd_write(sd, lconfig->ypos, OSD_VIDWIN0YP);
+			osd_write(sd, lconfig->ysize, OSD_VIDWIN0YL);
+			if ((sd->vpbe_type == VPBE_VERSION_2) &&
+				lconfig->pixfmt == PIXFMT_NV12) {
+				osd_write(sd, lconfig->ypos, OSD_VIDWIN1YP);
+				osd_write(sd, lconfig->ysize, OSD_VIDWIN1YL);
+			}
+		}
+		break;
+	case WIN_OSD1:
+		/*
+		 * The caller must ensure that OSD1 is disabled prior to
+		 * switching from a normal mode to attribute mode or from
+		 * attribute mode to a normal mode.
+		 */
+		if (lconfig->pixfmt == PIXFMT_OSD_ATTR) {
+			if (sd->vpbe_type == VPBE_VERSION_1) {
+				winmd_mask |= OSD_OSDWIN1MD_ATN1E |
+				OSD_OSDWIN1MD_RGB1E | OSD_OSDWIN1MD_CLUTS1 |
+				OSD_OSDWIN1MD_BLND1 | OSD_OSDWIN1MD_TE1;
+			} else {
+				winmd_mask |= OSD_OSDWIN1MD_BMP1MD |
+				OSD_OSDWIN1MD_CLUTS1 | OSD_OSDWIN1MD_BLND1 |
+				OSD_OSDWIN1MD_TE1;
+			}
+		} else {
+			if (sd->vpbe_type == VPBE_VERSION_1) {
+				winmd_mask |= OSD_OSDWIN1MD_RGB1E;
+				if (lconfig->pixfmt == PIXFMT_RGB565)
+					winmd |= OSD_OSDWIN1MD_RGB1E;
+			} else if ((sd->vpbe_type == VPBE_VERSION_3)
+				   || (sd->vpbe_type == VPBE_VERSION_2)) {
+				winmd_mask |= OSD_OSDWIN1MD_BMP1MD;
+				switch (lconfig->pixfmt) {
+				case PIXFMT_RGB565:
+					winmd |=
+					    (1 << OSD_OSDWIN1MD_BMP1MD_SHIFT);
+					break;
+				case PIXFMT_RGB888:
+					winmd |=
+					    (2 << OSD_OSDWIN1MD_BMP1MD_SHIFT);
+					_osd_enable_rgb888_pixblend(sd,
+							OSDWIN_OSD1);
+					break;
+				case PIXFMT_YCBCRI:
+				case PIXFMT_YCRCBI:
+					winmd |=
+					    (3 << OSD_OSDWIN1MD_BMP1MD_SHIFT);
+					break;
+				default:
+					break;
+				}
+			}
+
+			winmd_mask |= OSD_OSDWIN1MD_BMW1;
+			switch (lconfig->pixfmt) {
+			case PIXFMT_1BPP:
+				bmw = 0;
+				break;
+			case PIXFMT_2BPP:
+				bmw = 1;
+				break;
+			case PIXFMT_4BPP:
+				bmw = 2;
+				break;
+			case PIXFMT_8BPP:
+				bmw = 3;
+				break;
+			default:
+				break;
+			}
+			winmd |= (bmw << OSD_OSDWIN1MD_BMW1_SHIFT);
+		}
+
+		winmd_mask |= OSD_OSDWIN1MD_OFF1;
+		if (lconfig->interlaced)
+			winmd |= OSD_OSDWIN1MD_OFF1;
+
+		osd_modify(sd, winmd_mask, winmd, OSD_OSDWIN1MD);
+		osd_write(sd, lconfig->line_length >> 5, OSD_OSDWIN1OFST);
+		osd_write(sd, lconfig->xpos, OSD_OSDWIN1XP);
+		osd_write(sd, lconfig->xsize, OSD_OSDWIN1XL);
+		if (lconfig->interlaced) {
+			osd_write(sd, lconfig->ypos >> 1, OSD_OSDWIN1YP);
+			osd_write(sd, lconfig->ysize >> 1, OSD_OSDWIN1YL);
+		} else {
+			osd_write(sd, lconfig->ypos, OSD_OSDWIN1YP);
+			osd_write(sd, lconfig->ysize, OSD_OSDWIN1YL);
+		}
+		break;
+	case WIN_VID1:
+		winmd_mask |= OSD_VIDWINMD_VFF1;
+		if (lconfig->interlaced)
+			winmd |= OSD_VIDWINMD_VFF1;
+
+		osd_modify(sd, winmd_mask, winmd, OSD_VIDWINMD);
+		osd_write(sd, lconfig->line_length >> 5, OSD_VIDWIN1OFST);
+		osd_write(sd, lconfig->xpos, OSD_VIDWIN1XP);
+		osd_write(sd, lconfig->xsize, OSD_VIDWIN1XL);
+		/*
+		 * For YUV420P format the register contents are
+		 * duplicated in both VID registers
+		 */
+		if (sd->vpbe_type == VPBE_VERSION_2) {
+			if (lconfig->pixfmt == PIXFMT_NV12) {
+				/* other window also */
+				if (lconfig->interlaced) {
+					winmd_mask |= OSD_VIDWINMD_VFF0;
+					winmd |= OSD_VIDWINMD_VFF0;
+					osd_modify(sd, winmd_mask, winmd,
+						  OSD_VIDWINMD);
+				}
+				osd_modify(sd, OSD_MISCCTL_S420D,
+					   OSD_MISCCTL_S420D, OSD_MISCCTL);
+				osd_write(sd, lconfig->line_length >> 5,
+					  OSD_VIDWIN0OFST);
+				osd_write(sd, lconfig->xpos, OSD_VIDWIN0XP);
+				osd_write(sd, lconfig->xsize, OSD_VIDWIN0XL);
+			} else {
+				osd_modify(sd, OSD_MISCCTL_S420D,
+					   ~OSD_MISCCTL_S420D, OSD_MISCCTL);
+			}
+		}
+
+		if (lconfig->interlaced) {
+			osd_write(sd, lconfig->ypos >> 1, OSD_VIDWIN1YP);
+			osd_write(sd, lconfig->ysize >> 1, OSD_VIDWIN1YL);
+			if ((sd->vpbe_type == VPBE_VERSION_2) &&
+				lconfig->pixfmt == PIXFMT_NV12) {
+				osd_write(sd, lconfig->ypos >> 1,
+					  OSD_VIDWIN0YP);
+				osd_write(sd, lconfig->ysize >> 1,
+					  OSD_VIDWIN0YL);
+			}
+		} else {
+			osd_write(sd, lconfig->ypos, OSD_VIDWIN1YP);
+			osd_write(sd, lconfig->ysize, OSD_VIDWIN1YL);
+			if ((sd->vpbe_type == VPBE_VERSION_2) &&
+				lconfig->pixfmt == PIXFMT_NV12) {
+				osd_write(sd, lconfig->ypos, OSD_VIDWIN0YP);
+				osd_write(sd, lconfig->ysize, OSD_VIDWIN0YL);
+			}
+		}
+		break;
+	}
+}
+
+static int osd_set_layer_config(struct osd_state *sd, enum osd_layer layer,
+				struct osd_layer_config *lconfig)
+{
+	struct osd_state *osd = sd;
+	struct osd_window_state *win = &osd->win[layer];
+	struct osd_layer_config *cfg = &win->lconfig;
+	unsigned long flags;
+	int reject_config;
+
+	spin_lock_irqsave(&osd->lock, flags);
+
+	reject_config = try_layer_config(sd, layer, lconfig);
+	if (reject_config) {
+		spin_unlock_irqrestore(&osd->lock, flags);
+		return reject_config;
+	}
+
+	/* update the current Cb/Cr order */
+	if (is_yc_pixfmt(lconfig->pixfmt))
+		osd->yc_pixfmt = lconfig->pixfmt;
+
+	/*
+	 * If we are switching OSD1 from normal mode to attribute mode or from
+	 * attribute mode to normal mode, then we must disable the window.
+	 */
+	if (layer == WIN_OSD1) {
+		if (((lconfig->pixfmt == PIXFMT_OSD_ATTR) &&
+		  (cfg->pixfmt != PIXFMT_OSD_ATTR)) ||
+		  ((lconfig->pixfmt != PIXFMT_OSD_ATTR) &&
+		  (cfg->pixfmt == PIXFMT_OSD_ATTR))) {
+			win->is_enabled = 0;
+			_osd_disable_layer(sd, layer);
+		}
+	}
+
+	_osd_set_layer_config(sd, layer, lconfig);
+
+	if (layer == WIN_OSD1) {
+		struct osd_osdwin_state *osdwin_state =
+		    &osd->osdwin[OSDWIN_OSD1];
+
+		if ((lconfig->pixfmt != PIXFMT_OSD_ATTR) &&
+		  (cfg->pixfmt == PIXFMT_OSD_ATTR)) {
+			/*
+			 * We just switched OSD1 from attribute mode to normal
+			 * mode, so we must initialize the CLUT select, the
+			 * blend factor, transparency colorkey enable, and
+			 * attenuation enable (DM6446 only) bits in the
+			 * OSDWIN1MD register.
+			 */
+			_osd_set_osd_clut(sd, OSDWIN_OSD1,
+						   osdwin_state->clut);
+			_osd_set_blending_factor(sd, OSDWIN_OSD1,
+							  osdwin_state->blend);
+			if (osdwin_state->colorkey_blending) {
+				_osd_enable_color_key(sd, OSDWIN_OSD1,
+							       osdwin_state->
+							       colorkey,
+							       lconfig->pixfmt);
+			} else
+				_osd_disable_color_key(sd, OSDWIN_OSD1);
+			_osd_set_rec601_attenuation(sd, OSDWIN_OSD1,
+						    osdwin_state->
+						    rec601_attenuation);
+		} else if ((lconfig->pixfmt == PIXFMT_OSD_ATTR) &&
+		  (cfg->pixfmt != PIXFMT_OSD_ATTR)) {
+			/*
+			 * We just switched OSD1 from normal mode to attribute
+			 * mode, so we must initialize the blink enable and
+			 * blink interval bits in the OSDATRMD register.
+			 */
+			_osd_set_blink_attribute(sd, osd->is_blinking,
+							  osd->blink);
+		}
+	}
+
+	/*
+	 * If we just switched to a 1-, 2-, or 4-bits-per-pixel bitmap format
+	 * then configure a default palette map.
+	 */
+	if ((lconfig->pixfmt != cfg->pixfmt) &&
+	  ((lconfig->pixfmt == PIXFMT_1BPP) ||
+	  (lconfig->pixfmt == PIXFMT_2BPP) ||
+	  (lconfig->pixfmt == PIXFMT_4BPP))) {
+		enum osd_win_layer osdwin =
+		    ((layer == WIN_OSD0) ? OSDWIN_OSD0 : OSDWIN_OSD1);
+		struct osd_osdwin_state *osdwin_state =
+		    &osd->osdwin[osdwin];
+		unsigned char clut_index;
+		unsigned char clut_entries = 0;
+
+		switch (lconfig->pixfmt) {
+		case PIXFMT_1BPP:
+			clut_entries = 2;
+			break;
+		case PIXFMT_2BPP:
+			clut_entries = 4;
+			break;
+		case PIXFMT_4BPP:
+			clut_entries = 16;
+			break;
+		default:
+			break;
+		}
+		/*
+		 * The default palette map maps the pixel value to the clut
+		 * index, i.e. pixel value 0 maps to clut entry 0, pixel value
+		 * 1 maps to clut entry 1, etc.
+		 */
+		for (clut_index = 0; clut_index < 16; clut_index++) {
+			osdwin_state->palette_map[clut_index] = clut_index;
+			if (clut_index < clut_entries) {
+				_osd_set_palette_map(sd, osdwin, clut_index,
+						     clut_index,
+						     lconfig->pixfmt);
+			}
+		}
+	}
+
+	*cfg = *lconfig;
+	/* DM6446: configure the RGB888 enable and window selection */
+	if (osd->win[WIN_VID0].lconfig.pixfmt == PIXFMT_RGB888)
+		_osd_enable_vid_rgb888(sd, WIN_VID0);
+	else if (osd->win[WIN_VID1].lconfig.pixfmt == PIXFMT_RGB888)
+		_osd_enable_vid_rgb888(sd, WIN_VID1);
+	else
+		_osd_disable_vid_rgb888(sd);
+
+	if (layer == WIN_VID0) {
+		osd->pingpong =
+		    _osd_dm6446_vid0_pingpong(sd, osd->field_inversion,
+						       win->fb_base_phys,
+						       cfg);
+	}
+
+	spin_unlock_irqrestore(&osd->lock, flags);
+
+	return 0;
+}
+
+static void osd_init_layer(struct osd_state *sd, enum osd_layer layer)
+{
+	struct osd_state *osd = sd;
+	struct osd_window_state *win = &osd->win[layer];
+	enum osd_win_layer osdwin;
+	struct osd_osdwin_state *osdwin_state;
+	struct osd_layer_config *cfg = &win->lconfig;
+	unsigned long flags;
+
+	spin_lock_irqsave(&osd->lock, flags);
+
+	win->is_enabled = 0;
+	_osd_disable_layer(sd, layer);
+
+	win->h_zoom = ZOOM_X1;
+	win->v_zoom = ZOOM_X1;
+	_osd_set_zoom(sd, layer, win->h_zoom, win->v_zoom);
+
+	win->fb_base_phys = 0;
+	_osd_start_layer(sd, layer, win->fb_base_phys, 0);
+
+	cfg->line_length = 0;
+	cfg->xsize = 0;
+	cfg->ysize = 0;
+	cfg->xpos = 0;
+	cfg->ypos = 0;
+	cfg->interlaced = 0;
+	switch (layer) {
+	case WIN_OSD0:
+	case WIN_OSD1:
+		osdwin = (layer == WIN_OSD0) ? OSDWIN_OSD0 : OSDWIN_OSD1;
+		osdwin_state = &osd->osdwin[osdwin];
+		/*
+		 * Other code relies on the fact that OSD windows default to a
+		 * bitmap pixel format when they are deallocated, so don't
+		 * change this default pixel format.
+		 */
+		cfg->pixfmt = PIXFMT_8BPP;
+		_osd_set_layer_config(sd, layer, cfg);
+		osdwin_state->clut = RAM_CLUT;
+		_osd_set_osd_clut(sd, osdwin, osdwin_state->clut);
+		osdwin_state->colorkey_blending = 0;
+		_osd_disable_color_key(sd, osdwin);
+		osdwin_state->blend = OSD_8_VID_0;
+		_osd_set_blending_factor(sd, osdwin, osdwin_state->blend);
+		osdwin_state->rec601_attenuation = 0;
+		_osd_set_rec601_attenuation(sd, osdwin,
+						     osdwin_state->
+						     rec601_attenuation);
+		if (osdwin == OSDWIN_OSD1) {
+			osd->is_blinking = 0;
+			osd->blink = BLINK_X1;
+		}
+		break;
+	case WIN_VID0:
+	case WIN_VID1:
+		cfg->pixfmt = osd->yc_pixfmt;
+		_osd_set_layer_config(sd, layer, cfg);
+		break;
+	}
+
+	spin_unlock_irqrestore(&osd->lock, flags);
+}
+
+static void osd_release_layer(struct osd_state *sd, enum osd_layer layer)
+{
+	struct osd_state *osd = sd;
+	struct osd_window_state *win = &osd->win[layer];
+	unsigned long flags;
+
+	spin_lock_irqsave(&osd->lock, flags);
+
+	if (!win->is_allocated) {
+		spin_unlock_irqrestore(&osd->lock, flags);
+		return;
+	}
+
+	spin_unlock_irqrestore(&osd->lock, flags);
+	osd_init_layer(sd, layer);
+	spin_lock_irqsave(&osd->lock, flags);
+
+	win->is_allocated = 0;
+
+	spin_unlock_irqrestore(&osd->lock, flags);
+}
+
+static int osd_request_layer(struct osd_state *sd, enum osd_layer layer)
+{
+	struct osd_state *osd = sd;
+	struct osd_window_state *win = &osd->win[layer];
+	unsigned long flags;
+
+	spin_lock_irqsave(&osd->lock, flags);
+
+	if (win->is_allocated) {
+		spin_unlock_irqrestore(&osd->lock, flags);
+		return -1;
+	}
+	win->is_allocated = 1;
+
+	spin_unlock_irqrestore(&osd->lock, flags);
+
+	return 0;
+}
+
+static void _osd_init(struct osd_state *sd)
+{
+	osd_write(sd, 0, OSD_MODE);
+	osd_write(sd, 0, OSD_VIDWINMD);
+	osd_write(sd, 0, OSD_OSDWIN0MD);
+	osd_write(sd, 0, OSD_OSDWIN1MD);
+	osd_write(sd, 0, OSD_RECTCUR);
+	osd_write(sd, 0, OSD_MISCCTL);
+	if (sd->vpbe_type == VPBE_VERSION_3) {
+		osd_write(sd, 0, OSD_VBNDRY);
+		osd_write(sd, 0, OSD_EXTMODE);
+		osd_write(sd, OSD_MISCCTL_DMANG, OSD_MISCCTL);
+	}
+}
+
+static void osd_set_left_margin(struct osd_state *sd, u32 val)
+{
+	osd_write(sd, val, OSD_BASEPX);
+}
+
+static void osd_set_top_margin(struct osd_state *sd, u32 val)
+{
+	osd_write(sd, val, OSD_BASEPY);
+}
+
+static int osd_initialize(struct osd_state *osd)
+{
+	if (osd == NULL)
+		return -ENODEV;
+	_osd_init(osd);
+
+	/* set default Cb/Cr order */
+	osd->yc_pixfmt = PIXFMT_YCBCRI;
+
+	if (osd->vpbe_type == VPBE_VERSION_3) {
+		/*
+		 * ROM CLUT1 on the DM355 is similar (identical?) to ROM CLUT0
+		 * on the DM6446, so make ROM_CLUT1 the default on the DM355.
+		 */
+		osd->rom_clut = ROM_CLUT1;
+	}
+
+	_osd_set_field_inversion(osd, osd->field_inversion);
+	_osd_set_rom_clut(osd, osd->rom_clut);
+
+	osd_init_layer(osd, WIN_OSD0);
+	osd_init_layer(osd, WIN_VID0);
+	osd_init_layer(osd, WIN_OSD1);
+	osd_init_layer(osd, WIN_VID1);
+
+	return 0;
+}
+
+static const struct vpbe_osd_ops osd_ops = {
+	.initialize = osd_initialize,
+	.request_layer = osd_request_layer,
+	.release_layer = osd_release_layer,
+	.enable_layer = osd_enable_layer,
+	.disable_layer = osd_disable_layer,
+	.set_layer_config = osd_set_layer_config,
+	.get_layer_config = osd_get_layer_config,
+	.start_layer = osd_start_layer,
+	.set_left_margin = osd_set_left_margin,
+	.set_top_margin = osd_set_top_margin,
+};
+
+static int osd_probe(struct platform_device *pdev)
+{
+	const struct platform_device_id *pdev_id;
+	struct osd_state *osd;
+	struct resource *res;
+
+	pdev_id = platform_get_device_id(pdev);
+	if (!pdev_id)
+		return -EINVAL;
+
+	osd = devm_kzalloc(&pdev->dev, sizeof(struct osd_state), GFP_KERNEL);
+	if (osd == NULL)
+		return -ENOMEM;
+
+
+	osd->dev = &pdev->dev;
+	osd->vpbe_type = pdev_id->driver_data;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	osd->osd_base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(osd->osd_base))
+		return PTR_ERR(osd->osd_base);
+
+	osd->osd_base_phys = res->start;
+	osd->osd_size = resource_size(res);
+	spin_lock_init(&osd->lock);
+	osd->ops = osd_ops;
+	platform_set_drvdata(pdev, osd);
+	dev_notice(osd->dev, "OSD sub device probe success\n");
+
+	return 0;
+}
+
+static int osd_remove(struct platform_device *pdev)
+{
+	return 0;
+}
+
+static struct platform_driver osd_driver = {
+	.probe		= osd_probe,
+	.remove		= osd_remove,
+	.driver		= {
+		.name	= MODULE_NAME,
+	},
+	.id_table	= vpbe_osd_devtype
+};
+
+module_platform_driver(osd_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("DaVinci OSD Manager Driver");
+MODULE_AUTHOR("Texas Instruments");
diff --git a/drivers/media/platform/davinci/vpbe_osd_regs.h b/drivers/media/platform/davinci/vpbe_osd_regs.h
new file mode 100644
index 0000000..584520f
--- /dev/null
+++ b/drivers/media/platform/davinci/vpbe_osd_regs.h
@@ -0,0 +1,364 @@
+/*
+ * Copyright (C) 2006-2010 Texas Instruments Inc
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation version 2.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#ifndef _VPBE_OSD_REGS_H
+#define _VPBE_OSD_REGS_H
+
+/* VPBE Global Registers */
+#define VPBE_PID				0x0
+#define VPBE_PCR				0x4
+
+/* VPSS CLock Registers */
+#define VPSSCLK_PID				0x00
+#define VPSSCLK_CLKCTRL				0x04
+
+/* VPSS Buffer Logic Registers */
+#define VPSSBL_PID				0x00
+#define VPSSBL_PCR				0x04
+#define VPSSBL_BCR				0x08
+#define VPSSBL_INTSTAT				0x0C
+#define VPSSBL_INTSEL				0x10
+#define VPSSBL_EVTSEL				0x14
+#define VPSSBL_MEMCTRL				0x18
+#define VPSSBL_CCDCMUX				0x1C
+
+/* DM365 ISP5 system configuration */
+#define ISP5_PID				0x0
+#define ISP5_PCCR				0x4
+#define ISP5_BCR				0x8
+#define ISP5_INTSTAT				0xC
+#define ISP5_INTSEL1				0x10
+#define ISP5_INTSEL2				0x14
+#define ISP5_INTSEL3				0x18
+#define ISP5_EVTSEL				0x1c
+#define ISP5_CCDCMUX				0x20
+
+/* VPBE On-Screen Display Subsystem Registers (OSD) */
+#define OSD_MODE				0x00
+#define OSD_VIDWINMD				0x04
+#define OSD_OSDWIN0MD				0x08
+#define OSD_OSDWIN1MD				0x0C
+#define OSD_OSDATRMD				0x0C
+#define OSD_RECTCUR				0x10
+#define OSD_VIDWIN0OFST				0x18
+#define OSD_VIDWIN1OFST				0x1C
+#define OSD_OSDWIN0OFST				0x20
+#define OSD_OSDWIN1OFST				0x24
+#define OSD_VIDWINADH				0x28
+#define OSD_VIDWIN0ADL				0x2C
+#define OSD_VIDWIN0ADR				0x2C
+#define OSD_VIDWIN1ADL				0x30
+#define OSD_VIDWIN1ADR				0x30
+#define OSD_OSDWINADH				0x34
+#define OSD_OSDWIN0ADL				0x38
+#define OSD_OSDWIN0ADR				0x38
+#define OSD_OSDWIN1ADL				0x3C
+#define OSD_OSDWIN1ADR				0x3C
+#define OSD_BASEPX				0x40
+#define OSD_BASEPY				0x44
+#define OSD_VIDWIN0XP				0x48
+#define OSD_VIDWIN0YP				0x4C
+#define OSD_VIDWIN0XL				0x50
+#define OSD_VIDWIN0YL				0x54
+#define OSD_VIDWIN1XP				0x58
+#define OSD_VIDWIN1YP				0x5C
+#define OSD_VIDWIN1XL				0x60
+#define OSD_VIDWIN1YL				0x64
+#define OSD_OSDWIN0XP				0x68
+#define OSD_OSDWIN0YP				0x6C
+#define OSD_OSDWIN0XL				0x70
+#define OSD_OSDWIN0YL				0x74
+#define OSD_OSDWIN1XP				0x78
+#define OSD_OSDWIN1YP				0x7C
+#define OSD_OSDWIN1XL				0x80
+#define OSD_OSDWIN1YL				0x84
+#define OSD_CURXP				0x88
+#define OSD_CURYP				0x8C
+#define OSD_CURXL				0x90
+#define OSD_CURYL				0x94
+#define OSD_W0BMP01				0xA0
+#define OSD_W0BMP23				0xA4
+#define OSD_W0BMP45				0xA8
+#define OSD_W0BMP67				0xAC
+#define OSD_W0BMP89				0xB0
+#define OSD_W0BMPAB				0xB4
+#define OSD_W0BMPCD				0xB8
+#define OSD_W0BMPEF				0xBC
+#define OSD_W1BMP01				0xC0
+#define OSD_W1BMP23				0xC4
+#define OSD_W1BMP45				0xC8
+#define OSD_W1BMP67				0xCC
+#define OSD_W1BMP89				0xD0
+#define OSD_W1BMPAB				0xD4
+#define OSD_W1BMPCD				0xD8
+#define OSD_W1BMPEF				0xDC
+#define OSD_VBNDRY				0xE0
+#define OSD_EXTMODE				0xE4
+#define OSD_MISCCTL				0xE8
+#define OSD_CLUTRAMYCB				0xEC
+#define OSD_CLUTRAMCR				0xF0
+#define OSD_TRANSPVAL				0xF4
+#define OSD_TRANSPVALL				0xF4
+#define OSD_TRANSPVALU				0xF8
+#define OSD_TRANSPBMPIDX			0xFC
+#define OSD_PPVWIN0ADR				0xFC
+
+/* bit definitions */
+#define VPBE_PCR_VENC_DIV			(1 << 1)
+#define VPBE_PCR_CLK_OFF			(1 << 0)
+
+#define VPSSBL_INTSTAT_HSSIINT			(1 << 14)
+#define VPSSBL_INTSTAT_CFALDINT			(1 << 13)
+#define VPSSBL_INTSTAT_IPIPE_INT5		(1 << 12)
+#define VPSSBL_INTSTAT_IPIPE_INT4		(1 << 11)
+#define VPSSBL_INTSTAT_IPIPE_INT3		(1 << 10)
+#define VPSSBL_INTSTAT_IPIPE_INT2		(1 << 9)
+#define VPSSBL_INTSTAT_IPIPE_INT1		(1 << 8)
+#define VPSSBL_INTSTAT_IPIPE_INT0		(1 << 7)
+#define VPSSBL_INTSTAT_IPIPEIFINT		(1 << 6)
+#define VPSSBL_INTSTAT_OSDINT			(1 << 5)
+#define VPSSBL_INTSTAT_VENCINT			(1 << 4)
+#define VPSSBL_INTSTAT_H3AINT			(1 << 3)
+#define VPSSBL_INTSTAT_CCDC_VDINT2		(1 << 2)
+#define VPSSBL_INTSTAT_CCDC_VDINT1		(1 << 1)
+#define VPSSBL_INTSTAT_CCDC_VDINT0		(1 << 0)
+
+/* DM365 ISP5 bit definitions */
+#define ISP5_INTSTAT_VENCINT			(1 << 21)
+#define ISP5_INTSTAT_OSDINT			(1 << 20)
+
+/* VMOD TVTYP options for HDMD=0 */
+#define SDTV_NTSC				0
+#define SDTV_PAL				1
+/* VMOD TVTYP options for HDMD=1 */
+#define HDTV_525P				0
+#define HDTV_625P				1
+#define HDTV_1080I				2
+#define HDTV_720P				3
+
+#define OSD_MODE_CS				(1 << 15)
+#define OSD_MODE_OVRSZ				(1 << 14)
+#define OSD_MODE_OHRSZ				(1 << 13)
+#define OSD_MODE_EF				(1 << 12)
+#define OSD_MODE_VVRSZ				(1 << 11)
+#define OSD_MODE_VHRSZ				(1 << 10)
+#define OSD_MODE_FSINV				(1 << 9)
+#define OSD_MODE_BCLUT				(1 << 8)
+#define OSD_MODE_CABG_SHIFT			0
+#define OSD_MODE_CABG				(0xff << 0)
+
+#define OSD_VIDWINMD_VFINV			(1 << 15)
+#define OSD_VIDWINMD_V1EFC			(1 << 14)
+#define OSD_VIDWINMD_VHZ1_SHIFT			12
+#define OSD_VIDWINMD_VHZ1			(3 << 12)
+#define OSD_VIDWINMD_VVZ1_SHIFT			10
+#define OSD_VIDWINMD_VVZ1			(3 << 10)
+#define OSD_VIDWINMD_VFF1			(1 << 9)
+#define OSD_VIDWINMD_ACT1			(1 << 8)
+#define OSD_VIDWINMD_V0EFC			(1 << 6)
+#define OSD_VIDWINMD_VHZ0_SHIFT			4
+#define OSD_VIDWINMD_VHZ0			(3 << 4)
+#define OSD_VIDWINMD_VVZ0_SHIFT			2
+#define OSD_VIDWINMD_VVZ0			(3 << 2)
+#define OSD_VIDWINMD_VFF0			(1 << 1)
+#define OSD_VIDWINMD_ACT0			(1 << 0)
+
+#define OSD_OSDWIN0MD_ATN0E			(1 << 14)
+#define OSD_OSDWIN0MD_RGB0E			(1 << 13)
+#define OSD_OSDWIN0MD_BMP0MD_SHIFT		13
+#define OSD_OSDWIN0MD_BMP0MD			(3 << 13)
+#define OSD_OSDWIN0MD_CLUTS0			(1 << 12)
+#define OSD_OSDWIN0MD_OHZ0_SHIFT		10
+#define OSD_OSDWIN0MD_OHZ0			(3 << 10)
+#define OSD_OSDWIN0MD_OVZ0_SHIFT		8
+#define OSD_OSDWIN0MD_OVZ0			(3 << 8)
+#define OSD_OSDWIN0MD_BMW0_SHIFT		6
+#define OSD_OSDWIN0MD_BMW0			(3 << 6)
+#define OSD_OSDWIN0MD_BLND0_SHIFT		3
+#define OSD_OSDWIN0MD_BLND0			(7 << 3)
+#define OSD_OSDWIN0MD_TE0			(1 << 2)
+#define OSD_OSDWIN0MD_OFF0			(1 << 1)
+#define OSD_OSDWIN0MD_OACT0			(1 << 0)
+
+#define OSD_OSDWIN1MD_OASW			(1 << 15)
+#define OSD_OSDWIN1MD_ATN1E			(1 << 14)
+#define OSD_OSDWIN1MD_RGB1E			(1 << 13)
+#define OSD_OSDWIN1MD_BMP1MD_SHIFT		13
+#define OSD_OSDWIN1MD_BMP1MD			(3 << 13)
+#define OSD_OSDWIN1MD_CLUTS1			(1 << 12)
+#define OSD_OSDWIN1MD_OHZ1_SHIFT		10
+#define OSD_OSDWIN1MD_OHZ1			(3 << 10)
+#define OSD_OSDWIN1MD_OVZ1_SHIFT		8
+#define OSD_OSDWIN1MD_OVZ1			(3 << 8)
+#define OSD_OSDWIN1MD_BMW1_SHIFT		6
+#define OSD_OSDWIN1MD_BMW1			(3 << 6)
+#define OSD_OSDWIN1MD_BLND1_SHIFT		3
+#define OSD_OSDWIN1MD_BLND1			(7 << 3)
+#define OSD_OSDWIN1MD_TE1			(1 << 2)
+#define OSD_OSDWIN1MD_OFF1			(1 << 1)
+#define OSD_OSDWIN1MD_OACT1			(1 << 0)
+
+#define OSD_OSDATRMD_OASW			(1 << 15)
+#define OSD_OSDATRMD_OHZA_SHIFT			10
+#define OSD_OSDATRMD_OHZA			(3 << 10)
+#define OSD_OSDATRMD_OVZA_SHIFT			8
+#define OSD_OSDATRMD_OVZA			(3 << 8)
+#define OSD_OSDATRMD_BLNKINT_SHIFT		6
+#define OSD_OSDATRMD_BLNKINT			(3 << 6)
+#define OSD_OSDATRMD_OFFA			(1 << 1)
+#define OSD_OSDATRMD_BLNK			(1 << 0)
+
+#define OSD_RECTCUR_RCAD_SHIFT			8
+#define OSD_RECTCUR_RCAD			(0xff << 8)
+#define OSD_RECTCUR_CLUTSR			(1 << 7)
+#define OSD_RECTCUR_RCHW_SHIFT			4
+#define OSD_RECTCUR_RCHW			(7 << 4)
+#define OSD_RECTCUR_RCVW_SHIFT			1
+#define OSD_RECTCUR_RCVW			(7 << 1)
+#define OSD_RECTCUR_RCACT			(1 << 0)
+
+#define OSD_VIDWIN0OFST_V0LO			(0x1ff << 0)
+
+#define OSD_VIDWIN1OFST_V1LO			(0x1ff << 0)
+
+#define OSD_OSDWIN0OFST_O0LO			(0x1ff << 0)
+
+#define OSD_OSDWIN1OFST_O1LO			(0x1ff << 0)
+
+#define OSD_WINOFST_AH_SHIFT			9
+
+#define OSD_VIDWIN0OFST_V0AH			(0xf << 9)
+#define OSD_VIDWIN1OFST_V1AH			(0xf << 9)
+#define OSD_OSDWIN0OFST_O0AH			(0xf << 9)
+#define OSD_OSDWIN1OFST_O1AH			(0xf << 9)
+
+#define OSD_VIDWINADH_V1AH_SHIFT		8
+#define OSD_VIDWINADH_V1AH			(0x7f << 8)
+#define OSD_VIDWINADH_V0AH_SHIFT		0
+#define OSD_VIDWINADH_V0AH			(0x7f << 0)
+
+#define OSD_VIDWIN0ADL_V0AL			(0xffff << 0)
+
+#define OSD_VIDWIN1ADL_V1AL			(0xffff << 0)
+
+#define OSD_OSDWINADH_O1AH_SHIFT		8
+#define OSD_OSDWINADH_O1AH			(0x7f << 8)
+#define OSD_OSDWINADH_O0AH_SHIFT		0
+#define OSD_OSDWINADH_O0AH			(0x7f << 0)
+
+#define OSD_OSDWIN0ADL_O0AL			(0xffff << 0)
+
+#define OSD_OSDWIN1ADL_O1AL			(0xffff << 0)
+
+#define OSD_BASEPX_BPX				(0x3ff << 0)
+
+#define OSD_BASEPY_BPY				(0x1ff << 0)
+
+#define OSD_VIDWIN0XP_V0X			(0x7ff << 0)
+
+#define OSD_VIDWIN0YP_V0Y			(0x7ff << 0)
+
+#define OSD_VIDWIN0XL_V0W			(0x7ff << 0)
+
+#define OSD_VIDWIN0YL_V0H			(0x7ff << 0)
+
+#define OSD_VIDWIN1XP_V1X			(0x7ff << 0)
+
+#define OSD_VIDWIN1YP_V1Y			(0x7ff << 0)
+
+#define OSD_VIDWIN1XL_V1W			(0x7ff << 0)
+
+#define OSD_VIDWIN1YL_V1H			(0x7ff << 0)
+
+#define OSD_OSDWIN0XP_W0X			(0x7ff << 0)
+
+#define OSD_OSDWIN0YP_W0Y			(0x7ff << 0)
+
+#define OSD_OSDWIN0XL_W0W			(0x7ff << 0)
+
+#define OSD_OSDWIN0YL_W0H			(0x7ff << 0)
+
+#define OSD_OSDWIN1XP_W1X			(0x7ff << 0)
+
+#define OSD_OSDWIN1YP_W1Y			(0x7ff << 0)
+
+#define OSD_OSDWIN1XL_W1W			(0x7ff << 0)
+
+#define OSD_OSDWIN1YL_W1H			(0x7ff << 0)
+
+#define OSD_CURXP_RCSX				(0x7ff << 0)
+
+#define OSD_CURYP_RCSY				(0x7ff << 0)
+
+#define OSD_CURXL_RCSW				(0x7ff << 0)
+
+#define OSD_CURYL_RCSH				(0x7ff << 0)
+
+#define OSD_EXTMODE_EXPMDSEL			(1 << 15)
+#define OSD_EXTMODE_SCRNHEXP_SHIFT		13
+#define OSD_EXTMODE_SCRNHEXP			(3 << 13)
+#define OSD_EXTMODE_SCRNVEXP			(1 << 12)
+#define OSD_EXTMODE_OSD1BLDCHR			(1 << 11)
+#define OSD_EXTMODE_OSD0BLDCHR			(1 << 10)
+#define OSD_EXTMODE_ATNOSD1EN			(1 << 9)
+#define OSD_EXTMODE_ATNOSD0EN			(1 << 8)
+#define OSD_EXTMODE_OSDHRSZ15			(1 << 7)
+#define OSD_EXTMODE_VIDHRSZ15			(1 << 6)
+#define OSD_EXTMODE_ZMFILV1HEN			(1 << 5)
+#define OSD_EXTMODE_ZMFILV1VEN			(1 << 4)
+#define OSD_EXTMODE_ZMFILV0HEN			(1 << 3)
+#define OSD_EXTMODE_ZMFILV0VEN			(1 << 2)
+#define OSD_EXTMODE_EXPFILHEN			(1 << 1)
+#define OSD_EXTMODE_EXPFILVEN			(1 << 0)
+
+#define OSD_MISCCTL_BLDSEL			(1 << 15)
+#define OSD_MISCCTL_S420D			(1 << 14)
+#define OSD_MISCCTL_BMAPT			(1 << 13)
+#define OSD_MISCCTL_DM365M			(1 << 12)
+#define OSD_MISCCTL_RGBEN			(1 << 7)
+#define OSD_MISCCTL_RGBWIN			(1 << 6)
+#define OSD_MISCCTL_DMANG			(1 << 6)
+#define OSD_MISCCTL_TMON			(1 << 5)
+#define OSD_MISCCTL_RSEL			(1 << 4)
+#define OSD_MISCCTL_CPBSY			(1 << 3)
+#define OSD_MISCCTL_PPSW			(1 << 2)
+#define OSD_MISCCTL_PPRV			(1 << 1)
+
+#define OSD_CLUTRAMYCB_Y_SHIFT			8
+#define OSD_CLUTRAMYCB_Y			(0xff << 8)
+#define OSD_CLUTRAMYCB_CB_SHIFT			0
+#define OSD_CLUTRAMYCB_CB			(0xff << 0)
+
+#define OSD_CLUTRAMCR_CR_SHIFT			8
+#define OSD_CLUTRAMCR_CR			(0xff << 8)
+#define OSD_CLUTRAMCR_CADDR_SHIFT		0
+#define OSD_CLUTRAMCR_CADDR			(0xff << 0)
+
+#define OSD_TRANSPVAL_RGBTRANS			(0xffff << 0)
+
+#define OSD_TRANSPVALL_RGBL			(0xffff << 0)
+
+#define OSD_TRANSPVALU_Y_SHIFT			8
+#define OSD_TRANSPVALU_Y			(0xff << 8)
+#define OSD_TRANSPVALU_RGBU_SHIFT		0
+#define OSD_TRANSPVALU_RGBU			(0xff << 0)
+
+#define OSD_TRANSPBMPIDX_BMP1_SHIFT		8
+#define OSD_TRANSPBMPIDX_BMP1			(0xff << 8)
+#define OSD_TRANSPBMPIDX_BMP0_SHIFT		0
+#define OSD_TRANSPBMPIDX_BMP0			0xff
+
+#endif				/* _DAVINCI_VPBE_H_ */
diff --git a/drivers/media/platform/davinci/vpbe_venc.c b/drivers/media/platform/davinci/vpbe_venc.c
new file mode 100644
index 0000000..36ed146
--- /dev/null
+++ b/drivers/media/platform/davinci/vpbe_venc.c
@@ -0,0 +1,698 @@
+/*
+ * Copyright (C) 2010 Texas Instruments Inc
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation version 2.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/ctype.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/videodev2.h>
+#include <linux/slab.h>
+
+#include <mach/hardware.h>
+#include <mach/mux.h>
+#include <linux/platform_data/i2c-davinci.h>
+
+#include <linux/io.h>
+
+#include <media/davinci/vpbe_types.h>
+#include <media/davinci/vpbe_venc.h>
+#include <media/davinci/vpss.h>
+#include <media/v4l2-device.h>
+
+#include "vpbe_venc_regs.h"
+
+#define MODULE_NAME	"davinci-vpbe-venc"
+
+static struct platform_device_id vpbe_venc_devtype[] = {
+	{
+		.name = DM644X_VPBE_VENC_SUBDEV_NAME,
+		.driver_data = VPBE_VERSION_1,
+	}, {
+		.name = DM365_VPBE_VENC_SUBDEV_NAME,
+		.driver_data = VPBE_VERSION_2,
+	}, {
+		.name = DM355_VPBE_VENC_SUBDEV_NAME,
+		.driver_data = VPBE_VERSION_3,
+	},
+	{
+		/* sentinel */
+	}
+};
+
+MODULE_DEVICE_TABLE(platform, vpbe_venc_devtype);
+
+static int debug = 2;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "Debug level 0-2");
+
+struct venc_state {
+	struct v4l2_subdev sd;
+	struct venc_callback *callback;
+	struct venc_platform_data *pdata;
+	struct device *pdev;
+	u32 output;
+	v4l2_std_id std;
+	spinlock_t lock;
+	void __iomem *venc_base;
+	void __iomem *vdaccfg_reg;
+	enum vpbe_version venc_type;
+};
+
+static inline struct venc_state *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct venc_state, sd);
+}
+
+static inline u32 venc_read(struct v4l2_subdev *sd, u32 offset)
+{
+	struct venc_state *venc = to_state(sd);
+
+	return readl(venc->venc_base + offset);
+}
+
+static inline u32 venc_write(struct v4l2_subdev *sd, u32 offset, u32 val)
+{
+	struct venc_state *venc = to_state(sd);
+
+	writel(val, (venc->venc_base + offset));
+
+	return val;
+}
+
+static inline u32 venc_modify(struct v4l2_subdev *sd, u32 offset,
+				 u32 val, u32 mask)
+{
+	u32 new_val = (venc_read(sd, offset) & ~mask) | (val & mask);
+
+	venc_write(sd, offset, new_val);
+
+	return new_val;
+}
+
+static inline u32 vdaccfg_write(struct v4l2_subdev *sd, u32 val)
+{
+	struct venc_state *venc = to_state(sd);
+
+	writel(val, venc->vdaccfg_reg);
+
+	val = readl(venc->vdaccfg_reg);
+
+	return val;
+}
+
+#define VDAC_COMPONENT	0x543
+#define VDAC_S_VIDEO	0x210
+/* This function sets the dac of the VPBE for various outputs
+ */
+static int venc_set_dac(struct v4l2_subdev *sd, u32 out_index)
+{
+	switch (out_index) {
+	case 0:
+		v4l2_dbg(debug, 1, sd, "Setting output to Composite\n");
+		venc_write(sd, VENC_DACSEL, 0);
+		break;
+	case 1:
+		v4l2_dbg(debug, 1, sd, "Setting output to Component\n");
+		venc_write(sd, VENC_DACSEL, VDAC_COMPONENT);
+		break;
+	case 2:
+		v4l2_dbg(debug, 1, sd, "Setting output to S-video\n");
+		venc_write(sd, VENC_DACSEL, VDAC_S_VIDEO);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void venc_enabledigitaloutput(struct v4l2_subdev *sd, int benable)
+{
+	struct venc_state *venc = to_state(sd);
+
+	v4l2_dbg(debug, 2, sd, "venc_enabledigitaloutput\n");
+
+	if (benable) {
+		venc_write(sd, VENC_VMOD, 0);
+		venc_write(sd, VENC_CVBS, 0);
+		venc_write(sd, VENC_LCDOUT, 0);
+		venc_write(sd, VENC_HSPLS, 0);
+		venc_write(sd, VENC_HSTART, 0);
+		venc_write(sd, VENC_HVALID, 0);
+		venc_write(sd, VENC_HINT, 0);
+		venc_write(sd, VENC_VSPLS, 0);
+		venc_write(sd, VENC_VSTART, 0);
+		venc_write(sd, VENC_VVALID, 0);
+		venc_write(sd, VENC_VINT, 0);
+		venc_write(sd, VENC_YCCCTL, 0);
+		venc_write(sd, VENC_DACSEL, 0);
+
+	} else {
+		venc_write(sd, VENC_VMOD, 0);
+		/* disable VCLK output pin enable */
+		venc_write(sd, VENC_VIDCTL, 0x141);
+
+		/* Disable output sync pins */
+		venc_write(sd, VENC_SYNCCTL, 0);
+
+		/* Disable DCLOCK */
+		venc_write(sd, VENC_DCLKCTL, 0);
+		venc_write(sd, VENC_DRGBX1, 0x0000057C);
+
+		/* Disable LCD output control (accepting default polarity) */
+		venc_write(sd, VENC_LCDOUT, 0);
+		if (venc->venc_type != VPBE_VERSION_3)
+			venc_write(sd, VENC_CMPNT, 0x100);
+		venc_write(sd, VENC_HSPLS, 0);
+		venc_write(sd, VENC_HINT, 0);
+		venc_write(sd, VENC_HSTART, 0);
+		venc_write(sd, VENC_HVALID, 0);
+
+		venc_write(sd, VENC_VSPLS, 0);
+		venc_write(sd, VENC_VINT, 0);
+		venc_write(sd, VENC_VSTART, 0);
+		venc_write(sd, VENC_VVALID, 0);
+
+		venc_write(sd, VENC_HSDLY, 0);
+		venc_write(sd, VENC_VSDLY, 0);
+
+		venc_write(sd, VENC_YCCCTL, 0);
+		venc_write(sd, VENC_VSTARTA, 0);
+
+		/* Set OSD clock and OSD Sync Adavance registers */
+		venc_write(sd, VENC_OSDCLK0, 1);
+		venc_write(sd, VENC_OSDCLK1, 2);
+	}
+}
+
+static void
+venc_enable_vpss_clock(int venc_type,
+		       enum vpbe_enc_timings_type type,
+		       unsigned int pclock)
+{
+	if (venc_type == VPBE_VERSION_1)
+		return;
+
+	if (venc_type == VPBE_VERSION_2 && (type == VPBE_ENC_STD || (type ==
+	    VPBE_ENC_DV_TIMINGS && pclock <= 27000000))) {
+		vpss_enable_clock(VPSS_VENC_CLOCK_SEL, 1);
+		vpss_enable_clock(VPSS_VPBE_CLOCK, 1);
+		return;
+	}
+
+	if (venc_type == VPBE_VERSION_3 && type == VPBE_ENC_STD)
+		vpss_enable_clock(VPSS_VENC_CLOCK_SEL, 0);
+}
+
+#define VDAC_CONFIG_SD_V3	0x0E21A6B6
+#define VDAC_CONFIG_SD_V2	0x081141CF
+/*
+ * setting NTSC mode
+ */
+static int venc_set_ntsc(struct v4l2_subdev *sd)
+{
+	u32 val;
+	struct venc_state *venc = to_state(sd);
+	struct venc_platform_data *pdata = venc->pdata;
+
+	v4l2_dbg(debug, 2, sd, "venc_set_ntsc\n");
+
+	/* Setup clock at VPSS & VENC for SD */
+	vpss_enable_clock(VPSS_VENC_CLOCK_SEL, 1);
+	if (pdata->setup_clock(VPBE_ENC_STD, V4L2_STD_525_60) < 0)
+		return -EINVAL;
+
+	venc_enable_vpss_clock(venc->venc_type, VPBE_ENC_STD, V4L2_STD_525_60);
+	venc_enabledigitaloutput(sd, 0);
+
+	if (venc->venc_type == VPBE_VERSION_3) {
+		venc_write(sd, VENC_CLKCTL, 0x01);
+		venc_write(sd, VENC_VIDCTL, 0);
+		val = vdaccfg_write(sd, VDAC_CONFIG_SD_V3);
+	} else if (venc->venc_type == VPBE_VERSION_2) {
+		venc_write(sd, VENC_CLKCTL, 0x01);
+		venc_write(sd, VENC_VIDCTL, 0);
+		vdaccfg_write(sd, VDAC_CONFIG_SD_V2);
+	} else {
+		/* to set VENC CLK DIV to 1 - final clock is 54 MHz */
+		venc_modify(sd, VENC_VIDCTL, 0, 1 << 1);
+		/* Set REC656 Mode */
+		venc_write(sd, VENC_YCCCTL, 0x1);
+		venc_modify(sd, VENC_VDPRO, 0, VENC_VDPRO_DAFRQ);
+		venc_modify(sd, VENC_VDPRO, 0, VENC_VDPRO_DAUPS);
+	}
+
+	venc_write(sd, VENC_VMOD, 0);
+	venc_modify(sd, VENC_VMOD, (1 << VENC_VMOD_VIE_SHIFT),
+			VENC_VMOD_VIE);
+	venc_modify(sd, VENC_VMOD, (0 << VENC_VMOD_VMD), VENC_VMOD_VMD);
+	venc_modify(sd, VENC_VMOD, (0 << VENC_VMOD_TVTYP_SHIFT),
+			VENC_VMOD_TVTYP);
+	venc_write(sd, VENC_DACTST, 0x0);
+	venc_modify(sd, VENC_VMOD, VENC_VMOD_VENC, VENC_VMOD_VENC);
+
+	return 0;
+}
+
+/*
+ * setting PAL mode
+ */
+static int venc_set_pal(struct v4l2_subdev *sd)
+{
+	struct venc_state *venc = to_state(sd);
+
+	v4l2_dbg(debug, 2, sd, "venc_set_pal\n");
+
+	/* Setup clock at VPSS & VENC for SD */
+	vpss_enable_clock(VPSS_VENC_CLOCK_SEL, 1);
+	if (venc->pdata->setup_clock(VPBE_ENC_STD, V4L2_STD_625_50) < 0)
+		return -EINVAL;
+
+	venc_enable_vpss_clock(venc->venc_type, VPBE_ENC_STD, V4L2_STD_625_50);
+	venc_enabledigitaloutput(sd, 0);
+
+	if (venc->venc_type == VPBE_VERSION_3) {
+		venc_write(sd, VENC_CLKCTL, 0x1);
+		venc_write(sd, VENC_VIDCTL, 0);
+		vdaccfg_write(sd, VDAC_CONFIG_SD_V3);
+	} else if (venc->venc_type == VPBE_VERSION_2) {
+		venc_write(sd, VENC_CLKCTL, 0x1);
+		venc_write(sd, VENC_VIDCTL, 0);
+		vdaccfg_write(sd, VDAC_CONFIG_SD_V2);
+	} else {
+		/* to set VENC CLK DIV to 1 - final clock is 54 MHz */
+		venc_modify(sd, VENC_VIDCTL, 0, 1 << 1);
+		/* Set REC656 Mode */
+		venc_write(sd, VENC_YCCCTL, 0x1);
+	}
+
+	venc_modify(sd, VENC_SYNCCTL, 1 << VENC_SYNCCTL_OVD_SHIFT,
+			VENC_SYNCCTL_OVD);
+	venc_write(sd, VENC_VMOD, 0);
+	venc_modify(sd, VENC_VMOD,
+			(1 << VENC_VMOD_VIE_SHIFT),
+			VENC_VMOD_VIE);
+	venc_modify(sd, VENC_VMOD,
+			(0 << VENC_VMOD_VMD), VENC_VMOD_VMD);
+	venc_modify(sd, VENC_VMOD,
+			(1 << VENC_VMOD_TVTYP_SHIFT),
+			VENC_VMOD_TVTYP);
+	venc_write(sd, VENC_DACTST, 0x0);
+	venc_modify(sd, VENC_VMOD, VENC_VMOD_VENC, VENC_VMOD_VENC);
+
+	return 0;
+}
+
+#define VDAC_CONFIG_HD_V2	0x081141EF
+/*
+ * venc_set_480p59_94
+ *
+ * This function configures the video encoder to EDTV(525p) component setting.
+ */
+static int venc_set_480p59_94(struct v4l2_subdev *sd)
+{
+	struct venc_state *venc = to_state(sd);
+	struct venc_platform_data *pdata = venc->pdata;
+
+	v4l2_dbg(debug, 2, sd, "venc_set_480p59_94\n");
+	if (venc->venc_type != VPBE_VERSION_1 &&
+	    venc->venc_type != VPBE_VERSION_2)
+		return -EINVAL;
+
+	/* Setup clock at VPSS & VENC for SD */
+	if (pdata->setup_clock(VPBE_ENC_DV_TIMINGS, 27000000) < 0)
+		return -EINVAL;
+
+	venc_enable_vpss_clock(venc->venc_type, VPBE_ENC_DV_TIMINGS, 27000000);
+	venc_enabledigitaloutput(sd, 0);
+
+	if (venc->venc_type == VPBE_VERSION_2)
+		vdaccfg_write(sd, VDAC_CONFIG_HD_V2);
+	venc_write(sd, VENC_OSDCLK0, 0);
+	venc_write(sd, VENC_OSDCLK1, 1);
+
+	if (venc->venc_type == VPBE_VERSION_1) {
+		venc_modify(sd, VENC_VDPRO, VENC_VDPRO_DAFRQ,
+			    VENC_VDPRO_DAFRQ);
+		venc_modify(sd, VENC_VDPRO, VENC_VDPRO_DAUPS,
+			    VENC_VDPRO_DAUPS);
+	}
+
+	venc_write(sd, VENC_VMOD, 0);
+	venc_modify(sd, VENC_VMOD, (1 << VENC_VMOD_VIE_SHIFT),
+		    VENC_VMOD_VIE);
+	venc_modify(sd, VENC_VMOD, VENC_VMOD_HDMD, VENC_VMOD_HDMD);
+	venc_modify(sd, VENC_VMOD, (HDTV_525P << VENC_VMOD_TVTYP_SHIFT),
+		    VENC_VMOD_TVTYP);
+	venc_modify(sd, VENC_VMOD, VENC_VMOD_VDMD_YCBCR8 <<
+		    VENC_VMOD_VDMD_SHIFT, VENC_VMOD_VDMD);
+
+	venc_modify(sd, VENC_VMOD, VENC_VMOD_VENC, VENC_VMOD_VENC);
+
+	return 0;
+}
+
+/*
+ * venc_set_625p
+ *
+ * This function configures the video encoder to HDTV(625p) component setting
+ */
+static int venc_set_576p50(struct v4l2_subdev *sd)
+{
+	struct venc_state *venc = to_state(sd);
+	struct venc_platform_data *pdata = venc->pdata;
+
+	v4l2_dbg(debug, 2, sd, "venc_set_576p50\n");
+
+	if (venc->venc_type != VPBE_VERSION_1 &&
+	    venc->venc_type != VPBE_VERSION_2)
+		return -EINVAL;
+	/* Setup clock at VPSS & VENC for SD */
+	if (pdata->setup_clock(VPBE_ENC_DV_TIMINGS, 27000000) < 0)
+		return -EINVAL;
+
+	venc_enable_vpss_clock(venc->venc_type, VPBE_ENC_DV_TIMINGS, 27000000);
+	venc_enabledigitaloutput(sd, 0);
+
+	if (venc->venc_type == VPBE_VERSION_2)
+		vdaccfg_write(sd, VDAC_CONFIG_HD_V2);
+
+	venc_write(sd, VENC_OSDCLK0, 0);
+	venc_write(sd, VENC_OSDCLK1, 1);
+
+	if (venc->venc_type == VPBE_VERSION_1) {
+		venc_modify(sd, VENC_VDPRO, VENC_VDPRO_DAFRQ,
+			    VENC_VDPRO_DAFRQ);
+		venc_modify(sd, VENC_VDPRO, VENC_VDPRO_DAUPS,
+			    VENC_VDPRO_DAUPS);
+	}
+
+	venc_write(sd, VENC_VMOD, 0);
+	venc_modify(sd, VENC_VMOD, (1 << VENC_VMOD_VIE_SHIFT),
+		    VENC_VMOD_VIE);
+	venc_modify(sd, VENC_VMOD, VENC_VMOD_HDMD, VENC_VMOD_HDMD);
+	venc_modify(sd, VENC_VMOD, (HDTV_625P << VENC_VMOD_TVTYP_SHIFT),
+		    VENC_VMOD_TVTYP);
+
+	venc_modify(sd, VENC_VMOD, VENC_VMOD_VDMD_YCBCR8 <<
+		    VENC_VMOD_VDMD_SHIFT, VENC_VMOD_VDMD);
+	venc_modify(sd, VENC_VMOD, VENC_VMOD_VENC, VENC_VMOD_VENC);
+
+	return 0;
+}
+
+/*
+ * venc_set_720p60_internal - Setup 720p60 in venc for dm365 only
+ */
+static int venc_set_720p60_internal(struct v4l2_subdev *sd)
+{
+	struct venc_state *venc = to_state(sd);
+	struct venc_platform_data *pdata = venc->pdata;
+
+	if (pdata->setup_clock(VPBE_ENC_DV_TIMINGS, 74250000) < 0)
+		return -EINVAL;
+
+	venc_enable_vpss_clock(venc->venc_type, VPBE_ENC_DV_TIMINGS, 74250000);
+	venc_enabledigitaloutput(sd, 0);
+
+	venc_write(sd, VENC_OSDCLK0, 0);
+	venc_write(sd, VENC_OSDCLK1, 1);
+
+	venc_write(sd, VENC_VMOD, 0);
+	/* DM365 component HD mode */
+	venc_modify(sd, VENC_VMOD, (1 << VENC_VMOD_VIE_SHIFT),
+	    VENC_VMOD_VIE);
+	venc_modify(sd, VENC_VMOD, VENC_VMOD_HDMD, VENC_VMOD_HDMD);
+	venc_modify(sd, VENC_VMOD, (HDTV_720P << VENC_VMOD_TVTYP_SHIFT),
+		    VENC_VMOD_TVTYP);
+	venc_modify(sd, VENC_VMOD, VENC_VMOD_VENC, VENC_VMOD_VENC);
+	venc_write(sd, VENC_XHINTVL, 0);
+	return 0;
+}
+
+/*
+ * venc_set_1080i30_internal - Setup 1080i30 in venc for dm365 only
+ */
+static int venc_set_1080i30_internal(struct v4l2_subdev *sd)
+{
+	struct venc_state *venc = to_state(sd);
+	struct venc_platform_data *pdata = venc->pdata;
+
+	if (pdata->setup_clock(VPBE_ENC_DV_TIMINGS, 74250000) < 0)
+		return -EINVAL;
+
+	venc_enable_vpss_clock(venc->venc_type, VPBE_ENC_DV_TIMINGS, 74250000);
+	venc_enabledigitaloutput(sd, 0);
+
+	venc_write(sd, VENC_OSDCLK0, 0);
+	venc_write(sd, VENC_OSDCLK1, 1);
+
+
+	venc_write(sd, VENC_VMOD, 0);
+	/* DM365 component HD mode */
+	venc_modify(sd, VENC_VMOD, (1 << VENC_VMOD_VIE_SHIFT),
+		    VENC_VMOD_VIE);
+	venc_modify(sd, VENC_VMOD, VENC_VMOD_HDMD, VENC_VMOD_HDMD);
+	venc_modify(sd, VENC_VMOD, (HDTV_1080I << VENC_VMOD_TVTYP_SHIFT),
+		    VENC_VMOD_TVTYP);
+	venc_modify(sd, VENC_VMOD, VENC_VMOD_VENC, VENC_VMOD_VENC);
+	venc_write(sd, VENC_XHINTVL, 0);
+	return 0;
+}
+
+static int venc_s_std_output(struct v4l2_subdev *sd, v4l2_std_id norm)
+{
+	v4l2_dbg(debug, 1, sd, "venc_s_std_output\n");
+
+	if (norm & V4L2_STD_525_60)
+		return venc_set_ntsc(sd);
+	else if (norm & V4L2_STD_625_50)
+		return venc_set_pal(sd);
+
+	return -EINVAL;
+}
+
+static int venc_s_dv_timings(struct v4l2_subdev *sd,
+			    struct v4l2_dv_timings *dv_timings)
+{
+	struct venc_state *venc = to_state(sd);
+	u32 height = dv_timings->bt.height;
+	int ret;
+
+	v4l2_dbg(debug, 1, sd, "venc_s_dv_timings\n");
+
+	if (height == 576)
+		return venc_set_576p50(sd);
+	else if (height == 480)
+		return venc_set_480p59_94(sd);
+	else if ((height == 720) &&
+			(venc->venc_type == VPBE_VERSION_2)) {
+		/* TBD setup internal 720p mode here */
+		ret = venc_set_720p60_internal(sd);
+		/* for DM365 VPBE, there is DAC inside */
+		vdaccfg_write(sd, VDAC_CONFIG_HD_V2);
+		return ret;
+	} else if ((height == 1080) &&
+		(venc->venc_type == VPBE_VERSION_2)) {
+		/* TBD setup internal 1080i mode here */
+		ret = venc_set_1080i30_internal(sd);
+		/* for DM365 VPBE, there is DAC inside */
+		vdaccfg_write(sd, VDAC_CONFIG_HD_V2);
+		return ret;
+	}
+	return -EINVAL;
+}
+
+static int venc_s_routing(struct v4l2_subdev *sd, u32 input, u32 output,
+			  u32 config)
+{
+	struct venc_state *venc = to_state(sd);
+	int ret;
+
+	v4l2_dbg(debug, 1, sd, "venc_s_routing\n");
+
+	ret = venc_set_dac(sd, output);
+	if (!ret)
+		venc->output = output;
+
+	return ret;
+}
+
+static long venc_ioctl(struct v4l2_subdev *sd,
+			unsigned int cmd,
+			void *arg)
+{
+	u32 val;
+
+	switch (cmd) {
+	case VENC_GET_FLD:
+		val = venc_read(sd, VENC_VSTAT);
+		*((int *)arg) = ((val & VENC_VSTAT_FIDST) ==
+		VENC_VSTAT_FIDST);
+		break;
+	default:
+		v4l2_err(sd, "Wrong IOCTL cmd\n");
+		break;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops venc_core_ops = {
+	.ioctl      = venc_ioctl,
+};
+
+static const struct v4l2_subdev_video_ops venc_video_ops = {
+	.s_routing = venc_s_routing,
+	.s_std_output = venc_s_std_output,
+	.s_dv_timings = venc_s_dv_timings,
+};
+
+static const struct v4l2_subdev_ops venc_ops = {
+	.core = &venc_core_ops,
+	.video = &venc_video_ops,
+};
+
+static int venc_initialize(struct v4l2_subdev *sd)
+{
+	struct venc_state *venc = to_state(sd);
+	int ret;
+
+	/* Set default to output to composite and std to NTSC */
+	venc->output = 0;
+	venc->std = V4L2_STD_525_60;
+
+	ret = venc_s_routing(sd, 0, venc->output, 0);
+	if (ret < 0) {
+		v4l2_err(sd, "Error setting output during init\n");
+		return -EINVAL;
+	}
+
+	ret = venc_s_std_output(sd, venc->std);
+	if (ret < 0) {
+		v4l2_err(sd, "Error setting std during init\n");
+		return -EINVAL;
+	}
+
+	return ret;
+}
+
+static int venc_device_get(struct device *dev, void *data)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct venc_state **venc = data;
+
+	if (strstr(pdev->name, "vpbe-venc") != NULL)
+		*venc = platform_get_drvdata(pdev);
+
+	return 0;
+}
+
+struct v4l2_subdev *venc_sub_dev_init(struct v4l2_device *v4l2_dev,
+		const char *venc_name)
+{
+	struct venc_state *venc;
+	int err;
+
+	err = bus_for_each_dev(&platform_bus_type, NULL, &venc,
+			venc_device_get);
+	if (venc == NULL)
+		return NULL;
+
+	v4l2_subdev_init(&venc->sd, &venc_ops);
+
+	strcpy(venc->sd.name, venc_name);
+	if (v4l2_device_register_subdev(v4l2_dev, &venc->sd) < 0) {
+		v4l2_err(v4l2_dev,
+			"vpbe unable to register venc sub device\n");
+		return NULL;
+	}
+	if (venc_initialize(&venc->sd)) {
+		v4l2_err(v4l2_dev,
+			"vpbe venc initialization failed\n");
+		return NULL;
+	}
+
+	return &venc->sd;
+}
+EXPORT_SYMBOL(venc_sub_dev_init);
+
+static int venc_probe(struct platform_device *pdev)
+{
+	const struct platform_device_id *pdev_id;
+	struct venc_state *venc;
+	struct resource *res;
+
+	if (!pdev->dev.platform_data) {
+		dev_err(&pdev->dev, "No platform data for VENC sub device");
+		return -EINVAL;
+	}
+
+	pdev_id = platform_get_device_id(pdev);
+	if (!pdev_id)
+		return -EINVAL;
+
+	venc = devm_kzalloc(&pdev->dev, sizeof(struct venc_state), GFP_KERNEL);
+	if (venc == NULL)
+		return -ENOMEM;
+
+	venc->venc_type = pdev_id->driver_data;
+	venc->pdev = &pdev->dev;
+	venc->pdata = pdev->dev.platform_data;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	venc->venc_base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(venc->venc_base))
+		return PTR_ERR(venc->venc_base);
+
+	if (venc->venc_type != VPBE_VERSION_1) {
+		res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+
+		venc->vdaccfg_reg = devm_ioremap_resource(&pdev->dev, res);
+		if (IS_ERR(venc->vdaccfg_reg))
+			return PTR_ERR(venc->vdaccfg_reg);
+	}
+	spin_lock_init(&venc->lock);
+	platform_set_drvdata(pdev, venc);
+	dev_notice(venc->pdev, "VENC sub device probe success\n");
+
+	return 0;
+}
+
+static int venc_remove(struct platform_device *pdev)
+{
+	return 0;
+}
+
+static struct platform_driver venc_driver = {
+	.probe		= venc_probe,
+	.remove		= venc_remove,
+	.driver		= {
+		.name	= MODULE_NAME,
+	},
+	.id_table	= vpbe_venc_devtype
+};
+
+module_platform_driver(venc_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("VPBE VENC Driver");
+MODULE_AUTHOR("Texas Instruments");
diff --git a/drivers/media/platform/davinci/vpbe_venc_regs.h b/drivers/media/platform/davinci/vpbe_venc_regs.h
new file mode 100644
index 0000000..947cb15
--- /dev/null
+++ b/drivers/media/platform/davinci/vpbe_venc_regs.h
@@ -0,0 +1,177 @@
+/*
+ * Copyright (C) 2006-2010 Texas Instruments Inc
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation version 2..
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#ifndef _VPBE_VENC_REGS_H
+#define _VPBE_VENC_REGS_H
+
+/* VPBE Video Encoder / Digital LCD Subsystem Registers (VENC) */
+#define VENC_VMOD				0x00
+#define VENC_VIDCTL				0x04
+#define VENC_VDPRO				0x08
+#define VENC_SYNCCTL				0x0C
+#define VENC_HSPLS				0x10
+#define VENC_VSPLS				0x14
+#define VENC_HINT				0x18
+#define VENC_HSTART				0x1C
+#define VENC_HVALID				0x20
+#define VENC_VINT				0x24
+#define VENC_VSTART				0x28
+#define VENC_VVALID				0x2C
+#define VENC_HSDLY				0x30
+#define VENC_VSDLY				0x34
+#define VENC_YCCCTL				0x38
+#define VENC_RGBCTL				0x3C
+#define VENC_RGBCLP				0x40
+#define VENC_LINECTL				0x44
+#define VENC_CULLLINE				0x48
+#define VENC_LCDOUT				0x4C
+#define VENC_BRTS				0x50
+#define VENC_BRTW				0x54
+#define VENC_ACCTL				0x58
+#define VENC_PWMP				0x5C
+#define VENC_PWMW				0x60
+#define VENC_DCLKCTL				0x64
+#define VENC_DCLKPTN0				0x68
+#define VENC_DCLKPTN1				0x6C
+#define VENC_DCLKPTN2				0x70
+#define VENC_DCLKPTN3				0x74
+#define VENC_DCLKPTN0A				0x78
+#define VENC_DCLKPTN1A				0x7C
+#define VENC_DCLKPTN2A				0x80
+#define VENC_DCLKPTN3A				0x84
+#define VENC_DCLKHS				0x88
+#define VENC_DCLKHSA				0x8C
+#define VENC_DCLKHR				0x90
+#define VENC_DCLKVS				0x94
+#define VENC_DCLKVR				0x98
+#define VENC_CAPCTL				0x9C
+#define VENC_CAPDO				0xA0
+#define VENC_CAPDE				0xA4
+#define VENC_ATR0				0xA8
+#define VENC_ATR1				0xAC
+#define VENC_ATR2				0xB0
+#define VENC_VSTAT				0xB8
+#define VENC_RAMADR				0xBC
+#define VENC_RAMPORT				0xC0
+#define VENC_DACTST				0xC4
+#define VENC_YCOLVL				0xC8
+#define VENC_SCPROG				0xCC
+#define VENC_CVBS				0xDC
+#define VENC_CMPNT				0xE0
+#define VENC_ETMG0				0xE4
+#define VENC_ETMG1				0xE8
+#define VENC_ETMG2				0xEC
+#define VENC_ETMG3				0xF0
+#define VENC_DACSEL				0xF4
+#define VENC_ARGBX0				0x100
+#define VENC_ARGBX1				0x104
+#define VENC_ARGBX2				0x108
+#define VENC_ARGBX3				0x10C
+#define VENC_ARGBX4				0x110
+#define VENC_DRGBX0				0x114
+#define VENC_DRGBX1				0x118
+#define VENC_DRGBX2				0x11C
+#define VENC_DRGBX3				0x120
+#define VENC_DRGBX4				0x124
+#define VENC_VSTARTA				0x128
+#define VENC_OSDCLK0				0x12C
+#define VENC_OSDCLK1				0x130
+#define VENC_HVLDCL0				0x134
+#define VENC_HVLDCL1				0x138
+#define VENC_OSDHADV				0x13C
+#define VENC_CLKCTL				0x140
+#define VENC_GAMCTL				0x144
+#define VENC_XHINTVL				0x174
+
+/* bit definitions */
+#define VPBE_PCR_VENC_DIV			(1 << 1)
+#define VPBE_PCR_CLK_OFF			(1 << 0)
+
+#define VENC_VMOD_VDMD_SHIFT			12
+#define VENC_VMOD_VDMD_YCBCR16			0
+#define VENC_VMOD_VDMD_YCBCR8			1
+#define VENC_VMOD_VDMD_RGB666			2
+#define VENC_VMOD_VDMD_RGB8			3
+#define VENC_VMOD_VDMD_EPSON			4
+#define VENC_VMOD_VDMD_CASIO			5
+#define VENC_VMOD_VDMD_UDISPQVGA		6
+#define VENC_VMOD_VDMD_STNLCD			7
+#define VENC_VMOD_VIE_SHIFT			1
+#define VENC_VMOD_VDMD				(7 << 12)
+#define VENC_VMOD_ITLCL				(1 << 11)
+#define VENC_VMOD_ITLC				(1 << 10)
+#define VENC_VMOD_NSIT				(1 << 9)
+#define VENC_VMOD_HDMD				(1 << 8)
+#define VENC_VMOD_TVTYP_SHIFT			6
+#define VENC_VMOD_TVTYP				(3 << 6)
+#define VENC_VMOD_SLAVE				(1 << 5)
+#define VENC_VMOD_VMD				(1 << 4)
+#define VENC_VMOD_BLNK				(1 << 3)
+#define VENC_VMOD_VIE				(1 << 1)
+#define VENC_VMOD_VENC				(1 << 0)
+
+/* VMOD TVTYP options for HDMD=0 */
+#define SDTV_NTSC				0
+#define SDTV_PAL				1
+/* VMOD TVTYP options for HDMD=1 */
+#define HDTV_525P				0
+#define HDTV_625P				1
+#define HDTV_1080I				2
+#define HDTV_720P				3
+
+#define VENC_VIDCTL_VCLKP			(1 << 14)
+#define VENC_VIDCTL_VCLKE_SHIFT			13
+#define VENC_VIDCTL_VCLKE			(1 << 13)
+#define VENC_VIDCTL_VCLKZ_SHIFT			12
+#define VENC_VIDCTL_VCLKZ			(1 << 12)
+#define VENC_VIDCTL_SYDIR_SHIFT			8
+#define VENC_VIDCTL_SYDIR			(1 << 8)
+#define VENC_VIDCTL_DOMD_SHIFT			4
+#define VENC_VIDCTL_DOMD			(3 << 4)
+#define VENC_VIDCTL_YCDIR_SHIFT			0
+#define VENC_VIDCTL_YCDIR			(1 << 0)
+
+#define VENC_VDPRO_ATYCC_SHIFT			5
+#define VENC_VDPRO_ATYCC			(1 << 5)
+#define VENC_VDPRO_ATCOM_SHIFT			4
+#define VENC_VDPRO_ATCOM			(1 << 4)
+#define VENC_VDPRO_DAFRQ			(1 << 3)
+#define VENC_VDPRO_DAUPS			(1 << 2)
+#define VENC_VDPRO_CUPS				(1 << 1)
+#define VENC_VDPRO_YUPS				(1 << 0)
+
+#define VENC_SYNCCTL_VPL_SHIFT			3
+#define VENC_SYNCCTL_VPL			(1 << 3)
+#define VENC_SYNCCTL_HPL_SHIFT			2
+#define VENC_SYNCCTL_HPL			(1 << 2)
+#define VENC_SYNCCTL_SYEV_SHIFT			1
+#define VENC_SYNCCTL_SYEV			(1 << 1)
+#define VENC_SYNCCTL_SYEH_SHIFT			0
+#define VENC_SYNCCTL_SYEH			(1 << 0)
+#define VENC_SYNCCTL_OVD_SHIFT			14
+#define VENC_SYNCCTL_OVD			(1 << 14)
+
+#define VENC_DCLKCTL_DCKEC_SHIFT		11
+#define VENC_DCLKCTL_DCKEC			(1 << 11)
+#define VENC_DCLKCTL_DCKPW_SHIFT		0
+#define VENC_DCLKCTL_DCKPW			(0x3f << 0)
+
+#define VENC_VSTAT_FIDST			(1 << 4)
+
+#define VENC_CMPNT_MRGB_SHIFT			14
+#define VENC_CMPNT_MRGB				(1 << 14)
+
+#endif				/* _VPBE_VENC_REGS_H */
diff --git a/drivers/media/platform/davinci/vpfe_capture.c b/drivers/media/platform/davinci/vpfe_capture.c
new file mode 100644
index 0000000..1f656a3
--- /dev/null
+++ b/drivers/media/platform/davinci/vpfe_capture.c
@@ -0,0 +1,2013 @@
+/*
+ * Copyright (C) 2008-2009 Texas Instruments Inc
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
+ *
+ * Driver name : VPFE Capture driver
+ *    VPFE Capture driver allows applications to capture and stream video
+ *    frames on DaVinci SoCs (DM6446, DM355 etc) from a YUV source such as
+ *    TVP5146 or  Raw Bayer RGB image data from an image sensor
+ *    such as Microns' MT9T001, MT9T031 etc.
+ *
+ *    These SoCs have, in common, a Video Processing Subsystem (VPSS) that
+ *    consists of a Video Processing Front End (VPFE) for capturing
+ *    video/raw image data and Video Processing Back End (VPBE) for displaying
+ *    YUV data through an in-built analog encoder or Digital LCD port. This
+ *    driver is for capture through VPFE. A typical EVM using these SoCs have
+ *    following high level configuration.
+ *
+ *
+ *    decoder(TVP5146/		YUV/
+ * 	     MT9T001)   -->  Raw Bayer RGB ---> MUX -> VPFE (CCDC/ISIF)
+ *    				data input              |      |
+ *							V      |
+ *						      SDRAM    |
+ *							       V
+ *							   Image Processor
+ *							       |
+ *							       V
+ *							     SDRAM
+ *    The data flow happens from a decoder connected to the VPFE over a
+ *    YUV embedded (BT.656/BT.1120) or separate sync or raw bayer rgb interface
+ *    and to the input of VPFE through an optional MUX (if more inputs are
+ *    to be interfaced on the EVM). The input data is first passed through
+ *    CCDC (CCD Controller, a.k.a Image Sensor Interface, ISIF). The CCDC
+ *    does very little or no processing on YUV data and does pre-process Raw
+ *    Bayer RGB data through modules such as Defect Pixel Correction (DFC)
+ *    Color Space Conversion (CSC), data gain/offset etc. After this, data
+ *    can be written to SDRAM or can be connected to the image processing
+ *    block such as IPIPE (on DM355 only).
+ *
+ *    Features supported
+ *  		- MMAP IO
+ *		- Capture using TVP5146 over BT.656
+ *		- support for interfacing decoders using sub device model
+ *		- Work with DM355 or DM6446 CCDC to do Raw Bayer RGB/YUV
+ *		  data capture to SDRAM.
+ *    TODO list
+ *		- Support multiple REQBUF after open
+ *		- Support for de-allocating buffers through REQBUF
+ *		- Support for Raw Bayer RGB capture
+ *		- Support for chaining Image Processor
+ *		- Support for static allocation of buffers
+ *		- Support for USERPTR IO
+ *		- Support for STREAMON before QBUF
+ *		- Support for control ioctls
+ */
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <media/v4l2-common.h>
+#include <linux/io.h>
+#include <media/davinci/vpfe_capture.h>
+#include "ccdc_hw_device.h"
+
+static int debug;
+static u32 numbuffers = 3;
+static u32 bufsize = (720 * 576 * 2);
+
+module_param(numbuffers, uint, S_IRUGO);
+module_param(bufsize, uint, S_IRUGO);
+module_param(debug, int, 0644);
+
+MODULE_PARM_DESC(numbuffers, "buffer count (default:3)");
+MODULE_PARM_DESC(bufsize, "buffer size in bytes (default:720 x 576 x 2)");
+MODULE_PARM_DESC(debug, "Debug level 0-1");
+
+MODULE_DESCRIPTION("VPFE Video for Linux Capture Driver");
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Texas Instruments");
+
+/* standard information */
+struct vpfe_standard {
+	v4l2_std_id std_id;
+	unsigned int width;
+	unsigned int height;
+	struct v4l2_fract pixelaspect;
+	/* 0 - progressive, 1 - interlaced */
+	int frame_format;
+};
+
+/* ccdc configuration */
+struct ccdc_config {
+	/* This make sure vpfe is probed and ready to go */
+	int vpfe_probed;
+	/* name of ccdc device */
+	char name[32];
+};
+
+/* data structures */
+static struct vpfe_config_params config_params = {
+	.min_numbuffers = 3,
+	.numbuffers = 3,
+	.min_bufsize = 720 * 480 * 2,
+	.device_bufsize = 720 * 576 * 2,
+};
+
+/* ccdc device registered */
+static struct ccdc_hw_device *ccdc_dev;
+/* lock for accessing ccdc information */
+static DEFINE_MUTEX(ccdc_lock);
+/* ccdc configuration */
+static struct ccdc_config *ccdc_cfg;
+
+static const struct vpfe_standard vpfe_standards[] = {
+	{V4L2_STD_525_60, 720, 480, {11, 10}, 1},
+	{V4L2_STD_625_50, 720, 576, {54, 59}, 1},
+};
+
+/* Used when raw Bayer image from ccdc is directly captured to SDRAM */
+static const struct vpfe_pixel_format vpfe_pix_fmts[] = {
+	{
+		.fmtdesc = {
+			.index = 0,
+			.type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
+			.description = "Bayer GrRBGb 8bit A-Law compr.",
+			.pixelformat = V4L2_PIX_FMT_SBGGR8,
+		},
+		.bpp = 1,
+	},
+	{
+		.fmtdesc = {
+			.index = 1,
+			.type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
+			.description = "Bayer GrRBGb - 16bit",
+			.pixelformat = V4L2_PIX_FMT_SBGGR16,
+		},
+		.bpp = 2,
+	},
+	{
+		.fmtdesc = {
+			.index = 2,
+			.type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
+			.description = "Bayer GrRBGb 8bit DPCM compr.",
+			.pixelformat = V4L2_PIX_FMT_SGRBG10DPCM8,
+		},
+		.bpp = 1,
+	},
+	{
+		.fmtdesc = {
+			.index = 3,
+			.type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
+			.description = "YCbCr 4:2:2 Interleaved UYVY",
+			.pixelformat = V4L2_PIX_FMT_UYVY,
+		},
+		.bpp = 2,
+	},
+	{
+		.fmtdesc = {
+			.index = 4,
+			.type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
+			.description = "YCbCr 4:2:2 Interleaved YUYV",
+			.pixelformat = V4L2_PIX_FMT_YUYV,
+		},
+		.bpp = 2,
+	},
+	{
+		.fmtdesc = {
+			.index = 5,
+			.type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
+			.description = "Y/CbCr 4:2:0 - Semi planar",
+			.pixelformat = V4L2_PIX_FMT_NV12,
+		},
+		.bpp = 1,
+	},
+};
+
+/*
+ * vpfe_lookup_pix_format()
+ * lookup an entry in the vpfe pix format table based on pix_format
+ */
+static const struct vpfe_pixel_format *vpfe_lookup_pix_format(u32 pix_format)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(vpfe_pix_fmts); i++) {
+		if (pix_format == vpfe_pix_fmts[i].fmtdesc.pixelformat)
+			return &vpfe_pix_fmts[i];
+	}
+	return NULL;
+}
+
+/*
+ * vpfe_register_ccdc_device. CCDC module calls this to
+ * register with vpfe capture
+ */
+int vpfe_register_ccdc_device(struct ccdc_hw_device *dev)
+{
+	int ret = 0;
+	printk(KERN_NOTICE "vpfe_register_ccdc_device: %s\n", dev->name);
+
+	BUG_ON(!dev->hw_ops.open);
+	BUG_ON(!dev->hw_ops.enable);
+	BUG_ON(!dev->hw_ops.set_hw_if_params);
+	BUG_ON(!dev->hw_ops.configure);
+	BUG_ON(!dev->hw_ops.set_buftype);
+	BUG_ON(!dev->hw_ops.get_buftype);
+	BUG_ON(!dev->hw_ops.enum_pix);
+	BUG_ON(!dev->hw_ops.set_frame_format);
+	BUG_ON(!dev->hw_ops.get_frame_format);
+	BUG_ON(!dev->hw_ops.get_pixel_format);
+	BUG_ON(!dev->hw_ops.set_pixel_format);
+	BUG_ON(!dev->hw_ops.set_image_window);
+	BUG_ON(!dev->hw_ops.get_image_window);
+	BUG_ON(!dev->hw_ops.get_line_length);
+	BUG_ON(!dev->hw_ops.getfid);
+
+	mutex_lock(&ccdc_lock);
+	if (NULL == ccdc_cfg) {
+		/*
+		 * TODO. Will this ever happen? if so, we need to fix it.
+		 * Proabably we need to add the request to a linked list and
+		 * walk through it during vpfe probe
+		 */
+		printk(KERN_ERR "vpfe capture not initialized\n");
+		ret = -EFAULT;
+		goto unlock;
+	}
+
+	if (strcmp(dev->name, ccdc_cfg->name)) {
+		/* ignore this ccdc */
+		ret = -EINVAL;
+		goto unlock;
+	}
+
+	if (ccdc_dev) {
+		printk(KERN_ERR "ccdc already registered\n");
+		ret = -EINVAL;
+		goto unlock;
+	}
+
+	ccdc_dev = dev;
+unlock:
+	mutex_unlock(&ccdc_lock);
+	return ret;
+}
+EXPORT_SYMBOL(vpfe_register_ccdc_device);
+
+/*
+ * vpfe_unregister_ccdc_device. CCDC module calls this to
+ * unregister with vpfe capture
+ */
+void vpfe_unregister_ccdc_device(struct ccdc_hw_device *dev)
+{
+	if (NULL == dev) {
+		printk(KERN_ERR "invalid ccdc device ptr\n");
+		return;
+	}
+
+	printk(KERN_NOTICE "vpfe_unregister_ccdc_device, dev->name = %s\n",
+		dev->name);
+
+	if (strcmp(dev->name, ccdc_cfg->name)) {
+		/* ignore this ccdc */
+		return;
+	}
+
+	mutex_lock(&ccdc_lock);
+	ccdc_dev = NULL;
+	mutex_unlock(&ccdc_lock);
+	return;
+}
+EXPORT_SYMBOL(vpfe_unregister_ccdc_device);
+
+/*
+ * vpfe_get_ccdc_image_format - Get image parameters based on CCDC settings
+ */
+static int vpfe_get_ccdc_image_format(struct vpfe_device *vpfe_dev,
+				 struct v4l2_format *f)
+{
+	struct v4l2_rect image_win;
+	enum ccdc_buftype buf_type;
+	enum ccdc_frmfmt frm_fmt;
+
+	memset(f, 0, sizeof(*f));
+	f->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	ccdc_dev->hw_ops.get_image_window(&image_win);
+	f->fmt.pix.width = image_win.width;
+	f->fmt.pix.height = image_win.height;
+	f->fmt.pix.bytesperline = ccdc_dev->hw_ops.get_line_length();
+	f->fmt.pix.sizeimage = f->fmt.pix.bytesperline *
+				f->fmt.pix.height;
+	buf_type = ccdc_dev->hw_ops.get_buftype();
+	f->fmt.pix.pixelformat = ccdc_dev->hw_ops.get_pixel_format();
+	frm_fmt = ccdc_dev->hw_ops.get_frame_format();
+	if (frm_fmt == CCDC_FRMFMT_PROGRESSIVE)
+		f->fmt.pix.field = V4L2_FIELD_NONE;
+	else if (frm_fmt == CCDC_FRMFMT_INTERLACED) {
+		if (buf_type == CCDC_BUFTYPE_FLD_INTERLEAVED)
+			f->fmt.pix.field = V4L2_FIELD_INTERLACED;
+		else if (buf_type == CCDC_BUFTYPE_FLD_SEPARATED)
+			f->fmt.pix.field = V4L2_FIELD_SEQ_TB;
+		else {
+			v4l2_err(&vpfe_dev->v4l2_dev, "Invalid buf_type\n");
+			return -EINVAL;
+		}
+	} else {
+		v4l2_err(&vpfe_dev->v4l2_dev, "Invalid frm_fmt\n");
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/*
+ * vpfe_config_ccdc_image_format()
+ * For a pix format, configure ccdc to setup the capture
+ */
+static int vpfe_config_ccdc_image_format(struct vpfe_device *vpfe_dev)
+{
+	enum ccdc_frmfmt frm_fmt = CCDC_FRMFMT_INTERLACED;
+	int ret = 0;
+
+	if (ccdc_dev->hw_ops.set_pixel_format(
+			vpfe_dev->fmt.fmt.pix.pixelformat) < 0) {
+		v4l2_err(&vpfe_dev->v4l2_dev,
+			"couldn't set pix format in ccdc\n");
+		return -EINVAL;
+	}
+	/* configure the image window */
+	ccdc_dev->hw_ops.set_image_window(&vpfe_dev->crop);
+
+	switch (vpfe_dev->fmt.fmt.pix.field) {
+	case V4L2_FIELD_INTERLACED:
+		/* do nothing, since it is default */
+		ret = ccdc_dev->hw_ops.set_buftype(
+				CCDC_BUFTYPE_FLD_INTERLEAVED);
+		break;
+	case V4L2_FIELD_NONE:
+		frm_fmt = CCDC_FRMFMT_PROGRESSIVE;
+		/* buffer type only applicable for interlaced scan */
+		break;
+	case V4L2_FIELD_SEQ_TB:
+		ret = ccdc_dev->hw_ops.set_buftype(
+				CCDC_BUFTYPE_FLD_SEPARATED);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/* set the frame format */
+	if (!ret)
+		ret = ccdc_dev->hw_ops.set_frame_format(frm_fmt);
+	return ret;
+}
+/*
+ * vpfe_config_image_format()
+ * For a given standard, this functions sets up the default
+ * pix format & crop values in the vpfe device and ccdc.  It first
+ * starts with defaults based values from the standard table.
+ * It then checks if sub device supports get_fmt and then override the
+ * values based on that.Sets crop values to match with scan resolution
+ * starting at 0,0. It calls vpfe_config_ccdc_image_format() set the
+ * values in ccdc
+ */
+static int vpfe_config_image_format(struct vpfe_device *vpfe_dev,
+				    v4l2_std_id std_id)
+{
+	struct vpfe_subdev_info *sdinfo = vpfe_dev->current_subdev;
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_mbus_framefmt *mbus_fmt = &fmt.format;
+	struct v4l2_pix_format *pix = &vpfe_dev->fmt.fmt.pix;
+	int i, ret = 0;
+
+	for (i = 0; i < ARRAY_SIZE(vpfe_standards); i++) {
+		if (vpfe_standards[i].std_id & std_id) {
+			vpfe_dev->std_info.active_pixels =
+					vpfe_standards[i].width;
+			vpfe_dev->std_info.active_lines =
+					vpfe_standards[i].height;
+			vpfe_dev->std_info.frame_format =
+					vpfe_standards[i].frame_format;
+			vpfe_dev->std_index = i;
+			break;
+		}
+	}
+
+	if (i ==  ARRAY_SIZE(vpfe_standards)) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "standard not supported\n");
+		return -EINVAL;
+	}
+
+	vpfe_dev->crop.top = 0;
+	vpfe_dev->crop.left = 0;
+	vpfe_dev->crop.width = vpfe_dev->std_info.active_pixels;
+	vpfe_dev->crop.height = vpfe_dev->std_info.active_lines;
+	pix->width = vpfe_dev->crop.width;
+	pix->height = vpfe_dev->crop.height;
+
+	/* first field and frame format based on standard frame format */
+	if (vpfe_dev->std_info.frame_format) {
+		pix->field = V4L2_FIELD_INTERLACED;
+		/* assume V4L2_PIX_FMT_UYVY as default */
+		pix->pixelformat = V4L2_PIX_FMT_UYVY;
+		v4l2_fill_mbus_format(mbus_fmt, pix,
+				MEDIA_BUS_FMT_YUYV10_2X10);
+	} else {
+		pix->field = V4L2_FIELD_NONE;
+		/* assume V4L2_PIX_FMT_SBGGR8 */
+		pix->pixelformat = V4L2_PIX_FMT_SBGGR8;
+		v4l2_fill_mbus_format(mbus_fmt, pix,
+				MEDIA_BUS_FMT_SBGGR8_1X8);
+	}
+
+	/* if sub device supports get_fmt, override the defaults */
+	ret = v4l2_device_call_until_err(&vpfe_dev->v4l2_dev,
+			sdinfo->grp_id, pad, get_fmt, NULL, &fmt);
+
+	if (ret && ret != -ENOIOCTLCMD) {
+		v4l2_err(&vpfe_dev->v4l2_dev,
+			"error in getting get_fmt from sub device\n");
+		return ret;
+	}
+	v4l2_fill_pix_format(pix, mbus_fmt);
+	pix->bytesperline = pix->width * 2;
+	pix->sizeimage = pix->bytesperline * pix->height;
+
+	/* Sets the values in CCDC */
+	ret = vpfe_config_ccdc_image_format(vpfe_dev);
+	if (ret)
+		return ret;
+
+	/* Update the values of sizeimage and bytesperline */
+	pix->bytesperline = ccdc_dev->hw_ops.get_line_length();
+	pix->sizeimage = pix->bytesperline * pix->height;
+
+	return 0;
+}
+
+static int vpfe_initialize_device(struct vpfe_device *vpfe_dev)
+{
+	int ret = 0;
+
+	/* set first input of current subdevice as the current input */
+	vpfe_dev->current_input = 0;
+
+	/* set default standard */
+	vpfe_dev->std_index = 0;
+
+	/* Configure the default format information */
+	ret = vpfe_config_image_format(vpfe_dev,
+				vpfe_standards[vpfe_dev->std_index].std_id);
+	if (ret)
+		return ret;
+
+	/* now open the ccdc device to initialize it */
+	mutex_lock(&ccdc_lock);
+	if (NULL == ccdc_dev) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "ccdc device not registered\n");
+		ret = -ENODEV;
+		goto unlock;
+	}
+
+	if (!try_module_get(ccdc_dev->owner)) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "Couldn't lock ccdc module\n");
+		ret = -ENODEV;
+		goto unlock;
+	}
+	ret = ccdc_dev->hw_ops.open(vpfe_dev->pdev);
+	if (!ret)
+		vpfe_dev->initialized = 1;
+
+	/* Clear all VPFE/CCDC interrupts */
+	if (vpfe_dev->cfg->clr_intr)
+		vpfe_dev->cfg->clr_intr(-1);
+
+unlock:
+	mutex_unlock(&ccdc_lock);
+	return ret;
+}
+
+/*
+ * vpfe_open : It creates object of file handle structure and
+ * stores it in private_data  member of filepointer
+ */
+static int vpfe_open(struct file *file)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+	struct video_device *vdev = video_devdata(file);
+	struct vpfe_fh *fh;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_open\n");
+
+	if (!vpfe_dev->cfg->num_subdevs) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "No decoder registered\n");
+		return -ENODEV;
+	}
+
+	/* Allocate memory for the file handle object */
+	fh = kmalloc(sizeof(struct vpfe_fh), GFP_KERNEL);
+	if (NULL == fh) {
+		v4l2_err(&vpfe_dev->v4l2_dev,
+			"unable to allocate memory for file handle object\n");
+		return -ENOMEM;
+	}
+	/* store pointer to fh in private_data member of file */
+	file->private_data = fh;
+	fh->vpfe_dev = vpfe_dev;
+	v4l2_fh_init(&fh->fh, vdev);
+	mutex_lock(&vpfe_dev->lock);
+	/* If decoder is not initialized. initialize it */
+	if (!vpfe_dev->initialized) {
+		if (vpfe_initialize_device(vpfe_dev)) {
+			mutex_unlock(&vpfe_dev->lock);
+			return -ENODEV;
+		}
+	}
+	/* Increment device usrs counter */
+	vpfe_dev->usrs++;
+	/* Set io_allowed member to false */
+	fh->io_allowed = 0;
+	v4l2_fh_add(&fh->fh);
+	mutex_unlock(&vpfe_dev->lock);
+	return 0;
+}
+
+static void vpfe_schedule_next_buffer(struct vpfe_device *vpfe_dev)
+{
+	unsigned long addr;
+
+	vpfe_dev->next_frm = list_entry(vpfe_dev->dma_queue.next,
+					struct videobuf_buffer, queue);
+	list_del(&vpfe_dev->next_frm->queue);
+	vpfe_dev->next_frm->state = VIDEOBUF_ACTIVE;
+	addr = videobuf_to_dma_contig(vpfe_dev->next_frm);
+
+	ccdc_dev->hw_ops.setfbaddr(addr);
+}
+
+static void vpfe_schedule_bottom_field(struct vpfe_device *vpfe_dev)
+{
+	unsigned long addr;
+
+	addr = videobuf_to_dma_contig(vpfe_dev->cur_frm);
+	addr += vpfe_dev->field_off;
+	ccdc_dev->hw_ops.setfbaddr(addr);
+}
+
+static void vpfe_process_buffer_complete(struct vpfe_device *vpfe_dev)
+{
+	v4l2_get_timestamp(&vpfe_dev->cur_frm->ts);
+	vpfe_dev->cur_frm->state = VIDEOBUF_DONE;
+	vpfe_dev->cur_frm->size = vpfe_dev->fmt.fmt.pix.sizeimage;
+	wake_up_interruptible(&vpfe_dev->cur_frm->done);
+	vpfe_dev->cur_frm = vpfe_dev->next_frm;
+}
+
+/* ISR for VINT0*/
+static irqreturn_t vpfe_isr(int irq, void *dev_id)
+{
+	struct vpfe_device *vpfe_dev = dev_id;
+	enum v4l2_field field;
+	int fid;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "\nStarting vpfe_isr...\n");
+	field = vpfe_dev->fmt.fmt.pix.field;
+
+	/* if streaming not started, don't do anything */
+	if (!vpfe_dev->started)
+		goto clear_intr;
+
+	/* only for 6446 this will be applicable */
+	if (NULL != ccdc_dev->hw_ops.reset)
+		ccdc_dev->hw_ops.reset();
+
+	if (field == V4L2_FIELD_NONE) {
+		/* handle progressive frame capture */
+		v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev,
+			"frame format is progressive...\n");
+		if (vpfe_dev->cur_frm != vpfe_dev->next_frm)
+			vpfe_process_buffer_complete(vpfe_dev);
+		goto clear_intr;
+	}
+
+	/* interlaced or TB capture check which field we are in hardware */
+	fid = ccdc_dev->hw_ops.getfid();
+
+	/* switch the software maintained field id */
+	vpfe_dev->field_id ^= 1;
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "field id = %x:%x.\n",
+		fid, vpfe_dev->field_id);
+	if (fid == vpfe_dev->field_id) {
+		/* we are in-sync here,continue */
+		if (fid == 0) {
+			/*
+			 * One frame is just being captured. If the next frame
+			 * is available, release the current frame and move on
+			 */
+			if (vpfe_dev->cur_frm != vpfe_dev->next_frm)
+				vpfe_process_buffer_complete(vpfe_dev);
+			/*
+			 * based on whether the two fields are stored
+			 * interleavely or separately in memory, reconfigure
+			 * the CCDC memory address
+			 */
+			if (field == V4L2_FIELD_SEQ_TB) {
+				vpfe_schedule_bottom_field(vpfe_dev);
+			}
+			goto clear_intr;
+		}
+		/*
+		 * if one field is just being captured configure
+		 * the next frame get the next frame from the empty
+		 * queue if no frame is available hold on to the
+		 * current buffer
+		 */
+		spin_lock(&vpfe_dev->dma_queue_lock);
+		if (!list_empty(&vpfe_dev->dma_queue) &&
+		    vpfe_dev->cur_frm == vpfe_dev->next_frm)
+			vpfe_schedule_next_buffer(vpfe_dev);
+		spin_unlock(&vpfe_dev->dma_queue_lock);
+	} else if (fid == 0) {
+		/*
+		 * out of sync. Recover from any hardware out-of-sync.
+		 * May loose one frame
+		 */
+		vpfe_dev->field_id = fid;
+	}
+clear_intr:
+	if (vpfe_dev->cfg->clr_intr)
+		vpfe_dev->cfg->clr_intr(irq);
+
+	return IRQ_HANDLED;
+}
+
+/* vdint1_isr - isr handler for VINT1 interrupt */
+static irqreturn_t vdint1_isr(int irq, void *dev_id)
+{
+	struct vpfe_device *vpfe_dev = dev_id;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "\nInside vdint1_isr...\n");
+
+	/* if streaming not started, don't do anything */
+	if (!vpfe_dev->started) {
+		if (vpfe_dev->cfg->clr_intr)
+			vpfe_dev->cfg->clr_intr(irq);
+		return IRQ_HANDLED;
+	}
+
+	spin_lock(&vpfe_dev->dma_queue_lock);
+	if ((vpfe_dev->fmt.fmt.pix.field == V4L2_FIELD_NONE) &&
+	    !list_empty(&vpfe_dev->dma_queue) &&
+	    vpfe_dev->cur_frm == vpfe_dev->next_frm)
+		vpfe_schedule_next_buffer(vpfe_dev);
+	spin_unlock(&vpfe_dev->dma_queue_lock);
+
+	if (vpfe_dev->cfg->clr_intr)
+		vpfe_dev->cfg->clr_intr(irq);
+
+	return IRQ_HANDLED;
+}
+
+static void vpfe_detach_irq(struct vpfe_device *vpfe_dev)
+{
+	enum ccdc_frmfmt frame_format;
+
+	frame_format = ccdc_dev->hw_ops.get_frame_format();
+	if (frame_format == CCDC_FRMFMT_PROGRESSIVE)
+		free_irq(vpfe_dev->ccdc_irq1, vpfe_dev);
+}
+
+static int vpfe_attach_irq(struct vpfe_device *vpfe_dev)
+{
+	enum ccdc_frmfmt frame_format;
+
+	frame_format = ccdc_dev->hw_ops.get_frame_format();
+	if (frame_format == CCDC_FRMFMT_PROGRESSIVE) {
+		return request_irq(vpfe_dev->ccdc_irq1, vdint1_isr,
+				    0, "vpfe_capture1",
+				    vpfe_dev);
+	}
+	return 0;
+}
+
+/* vpfe_stop_ccdc_capture: stop streaming in ccdc/isif */
+static void vpfe_stop_ccdc_capture(struct vpfe_device *vpfe_dev)
+{
+	vpfe_dev->started = 0;
+	ccdc_dev->hw_ops.enable(0);
+	if (ccdc_dev->hw_ops.enable_out_to_sdram)
+		ccdc_dev->hw_ops.enable_out_to_sdram(0);
+}
+
+/*
+ * vpfe_release : This function deletes buffer queue, frees the
+ * buffers and the vpfe file  handle
+ */
+static int vpfe_release(struct file *file)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+	struct vpfe_fh *fh = file->private_data;
+	struct vpfe_subdev_info *sdinfo;
+	int ret;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_release\n");
+
+	/* Get the device lock */
+	mutex_lock(&vpfe_dev->lock);
+	/* if this instance is doing IO */
+	if (fh->io_allowed) {
+		if (vpfe_dev->started) {
+			sdinfo = vpfe_dev->current_subdev;
+			ret = v4l2_device_call_until_err(&vpfe_dev->v4l2_dev,
+							 sdinfo->grp_id,
+							 video, s_stream, 0);
+			if (ret && (ret != -ENOIOCTLCMD))
+				v4l2_err(&vpfe_dev->v4l2_dev,
+				"stream off failed in subdev\n");
+			vpfe_stop_ccdc_capture(vpfe_dev);
+			vpfe_detach_irq(vpfe_dev);
+			videobuf_streamoff(&vpfe_dev->buffer_queue);
+		}
+		vpfe_dev->io_usrs = 0;
+		vpfe_dev->numbuffers = config_params.numbuffers;
+		videobuf_stop(&vpfe_dev->buffer_queue);
+		videobuf_mmap_free(&vpfe_dev->buffer_queue);
+	}
+
+	/* Decrement device usrs counter */
+	vpfe_dev->usrs--;
+	v4l2_fh_del(&fh->fh);
+	v4l2_fh_exit(&fh->fh);
+	/* If this is the last file handle */
+	if (!vpfe_dev->usrs) {
+		vpfe_dev->initialized = 0;
+		if (ccdc_dev->hw_ops.close)
+			ccdc_dev->hw_ops.close(vpfe_dev->pdev);
+		module_put(ccdc_dev->owner);
+	}
+	mutex_unlock(&vpfe_dev->lock);
+	file->private_data = NULL;
+	/* Free memory allocated to file handle object */
+	kfree(fh);
+	return 0;
+}
+
+/*
+ * vpfe_mmap : It is used to map kernel space buffers
+ * into user spaces
+ */
+static int vpfe_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	/* Get the device object and file handle object */
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_mmap\n");
+
+	return videobuf_mmap_mapper(&vpfe_dev->buffer_queue, vma);
+}
+
+/*
+ * vpfe_poll: It is used for select/poll system call
+ */
+static unsigned int vpfe_poll(struct file *file, poll_table *wait)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_poll\n");
+
+	if (vpfe_dev->started)
+		return videobuf_poll_stream(file,
+					    &vpfe_dev->buffer_queue, wait);
+	return 0;
+}
+
+/* vpfe capture driver file operations */
+static const struct v4l2_file_operations vpfe_fops = {
+	.owner = THIS_MODULE,
+	.open = vpfe_open,
+	.release = vpfe_release,
+	.unlocked_ioctl = video_ioctl2,
+	.mmap = vpfe_mmap,
+	.poll = vpfe_poll
+};
+
+/*
+ * vpfe_check_format()
+ * This function adjust the input pixel format as per hardware
+ * capabilities and update the same in pixfmt.
+ * Following algorithm used :-
+ *
+ *	If given pixformat is not in the vpfe list of pix formats or not
+ *	supported by the hardware, current value of pixformat in the device
+ *	is used
+ *	If given field is not supported, then current field is used. If field
+ *	is different from current, then it is matched with that from sub device.
+ *	Minimum height is 2 lines for interlaced or tb field and 1 line for
+ *	progressive. Maximum height is clamped to active active lines of scan
+ *	Minimum width is 32 bytes in memory and width is clamped to active
+ *	pixels of scan.
+ *	bytesperline is a multiple of 32.
+ */
+static const struct vpfe_pixel_format *
+	vpfe_check_format(struct vpfe_device *vpfe_dev,
+			  struct v4l2_pix_format *pixfmt)
+{
+	u32 min_height = 1, min_width = 32, max_width, max_height;
+	const struct vpfe_pixel_format *vpfe_pix_fmt;
+	u32 pix;
+	int temp, found;
+
+	vpfe_pix_fmt = vpfe_lookup_pix_format(pixfmt->pixelformat);
+	if (NULL == vpfe_pix_fmt) {
+		/*
+		 * use current pixel format in the vpfe device. We
+		 * will find this pix format in the table
+		 */
+		pixfmt->pixelformat = vpfe_dev->fmt.fmt.pix.pixelformat;
+		vpfe_pix_fmt = vpfe_lookup_pix_format(pixfmt->pixelformat);
+	}
+
+	/* check if hw supports it */
+	temp = 0;
+	found = 0;
+	while (ccdc_dev->hw_ops.enum_pix(&pix, temp) >= 0) {
+		if (vpfe_pix_fmt->fmtdesc.pixelformat == pix) {
+			found = 1;
+			break;
+		}
+		temp++;
+	}
+
+	if (!found) {
+		/* use current pixel format */
+		pixfmt->pixelformat = vpfe_dev->fmt.fmt.pix.pixelformat;
+		/*
+		 * Since this is currently used in the vpfe device, we
+		 * will find this pix format in the table
+		 */
+		vpfe_pix_fmt = vpfe_lookup_pix_format(pixfmt->pixelformat);
+	}
+
+	/* check what field format is supported */
+	if (pixfmt->field == V4L2_FIELD_ANY) {
+		/* if field is any, use current value as default */
+		pixfmt->field = vpfe_dev->fmt.fmt.pix.field;
+	}
+
+	/*
+	 * if field is not same as current field in the vpfe device
+	 * try matching the field with the sub device field
+	 */
+	if (vpfe_dev->fmt.fmt.pix.field != pixfmt->field) {
+		/*
+		 * If field value is not in the supported fields, use current
+		 * field used in the device as default
+		 */
+		switch (pixfmt->field) {
+		case V4L2_FIELD_INTERLACED:
+		case V4L2_FIELD_SEQ_TB:
+			/* if sub device is supporting progressive, use that */
+			if (!vpfe_dev->std_info.frame_format)
+				pixfmt->field = V4L2_FIELD_NONE;
+			break;
+		case V4L2_FIELD_NONE:
+			if (vpfe_dev->std_info.frame_format)
+				pixfmt->field = V4L2_FIELD_INTERLACED;
+			break;
+
+		default:
+			/* use current field as default */
+			pixfmt->field = vpfe_dev->fmt.fmt.pix.field;
+			break;
+		}
+	}
+
+	/* Now adjust image resolutions supported */
+	if (pixfmt->field == V4L2_FIELD_INTERLACED ||
+	    pixfmt->field == V4L2_FIELD_SEQ_TB)
+		min_height = 2;
+
+	max_width = vpfe_dev->std_info.active_pixels;
+	max_height = vpfe_dev->std_info.active_lines;
+	min_width /= vpfe_pix_fmt->bpp;
+
+	v4l2_info(&vpfe_dev->v4l2_dev, "width = %d, height = %d, bpp = %d\n",
+		  pixfmt->width, pixfmt->height, vpfe_pix_fmt->bpp);
+
+	pixfmt->width = clamp((pixfmt->width), min_width, max_width);
+	pixfmt->height = clamp((pixfmt->height), min_height, max_height);
+
+	/* If interlaced, adjust height to be a multiple of 2 */
+	if (pixfmt->field == V4L2_FIELD_INTERLACED)
+		pixfmt->height &= (~1);
+	/*
+	 * recalculate bytesperline and sizeimage since width
+	 * and height might have changed
+	 */
+	pixfmt->bytesperline = (((pixfmt->width * vpfe_pix_fmt->bpp) + 31)
+				& ~31);
+	if (pixfmt->pixelformat == V4L2_PIX_FMT_NV12)
+		pixfmt->sizeimage =
+			pixfmt->bytesperline * pixfmt->height +
+			((pixfmt->bytesperline * pixfmt->height) >> 1);
+	else
+		pixfmt->sizeimage = pixfmt->bytesperline * pixfmt->height;
+
+	v4l2_info(&vpfe_dev->v4l2_dev, "adjusted width = %d, height ="
+		 " %d, bpp = %d, bytesperline = %d, sizeimage = %d\n",
+		 pixfmt->width, pixfmt->height, vpfe_pix_fmt->bpp,
+		 pixfmt->bytesperline, pixfmt->sizeimage);
+	return vpfe_pix_fmt;
+}
+
+static int vpfe_querycap(struct file *file, void  *priv,
+			       struct v4l2_capability *cap)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_querycap\n");
+
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	strlcpy(cap->driver, CAPTURE_DRV_NAME, sizeof(cap->driver));
+	strlcpy(cap->bus_info, "VPFE", sizeof(cap->bus_info));
+	strlcpy(cap->card, vpfe_dev->cfg->card_name, sizeof(cap->card));
+	return 0;
+}
+
+static int vpfe_g_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *fmt)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_g_fmt_vid_cap\n");
+	/* Fill in the information about format */
+	*fmt = vpfe_dev->fmt;
+	return 0;
+}
+
+static int vpfe_enum_fmt_vid_cap(struct file *file, void  *priv,
+				   struct v4l2_fmtdesc *fmt)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+	const struct vpfe_pixel_format *pix_fmt;
+	int temp_index;
+	u32 pix;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_enum_fmt_vid_cap\n");
+
+	if (ccdc_dev->hw_ops.enum_pix(&pix, fmt->index) < 0)
+		return -EINVAL;
+
+	/* Fill in the information about format */
+	pix_fmt = vpfe_lookup_pix_format(pix);
+	if (NULL != pix_fmt) {
+		temp_index = fmt->index;
+		*fmt = pix_fmt->fmtdesc;
+		fmt->index = temp_index;
+		return 0;
+	}
+	return -EINVAL;
+}
+
+static int vpfe_s_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *fmt)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+	const struct vpfe_pixel_format *pix_fmts;
+	int ret = 0;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_s_fmt_vid_cap\n");
+
+	/* If streaming is started, return error */
+	if (vpfe_dev->started) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "Streaming is started\n");
+		return -EBUSY;
+	}
+
+	/* Check for valid frame format */
+	pix_fmts = vpfe_check_format(vpfe_dev, &fmt->fmt.pix);
+
+	if (NULL == pix_fmts)
+		return -EINVAL;
+
+	/* store the pixel format in the device  object */
+	ret = mutex_lock_interruptible(&vpfe_dev->lock);
+	if (ret)
+		return ret;
+
+	/* First detach any IRQ if currently attached */
+	vpfe_detach_irq(vpfe_dev);
+	vpfe_dev->fmt = *fmt;
+	/* set image capture parameters in the ccdc */
+	ret = vpfe_config_ccdc_image_format(vpfe_dev);
+	mutex_unlock(&vpfe_dev->lock);
+	return ret;
+}
+
+static int vpfe_try_fmt_vid_cap(struct file *file, void *priv,
+				  struct v4l2_format *f)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+	const struct vpfe_pixel_format *pix_fmts;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_try_fmt_vid_cap\n");
+
+	pix_fmts = vpfe_check_format(vpfe_dev, &f->fmt.pix);
+	if (NULL == pix_fmts)
+		return -EINVAL;
+	return 0;
+}
+
+/*
+ * vpfe_get_subdev_input_index - Get subdev index and subdev input index for a
+ * given app input index
+ */
+static int vpfe_get_subdev_input_index(struct vpfe_device *vpfe_dev,
+					int *subdev_index,
+					int *subdev_input_index,
+					int app_input_index)
+{
+	struct vpfe_config *cfg = vpfe_dev->cfg;
+	struct vpfe_subdev_info *sdinfo;
+	int i, j = 0;
+
+	for (i = 0; i < cfg->num_subdevs; i++) {
+		sdinfo = &cfg->sub_devs[i];
+		if (app_input_index < (j + sdinfo->num_inputs)) {
+			*subdev_index = i;
+			*subdev_input_index = app_input_index - j;
+			return 0;
+		}
+		j += sdinfo->num_inputs;
+	}
+	return -EINVAL;
+}
+
+/*
+ * vpfe_get_app_input - Get app input index for a given subdev input index
+ * driver stores the input index of the current sub device and translate it
+ * when application request the current input
+ */
+static int vpfe_get_app_input_index(struct vpfe_device *vpfe_dev,
+				    int *app_input_index)
+{
+	struct vpfe_config *cfg = vpfe_dev->cfg;
+	struct vpfe_subdev_info *sdinfo;
+	int i, j = 0;
+
+	for (i = 0; i < cfg->num_subdevs; i++) {
+		sdinfo = &cfg->sub_devs[i];
+		if (!strcmp(sdinfo->name, vpfe_dev->current_subdev->name)) {
+			if (vpfe_dev->current_input >= sdinfo->num_inputs)
+				return -1;
+			*app_input_index = j + vpfe_dev->current_input;
+			return 0;
+		}
+		j += sdinfo->num_inputs;
+	}
+	return -EINVAL;
+}
+
+static int vpfe_enum_input(struct file *file, void *priv,
+				 struct v4l2_input *inp)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+	struct vpfe_subdev_info *sdinfo;
+	int subdev, index ;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_enum_input\n");
+
+	if (vpfe_get_subdev_input_index(vpfe_dev,
+					&subdev,
+					&index,
+					inp->index) < 0) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "input information not found"
+			 " for the subdev\n");
+		return -EINVAL;
+	}
+	sdinfo = &vpfe_dev->cfg->sub_devs[subdev];
+	memcpy(inp, &sdinfo->inputs[index], sizeof(struct v4l2_input));
+	return 0;
+}
+
+static int vpfe_g_input(struct file *file, void *priv, unsigned int *index)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_g_input\n");
+
+	return vpfe_get_app_input_index(vpfe_dev, index);
+}
+
+
+static int vpfe_s_input(struct file *file, void *priv, unsigned int index)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+	struct v4l2_subdev *sd;
+	struct vpfe_subdev_info *sdinfo;
+	int subdev_index, inp_index;
+	struct vpfe_route *route;
+	u32 input = 0, output = 0;
+	int ret = -EINVAL;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_s_input\n");
+
+	ret = mutex_lock_interruptible(&vpfe_dev->lock);
+	if (ret)
+		return ret;
+
+	/*
+	 * If streaming is started return device busy
+	 * error
+	 */
+	if (vpfe_dev->started) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "Streaming is on\n");
+		ret = -EBUSY;
+		goto unlock_out;
+	}
+	ret = vpfe_get_subdev_input_index(vpfe_dev,
+					  &subdev_index,
+					  &inp_index,
+					  index);
+	if (ret < 0) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "invalid input index\n");
+		goto unlock_out;
+	}
+
+	sdinfo = &vpfe_dev->cfg->sub_devs[subdev_index];
+	sd = vpfe_dev->sd[subdev_index];
+	route = &sdinfo->routes[inp_index];
+	if (route && sdinfo->can_route) {
+		input = route->input;
+		output = route->output;
+	}
+
+	if (sd)
+		ret = v4l2_subdev_call(sd, video, s_routing, input, output, 0);
+
+	if (ret) {
+		v4l2_err(&vpfe_dev->v4l2_dev,
+			"vpfe_doioctl:error in setting input in decoder\n");
+		ret = -EINVAL;
+		goto unlock_out;
+	}
+	vpfe_dev->current_subdev = sdinfo;
+	if (sd)
+		vpfe_dev->v4l2_dev.ctrl_handler = sd->ctrl_handler;
+	vpfe_dev->current_input = index;
+	vpfe_dev->std_index = 0;
+
+	/* set the bus/interface parameter for the sub device in ccdc */
+	ret = ccdc_dev->hw_ops.set_hw_if_params(&sdinfo->ccdc_if_params);
+	if (ret)
+		goto unlock_out;
+
+	/* set the default image parameters in the device */
+	ret = vpfe_config_image_format(vpfe_dev,
+				vpfe_standards[vpfe_dev->std_index].std_id);
+unlock_out:
+	mutex_unlock(&vpfe_dev->lock);
+	return ret;
+}
+
+static int vpfe_querystd(struct file *file, void *priv, v4l2_std_id *std_id)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+	struct vpfe_subdev_info *sdinfo;
+	int ret = 0;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_querystd\n");
+
+	ret = mutex_lock_interruptible(&vpfe_dev->lock);
+	sdinfo = vpfe_dev->current_subdev;
+	if (ret)
+		return ret;
+	/* Call querystd function of decoder device */
+	ret = v4l2_device_call_until_err(&vpfe_dev->v4l2_dev, sdinfo->grp_id,
+					 video, querystd, std_id);
+	mutex_unlock(&vpfe_dev->lock);
+	return ret;
+}
+
+static int vpfe_s_std(struct file *file, void *priv, v4l2_std_id std_id)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+	struct vpfe_subdev_info *sdinfo;
+	int ret = 0;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_s_std\n");
+
+	/* Call decoder driver function to set the standard */
+	ret = mutex_lock_interruptible(&vpfe_dev->lock);
+	if (ret)
+		return ret;
+
+	sdinfo = vpfe_dev->current_subdev;
+	/* If streaming is started, return device busy error */
+	if (vpfe_dev->started) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "streaming is started\n");
+		ret = -EBUSY;
+		goto unlock_out;
+	}
+
+	ret = v4l2_device_call_until_err(&vpfe_dev->v4l2_dev, sdinfo->grp_id,
+					 video, s_std, std_id);
+	if (ret < 0) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "Failed to set standard\n");
+		goto unlock_out;
+	}
+	ret = vpfe_config_image_format(vpfe_dev, std_id);
+
+unlock_out:
+	mutex_unlock(&vpfe_dev->lock);
+	return ret;
+}
+
+static int vpfe_g_std(struct file *file, void *priv, v4l2_std_id *std_id)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_g_std\n");
+
+	*std_id = vpfe_standards[vpfe_dev->std_index].std_id;
+	return 0;
+}
+/*
+ *  Videobuf operations
+ */
+static int vpfe_videobuf_setup(struct videobuf_queue *vq,
+				unsigned int *count,
+				unsigned int *size)
+{
+	struct vpfe_fh *fh = vq->priv_data;
+	struct vpfe_device *vpfe_dev = fh->vpfe_dev;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_buffer_setup\n");
+	*size = vpfe_dev->fmt.fmt.pix.sizeimage;
+	if (vpfe_dev->memory == V4L2_MEMORY_MMAP &&
+		vpfe_dev->fmt.fmt.pix.sizeimage > config_params.device_bufsize)
+		*size = config_params.device_bufsize;
+
+	if (*count < config_params.min_numbuffers)
+		*count = config_params.min_numbuffers;
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev,
+		"count=%d, size=%d\n", *count, *size);
+	return 0;
+}
+
+static int vpfe_videobuf_prepare(struct videobuf_queue *vq,
+				struct videobuf_buffer *vb,
+				enum v4l2_field field)
+{
+	struct vpfe_fh *fh = vq->priv_data;
+	struct vpfe_device *vpfe_dev = fh->vpfe_dev;
+	unsigned long addr;
+	int ret;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_buffer_prepare\n");
+
+	/* If buffer is not initialized, initialize it */
+	if (VIDEOBUF_NEEDS_INIT == vb->state) {
+		vb->width = vpfe_dev->fmt.fmt.pix.width;
+		vb->height = vpfe_dev->fmt.fmt.pix.height;
+		vb->size = vpfe_dev->fmt.fmt.pix.sizeimage;
+		vb->field = field;
+
+		ret = videobuf_iolock(vq, vb, NULL);
+		if (ret < 0)
+			return ret;
+
+		addr = videobuf_to_dma_contig(vb);
+		/* Make sure user addresses are aligned to 32 bytes */
+		if (!ALIGN(addr, 32))
+			return -EINVAL;
+
+		vb->state = VIDEOBUF_PREPARED;
+	}
+	return 0;
+}
+
+static void vpfe_videobuf_queue(struct videobuf_queue *vq,
+				struct videobuf_buffer *vb)
+{
+	/* Get the file handle object and device object */
+	struct vpfe_fh *fh = vq->priv_data;
+	struct vpfe_device *vpfe_dev = fh->vpfe_dev;
+	unsigned long flags;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_buffer_queue\n");
+
+	/* add the buffer to the DMA queue */
+	spin_lock_irqsave(&vpfe_dev->dma_queue_lock, flags);
+	list_add_tail(&vb->queue, &vpfe_dev->dma_queue);
+	spin_unlock_irqrestore(&vpfe_dev->dma_queue_lock, flags);
+
+	/* Change state of the buffer */
+	vb->state = VIDEOBUF_QUEUED;
+}
+
+static void vpfe_videobuf_release(struct videobuf_queue *vq,
+				  struct videobuf_buffer *vb)
+{
+	struct vpfe_fh *fh = vq->priv_data;
+	struct vpfe_device *vpfe_dev = fh->vpfe_dev;
+	unsigned long flags;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_videobuf_release\n");
+
+	/*
+	 * We need to flush the buffer from the dma queue since
+	 * they are de-allocated
+	 */
+	spin_lock_irqsave(&vpfe_dev->dma_queue_lock, flags);
+	INIT_LIST_HEAD(&vpfe_dev->dma_queue);
+	spin_unlock_irqrestore(&vpfe_dev->dma_queue_lock, flags);
+	videobuf_dma_contig_free(vq, vb);
+	vb->state = VIDEOBUF_NEEDS_INIT;
+}
+
+static struct videobuf_queue_ops vpfe_videobuf_qops = {
+	.buf_setup      = vpfe_videobuf_setup,
+	.buf_prepare    = vpfe_videobuf_prepare,
+	.buf_queue      = vpfe_videobuf_queue,
+	.buf_release    = vpfe_videobuf_release,
+};
+
+/*
+ * vpfe_reqbufs. currently support REQBUF only once opening
+ * the device.
+ */
+static int vpfe_reqbufs(struct file *file, void *priv,
+			struct v4l2_requestbuffers *req_buf)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+	struct vpfe_fh *fh = file->private_data;
+	int ret = 0;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_reqbufs\n");
+
+	if (V4L2_BUF_TYPE_VIDEO_CAPTURE != req_buf->type) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "Invalid buffer type\n");
+		return -EINVAL;
+	}
+
+	ret = mutex_lock_interruptible(&vpfe_dev->lock);
+	if (ret)
+		return ret;
+
+	if (vpfe_dev->io_usrs != 0) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "Only one IO user allowed\n");
+		ret = -EBUSY;
+		goto unlock_out;
+	}
+
+	vpfe_dev->memory = req_buf->memory;
+	videobuf_queue_dma_contig_init(&vpfe_dev->buffer_queue,
+				&vpfe_videobuf_qops,
+				vpfe_dev->pdev,
+				&vpfe_dev->irqlock,
+				req_buf->type,
+				vpfe_dev->fmt.fmt.pix.field,
+				sizeof(struct videobuf_buffer),
+				fh, NULL);
+
+	fh->io_allowed = 1;
+	vpfe_dev->io_usrs = 1;
+	INIT_LIST_HEAD(&vpfe_dev->dma_queue);
+	ret = videobuf_reqbufs(&vpfe_dev->buffer_queue, req_buf);
+unlock_out:
+	mutex_unlock(&vpfe_dev->lock);
+	return ret;
+}
+
+static int vpfe_querybuf(struct file *file, void *priv,
+			 struct v4l2_buffer *buf)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_querybuf\n");
+
+	if (V4L2_BUF_TYPE_VIDEO_CAPTURE != buf->type) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "Invalid buf type\n");
+		return  -EINVAL;
+	}
+
+	if (vpfe_dev->memory != V4L2_MEMORY_MMAP) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "Invalid memory\n");
+		return -EINVAL;
+	}
+	/* Call videobuf_querybuf to get information */
+	return videobuf_querybuf(&vpfe_dev->buffer_queue, buf);
+}
+
+static int vpfe_qbuf(struct file *file, void *priv,
+		     struct v4l2_buffer *p)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+	struct vpfe_fh *fh = file->private_data;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_qbuf\n");
+
+	if (V4L2_BUF_TYPE_VIDEO_CAPTURE != p->type) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "Invalid buf type\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * If this file handle is not allowed to do IO,
+	 * return error
+	 */
+	if (!fh->io_allowed) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "fh->io_allowed\n");
+		return -EACCES;
+	}
+	return videobuf_qbuf(&vpfe_dev->buffer_queue, p);
+}
+
+static int vpfe_dqbuf(struct file *file, void *priv,
+		      struct v4l2_buffer *buf)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_dqbuf\n");
+
+	if (V4L2_BUF_TYPE_VIDEO_CAPTURE != buf->type) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "Invalid buf type\n");
+		return -EINVAL;
+	}
+	return videobuf_dqbuf(&vpfe_dev->buffer_queue,
+				      buf, file->f_flags & O_NONBLOCK);
+}
+
+/*
+ * vpfe_calculate_offsets : This function calculates buffers offset
+ * for top and bottom field
+ */
+static void vpfe_calculate_offsets(struct vpfe_device *vpfe_dev)
+{
+	struct v4l2_rect image_win;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_calculate_offsets\n");
+
+	ccdc_dev->hw_ops.get_image_window(&image_win);
+	vpfe_dev->field_off = image_win.height * image_win.width;
+}
+
+/* vpfe_start_ccdc_capture: start streaming in ccdc/isif */
+static void vpfe_start_ccdc_capture(struct vpfe_device *vpfe_dev)
+{
+	ccdc_dev->hw_ops.enable(1);
+	if (ccdc_dev->hw_ops.enable_out_to_sdram)
+		ccdc_dev->hw_ops.enable_out_to_sdram(1);
+	vpfe_dev->started = 1;
+}
+
+/*
+ * vpfe_streamon. Assume the DMA queue is not empty.
+ * application is expected to call QBUF before calling
+ * this ioctl. If not, driver returns error
+ */
+static int vpfe_streamon(struct file *file, void *priv,
+			 enum v4l2_buf_type buf_type)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+	struct vpfe_fh *fh = file->private_data;
+	struct vpfe_subdev_info *sdinfo;
+	unsigned long addr;
+	int ret = 0;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_streamon\n");
+
+	if (V4L2_BUF_TYPE_VIDEO_CAPTURE != buf_type) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "Invalid buf type\n");
+		return -EINVAL;
+	}
+
+	/* If file handle is not allowed IO, return error */
+	if (!fh->io_allowed) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "fh->io_allowed\n");
+		return -EACCES;
+	}
+
+	sdinfo = vpfe_dev->current_subdev;
+	ret = v4l2_device_call_until_err(&vpfe_dev->v4l2_dev, sdinfo->grp_id,
+					video, s_stream, 1);
+
+	if (ret && (ret != -ENOIOCTLCMD)) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "stream on failed in subdev\n");
+		return -EINVAL;
+	}
+
+	/* If buffer queue is empty, return error */
+	if (list_empty(&vpfe_dev->buffer_queue.stream)) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "buffer queue is empty\n");
+		return -EIO;
+	}
+
+	/* Call videobuf_streamon to start streaming * in videobuf */
+	ret = videobuf_streamon(&vpfe_dev->buffer_queue);
+	if (ret)
+		return ret;
+
+
+	ret = mutex_lock_interruptible(&vpfe_dev->lock);
+	if (ret)
+		goto streamoff;
+	/* Get the next frame from the buffer queue */
+	vpfe_dev->next_frm = list_entry(vpfe_dev->dma_queue.next,
+					struct videobuf_buffer, queue);
+	vpfe_dev->cur_frm = vpfe_dev->next_frm;
+	/* Remove buffer from the buffer queue */
+	list_del(&vpfe_dev->cur_frm->queue);
+	/* Mark state of the current frame to active */
+	vpfe_dev->cur_frm->state = VIDEOBUF_ACTIVE;
+	/* Initialize field_id and started member */
+	vpfe_dev->field_id = 0;
+	addr = videobuf_to_dma_contig(vpfe_dev->cur_frm);
+
+	/* Calculate field offset */
+	vpfe_calculate_offsets(vpfe_dev);
+
+	if (vpfe_attach_irq(vpfe_dev) < 0) {
+		v4l2_err(&vpfe_dev->v4l2_dev,
+			 "Error in attaching interrupt handle\n");
+		ret = -EFAULT;
+		goto unlock_out;
+	}
+	if (ccdc_dev->hw_ops.configure() < 0) {
+		v4l2_err(&vpfe_dev->v4l2_dev,
+			 "Error in configuring ccdc\n");
+		ret = -EINVAL;
+		goto unlock_out;
+	}
+	ccdc_dev->hw_ops.setfbaddr((unsigned long)(addr));
+	vpfe_start_ccdc_capture(vpfe_dev);
+	mutex_unlock(&vpfe_dev->lock);
+	return ret;
+unlock_out:
+	mutex_unlock(&vpfe_dev->lock);
+streamoff:
+	ret = videobuf_streamoff(&vpfe_dev->buffer_queue);
+	return ret;
+}
+
+static int vpfe_streamoff(struct file *file, void *priv,
+			  enum v4l2_buf_type buf_type)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+	struct vpfe_fh *fh = file->private_data;
+	struct vpfe_subdev_info *sdinfo;
+	int ret = 0;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_streamoff\n");
+
+	if (V4L2_BUF_TYPE_VIDEO_CAPTURE != buf_type) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "Invalid buf type\n");
+		return -EINVAL;
+	}
+
+	/* If io is allowed for this file handle, return error */
+	if (!fh->io_allowed) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "fh->io_allowed\n");
+		return -EACCES;
+	}
+
+	/* If streaming is not started, return error */
+	if (!vpfe_dev->started) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "device started\n");
+		return -EINVAL;
+	}
+
+	ret = mutex_lock_interruptible(&vpfe_dev->lock);
+	if (ret)
+		return ret;
+
+	vpfe_stop_ccdc_capture(vpfe_dev);
+	vpfe_detach_irq(vpfe_dev);
+
+	sdinfo = vpfe_dev->current_subdev;
+	ret = v4l2_device_call_until_err(&vpfe_dev->v4l2_dev, sdinfo->grp_id,
+					video, s_stream, 0);
+
+	if (ret && (ret != -ENOIOCTLCMD))
+		v4l2_err(&vpfe_dev->v4l2_dev, "stream off failed in subdev\n");
+	ret = videobuf_streamoff(&vpfe_dev->buffer_queue);
+	mutex_unlock(&vpfe_dev->lock);
+	return ret;
+}
+
+static int vpfe_cropcap(struct file *file, void *priv,
+			      struct v4l2_cropcap *crop)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_cropcap\n");
+
+	if (vpfe_dev->std_index >= ARRAY_SIZE(vpfe_standards))
+		return -EINVAL;
+
+	memset(crop, 0, sizeof(struct v4l2_cropcap));
+	crop->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	crop->bounds.width = crop->defrect.width =
+		vpfe_standards[vpfe_dev->std_index].width;
+	crop->bounds.height = crop->defrect.height =
+		vpfe_standards[vpfe_dev->std_index].height;
+	crop->pixelaspect = vpfe_standards[vpfe_dev->std_index].pixelaspect;
+	return 0;
+}
+
+static int vpfe_g_crop(struct file *file, void *priv,
+			     struct v4l2_crop *crop)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_g_crop\n");
+
+	crop->c = vpfe_dev->crop;
+	return 0;
+}
+
+static int vpfe_s_crop(struct file *file, void *priv,
+			     const struct v4l2_crop *crop)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+	struct v4l2_rect rect = crop->c;
+	int ret = 0;
+
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_s_crop\n");
+
+	if (vpfe_dev->started) {
+		/* make sure streaming is not started */
+		v4l2_err(&vpfe_dev->v4l2_dev,
+			"Cannot change crop when streaming is ON\n");
+		return -EBUSY;
+	}
+
+	ret = mutex_lock_interruptible(&vpfe_dev->lock);
+	if (ret)
+		return ret;
+
+	if (rect.top < 0 || rect.left < 0) {
+		v4l2_err(&vpfe_dev->v4l2_dev,
+			"doesn't support negative values for top & left\n");
+		ret = -EINVAL;
+		goto unlock_out;
+	}
+
+	/* adjust the width to 16 pixel boundary */
+	rect.width = ((rect.width + 15) & ~0xf);
+
+	/* make sure parameters are valid */
+	if ((rect.left + rect.width >
+		vpfe_dev->std_info.active_pixels) ||
+	    (rect.top + rect.height >
+		vpfe_dev->std_info.active_lines)) {
+		v4l2_err(&vpfe_dev->v4l2_dev, "Error in S_CROP params\n");
+		ret = -EINVAL;
+		goto unlock_out;
+	}
+	ccdc_dev->hw_ops.set_image_window(&rect);
+	vpfe_dev->fmt.fmt.pix.width = rect.width;
+	vpfe_dev->fmt.fmt.pix.height = rect.height;
+	vpfe_dev->fmt.fmt.pix.bytesperline =
+		ccdc_dev->hw_ops.get_line_length();
+	vpfe_dev->fmt.fmt.pix.sizeimage =
+		vpfe_dev->fmt.fmt.pix.bytesperline *
+		vpfe_dev->fmt.fmt.pix.height;
+	vpfe_dev->crop = rect;
+unlock_out:
+	mutex_unlock(&vpfe_dev->lock);
+	return ret;
+}
+
+
+static long vpfe_param_handler(struct file *file, void *priv,
+		bool valid_prio, unsigned int cmd, void *param)
+{
+	struct vpfe_device *vpfe_dev = video_drvdata(file);
+	int ret = 0;
+
+	v4l2_dbg(2, debug, &vpfe_dev->v4l2_dev, "vpfe_param_handler\n");
+
+	if (vpfe_dev->started) {
+		/* only allowed if streaming is not started */
+		v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev,
+			"device already started\n");
+		return -EBUSY;
+	}
+
+	ret = mutex_lock_interruptible(&vpfe_dev->lock);
+	if (ret)
+		return ret;
+
+	switch (cmd) {
+	case VPFE_CMD_S_CCDC_RAW_PARAMS:
+		ret = -EINVAL;
+		v4l2_warn(&vpfe_dev->v4l2_dev,
+			"VPFE_CMD_S_CCDC_RAW_PARAMS not supported\n");
+		break;
+	default:
+		ret = -ENOTTY;
+	}
+unlock_out:
+	mutex_unlock(&vpfe_dev->lock);
+	return ret;
+}
+
+
+/* vpfe capture ioctl operations */
+static const struct v4l2_ioctl_ops vpfe_ioctl_ops = {
+	.vidioc_querycap	 = vpfe_querycap,
+	.vidioc_g_fmt_vid_cap    = vpfe_g_fmt_vid_cap,
+	.vidioc_enum_fmt_vid_cap = vpfe_enum_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap    = vpfe_s_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap  = vpfe_try_fmt_vid_cap,
+	.vidioc_enum_input	 = vpfe_enum_input,
+	.vidioc_g_input		 = vpfe_g_input,
+	.vidioc_s_input		 = vpfe_s_input,
+	.vidioc_querystd	 = vpfe_querystd,
+	.vidioc_s_std		 = vpfe_s_std,
+	.vidioc_g_std		 = vpfe_g_std,
+	.vidioc_reqbufs		 = vpfe_reqbufs,
+	.vidioc_querybuf	 = vpfe_querybuf,
+	.vidioc_qbuf		 = vpfe_qbuf,
+	.vidioc_dqbuf		 = vpfe_dqbuf,
+	.vidioc_streamon	 = vpfe_streamon,
+	.vidioc_streamoff	 = vpfe_streamoff,
+	.vidioc_cropcap		 = vpfe_cropcap,
+	.vidioc_g_crop		 = vpfe_g_crop,
+	.vidioc_s_crop		 = vpfe_s_crop,
+	.vidioc_default		 = vpfe_param_handler,
+};
+
+static struct vpfe_device *vpfe_initialize(void)
+{
+	struct vpfe_device *vpfe_dev;
+
+	/* Default number of buffers should be 3 */
+	if ((numbuffers > 0) &&
+	    (numbuffers < config_params.min_numbuffers))
+		numbuffers = config_params.min_numbuffers;
+
+	/*
+	 * Set buffer size to min buffers size if invalid buffer size is
+	 * given
+	 */
+	if (bufsize < config_params.min_bufsize)
+		bufsize = config_params.min_bufsize;
+
+	config_params.numbuffers = numbuffers;
+
+	if (numbuffers)
+		config_params.device_bufsize = bufsize;
+
+	/* Allocate memory for device objects */
+	vpfe_dev = kzalloc(sizeof(*vpfe_dev), GFP_KERNEL);
+
+	return vpfe_dev;
+}
+
+/*
+ * vpfe_probe : This function creates device entries by register
+ * itself to the V4L2 driver and initializes fields of each
+ * device objects
+ */
+static int vpfe_probe(struct platform_device *pdev)
+{
+	struct vpfe_subdev_info *sdinfo;
+	struct vpfe_config *vpfe_cfg;
+	struct resource *res1;
+	struct vpfe_device *vpfe_dev;
+	struct i2c_adapter *i2c_adap;
+	struct video_device *vfd;
+	int ret = -ENOMEM, i, j;
+	int num_subdevs = 0;
+
+	/* Get the pointer to the device object */
+	vpfe_dev = vpfe_initialize();
+
+	if (!vpfe_dev) {
+		v4l2_err(pdev->dev.driver,
+			"Failed to allocate memory for vpfe_dev\n");
+		return ret;
+	}
+
+	vpfe_dev->pdev = &pdev->dev;
+
+	if (NULL == pdev->dev.platform_data) {
+		v4l2_err(pdev->dev.driver, "Unable to get vpfe config\n");
+		ret = -ENODEV;
+		goto probe_free_dev_mem;
+	}
+
+	vpfe_cfg = pdev->dev.platform_data;
+	vpfe_dev->cfg = vpfe_cfg;
+	if (NULL == vpfe_cfg->ccdc ||
+	    NULL == vpfe_cfg->card_name ||
+	    NULL == vpfe_cfg->sub_devs) {
+		v4l2_err(pdev->dev.driver, "null ptr in vpfe_cfg\n");
+		ret = -ENOENT;
+		goto probe_free_dev_mem;
+	}
+
+	/* Allocate memory for ccdc configuration */
+	ccdc_cfg = kmalloc(sizeof(struct ccdc_config), GFP_KERNEL);
+	if (NULL == ccdc_cfg) {
+		v4l2_err(pdev->dev.driver,
+			 "Memory allocation failed for ccdc_cfg\n");
+		goto probe_free_dev_mem;
+	}
+
+	mutex_lock(&ccdc_lock);
+
+	strncpy(ccdc_cfg->name, vpfe_cfg->ccdc, 32);
+	/* Get VINT0 irq resource */
+	res1 = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res1) {
+		v4l2_err(pdev->dev.driver,
+			 "Unable to get interrupt for VINT0\n");
+		ret = -ENODEV;
+		goto probe_free_ccdc_cfg_mem;
+	}
+	vpfe_dev->ccdc_irq0 = res1->start;
+
+	/* Get VINT1 irq resource */
+	res1 = platform_get_resource(pdev, IORESOURCE_IRQ, 1);
+	if (!res1) {
+		v4l2_err(pdev->dev.driver,
+			 "Unable to get interrupt for VINT1\n");
+		ret = -ENODEV;
+		goto probe_free_ccdc_cfg_mem;
+	}
+	vpfe_dev->ccdc_irq1 = res1->start;
+
+	ret = request_irq(vpfe_dev->ccdc_irq0, vpfe_isr, 0,
+			  "vpfe_capture0", vpfe_dev);
+
+	if (0 != ret) {
+		v4l2_err(pdev->dev.driver, "Unable to request interrupt\n");
+		goto probe_free_ccdc_cfg_mem;
+	}
+
+	vfd = &vpfe_dev->video_dev;
+	/* Initialize field of video device */
+	vfd->release		= video_device_release_empty;
+	vfd->fops		= &vpfe_fops;
+	vfd->ioctl_ops		= &vpfe_ioctl_ops;
+	vfd->tvnorms		= 0;
+	vfd->v4l2_dev 		= &vpfe_dev->v4l2_dev;
+	snprintf(vfd->name, sizeof(vfd->name),
+		 "%s_V%d.%d.%d",
+		 CAPTURE_DRV_NAME,
+		 (VPFE_CAPTURE_VERSION_CODE >> 16) & 0xff,
+		 (VPFE_CAPTURE_VERSION_CODE >> 8) & 0xff,
+		 (VPFE_CAPTURE_VERSION_CODE) & 0xff);
+
+	ret = v4l2_device_register(&pdev->dev, &vpfe_dev->v4l2_dev);
+	if (ret) {
+		v4l2_err(pdev->dev.driver,
+			"Unable to register v4l2 device.\n");
+		goto probe_out_release_irq;
+	}
+	v4l2_info(&vpfe_dev->v4l2_dev, "v4l2 device registered\n");
+	spin_lock_init(&vpfe_dev->irqlock);
+	spin_lock_init(&vpfe_dev->dma_queue_lock);
+	mutex_init(&vpfe_dev->lock);
+
+	/* Initialize field of the device objects */
+	vpfe_dev->numbuffers = config_params.numbuffers;
+
+	/* register video device */
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev,
+		"trying to register vpfe device.\n");
+	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev,
+		"video_dev=%p\n", &vpfe_dev->video_dev);
+	vpfe_dev->fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	ret = video_register_device(&vpfe_dev->video_dev,
+				    VFL_TYPE_GRABBER, -1);
+
+	if (ret) {
+		v4l2_err(pdev->dev.driver,
+			"Unable to register video device.\n");
+		goto probe_out_v4l2_unregister;
+	}
+
+	v4l2_info(&vpfe_dev->v4l2_dev, "video device registered\n");
+	/* set the driver data in platform device */
+	platform_set_drvdata(pdev, vpfe_dev);
+	/* set driver private data */
+	video_set_drvdata(&vpfe_dev->video_dev, vpfe_dev);
+	i2c_adap = i2c_get_adapter(vpfe_cfg->i2c_adapter_id);
+	num_subdevs = vpfe_cfg->num_subdevs;
+	vpfe_dev->sd = kmalloc(sizeof(struct v4l2_subdev *) * num_subdevs,
+				GFP_KERNEL);
+	if (NULL == vpfe_dev->sd) {
+		v4l2_err(&vpfe_dev->v4l2_dev,
+			"unable to allocate memory for subdevice pointers\n");
+		ret = -ENOMEM;
+		goto probe_out_video_unregister;
+	}
+
+	for (i = 0; i < num_subdevs; i++) {
+		struct v4l2_input *inps;
+
+		sdinfo = &vpfe_cfg->sub_devs[i];
+
+		/* Load up the subdevice */
+		vpfe_dev->sd[i] =
+			v4l2_i2c_new_subdev_board(&vpfe_dev->v4l2_dev,
+						  i2c_adap,
+						  &sdinfo->board_info,
+						  NULL);
+		if (vpfe_dev->sd[i]) {
+			v4l2_info(&vpfe_dev->v4l2_dev,
+				  "v4l2 sub device %s registered\n",
+				  sdinfo->name);
+			vpfe_dev->sd[i]->grp_id = sdinfo->grp_id;
+			/* update tvnorms from the sub devices */
+			for (j = 0; j < sdinfo->num_inputs; j++) {
+				inps = &sdinfo->inputs[j];
+				vfd->tvnorms |= inps->std;
+			}
+		} else {
+			v4l2_info(&vpfe_dev->v4l2_dev,
+				  "v4l2 sub device %s register fails\n",
+				  sdinfo->name);
+			goto probe_sd_out;
+		}
+	}
+
+	/* set first sub device as current one */
+	vpfe_dev->current_subdev = &vpfe_cfg->sub_devs[0];
+	vpfe_dev->v4l2_dev.ctrl_handler = vpfe_dev->sd[0]->ctrl_handler;
+
+	/* We have at least one sub device to work with */
+	mutex_unlock(&ccdc_lock);
+	return 0;
+
+probe_sd_out:
+	kfree(vpfe_dev->sd);
+probe_out_video_unregister:
+	video_unregister_device(&vpfe_dev->video_dev);
+probe_out_v4l2_unregister:
+	v4l2_device_unregister(&vpfe_dev->v4l2_dev);
+probe_out_release_irq:
+	free_irq(vpfe_dev->ccdc_irq0, vpfe_dev);
+probe_free_ccdc_cfg_mem:
+	kfree(ccdc_cfg);
+	mutex_unlock(&ccdc_lock);
+probe_free_dev_mem:
+	kfree(vpfe_dev);
+	return ret;
+}
+
+/*
+ * vpfe_remove : It un-register device from V4L2 driver
+ */
+static int vpfe_remove(struct platform_device *pdev)
+{
+	struct vpfe_device *vpfe_dev = platform_get_drvdata(pdev);
+
+	v4l2_info(pdev->dev.driver, "vpfe_remove\n");
+
+	free_irq(vpfe_dev->ccdc_irq0, vpfe_dev);
+	kfree(vpfe_dev->sd);
+	v4l2_device_unregister(&vpfe_dev->v4l2_dev);
+	video_unregister_device(&vpfe_dev->video_dev);
+	kfree(vpfe_dev);
+	kfree(ccdc_cfg);
+	return 0;
+}
+
+static int vpfe_suspend(struct device *dev)
+{
+	return 0;
+}
+
+static int vpfe_resume(struct device *dev)
+{
+	return 0;
+}
+
+static const struct dev_pm_ops vpfe_dev_pm_ops = {
+	.suspend = vpfe_suspend,
+	.resume = vpfe_resume,
+};
+
+static struct platform_driver vpfe_driver = {
+	.driver = {
+		.name = CAPTURE_DRV_NAME,
+		.pm = &vpfe_dev_pm_ops,
+	},
+	.probe = vpfe_probe,
+	.remove = vpfe_remove,
+};
+
+module_platform_driver(vpfe_driver);
diff --git a/drivers/media/platform/davinci/vpif.c b/drivers/media/platform/davinci/vpif.c
new file mode 100644
index 0000000..0380cf2
--- /dev/null
+++ b/drivers/media/platform/davinci/vpif.c
@@ -0,0 +1,487 @@
+/*
+ * vpif - Video Port Interface driver
+ * VPIF is a receiver and transmitter for video data. It has two channels(0, 1)
+ * that receiveing video byte stream and two channels(2, 3) for video output.
+ * The hardware supports SDTV, HDTV formats, raw data capture.
+ * Currently, the driver supports NTSC and PAL standards.
+ *
+ * Copyright (C) 2009 Texas Instruments Incorporated - http://www.ti.com/
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed .as is. WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/spinlock.h>
+#include <linux/v4l2-dv-timings.h>
+
+#include "vpif.h"
+
+MODULE_DESCRIPTION("TI DaVinci Video Port Interface driver");
+MODULE_LICENSE("GPL");
+
+#define VPIF_CH0_MAX_MODES	22
+#define VPIF_CH1_MAX_MODES	2
+#define VPIF_CH2_MAX_MODES	15
+#define VPIF_CH3_MAX_MODES	2
+
+spinlock_t vpif_lock;
+EXPORT_SYMBOL_GPL(vpif_lock);
+
+void __iomem *vpif_base;
+EXPORT_SYMBOL_GPL(vpif_base);
+
+/**
+ * vpif_ch_params: video standard configuration parameters for vpif
+ * The table must include all presets from supported subdevices.
+ */
+const struct vpif_channel_config_params vpif_ch_params[] = {
+	/* HDTV formats */
+	{
+		.name = "480p59_94",
+		.width = 720,
+		.height = 480,
+		.frm_fmt = 1,
+		.ycmux_mode = 0,
+		.eav2sav = 138-8,
+		.sav2eav = 720,
+		.l1 = 1,
+		.l3 = 43,
+		.l5 = 523,
+		.vsize = 525,
+		.capture_format = 0,
+		.vbi_supported = 0,
+		.hd_sd = 1,
+		.dv_timings = V4L2_DV_BT_CEA_720X480P59_94,
+	},
+	{
+		.name = "576p50",
+		.width = 720,
+		.height = 576,
+		.frm_fmt = 1,
+		.ycmux_mode = 0,
+		.eav2sav = 144-8,
+		.sav2eav = 720,
+		.l1 = 1,
+		.l3 = 45,
+		.l5 = 621,
+		.vsize = 625,
+		.capture_format = 0,
+		.vbi_supported = 0,
+		.hd_sd = 1,
+		.dv_timings = V4L2_DV_BT_CEA_720X576P50,
+	},
+	{
+		.name = "720p50",
+		.width = 1280,
+		.height = 720,
+		.frm_fmt = 1,
+		.ycmux_mode = 0,
+		.eav2sav = 700-8,
+		.sav2eav = 1280,
+		.l1 = 1,
+		.l3 = 26,
+		.l5 = 746,
+		.vsize = 750,
+		.capture_format = 0,
+		.vbi_supported = 0,
+		.hd_sd = 1,
+		.dv_timings = V4L2_DV_BT_CEA_1280X720P50,
+	},
+	{
+		.name = "720p60",
+		.width = 1280,
+		.height = 720,
+		.frm_fmt = 1,
+		.ycmux_mode = 0,
+		.eav2sav = 370 - 8,
+		.sav2eav = 1280,
+		.l1 = 1,
+		.l3 = 26,
+		.l5 = 746,
+		.vsize = 750,
+		.capture_format = 0,
+		.vbi_supported = 0,
+		.hd_sd = 1,
+		.dv_timings = V4L2_DV_BT_CEA_1280X720P60,
+	},
+	{
+		.name = "1080I50",
+		.width = 1920,
+		.height = 1080,
+		.frm_fmt = 0,
+		.ycmux_mode = 0,
+		.eav2sav = 720 - 8,
+		.sav2eav = 1920,
+		.l1 = 1,
+		.l3 = 21,
+		.l5 = 561,
+		.l7 = 563,
+		.l9 = 584,
+		.l11 = 1124,
+		.vsize = 1125,
+		.capture_format = 0,
+		.vbi_supported = 0,
+		.hd_sd = 1,
+		.dv_timings = V4L2_DV_BT_CEA_1920X1080I50,
+	},
+	{
+		.name = "1080I60",
+		.width = 1920,
+		.height = 1080,
+		.frm_fmt = 0,
+		.ycmux_mode = 0,
+		.eav2sav = 280 - 8,
+		.sav2eav = 1920,
+		.l1 = 1,
+		.l3 = 21,
+		.l5 = 561,
+		.l7 = 563,
+		.l9 = 584,
+		.l11 = 1124,
+		.vsize = 1125,
+		.capture_format = 0,
+		.vbi_supported = 0,
+		.hd_sd = 1,
+		.dv_timings = V4L2_DV_BT_CEA_1920X1080I60,
+	},
+	{
+		.name = "1080p60",
+		.width = 1920,
+		.height = 1080,
+		.frm_fmt = 1,
+		.ycmux_mode = 0,
+		.eav2sav = 280 - 8,
+		.sav2eav = 1920,
+		.l1 = 1,
+		.l3 = 42,
+		.l5 = 1122,
+		.vsize = 1125,
+		.capture_format = 0,
+		.vbi_supported = 0,
+		.hd_sd = 1,
+		.dv_timings = V4L2_DV_BT_CEA_1920X1080P60,
+	},
+
+	/* SDTV formats */
+	{
+		.name = "NTSC_M",
+		.width = 720,
+		.height = 480,
+		.frm_fmt = 0,
+		.ycmux_mode = 1,
+		.eav2sav = 268,
+		.sav2eav = 1440,
+		.l1 = 1,
+		.l3 = 23,
+		.l5 = 263,
+		.l7 = 266,
+		.l9 = 286,
+		.l11 = 525,
+		.vsize = 525,
+		.capture_format = 0,
+		.vbi_supported = 1,
+		.hd_sd = 0,
+		.stdid = V4L2_STD_525_60,
+	},
+	{
+		.name = "PAL_BDGHIK",
+		.width = 720,
+		.height = 576,
+		.frm_fmt = 0,
+		.ycmux_mode = 1,
+		.eav2sav = 280,
+		.sav2eav = 1440,
+		.l1 = 1,
+		.l3 = 23,
+		.l5 = 311,
+		.l7 = 313,
+		.l9 = 336,
+		.l11 = 624,
+		.vsize = 625,
+		.capture_format = 0,
+		.vbi_supported = 1,
+		.hd_sd = 0,
+		.stdid = V4L2_STD_625_50,
+	},
+};
+EXPORT_SYMBOL_GPL(vpif_ch_params);
+
+const unsigned int vpif_ch_params_count = ARRAY_SIZE(vpif_ch_params);
+EXPORT_SYMBOL_GPL(vpif_ch_params_count);
+
+static inline void vpif_wr_bit(u32 reg, u32 bit, u32 val)
+{
+	if (val)
+		vpif_set_bit(reg, bit);
+	else
+		vpif_clr_bit(reg, bit);
+}
+
+/* This structure is used to keep track of VPIF size register's offsets */
+struct vpif_registers {
+	u32 h_cfg, v_cfg_00, v_cfg_01, v_cfg_02, v_cfg, ch_ctrl;
+	u32 line_offset, vanc0_strt, vanc0_size, vanc1_strt;
+	u32 vanc1_size, width_mask, len_mask;
+	u8 max_modes;
+};
+
+static const struct vpif_registers vpifregs[VPIF_NUM_CHANNELS] = {
+	/* Channel0 */
+	{
+		VPIF_CH0_H_CFG, VPIF_CH0_V_CFG_00, VPIF_CH0_V_CFG_01,
+		VPIF_CH0_V_CFG_02, VPIF_CH0_V_CFG_03, VPIF_CH0_CTRL,
+		VPIF_CH0_IMG_ADD_OFST, 0, 0, 0, 0, 0x1FFF, 0xFFF,
+		VPIF_CH0_MAX_MODES,
+	},
+	/* Channel1 */
+	{
+		VPIF_CH1_H_CFG, VPIF_CH1_V_CFG_00, VPIF_CH1_V_CFG_01,
+		VPIF_CH1_V_CFG_02, VPIF_CH1_V_CFG_03, VPIF_CH1_CTRL,
+		VPIF_CH1_IMG_ADD_OFST, 0, 0, 0, 0, 0x1FFF, 0xFFF,
+		VPIF_CH1_MAX_MODES,
+	},
+	/* Channel2 */
+	{
+		VPIF_CH2_H_CFG, VPIF_CH2_V_CFG_00, VPIF_CH2_V_CFG_01,
+		VPIF_CH2_V_CFG_02, VPIF_CH2_V_CFG_03, VPIF_CH2_CTRL,
+		VPIF_CH2_IMG_ADD_OFST, VPIF_CH2_VANC0_STRT, VPIF_CH2_VANC0_SIZE,
+		VPIF_CH2_VANC1_STRT, VPIF_CH2_VANC1_SIZE, 0x7FF, 0x7FF,
+		VPIF_CH2_MAX_MODES
+	},
+	/* Channel3 */
+	{
+		VPIF_CH3_H_CFG, VPIF_CH3_V_CFG_00, VPIF_CH3_V_CFG_01,
+		VPIF_CH3_V_CFG_02, VPIF_CH3_V_CFG_03, VPIF_CH3_CTRL,
+		VPIF_CH3_IMG_ADD_OFST, VPIF_CH3_VANC0_STRT, VPIF_CH3_VANC0_SIZE,
+		VPIF_CH3_VANC1_STRT, VPIF_CH3_VANC1_SIZE, 0x7FF, 0x7FF,
+		VPIF_CH3_MAX_MODES
+	},
+};
+
+/* vpif_set_mode_info:
+ * This function is used to set horizontal and vertical config parameters
+ * As per the standard in the channel, configure the values of L1, L3,
+ * L5, L7  L9, L11 in VPIF Register , also write width and height
+ */
+static void vpif_set_mode_info(const struct vpif_channel_config_params *config,
+				u8 channel_id, u8 config_channel_id)
+{
+	u32 value;
+
+	value = (config->eav2sav & vpifregs[config_channel_id].width_mask);
+	value <<= VPIF_CH_LEN_SHIFT;
+	value |= (config->sav2eav & vpifregs[config_channel_id].width_mask);
+	regw(value, vpifregs[channel_id].h_cfg);
+
+	value = (config->l1 & vpifregs[config_channel_id].len_mask);
+	value <<= VPIF_CH_LEN_SHIFT;
+	value |= (config->l3 & vpifregs[config_channel_id].len_mask);
+	regw(value, vpifregs[channel_id].v_cfg_00);
+
+	value = (config->l5 & vpifregs[config_channel_id].len_mask);
+	value <<= VPIF_CH_LEN_SHIFT;
+	value |= (config->l7 & vpifregs[config_channel_id].len_mask);
+	regw(value, vpifregs[channel_id].v_cfg_01);
+
+	value = (config->l9 & vpifregs[config_channel_id].len_mask);
+	value <<= VPIF_CH_LEN_SHIFT;
+	value |= (config->l11 & vpifregs[config_channel_id].len_mask);
+	regw(value, vpifregs[channel_id].v_cfg_02);
+
+	value = (config->vsize & vpifregs[config_channel_id].len_mask);
+	regw(value, vpifregs[channel_id].v_cfg);
+}
+
+/* config_vpif_params
+ * Function to set the parameters of a channel
+ * Mainly modifies the channel ciontrol register
+ * It sets frame format, yc mux mode
+ */
+static void config_vpif_params(struct vpif_params *vpifparams,
+				u8 channel_id, u8 found)
+{
+	const struct vpif_channel_config_params *config = &vpifparams->std_info;
+	u32 value, ch_nip, reg;
+	u8 start, end;
+	int i;
+
+	start = channel_id;
+	end = channel_id + found;
+
+	for (i = start; i < end; i++) {
+		reg = vpifregs[i].ch_ctrl;
+		if (channel_id < 2)
+			ch_nip = VPIF_CAPTURE_CH_NIP;
+		else
+			ch_nip = VPIF_DISPLAY_CH_NIP;
+
+		vpif_wr_bit(reg, ch_nip, config->frm_fmt);
+		vpif_wr_bit(reg, VPIF_CH_YC_MUX_BIT, config->ycmux_mode);
+		vpif_wr_bit(reg, VPIF_CH_INPUT_FIELD_FRAME_BIT,
+					vpifparams->video_params.storage_mode);
+
+		/* Set raster scanning SDR Format */
+		vpif_clr_bit(reg, VPIF_CH_SDR_FMT_BIT);
+		vpif_wr_bit(reg, VPIF_CH_DATA_MODE_BIT, config->capture_format);
+
+		if (channel_id > 1)	/* Set the Pixel enable bit */
+			vpif_set_bit(reg, VPIF_DISPLAY_PIX_EN_BIT);
+		else if (config->capture_format) {
+			/* Set the polarity of various pins */
+			vpif_wr_bit(reg, VPIF_CH_FID_POLARITY_BIT,
+					vpifparams->iface.fid_pol);
+			vpif_wr_bit(reg, VPIF_CH_V_VALID_POLARITY_BIT,
+					vpifparams->iface.vd_pol);
+			vpif_wr_bit(reg, VPIF_CH_H_VALID_POLARITY_BIT,
+					vpifparams->iface.hd_pol);
+
+			value = regr(reg);
+			/* Set data width */
+			value &= ~(0x3u <<
+					VPIF_CH_DATA_WIDTH_BIT);
+			value |= ((vpifparams->params.data_sz) <<
+						     VPIF_CH_DATA_WIDTH_BIT);
+			regw(value, reg);
+		}
+
+		/* Write the pitch in the driver */
+		regw((vpifparams->video_params.hpitch),
+						vpifregs[i].line_offset);
+	}
+}
+
+/* vpif_set_video_params
+ * This function is used to set video parameters in VPIF register
+ */
+int vpif_set_video_params(struct vpif_params *vpifparams, u8 channel_id)
+{
+	const struct vpif_channel_config_params *config = &vpifparams->std_info;
+	int found = 1;
+
+	vpif_set_mode_info(config, channel_id, channel_id);
+	if (!config->ycmux_mode) {
+		/* YC are on separate channels (HDTV formats) */
+		vpif_set_mode_info(config, channel_id + 1, channel_id);
+		found = 2;
+	}
+
+	config_vpif_params(vpifparams, channel_id, found);
+
+	regw(0x80, VPIF_REQ_SIZE);
+	regw(0x01, VPIF_EMULATION_CTRL);
+
+	return found;
+}
+EXPORT_SYMBOL(vpif_set_video_params);
+
+void vpif_set_vbi_display_params(struct vpif_vbi_params *vbiparams,
+				u8 channel_id)
+{
+	u32 value;
+
+	value = 0x3F8 & (vbiparams->hstart0);
+	value |= 0x3FFFFFF & ((vbiparams->vstart0) << 16);
+	regw(value, vpifregs[channel_id].vanc0_strt);
+
+	value = 0x3F8 & (vbiparams->hstart1);
+	value |= 0x3FFFFFF & ((vbiparams->vstart1) << 16);
+	regw(value, vpifregs[channel_id].vanc1_strt);
+
+	value = 0x3F8 & (vbiparams->hsize0);
+	value |= 0x3FFFFFF & ((vbiparams->vsize0) << 16);
+	regw(value, vpifregs[channel_id].vanc0_size);
+
+	value = 0x3F8 & (vbiparams->hsize1);
+	value |= 0x3FFFFFF & ((vbiparams->vsize1) << 16);
+	regw(value, vpifregs[channel_id].vanc1_size);
+
+}
+EXPORT_SYMBOL(vpif_set_vbi_display_params);
+
+int vpif_channel_getfid(u8 channel_id)
+{
+	return (regr(vpifregs[channel_id].ch_ctrl) & VPIF_CH_FID_MASK)
+					>> VPIF_CH_FID_SHIFT;
+}
+EXPORT_SYMBOL(vpif_channel_getfid);
+
+static int vpif_probe(struct platform_device *pdev)
+{
+	static struct resource	*res;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	vpif_base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(vpif_base))
+		return PTR_ERR(vpif_base);
+
+	pm_runtime_enable(&pdev->dev);
+	pm_runtime_get(&pdev->dev);
+
+	spin_lock_init(&vpif_lock);
+	dev_info(&pdev->dev, "vpif probe success\n");
+	return 0;
+}
+
+static int vpif_remove(struct platform_device *pdev)
+{
+	pm_runtime_disable(&pdev->dev);
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int vpif_suspend(struct device *dev)
+{
+	pm_runtime_put(dev);
+	return 0;
+}
+
+static int vpif_resume(struct device *dev)
+{
+	pm_runtime_get(dev);
+	return 0;
+}
+
+static const struct dev_pm_ops vpif_pm = {
+	.suspend        = vpif_suspend,
+	.resume         = vpif_resume,
+};
+
+#define vpif_pm_ops (&vpif_pm)
+#else
+#define vpif_pm_ops NULL
+#endif
+
+static struct platform_driver vpif_driver = {
+	.driver = {
+		.name	= "vpif",
+		.pm	= vpif_pm_ops,
+	},
+	.remove = vpif_remove,
+	.probe = vpif_probe,
+};
+
+static void vpif_exit(void)
+{
+	platform_driver_unregister(&vpif_driver);
+}
+
+static int __init vpif_init(void)
+{
+	return platform_driver_register(&vpif_driver);
+}
+subsys_initcall(vpif_init);
+module_exit(vpif_exit);
+
diff --git a/drivers/media/platform/davinci/vpif.h b/drivers/media/platform/davinci/vpif.h
new file mode 100644
index 0000000..9956e67
--- /dev/null
+++ b/drivers/media/platform/davinci/vpif.h
@@ -0,0 +1,688 @@
+/*
+ * VPIF header file
+ *
+ * Copyright (C) 2009 Texas Instruments Incorporated - http://www.ti.com/
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed .as is. WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef VPIF_H
+#define VPIF_H
+
+#include <linux/io.h>
+#include <linux/videodev2.h>
+#include <media/davinci/vpif_types.h>
+
+/* Maximum channel allowed */
+#define VPIF_NUM_CHANNELS		(4)
+#define VPIF_CAPTURE_NUM_CHANNELS	(2)
+#define VPIF_DISPLAY_NUM_CHANNELS	(2)
+
+/* Macros to read/write registers */
+extern void __iomem *vpif_base;
+extern spinlock_t vpif_lock;
+
+#define regr(reg)               readl((reg) + vpif_base)
+#define regw(value, reg)        writel(value, (reg + vpif_base))
+
+/* Register Address Offsets */
+#define VPIF_PID			(0x0000)
+#define VPIF_CH0_CTRL			(0x0004)
+#define VPIF_CH1_CTRL			(0x0008)
+#define VPIF_CH2_CTRL			(0x000C)
+#define VPIF_CH3_CTRL			(0x0010)
+
+#define VPIF_INTEN			(0x0020)
+#define VPIF_INTEN_SET			(0x0024)
+#define VPIF_INTEN_CLR			(0x0028)
+#define VPIF_STATUS			(0x002C)
+#define VPIF_STATUS_CLR			(0x0030)
+#define VPIF_EMULATION_CTRL		(0x0034)
+#define VPIF_REQ_SIZE			(0x0038)
+
+#define VPIF_CH0_TOP_STRT_ADD_LUMA	(0x0040)
+#define VPIF_CH0_BTM_STRT_ADD_LUMA	(0x0044)
+#define VPIF_CH0_TOP_STRT_ADD_CHROMA	(0x0048)
+#define VPIF_CH0_BTM_STRT_ADD_CHROMA	(0x004c)
+#define VPIF_CH0_TOP_STRT_ADD_HANC	(0x0050)
+#define VPIF_CH0_BTM_STRT_ADD_HANC	(0x0054)
+#define VPIF_CH0_TOP_STRT_ADD_VANC	(0x0058)
+#define VPIF_CH0_BTM_STRT_ADD_VANC	(0x005c)
+#define VPIF_CH0_SP_CFG			(0x0060)
+#define VPIF_CH0_IMG_ADD_OFST		(0x0064)
+#define VPIF_CH0_HANC_ADD_OFST		(0x0068)
+#define VPIF_CH0_H_CFG			(0x006c)
+#define VPIF_CH0_V_CFG_00		(0x0070)
+#define VPIF_CH0_V_CFG_01		(0x0074)
+#define VPIF_CH0_V_CFG_02		(0x0078)
+#define VPIF_CH0_V_CFG_03		(0x007c)
+
+#define VPIF_CH1_TOP_STRT_ADD_LUMA	(0x0080)
+#define VPIF_CH1_BTM_STRT_ADD_LUMA	(0x0084)
+#define VPIF_CH1_TOP_STRT_ADD_CHROMA	(0x0088)
+#define VPIF_CH1_BTM_STRT_ADD_CHROMA	(0x008c)
+#define VPIF_CH1_TOP_STRT_ADD_HANC	(0x0090)
+#define VPIF_CH1_BTM_STRT_ADD_HANC	(0x0094)
+#define VPIF_CH1_TOP_STRT_ADD_VANC	(0x0098)
+#define VPIF_CH1_BTM_STRT_ADD_VANC	(0x009c)
+#define VPIF_CH1_SP_CFG			(0x00a0)
+#define VPIF_CH1_IMG_ADD_OFST		(0x00a4)
+#define VPIF_CH1_HANC_ADD_OFST		(0x00a8)
+#define VPIF_CH1_H_CFG			(0x00ac)
+#define VPIF_CH1_V_CFG_00		(0x00b0)
+#define VPIF_CH1_V_CFG_01		(0x00b4)
+#define VPIF_CH1_V_CFG_02		(0x00b8)
+#define VPIF_CH1_V_CFG_03		(0x00bc)
+
+#define VPIF_CH2_TOP_STRT_ADD_LUMA	(0x00c0)
+#define VPIF_CH2_BTM_STRT_ADD_LUMA	(0x00c4)
+#define VPIF_CH2_TOP_STRT_ADD_CHROMA	(0x00c8)
+#define VPIF_CH2_BTM_STRT_ADD_CHROMA	(0x00cc)
+#define VPIF_CH2_TOP_STRT_ADD_HANC	(0x00d0)
+#define VPIF_CH2_BTM_STRT_ADD_HANC	(0x00d4)
+#define VPIF_CH2_TOP_STRT_ADD_VANC	(0x00d8)
+#define VPIF_CH2_BTM_STRT_ADD_VANC	(0x00dc)
+#define VPIF_CH2_SP_CFG			(0x00e0)
+#define VPIF_CH2_IMG_ADD_OFST		(0x00e4)
+#define VPIF_CH2_HANC_ADD_OFST		(0x00e8)
+#define VPIF_CH2_H_CFG			(0x00ec)
+#define VPIF_CH2_V_CFG_00		(0x00f0)
+#define VPIF_CH2_V_CFG_01		(0x00f4)
+#define VPIF_CH2_V_CFG_02		(0x00f8)
+#define VPIF_CH2_V_CFG_03		(0x00fc)
+#define VPIF_CH2_HANC0_STRT		(0x0100)
+#define VPIF_CH2_HANC0_SIZE		(0x0104)
+#define VPIF_CH2_HANC1_STRT		(0x0108)
+#define VPIF_CH2_HANC1_SIZE		(0x010c)
+#define VPIF_CH2_VANC0_STRT		(0x0110)
+#define VPIF_CH2_VANC0_SIZE		(0x0114)
+#define VPIF_CH2_VANC1_STRT		(0x0118)
+#define VPIF_CH2_VANC1_SIZE		(0x011c)
+
+#define VPIF_CH3_TOP_STRT_ADD_LUMA	(0x0140)
+#define VPIF_CH3_BTM_STRT_ADD_LUMA	(0x0144)
+#define VPIF_CH3_TOP_STRT_ADD_CHROMA	(0x0148)
+#define VPIF_CH3_BTM_STRT_ADD_CHROMA	(0x014c)
+#define VPIF_CH3_TOP_STRT_ADD_HANC	(0x0150)
+#define VPIF_CH3_BTM_STRT_ADD_HANC	(0x0154)
+#define VPIF_CH3_TOP_STRT_ADD_VANC	(0x0158)
+#define VPIF_CH3_BTM_STRT_ADD_VANC	(0x015c)
+#define VPIF_CH3_SP_CFG			(0x0160)
+#define VPIF_CH3_IMG_ADD_OFST		(0x0164)
+#define VPIF_CH3_HANC_ADD_OFST		(0x0168)
+#define VPIF_CH3_H_CFG			(0x016c)
+#define VPIF_CH3_V_CFG_00		(0x0170)
+#define VPIF_CH3_V_CFG_01		(0x0174)
+#define VPIF_CH3_V_CFG_02		(0x0178)
+#define VPIF_CH3_V_CFG_03		(0x017c)
+#define VPIF_CH3_HANC0_STRT		(0x0180)
+#define VPIF_CH3_HANC0_SIZE		(0x0184)
+#define VPIF_CH3_HANC1_STRT		(0x0188)
+#define VPIF_CH3_HANC1_SIZE		(0x018c)
+#define VPIF_CH3_VANC0_STRT		(0x0190)
+#define VPIF_CH3_VANC0_SIZE		(0x0194)
+#define VPIF_CH3_VANC1_STRT		(0x0198)
+#define VPIF_CH3_VANC1_SIZE		(0x019c)
+
+#define VPIF_IODFT_CTRL			(0x01c0)
+
+/* Functions for bit Manipulation */
+static inline void vpif_set_bit(u32 reg, u32 bit)
+{
+	regw((regr(reg)) | (0x01 << bit), reg);
+}
+
+static inline void vpif_clr_bit(u32 reg, u32 bit)
+{
+	regw(((regr(reg)) & ~(0x01 << bit)), reg);
+}
+
+/* Macro for Generating mask */
+#ifdef GENERATE_MASK
+#undef GENERATE_MASK
+#endif
+
+#define GENERATE_MASK(bits, pos) \
+		((((0xFFFFFFFF) << (32 - bits)) >> (32 - bits)) << pos)
+
+/* Bit positions in the channel control registers */
+#define VPIF_CH_DATA_MODE_BIT	(2)
+#define VPIF_CH_YC_MUX_BIT	(3)
+#define VPIF_CH_SDR_FMT_BIT	(4)
+#define VPIF_CH_HANC_EN_BIT	(8)
+#define VPIF_CH_VANC_EN_BIT	(9)
+
+#define VPIF_CAPTURE_CH_NIP	(10)
+#define VPIF_DISPLAY_CH_NIP	(11)
+
+#define VPIF_DISPLAY_PIX_EN_BIT	(10)
+
+#define VPIF_CH_INPUT_FIELD_FRAME_BIT	(12)
+
+#define VPIF_CH_FID_POLARITY_BIT	(15)
+#define VPIF_CH_V_VALID_POLARITY_BIT	(14)
+#define VPIF_CH_H_VALID_POLARITY_BIT	(13)
+#define VPIF_CH_DATA_WIDTH_BIT		(28)
+
+#define VPIF_CH_CLK_EDGE_CTRL_BIT	(31)
+
+/* Mask various length */
+#define VPIF_CH_EAVSAV_MASK	GENERATE_MASK(13, 0)
+#define VPIF_CH_LEN_MASK	GENERATE_MASK(12, 0)
+#define VPIF_CH_WIDTH_MASK	GENERATE_MASK(13, 0)
+#define VPIF_CH_LEN_SHIFT	(16)
+
+/* VPIF masks for registers */
+#define VPIF_REQ_SIZE_MASK	(0x1ff)
+
+/* bit posotion of interrupt vpif_ch_intr register */
+#define VPIF_INTEN_FRAME_CH0	(0x00000001)
+#define VPIF_INTEN_FRAME_CH1	(0x00000002)
+#define VPIF_INTEN_FRAME_CH2	(0x00000004)
+#define VPIF_INTEN_FRAME_CH3	(0x00000008)
+
+/* bit position of clock and channel enable in vpif_chn_ctrl register */
+
+#define VPIF_CH0_CLK_EN		(0x00000002)
+#define VPIF_CH0_EN		(0x00000001)
+#define VPIF_CH1_CLK_EN		(0x00000002)
+#define VPIF_CH1_EN		(0x00000001)
+#define VPIF_CH2_CLK_EN		(0x00000002)
+#define VPIF_CH2_EN		(0x00000001)
+#define VPIF_CH3_CLK_EN		(0x00000002)
+#define VPIF_CH3_EN		(0x00000001)
+#define VPIF_CH_CLK_EN		(0x00000002)
+#define VPIF_CH_EN		(0x00000001)
+
+#define VPIF_INT_TOP	(0x00)
+#define VPIF_INT_BOTTOM	(0x01)
+#define VPIF_INT_BOTH	(0x02)
+
+#define VPIF_CH0_INT_CTRL_SHIFT	(6)
+#define VPIF_CH1_INT_CTRL_SHIFT	(6)
+#define VPIF_CH2_INT_CTRL_SHIFT	(6)
+#define VPIF_CH3_INT_CTRL_SHIFT	(6)
+#define VPIF_CH_INT_CTRL_SHIFT	(6)
+
+#define VPIF_CH2_CLIP_ANC_EN	14
+#define VPIF_CH2_CLIP_ACTIVE_EN	13
+
+#define VPIF_CH3_CLIP_ANC_EN	14
+#define VPIF_CH3_CLIP_ACTIVE_EN	13
+
+/* enabled interrupt on both the fields on vpid_ch0_ctrl register */
+#define channel0_intr_assert()	(regw((regr(VPIF_CH0_CTRL)|\
+	(VPIF_INT_BOTH << VPIF_CH0_INT_CTRL_SHIFT)), VPIF_CH0_CTRL))
+
+/* enabled interrupt on both the fields on vpid_ch1_ctrl register */
+#define channel1_intr_assert()	(regw((regr(VPIF_CH1_CTRL)|\
+	(VPIF_INT_BOTH << VPIF_CH1_INT_CTRL_SHIFT)), VPIF_CH1_CTRL))
+
+/* enabled interrupt on both the fields on vpid_ch0_ctrl register */
+#define channel2_intr_assert() 	(regw((regr(VPIF_CH2_CTRL)|\
+	(VPIF_INT_BOTH << VPIF_CH2_INT_CTRL_SHIFT)), VPIF_CH2_CTRL))
+
+/* enabled interrupt on both the fields on vpid_ch1_ctrl register */
+#define channel3_intr_assert() 	(regw((regr(VPIF_CH3_CTRL)|\
+	(VPIF_INT_BOTH << VPIF_CH3_INT_CTRL_SHIFT)), VPIF_CH3_CTRL))
+
+#define VPIF_CH_FID_MASK	(0x20)
+#define VPIF_CH_FID_SHIFT	(5)
+
+#define VPIF_NTSC_VBI_START_FIELD0	(1)
+#define VPIF_NTSC_VBI_START_FIELD1	(263)
+#define VPIF_PAL_VBI_START_FIELD0	(624)
+#define VPIF_PAL_VBI_START_FIELD1	(311)
+
+#define VPIF_NTSC_HBI_START_FIELD0	(1)
+#define VPIF_NTSC_HBI_START_FIELD1	(263)
+#define VPIF_PAL_HBI_START_FIELD0	(624)
+#define VPIF_PAL_HBI_START_FIELD1	(311)
+
+#define VPIF_NTSC_VBI_COUNT_FIELD0	(20)
+#define VPIF_NTSC_VBI_COUNT_FIELD1	(19)
+#define VPIF_PAL_VBI_COUNT_FIELD0	(24)
+#define VPIF_PAL_VBI_COUNT_FIELD1	(25)
+
+#define VPIF_NTSC_HBI_COUNT_FIELD0	(263)
+#define VPIF_NTSC_HBI_COUNT_FIELD1	(262)
+#define VPIF_PAL_HBI_COUNT_FIELD0	(312)
+#define VPIF_PAL_HBI_COUNT_FIELD1	(313)
+
+#define VPIF_NTSC_VBI_SAMPLES_PER_LINE	(720)
+#define VPIF_PAL_VBI_SAMPLES_PER_LINE	(720)
+#define VPIF_NTSC_HBI_SAMPLES_PER_LINE	(268)
+#define VPIF_PAL_HBI_SAMPLES_PER_LINE	(280)
+
+#define VPIF_CH_VANC_EN			(0x20)
+#define VPIF_DMA_REQ_SIZE		(0x080)
+#define VPIF_EMULATION_DISABLE		(0x01)
+
+extern u8 irq_vpif_capture_channel[VPIF_NUM_CHANNELS];
+
+/* inline function to enable/disable channel0 */
+static inline void enable_channel0(int enable)
+{
+	if (enable)
+		regw((regr(VPIF_CH0_CTRL) | (VPIF_CH0_EN)), VPIF_CH0_CTRL);
+	else
+		regw((regr(VPIF_CH0_CTRL) & (~VPIF_CH0_EN)), VPIF_CH0_CTRL);
+}
+
+/* inline function to enable/disable channel1 */
+static inline void enable_channel1(int enable)
+{
+	if (enable)
+		regw((regr(VPIF_CH1_CTRL) | (VPIF_CH1_EN)), VPIF_CH1_CTRL);
+	else
+		regw((regr(VPIF_CH1_CTRL) & (~VPIF_CH1_EN)), VPIF_CH1_CTRL);
+}
+
+/* inline function to enable interrupt for channel0 */
+static inline void channel0_intr_enable(int enable)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&vpif_lock, flags);
+
+	if (enable) {
+		regw((regr(VPIF_INTEN) | 0x10), VPIF_INTEN);
+		regw((regr(VPIF_INTEN_SET) | 0x10), VPIF_INTEN_SET);
+
+		regw((regr(VPIF_INTEN) | VPIF_INTEN_FRAME_CH0), VPIF_INTEN);
+		regw((regr(VPIF_INTEN_SET) | VPIF_INTEN_FRAME_CH0),
+							VPIF_INTEN_SET);
+	} else {
+		regw((regr(VPIF_INTEN) & (~VPIF_INTEN_FRAME_CH0)), VPIF_INTEN);
+		regw((regr(VPIF_INTEN_SET) | VPIF_INTEN_FRAME_CH0),
+							VPIF_INTEN_SET);
+	}
+	spin_unlock_irqrestore(&vpif_lock, flags);
+}
+
+/* inline function to enable interrupt for channel1 */
+static inline void channel1_intr_enable(int enable)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&vpif_lock, flags);
+
+	if (enable) {
+		regw((regr(VPIF_INTEN) | 0x10), VPIF_INTEN);
+		regw((regr(VPIF_INTEN_SET) | 0x10), VPIF_INTEN_SET);
+
+		regw((regr(VPIF_INTEN) | VPIF_INTEN_FRAME_CH1), VPIF_INTEN);
+		regw((regr(VPIF_INTEN_SET) | VPIF_INTEN_FRAME_CH1),
+							VPIF_INTEN_SET);
+	} else {
+		regw((regr(VPIF_INTEN) & (~VPIF_INTEN_FRAME_CH1)), VPIF_INTEN);
+		regw((regr(VPIF_INTEN_SET) | VPIF_INTEN_FRAME_CH1),
+							VPIF_INTEN_SET);
+	}
+	spin_unlock_irqrestore(&vpif_lock, flags);
+}
+
+/* inline function to set buffer addresses in case of Y/C non mux mode */
+static inline void ch0_set_videobuf_addr_yc_nmux(unsigned long top_strt_luma,
+						 unsigned long btm_strt_luma,
+						 unsigned long top_strt_chroma,
+						 unsigned long btm_strt_chroma)
+{
+	regw(top_strt_luma, VPIF_CH0_TOP_STRT_ADD_LUMA);
+	regw(btm_strt_luma, VPIF_CH0_BTM_STRT_ADD_LUMA);
+	regw(top_strt_chroma, VPIF_CH1_TOP_STRT_ADD_CHROMA);
+	regw(btm_strt_chroma, VPIF_CH1_BTM_STRT_ADD_CHROMA);
+}
+
+/* inline function to set buffer addresses in VPIF registers for video data */
+static inline void ch0_set_videobuf_addr(unsigned long top_strt_luma,
+					 unsigned long btm_strt_luma,
+					 unsigned long top_strt_chroma,
+					 unsigned long btm_strt_chroma)
+{
+	regw(top_strt_luma, VPIF_CH0_TOP_STRT_ADD_LUMA);
+	regw(btm_strt_luma, VPIF_CH0_BTM_STRT_ADD_LUMA);
+	regw(top_strt_chroma, VPIF_CH0_TOP_STRT_ADD_CHROMA);
+	regw(btm_strt_chroma, VPIF_CH0_BTM_STRT_ADD_CHROMA);
+}
+
+static inline void ch1_set_videobuf_addr(unsigned long top_strt_luma,
+					 unsigned long btm_strt_luma,
+					 unsigned long top_strt_chroma,
+					 unsigned long btm_strt_chroma)
+{
+
+	regw(top_strt_luma, VPIF_CH1_TOP_STRT_ADD_LUMA);
+	regw(btm_strt_luma, VPIF_CH1_BTM_STRT_ADD_LUMA);
+	regw(top_strt_chroma, VPIF_CH1_TOP_STRT_ADD_CHROMA);
+	regw(btm_strt_chroma, VPIF_CH1_BTM_STRT_ADD_CHROMA);
+}
+
+static inline void ch0_set_vbi_addr(unsigned long top_vbi,
+	unsigned long btm_vbi, unsigned long a, unsigned long b)
+{
+	regw(top_vbi, VPIF_CH0_TOP_STRT_ADD_VANC);
+	regw(btm_vbi, VPIF_CH0_BTM_STRT_ADD_VANC);
+}
+
+static inline void ch0_set_hbi_addr(unsigned long top_vbi,
+	unsigned long btm_vbi, unsigned long a, unsigned long b)
+{
+	regw(top_vbi, VPIF_CH0_TOP_STRT_ADD_HANC);
+	regw(btm_vbi, VPIF_CH0_BTM_STRT_ADD_HANC);
+}
+
+static inline void ch1_set_vbi_addr(unsigned long top_vbi,
+	unsigned long btm_vbi, unsigned long a, unsigned long b)
+{
+	regw(top_vbi, VPIF_CH1_TOP_STRT_ADD_VANC);
+	regw(btm_vbi, VPIF_CH1_BTM_STRT_ADD_VANC);
+}
+
+static inline void ch1_set_hbi_addr(unsigned long top_vbi,
+	unsigned long btm_vbi, unsigned long a, unsigned long b)
+{
+	regw(top_vbi, VPIF_CH1_TOP_STRT_ADD_HANC);
+	regw(btm_vbi, VPIF_CH1_BTM_STRT_ADD_HANC);
+}
+
+/* Inline function to enable raw vbi in the given channel */
+static inline void disable_raw_feature(u8 channel_id, u8 index)
+{
+	u32 ctrl_reg;
+	if (0 == channel_id)
+		ctrl_reg = VPIF_CH0_CTRL;
+	else
+		ctrl_reg = VPIF_CH1_CTRL;
+
+	if (1 == index)
+		vpif_clr_bit(ctrl_reg, VPIF_CH_VANC_EN_BIT);
+	else
+		vpif_clr_bit(ctrl_reg, VPIF_CH_HANC_EN_BIT);
+}
+
+static inline void enable_raw_feature(u8 channel_id, u8 index)
+{
+	u32 ctrl_reg;
+	if (0 == channel_id)
+		ctrl_reg = VPIF_CH0_CTRL;
+	else
+		ctrl_reg = VPIF_CH1_CTRL;
+
+	if (1 == index)
+		vpif_set_bit(ctrl_reg, VPIF_CH_VANC_EN_BIT);
+	else
+		vpif_set_bit(ctrl_reg, VPIF_CH_HANC_EN_BIT);
+}
+
+/* inline function to enable/disable channel2 */
+static inline void enable_channel2(int enable)
+{
+	if (enable) {
+		regw((regr(VPIF_CH2_CTRL) | (VPIF_CH2_CLK_EN)), VPIF_CH2_CTRL);
+		regw((regr(VPIF_CH2_CTRL) | (VPIF_CH2_EN)), VPIF_CH2_CTRL);
+	} else {
+		regw((regr(VPIF_CH2_CTRL) & (~VPIF_CH2_CLK_EN)), VPIF_CH2_CTRL);
+		regw((regr(VPIF_CH2_CTRL) & (~VPIF_CH2_EN)), VPIF_CH2_CTRL);
+	}
+}
+
+/* inline function to enable/disable channel3 */
+static inline void enable_channel3(int enable)
+{
+	if (enable) {
+		regw((regr(VPIF_CH3_CTRL) | (VPIF_CH3_CLK_EN)), VPIF_CH3_CTRL);
+		regw((regr(VPIF_CH3_CTRL) | (VPIF_CH3_EN)), VPIF_CH3_CTRL);
+	} else {
+		regw((regr(VPIF_CH3_CTRL) & (~VPIF_CH3_CLK_EN)), VPIF_CH3_CTRL);
+		regw((regr(VPIF_CH3_CTRL) & (~VPIF_CH3_EN)), VPIF_CH3_CTRL);
+	}
+}
+
+/* inline function to enable interrupt for channel2 */
+static inline void channel2_intr_enable(int enable)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&vpif_lock, flags);
+
+	if (enable) {
+		regw((regr(VPIF_INTEN) | 0x10), VPIF_INTEN);
+		regw((regr(VPIF_INTEN_SET) | 0x10), VPIF_INTEN_SET);
+		regw((regr(VPIF_INTEN) | VPIF_INTEN_FRAME_CH2), VPIF_INTEN);
+		regw((regr(VPIF_INTEN_SET) | VPIF_INTEN_FRAME_CH2),
+							VPIF_INTEN_SET);
+	} else {
+		regw((regr(VPIF_INTEN) & (~VPIF_INTEN_FRAME_CH2)), VPIF_INTEN);
+		regw((regr(VPIF_INTEN_SET) | VPIF_INTEN_FRAME_CH2),
+							VPIF_INTEN_SET);
+	}
+	spin_unlock_irqrestore(&vpif_lock, flags);
+}
+
+/* inline function to enable interrupt for channel3 */
+static inline void channel3_intr_enable(int enable)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&vpif_lock, flags);
+
+	if (enable) {
+		regw((regr(VPIF_INTEN) | 0x10), VPIF_INTEN);
+		regw((regr(VPIF_INTEN_SET) | 0x10), VPIF_INTEN_SET);
+
+		regw((regr(VPIF_INTEN) | VPIF_INTEN_FRAME_CH3), VPIF_INTEN);
+		regw((regr(VPIF_INTEN_SET) | VPIF_INTEN_FRAME_CH3),
+							VPIF_INTEN_SET);
+	} else {
+		regw((regr(VPIF_INTEN) & (~VPIF_INTEN_FRAME_CH3)), VPIF_INTEN);
+		regw((regr(VPIF_INTEN_SET) | VPIF_INTEN_FRAME_CH3),
+							VPIF_INTEN_SET);
+	}
+	spin_unlock_irqrestore(&vpif_lock, flags);
+}
+
+/* inline function to enable raw vbi data for channel2 */
+static inline void channel2_raw_enable(int enable, u8 index)
+{
+	u32 mask;
+
+	if (1 == index)
+		mask = VPIF_CH_VANC_EN_BIT;
+	else
+		mask = VPIF_CH_HANC_EN_BIT;
+
+	if (enable)
+		vpif_set_bit(VPIF_CH2_CTRL, mask);
+	else
+		vpif_clr_bit(VPIF_CH2_CTRL, mask);
+}
+
+/* inline function to enable raw vbi data for channel3*/
+static inline void channel3_raw_enable(int enable, u8 index)
+{
+	u32 mask;
+
+	if (1 == index)
+		mask = VPIF_CH_VANC_EN_BIT;
+	else
+		mask = VPIF_CH_HANC_EN_BIT;
+
+	if (enable)
+		vpif_set_bit(VPIF_CH3_CTRL, mask);
+	else
+		vpif_clr_bit(VPIF_CH3_CTRL, mask);
+}
+
+/* function to enable clipping (for both active and blanking regions) on ch 2 */
+static inline void channel2_clipping_enable(int enable)
+{
+	if (enable) {
+		vpif_set_bit(VPIF_CH2_CTRL, VPIF_CH2_CLIP_ANC_EN);
+		vpif_set_bit(VPIF_CH2_CTRL, VPIF_CH2_CLIP_ACTIVE_EN);
+	} else {
+		vpif_clr_bit(VPIF_CH2_CTRL, VPIF_CH2_CLIP_ANC_EN);
+		vpif_clr_bit(VPIF_CH2_CTRL, VPIF_CH2_CLIP_ACTIVE_EN);
+	}
+}
+
+/* function to enable clipping (for both active and blanking regions) on ch 3 */
+static inline void channel3_clipping_enable(int enable)
+{
+	if (enable) {
+		vpif_set_bit(VPIF_CH3_CTRL, VPIF_CH3_CLIP_ANC_EN);
+		vpif_set_bit(VPIF_CH3_CTRL, VPIF_CH3_CLIP_ACTIVE_EN);
+	} else {
+		vpif_clr_bit(VPIF_CH3_CTRL, VPIF_CH3_CLIP_ANC_EN);
+		vpif_clr_bit(VPIF_CH3_CTRL, VPIF_CH3_CLIP_ACTIVE_EN);
+	}
+}
+
+/* inline function to set buffer addresses in case of Y/C non mux mode */
+static inline void ch2_set_videobuf_addr_yc_nmux(unsigned long top_strt_luma,
+						 unsigned long btm_strt_luma,
+						 unsigned long top_strt_chroma,
+						 unsigned long btm_strt_chroma)
+{
+	regw(top_strt_luma, VPIF_CH2_TOP_STRT_ADD_LUMA);
+	regw(btm_strt_luma, VPIF_CH2_BTM_STRT_ADD_LUMA);
+	regw(top_strt_chroma, VPIF_CH3_TOP_STRT_ADD_CHROMA);
+	regw(btm_strt_chroma, VPIF_CH3_BTM_STRT_ADD_CHROMA);
+}
+
+/* inline function to set buffer addresses in VPIF registers for video data */
+static inline void ch2_set_videobuf_addr(unsigned long top_strt_luma,
+					 unsigned long btm_strt_luma,
+					 unsigned long top_strt_chroma,
+					 unsigned long btm_strt_chroma)
+{
+	regw(top_strt_luma, VPIF_CH2_TOP_STRT_ADD_LUMA);
+	regw(btm_strt_luma, VPIF_CH2_BTM_STRT_ADD_LUMA);
+	regw(top_strt_chroma, VPIF_CH2_TOP_STRT_ADD_CHROMA);
+	regw(btm_strt_chroma, VPIF_CH2_BTM_STRT_ADD_CHROMA);
+}
+
+static inline void ch3_set_videobuf_addr(unsigned long top_strt_luma,
+					 unsigned long btm_strt_luma,
+					 unsigned long top_strt_chroma,
+					 unsigned long btm_strt_chroma)
+{
+	regw(top_strt_luma, VPIF_CH3_TOP_STRT_ADD_LUMA);
+	regw(btm_strt_luma, VPIF_CH3_BTM_STRT_ADD_LUMA);
+	regw(top_strt_chroma, VPIF_CH3_TOP_STRT_ADD_CHROMA);
+	regw(btm_strt_chroma, VPIF_CH3_BTM_STRT_ADD_CHROMA);
+}
+
+/* inline function to set buffer addresses in VPIF registers for vbi data */
+static inline void ch2_set_vbi_addr(unsigned long top_strt_luma,
+					 unsigned long btm_strt_luma,
+					 unsigned long top_strt_chroma,
+					 unsigned long btm_strt_chroma)
+{
+	regw(top_strt_luma, VPIF_CH2_TOP_STRT_ADD_VANC);
+	regw(btm_strt_luma, VPIF_CH2_BTM_STRT_ADD_VANC);
+}
+
+static inline void ch3_set_vbi_addr(unsigned long top_strt_luma,
+					 unsigned long btm_strt_luma,
+					 unsigned long top_strt_chroma,
+					 unsigned long btm_strt_chroma)
+{
+	regw(top_strt_luma, VPIF_CH3_TOP_STRT_ADD_VANC);
+	regw(btm_strt_luma, VPIF_CH3_BTM_STRT_ADD_VANC);
+}
+
+static inline int vpif_intr_status(int channel)
+{
+	int status = 0;
+	int mask;
+
+	if (channel < 0 || channel > 3)
+		return 0;
+
+	mask = 1 << channel;
+	status = regr(VPIF_STATUS) & mask;
+	regw(status, VPIF_STATUS_CLR);
+
+	return status;
+}
+
+#define VPIF_MAX_NAME	(30)
+
+/* This structure will store size parameters as per the mode selected by user */
+struct vpif_channel_config_params {
+	char name[VPIF_MAX_NAME];	/* Name of the mode */
+	u16 width;			/* Indicates width of the image */
+	u16 height;			/* Indicates height of the image */
+	u8 frm_fmt;			/* Interlaced (0) or progressive (1) */
+	u8 ycmux_mode;			/* This mode requires one (0) or two (1)
+					   channels */
+	u16 eav2sav;			/* length of eav 2 sav */
+	u16 sav2eav;			/* length of sav 2 eav */
+	u16 l1, l3, l5, l7, l9, l11;	/* Other parameter configurations */
+	u16 vsize;			/* Vertical size of the image */
+	u8 capture_format;		/* Indicates whether capture format
+					 * is in BT or in CCD/CMOS */
+	u8  vbi_supported;		/* Indicates whether this mode
+					 * supports capturing vbi or not */
+	u8 hd_sd;			/* HDTV (1) or SDTV (0) format */
+	v4l2_std_id stdid;		/* SDTV format */
+	struct v4l2_dv_timings dv_timings;	/* HDTV format */
+};
+
+extern const unsigned int vpif_ch_params_count;
+extern const struct vpif_channel_config_params vpif_ch_params[];
+
+struct vpif_video_params;
+struct vpif_params;
+struct vpif_vbi_params;
+
+int vpif_set_video_params(struct vpif_params *vpifparams, u8 channel_id);
+void vpif_set_vbi_display_params(struct vpif_vbi_params *vbiparams,
+							u8 channel_id);
+int vpif_channel_getfid(u8 channel_id);
+
+enum data_size {
+	_8BITS = 0,
+	_10BITS,
+	_12BITS,
+};
+
+/* Structure for vpif parameters for raw vbi data */
+struct vpif_vbi_params {
+	__u32 hstart0;  /* Horizontal start of raw vbi data for first field */
+	__u32 vstart0;  /* Vertical start of raw vbi data for first field */
+	__u32 hsize0;   /* Horizontal size of raw vbi data for first field */
+	__u32 vsize0;   /* Vertical size of raw vbi data for first field */
+	__u32 hstart1;  /* Horizontal start of raw vbi data for second field */
+	__u32 vstart1;  /* Vertical start of raw vbi data for second field */
+	__u32 hsize1;   /* Horizontal size of raw vbi data for second field */
+	__u32 vsize1;   /* Vertical size of raw vbi data for second field */
+};
+
+/* structure for vpif parameters */
+struct vpif_video_params {
+	__u8 storage_mode;	/* Indicates field or frame mode */
+	unsigned long hpitch;
+	v4l2_std_id stdid;
+};
+
+struct vpif_params {
+	struct vpif_interface iface;
+	struct vpif_video_params video_params;
+	struct vpif_channel_config_params std_info;
+	union param {
+		struct vpif_vbi_params	vbi_params;
+		enum data_size data_sz;
+	} params;
+};
+
+#endif				/* End of #ifndef VPIF_H */
+
diff --git a/drivers/media/platform/davinci/vpif_capture.c b/drivers/media/platform/davinci/vpif_capture.c
new file mode 100644
index 0000000..c1e573b
--- /dev/null
+++ b/drivers/media/platform/davinci/vpif_capture.c
@@ -0,0 +1,1640 @@
+/*
+ * Copyright (C) 2009 Texas Instruments Inc
+ * Copyright (C) 2014 Lad, Prabhakar <prabhakar.csengg@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
+ *
+ * TODO : add support for VBI & HBI data service
+ *	  add static buffer allocation
+ */
+
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#include <media/v4l2-ioctl.h>
+
+#include "vpif.h"
+#include "vpif_capture.h"
+
+MODULE_DESCRIPTION("TI DaVinci VPIF Capture driver");
+MODULE_LICENSE("GPL");
+MODULE_VERSION(VPIF_CAPTURE_VERSION);
+
+#define vpif_err(fmt, arg...)	v4l2_err(&vpif_obj.v4l2_dev, fmt, ## arg)
+#define vpif_dbg(level, debug, fmt, arg...)	\
+		v4l2_dbg(level, debug, &vpif_obj.v4l2_dev, fmt, ## arg)
+
+static int debug = 1;
+
+module_param(debug, int, 0644);
+
+MODULE_PARM_DESC(debug, "Debug level 0-1");
+
+#define VPIF_DRIVER_NAME	"vpif_capture"
+
+/* global variables */
+static struct vpif_device vpif_obj = { {NULL} };
+static struct device *vpif_dev;
+static void vpif_calculate_offsets(struct channel_obj *ch);
+static void vpif_config_addr(struct channel_obj *ch, int muxmode);
+
+static u8 channel_first_int[VPIF_NUMBER_OF_OBJECTS][2] = { {1, 1} };
+
+/* Is set to 1 in case of SDTV formats, 2 in case of HDTV formats. */
+static int ycmux_mode;
+
+static inline
+struct vpif_cap_buffer *to_vpif_buffer(struct vb2_v4l2_buffer *vb)
+{
+	return container_of(vb, struct vpif_cap_buffer, vb);
+}
+
+/**
+ * vpif_buffer_prepare :  callback function for buffer prepare
+ * @vb: ptr to vb2_buffer
+ *
+ * This is the callback function for buffer prepare when vb2_qbuf()
+ * function is called. The buffer is prepared and user space virtual address
+ * or user address is converted into  physical address
+ */
+static int vpif_buffer_prepare(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct vb2_queue *q = vb->vb2_queue;
+	struct channel_obj *ch = vb2_get_drv_priv(q);
+	struct common_obj *common;
+	unsigned long addr;
+
+	vpif_dbg(2, debug, "vpif_buffer_prepare\n");
+
+	common = &ch->common[VPIF_VIDEO_INDEX];
+
+	vb2_set_plane_payload(vb, 0, common->fmt.fmt.pix.sizeimage);
+	if (vb2_get_plane_payload(vb, 0) > vb2_plane_size(vb, 0))
+		return -EINVAL;
+
+	vbuf->field = common->fmt.fmt.pix.field;
+
+	addr = vb2_dma_contig_plane_dma_addr(vb, 0);
+	if (!IS_ALIGNED((addr + common->ytop_off), 8) ||
+		!IS_ALIGNED((addr + common->ybtm_off), 8) ||
+		!IS_ALIGNED((addr + common->ctop_off), 8) ||
+		!IS_ALIGNED((addr + common->cbtm_off), 8)) {
+		vpif_dbg(1, debug, "offset is not aligned\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/**
+ * vpif_buffer_queue_setup : Callback function for buffer setup.
+ * @vq: vb2_queue ptr
+ * @fmt: v4l2 format
+ * @nbuffers: ptr to number of buffers requested by application
+ * @nplanes:: contains number of distinct video planes needed to hold a frame
+ * @sizes[]: contains the size (in bytes) of each plane.
+ * @alloc_ctxs: ptr to allocation context
+ *
+ * This callback function is called when reqbuf() is called to adjust
+ * the buffer count and buffer size
+ */
+static int vpif_buffer_queue_setup(struct vb2_queue *vq,
+				const void *parg,
+				unsigned int *nbuffers, unsigned int *nplanes,
+				unsigned int sizes[], void *alloc_ctxs[])
+{
+	const struct v4l2_format *fmt = parg;
+	struct channel_obj *ch = vb2_get_drv_priv(vq);
+	struct common_obj *common;
+
+	common = &ch->common[VPIF_VIDEO_INDEX];
+
+	vpif_dbg(2, debug, "vpif_buffer_setup\n");
+
+	if (fmt && fmt->fmt.pix.sizeimage < common->fmt.fmt.pix.sizeimage)
+		return -EINVAL;
+
+	if (vq->num_buffers + *nbuffers < 3)
+		*nbuffers = 3 - vq->num_buffers;
+
+	*nplanes = 1;
+	sizes[0] = fmt ? fmt->fmt.pix.sizeimage : common->fmt.fmt.pix.sizeimage;
+	alloc_ctxs[0] = common->alloc_ctx;
+
+	/* Calculate the offset for Y and C data in the buffer */
+	vpif_calculate_offsets(ch);
+
+	return 0;
+}
+
+/**
+ * vpif_buffer_queue : Callback function to add buffer to DMA queue
+ * @vb: ptr to vb2_buffer
+ */
+static void vpif_buffer_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct channel_obj *ch = vb2_get_drv_priv(vb->vb2_queue);
+	struct vpif_cap_buffer *buf = to_vpif_buffer(vbuf);
+	struct common_obj *common;
+	unsigned long flags;
+
+	common = &ch->common[VPIF_VIDEO_INDEX];
+
+	vpif_dbg(2, debug, "vpif_buffer_queue\n");
+
+	spin_lock_irqsave(&common->irqlock, flags);
+	/* add the buffer to the DMA queue */
+	list_add_tail(&buf->list, &common->dma_queue);
+	spin_unlock_irqrestore(&common->irqlock, flags);
+}
+
+/**
+ * vpif_start_streaming : Starts the DMA engine for streaming
+ * @vb: ptr to vb2_buffer
+ * @count: number of buffers
+ */
+static int vpif_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct vpif_capture_config *vpif_config_data =
+					vpif_dev->platform_data;
+	struct channel_obj *ch = vb2_get_drv_priv(vq);
+	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
+	struct vpif_params *vpif = &ch->vpifparams;
+	struct vpif_cap_buffer *buf, *tmp;
+	unsigned long addr, flags;
+	int ret;
+
+	spin_lock_irqsave(&common->irqlock, flags);
+
+	/* Initialize field_id */
+	ch->field_id = 0;
+
+	/* configure 1 or 2 channel mode */
+	if (vpif_config_data->setup_input_channel_mode) {
+		ret = vpif_config_data->
+			setup_input_channel_mode(vpif->std_info.ycmux_mode);
+		if (ret < 0) {
+			vpif_dbg(1, debug, "can't set vpif channel mode\n");
+			goto err;
+		}
+	}
+
+	ret = v4l2_subdev_call(ch->sd, video, s_stream, 1);
+	if (ret && ret != -ENOIOCTLCMD && ret != -ENODEV) {
+		vpif_dbg(1, debug, "stream on failed in subdev\n");
+		goto err;
+	}
+
+	/* Call vpif_set_params function to set the parameters and addresses */
+	ret = vpif_set_video_params(vpif, ch->channel_id);
+	if (ret < 0) {
+		vpif_dbg(1, debug, "can't set video params\n");
+		goto err;
+	}
+
+	ycmux_mode = ret;
+	vpif_config_addr(ch, ret);
+
+	/* Get the next frame from the buffer queue */
+	common->cur_frm = common->next_frm = list_entry(common->dma_queue.next,
+				    struct vpif_cap_buffer, list);
+	/* Remove buffer from the buffer queue */
+	list_del(&common->cur_frm->list);
+	spin_unlock_irqrestore(&common->irqlock, flags);
+
+	addr = vb2_dma_contig_plane_dma_addr(&common->cur_frm->vb.vb2_buf, 0);
+
+	common->set_addr(addr + common->ytop_off,
+			 addr + common->ybtm_off,
+			 addr + common->ctop_off,
+			 addr + common->cbtm_off);
+
+	/**
+	 * Set interrupt for both the fields in VPIF Register enable channel in
+	 * VPIF register
+	 */
+	channel_first_int[VPIF_VIDEO_INDEX][ch->channel_id] = 1;
+	if (VPIF_CHANNEL0_VIDEO == ch->channel_id) {
+		channel0_intr_assert();
+		channel0_intr_enable(1);
+		enable_channel0(1);
+	}
+	if (VPIF_CHANNEL1_VIDEO == ch->channel_id ||
+		ycmux_mode == 2) {
+		channel1_intr_assert();
+		channel1_intr_enable(1);
+		enable_channel1(1);
+	}
+
+	return 0;
+
+err:
+	list_for_each_entry_safe(buf, tmp, &common->dma_queue, list) {
+		list_del(&buf->list);
+		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_QUEUED);
+	}
+	spin_unlock_irqrestore(&common->irqlock, flags);
+
+	return ret;
+}
+
+/**
+ * vpif_stop_streaming : Stop the DMA engine
+ * @vq: ptr to vb2_queue
+ *
+ * This callback stops the DMA engine and any remaining buffers
+ * in the DMA queue are released.
+ */
+static void vpif_stop_streaming(struct vb2_queue *vq)
+{
+	struct channel_obj *ch = vb2_get_drv_priv(vq);
+	struct common_obj *common;
+	unsigned long flags;
+	int ret;
+
+	common = &ch->common[VPIF_VIDEO_INDEX];
+
+	/* Disable channel as per its device type and channel id */
+	if (VPIF_CHANNEL0_VIDEO == ch->channel_id) {
+		enable_channel0(0);
+		channel0_intr_enable(0);
+	}
+	if (VPIF_CHANNEL1_VIDEO == ch->channel_id ||
+		ycmux_mode == 2) {
+		enable_channel1(0);
+		channel1_intr_enable(0);
+	}
+
+	ycmux_mode = 0;
+
+	ret = v4l2_subdev_call(ch->sd, video, s_stream, 0);
+	if (ret && ret != -ENOIOCTLCMD && ret != -ENODEV)
+		vpif_dbg(1, debug, "stream off failed in subdev\n");
+
+	/* release all active buffers */
+	spin_lock_irqsave(&common->irqlock, flags);
+	if (common->cur_frm == common->next_frm) {
+		vb2_buffer_done(&common->cur_frm->vb.vb2_buf,
+				VB2_BUF_STATE_ERROR);
+	} else {
+		if (common->cur_frm != NULL)
+			vb2_buffer_done(&common->cur_frm->vb.vb2_buf,
+					VB2_BUF_STATE_ERROR);
+		if (common->next_frm != NULL)
+			vb2_buffer_done(&common->next_frm->vb.vb2_buf,
+					VB2_BUF_STATE_ERROR);
+	}
+
+	while (!list_empty(&common->dma_queue)) {
+		common->next_frm = list_entry(common->dma_queue.next,
+						struct vpif_cap_buffer, list);
+		list_del(&common->next_frm->list);
+		vb2_buffer_done(&common->next_frm->vb.vb2_buf,
+				VB2_BUF_STATE_ERROR);
+	}
+	spin_unlock_irqrestore(&common->irqlock, flags);
+}
+
+static struct vb2_ops video_qops = {
+	.queue_setup		= vpif_buffer_queue_setup,
+	.buf_prepare		= vpif_buffer_prepare,
+	.start_streaming	= vpif_start_streaming,
+	.stop_streaming		= vpif_stop_streaming,
+	.buf_queue		= vpif_buffer_queue,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
+};
+
+/**
+ * vpif_process_buffer_complete: process a completed buffer
+ * @common: ptr to common channel object
+ *
+ * This function time stamp the buffer and mark it as DONE. It also
+ * wake up any process waiting on the QUEUE and set the next buffer
+ * as current
+ */
+static void vpif_process_buffer_complete(struct common_obj *common)
+{
+	v4l2_get_timestamp(&common->cur_frm->vb.timestamp);
+	vb2_buffer_done(&common->cur_frm->vb.vb2_buf, VB2_BUF_STATE_DONE);
+	/* Make curFrm pointing to nextFrm */
+	common->cur_frm = common->next_frm;
+}
+
+/**
+ * vpif_schedule_next_buffer: set next buffer address for capture
+ * @common : ptr to common channel object
+ *
+ * This function will get next buffer from the dma queue and
+ * set the buffer address in the vpif register for capture.
+ * the buffer is marked active
+ */
+static void vpif_schedule_next_buffer(struct common_obj *common)
+{
+	unsigned long addr = 0;
+
+	spin_lock(&common->irqlock);
+	common->next_frm = list_entry(common->dma_queue.next,
+				     struct vpif_cap_buffer, list);
+	/* Remove that buffer from the buffer queue */
+	list_del(&common->next_frm->list);
+	spin_unlock(&common->irqlock);
+	addr = vb2_dma_contig_plane_dma_addr(&common->next_frm->vb.vb2_buf, 0);
+
+	/* Set top and bottom field addresses in VPIF registers */
+	common->set_addr(addr + common->ytop_off,
+			 addr + common->ybtm_off,
+			 addr + common->ctop_off,
+			 addr + common->cbtm_off);
+}
+
+/**
+ * vpif_channel_isr : ISR handler for vpif capture
+ * @irq: irq number
+ * @dev_id: dev_id ptr
+ *
+ * It changes status of the captured buffer, takes next buffer from the queue
+ * and sets its address in VPIF registers
+ */
+static irqreturn_t vpif_channel_isr(int irq, void *dev_id)
+{
+	struct vpif_device *dev = &vpif_obj;
+	struct common_obj *common;
+	struct channel_obj *ch;
+	int channel_id = 0;
+	int fid = -1, i;
+
+	channel_id = *(int *)(dev_id);
+	if (!vpif_intr_status(channel_id))
+		return IRQ_NONE;
+
+	ch = dev->dev[channel_id];
+
+	for (i = 0; i < VPIF_NUMBER_OF_OBJECTS; i++) {
+		common = &ch->common[i];
+		/* skip If streaming is not started in this channel */
+		/* Check the field format */
+		if (1 == ch->vpifparams.std_info.frm_fmt) {
+			/* Progressive mode */
+			spin_lock(&common->irqlock);
+			if (list_empty(&common->dma_queue)) {
+				spin_unlock(&common->irqlock);
+				continue;
+			}
+			spin_unlock(&common->irqlock);
+
+			if (!channel_first_int[i][channel_id])
+				vpif_process_buffer_complete(common);
+
+			channel_first_int[i][channel_id] = 0;
+
+			vpif_schedule_next_buffer(common);
+
+
+			channel_first_int[i][channel_id] = 0;
+		} else {
+			/**
+			 * Interlaced mode. If it is first interrupt, ignore
+			 * it
+			 */
+			if (channel_first_int[i][channel_id]) {
+				channel_first_int[i][channel_id] = 0;
+				continue;
+			}
+			if (0 == i) {
+				ch->field_id ^= 1;
+				/* Get field id from VPIF registers */
+				fid = vpif_channel_getfid(ch->channel_id);
+				if (fid != ch->field_id) {
+					/**
+					 * If field id does not match stored
+					 * field id, make them in sync
+					 */
+					if (0 == fid)
+						ch->field_id = fid;
+					return IRQ_HANDLED;
+				}
+			}
+			/* device field id and local field id are in sync */
+			if (0 == fid) {
+				/* this is even field */
+				if (common->cur_frm == common->next_frm)
+					continue;
+
+				/* mark the current buffer as done */
+				vpif_process_buffer_complete(common);
+			} else if (1 == fid) {
+				/* odd field */
+				spin_lock(&common->irqlock);
+				if (list_empty(&common->dma_queue) ||
+				    (common->cur_frm != common->next_frm)) {
+					spin_unlock(&common->irqlock);
+					continue;
+				}
+				spin_unlock(&common->irqlock);
+
+				vpif_schedule_next_buffer(common);
+			}
+		}
+	}
+	return IRQ_HANDLED;
+}
+
+/**
+ * vpif_update_std_info() - update standard related info
+ * @ch: ptr to channel object
+ *
+ * For a given standard selected by application, update values
+ * in the device data structures
+ */
+static int vpif_update_std_info(struct channel_obj *ch)
+{
+	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
+	struct vpif_params *vpifparams = &ch->vpifparams;
+	const struct vpif_channel_config_params *config;
+	struct vpif_channel_config_params *std_info = &vpifparams->std_info;
+	struct video_obj *vid_ch = &ch->video;
+	int index;
+
+	vpif_dbg(2, debug, "vpif_update_std_info\n");
+
+	for (index = 0; index < vpif_ch_params_count; index++) {
+		config = &vpif_ch_params[index];
+		if (config->hd_sd == 0) {
+			vpif_dbg(2, debug, "SD format\n");
+			if (config->stdid & vid_ch->stdid) {
+				memcpy(std_info, config, sizeof(*config));
+				break;
+			}
+		} else {
+			vpif_dbg(2, debug, "HD format\n");
+			if (!memcmp(&config->dv_timings, &vid_ch->dv_timings,
+				sizeof(vid_ch->dv_timings))) {
+				memcpy(std_info, config, sizeof(*config));
+				break;
+			}
+		}
+	}
+
+	/* standard not found */
+	if (index == vpif_ch_params_count)
+		return -EINVAL;
+
+	common->fmt.fmt.pix.width = std_info->width;
+	common->width = std_info->width;
+	common->fmt.fmt.pix.height = std_info->height;
+	common->height = std_info->height;
+	common->fmt.fmt.pix.sizeimage = common->height * common->width * 2;
+	common->fmt.fmt.pix.bytesperline = std_info->width;
+	vpifparams->video_params.hpitch = std_info->width;
+	vpifparams->video_params.storage_mode = std_info->frm_fmt;
+
+	if (vid_ch->stdid)
+		common->fmt.fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
+	else
+		common->fmt.fmt.pix.colorspace = V4L2_COLORSPACE_REC709;
+
+	if (ch->vpifparams.std_info.frm_fmt)
+		common->fmt.fmt.pix.field = V4L2_FIELD_NONE;
+	else
+		common->fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
+
+	if (ch->vpifparams.iface.if_type == VPIF_IF_RAW_BAYER)
+		common->fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_SBGGR8;
+	else
+		common->fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUV422P;
+
+	common->fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	return 0;
+}
+
+/**
+ * vpif_calculate_offsets : This function calculates buffers offsets
+ * @ch : ptr to channel object
+ *
+ * This function calculates buffer offsets for Y and C in the top and
+ * bottom field
+ */
+static void vpif_calculate_offsets(struct channel_obj *ch)
+{
+	unsigned int hpitch, sizeimage;
+	struct video_obj *vid_ch = &(ch->video);
+	struct vpif_params *vpifparams = &ch->vpifparams;
+	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
+	enum v4l2_field field = common->fmt.fmt.pix.field;
+
+	vpif_dbg(2, debug, "vpif_calculate_offsets\n");
+
+	if (V4L2_FIELD_ANY == field) {
+		if (vpifparams->std_info.frm_fmt)
+			vid_ch->buf_field = V4L2_FIELD_NONE;
+		else
+			vid_ch->buf_field = V4L2_FIELD_INTERLACED;
+	} else
+		vid_ch->buf_field = common->fmt.fmt.pix.field;
+
+	sizeimage = common->fmt.fmt.pix.sizeimage;
+
+	hpitch = common->fmt.fmt.pix.bytesperline;
+
+	if ((V4L2_FIELD_NONE == vid_ch->buf_field) ||
+	    (V4L2_FIELD_INTERLACED == vid_ch->buf_field)) {
+		/* Calculate offsets for Y top, Y Bottom, C top and C Bottom */
+		common->ytop_off = 0;
+		common->ybtm_off = hpitch;
+		common->ctop_off = sizeimage / 2;
+		common->cbtm_off = sizeimage / 2 + hpitch;
+	} else if (V4L2_FIELD_SEQ_TB == vid_ch->buf_field) {
+		/* Calculate offsets for Y top, Y Bottom, C top and C Bottom */
+		common->ytop_off = 0;
+		common->ybtm_off = sizeimage / 4;
+		common->ctop_off = sizeimage / 2;
+		common->cbtm_off = common->ctop_off + sizeimage / 4;
+	} else if (V4L2_FIELD_SEQ_BT == vid_ch->buf_field) {
+		/* Calculate offsets for Y top, Y Bottom, C top and C Bottom */
+		common->ybtm_off = 0;
+		common->ytop_off = sizeimage / 4;
+		common->cbtm_off = sizeimage / 2;
+		common->ctop_off = common->cbtm_off + sizeimage / 4;
+	}
+	if ((V4L2_FIELD_NONE == vid_ch->buf_field) ||
+	    (V4L2_FIELD_INTERLACED == vid_ch->buf_field))
+		vpifparams->video_params.storage_mode = 1;
+	else
+		vpifparams->video_params.storage_mode = 0;
+
+	if (1 == vpifparams->std_info.frm_fmt)
+		vpifparams->video_params.hpitch =
+		    common->fmt.fmt.pix.bytesperline;
+	else {
+		if ((field == V4L2_FIELD_ANY)
+		    || (field == V4L2_FIELD_INTERLACED))
+			vpifparams->video_params.hpitch =
+			    common->fmt.fmt.pix.bytesperline * 2;
+		else
+			vpifparams->video_params.hpitch =
+			    common->fmt.fmt.pix.bytesperline;
+	}
+
+	ch->vpifparams.video_params.stdid = vpifparams->std_info.stdid;
+}
+
+/**
+ * vpif_get_default_field() - Get default field type based on interface
+ * @vpif_params - ptr to vpif params
+ */
+static inline enum v4l2_field vpif_get_default_field(
+				struct vpif_interface *iface)
+{
+	return (iface->if_type == VPIF_IF_RAW_BAYER) ? V4L2_FIELD_NONE :
+						V4L2_FIELD_INTERLACED;
+}
+
+/**
+ * vpif_config_addr() - function to configure buffer address in vpif
+ * @ch - channel ptr
+ * @muxmode - channel mux mode
+ */
+static void vpif_config_addr(struct channel_obj *ch, int muxmode)
+{
+	struct common_obj *common;
+
+	vpif_dbg(2, debug, "vpif_config_addr\n");
+
+	common = &(ch->common[VPIF_VIDEO_INDEX]);
+
+	if (VPIF_CHANNEL1_VIDEO == ch->channel_id)
+		common->set_addr = ch1_set_videobuf_addr;
+	else if (2 == muxmode)
+		common->set_addr = ch0_set_videobuf_addr_yc_nmux;
+	else
+		common->set_addr = ch0_set_videobuf_addr;
+}
+
+/**
+ * vpif_input_to_subdev() - Maps input to sub device
+ * @vpif_cfg - global config ptr
+ * @chan_cfg - channel config ptr
+ * @input_index - Given input index from application
+ *
+ * lookup the sub device information for a given input index.
+ * we report all the inputs to application. inputs table also
+ * has sub device name for the each input
+ */
+static int vpif_input_to_subdev(
+		struct vpif_capture_config *vpif_cfg,
+		struct vpif_capture_chan_config *chan_cfg,
+		int input_index)
+{
+	struct vpif_subdev_info *subdev_info;
+	const char *subdev_name;
+	int i;
+
+	vpif_dbg(2, debug, "vpif_input_to_subdev\n");
+
+	subdev_name = chan_cfg->inputs[input_index].subdev_name;
+	if (subdev_name == NULL)
+		return -1;
+
+	/* loop through the sub device list to get the sub device info */
+	for (i = 0; i < vpif_cfg->subdev_count; i++) {
+		subdev_info = &vpif_cfg->subdev_info[i];
+		if (!strcmp(subdev_info->name, subdev_name))
+			return i;
+	}
+	return -1;
+}
+
+/**
+ * vpif_set_input() - Select an input
+ * @vpif_cfg - global config ptr
+ * @ch - channel
+ * @_index - Given input index from application
+ *
+ * Select the given input.
+ */
+static int vpif_set_input(
+		struct vpif_capture_config *vpif_cfg,
+		struct channel_obj *ch,
+		int index)
+{
+	struct vpif_capture_chan_config *chan_cfg =
+			&vpif_cfg->chan_config[ch->channel_id];
+	struct vpif_subdev_info *subdev_info = NULL;
+	struct v4l2_subdev *sd = NULL;
+	u32 input = 0, output = 0;
+	int sd_index;
+	int ret;
+
+	sd_index = vpif_input_to_subdev(vpif_cfg, chan_cfg, index);
+	if (sd_index >= 0) {
+		sd = vpif_obj.sd[sd_index];
+		subdev_info = &vpif_cfg->subdev_info[sd_index];
+	}
+
+	/* first setup input path from sub device to vpif */
+	if (sd && vpif_cfg->setup_input_path) {
+		ret = vpif_cfg->setup_input_path(ch->channel_id,
+				       subdev_info->name);
+		if (ret < 0) {
+			vpif_dbg(1, debug, "couldn't setup input path for the" \
+			" sub device %s, for input index %d\n",
+			subdev_info->name, index);
+			return ret;
+		}
+	}
+
+	if (sd) {
+		input = chan_cfg->inputs[index].input_route;
+		output = chan_cfg->inputs[index].output_route;
+		ret = v4l2_subdev_call(sd, video, s_routing,
+				input, output, 0);
+		if (ret < 0 && ret != -ENOIOCTLCMD) {
+			vpif_dbg(1, debug, "Failed to set input\n");
+			return ret;
+		}
+	}
+	ch->input_idx = index;
+	ch->sd = sd;
+	/* copy interface parameters to vpif */
+	ch->vpifparams.iface = chan_cfg->vpif_if;
+
+	/* update tvnorms from the sub device input info */
+	ch->video_dev.tvnorms = chan_cfg->inputs[index].input.std;
+	return 0;
+}
+
+/**
+ * vpif_querystd() - querystd handler
+ * @file: file ptr
+ * @priv: file handle
+ * @std_id: ptr to std id
+ *
+ * This function is called to detect standard at the selected input
+ */
+static int vpif_querystd(struct file *file, void *priv, v4l2_std_id *std_id)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
+	int ret = 0;
+
+	vpif_dbg(2, debug, "vpif_querystd\n");
+
+	/* Call querystd function of decoder device */
+	ret = v4l2_subdev_call(ch->sd, video, querystd, std_id);
+
+	if (ret == -ENOIOCTLCMD || ret == -ENODEV)
+		return -ENODATA;
+	if (ret) {
+		vpif_dbg(1, debug, "Failed to query standard for sub devices\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+/**
+ * vpif_g_std() - get STD handler
+ * @file: file ptr
+ * @priv: file handle
+ * @std_id: ptr to std id
+ */
+static int vpif_g_std(struct file *file, void *priv, v4l2_std_id *std)
+{
+	struct vpif_capture_config *config = vpif_dev->platform_data;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
+	struct vpif_capture_chan_config *chan_cfg;
+	struct v4l2_input input;
+
+	vpif_dbg(2, debug, "vpif_g_std\n");
+
+	if (config->chan_config[ch->channel_id].inputs == NULL)
+		return -ENODATA;
+
+	chan_cfg = &config->chan_config[ch->channel_id];
+	input = chan_cfg->inputs[ch->input_idx].input;
+	if (input.capabilities != V4L2_IN_CAP_STD)
+		return -ENODATA;
+
+	*std = ch->video.stdid;
+	return 0;
+}
+
+/**
+ * vpif_s_std() - set STD handler
+ * @file: file ptr
+ * @priv: file handle
+ * @std_id: ptr to std id
+ */
+static int vpif_s_std(struct file *file, void *priv, v4l2_std_id std_id)
+{
+	struct vpif_capture_config *config = vpif_dev->platform_data;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
+	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
+	struct vpif_capture_chan_config *chan_cfg;
+	struct v4l2_input input;
+	int ret;
+
+	vpif_dbg(2, debug, "vpif_s_std\n");
+
+	if (config->chan_config[ch->channel_id].inputs == NULL)
+		return -ENODATA;
+
+	chan_cfg = &config->chan_config[ch->channel_id];
+	input = chan_cfg->inputs[ch->input_idx].input;
+	if (input.capabilities != V4L2_IN_CAP_STD)
+		return -ENODATA;
+
+	if (vb2_is_busy(&common->buffer_queue))
+		return -EBUSY;
+
+	/* Call encoder subdevice function to set the standard */
+	ch->video.stdid = std_id;
+	memset(&ch->video.dv_timings, 0, sizeof(ch->video.dv_timings));
+
+	/* Get the information about the standard */
+	if (vpif_update_std_info(ch)) {
+		vpif_err("Error getting the standard info\n");
+		return -EINVAL;
+	}
+
+	/* set standard in the sub device */
+	ret = v4l2_subdev_call(ch->sd, video, s_std, std_id);
+	if (ret && ret != -ENOIOCTLCMD && ret != -ENODEV) {
+		vpif_dbg(1, debug, "Failed to set standard for sub devices\n");
+		return ret;
+	}
+	return 0;
+}
+
+/**
+ * vpif_enum_input() - ENUMINPUT handler
+ * @file: file ptr
+ * @priv: file handle
+ * @input: ptr to input structure
+ */
+static int vpif_enum_input(struct file *file, void *priv,
+				struct v4l2_input *input)
+{
+
+	struct vpif_capture_config *config = vpif_dev->platform_data;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
+	struct vpif_capture_chan_config *chan_cfg;
+
+	chan_cfg = &config->chan_config[ch->channel_id];
+
+	if (input->index >= chan_cfg->input_count)
+		return -EINVAL;
+
+	memcpy(input, &chan_cfg->inputs[input->index].input,
+		sizeof(*input));
+	return 0;
+}
+
+/**
+ * vpif_g_input() - Get INPUT handler
+ * @file: file ptr
+ * @priv: file handle
+ * @index: ptr to input index
+ */
+static int vpif_g_input(struct file *file, void *priv, unsigned int *index)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
+
+	*index = ch->input_idx;
+	return 0;
+}
+
+/**
+ * vpif_s_input() - Set INPUT handler
+ * @file: file ptr
+ * @priv: file handle
+ * @index: input index
+ */
+static int vpif_s_input(struct file *file, void *priv, unsigned int index)
+{
+	struct vpif_capture_config *config = vpif_dev->platform_data;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
+	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
+	struct vpif_capture_chan_config *chan_cfg;
+
+	chan_cfg = &config->chan_config[ch->channel_id];
+
+	if (index >= chan_cfg->input_count)
+		return -EINVAL;
+
+	if (vb2_is_busy(&common->buffer_queue))
+		return -EBUSY;
+
+	return vpif_set_input(config, ch, index);
+}
+
+/**
+ * vpif_enum_fmt_vid_cap() - ENUM_FMT handler
+ * @file: file ptr
+ * @priv: file handle
+ * @index: input index
+ */
+static int vpif_enum_fmt_vid_cap(struct file *file, void  *priv,
+					struct v4l2_fmtdesc *fmt)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
+
+	if (fmt->index != 0) {
+		vpif_dbg(1, debug, "Invalid format index\n");
+		return -EINVAL;
+	}
+
+	/* Fill in the information about format */
+	if (ch->vpifparams.iface.if_type == VPIF_IF_RAW_BAYER) {
+		fmt->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		strcpy(fmt->description, "Raw Mode -Bayer Pattern GrRBGb");
+		fmt->pixelformat = V4L2_PIX_FMT_SBGGR8;
+	} else {
+		fmt->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		strcpy(fmt->description, "YCbCr4:2:2 YC Planar");
+		fmt->pixelformat = V4L2_PIX_FMT_YUV422P;
+	}
+	return 0;
+}
+
+/**
+ * vpif_try_fmt_vid_cap() - TRY_FMT handler
+ * @file: file ptr
+ * @priv: file handle
+ * @fmt: ptr to v4l2 format structure
+ */
+static int vpif_try_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *fmt)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
+	struct v4l2_pix_format *pixfmt = &fmt->fmt.pix;
+	struct common_obj *common = &(ch->common[VPIF_VIDEO_INDEX]);
+	struct vpif_params *vpif_params = &ch->vpifparams;
+
+	/*
+	 * to supress v4l-compliance warnings silently correct
+	 * the pixelformat
+	 */
+	if (vpif_params->iface.if_type == VPIF_IF_RAW_BAYER) {
+		if (pixfmt->pixelformat != V4L2_PIX_FMT_SBGGR8)
+			pixfmt->pixelformat = V4L2_PIX_FMT_SBGGR8;
+	} else {
+		if (pixfmt->pixelformat != V4L2_PIX_FMT_YUV422P)
+			pixfmt->pixelformat = V4L2_PIX_FMT_YUV422P;
+	}
+
+	common->fmt.fmt.pix.pixelformat = pixfmt->pixelformat;
+
+	vpif_update_std_info(ch);
+
+	pixfmt->field = common->fmt.fmt.pix.field;
+	pixfmt->colorspace = common->fmt.fmt.pix.colorspace;
+	pixfmt->bytesperline = common->fmt.fmt.pix.width;
+	pixfmt->width = common->fmt.fmt.pix.width;
+	pixfmt->height = common->fmt.fmt.pix.height;
+	pixfmt->sizeimage = pixfmt->bytesperline * pixfmt->height * 2;
+	pixfmt->priv = 0;
+
+	return 0;
+}
+
+
+/**
+ * vpif_g_fmt_vid_cap() - Set INPUT handler
+ * @file: file ptr
+ * @priv: file handle
+ * @fmt: ptr to v4l2 format structure
+ */
+static int vpif_g_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *fmt)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
+	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
+
+	/* Check the validity of the buffer type */
+	if (common->fmt.type != fmt->type)
+		return -EINVAL;
+
+	/* Fill in the information about format */
+	*fmt = common->fmt;
+	return 0;
+}
+
+/**
+ * vpif_s_fmt_vid_cap() - Set FMT handler
+ * @file: file ptr
+ * @priv: file handle
+ * @fmt: ptr to v4l2 format structure
+ */
+static int vpif_s_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *fmt)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
+	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
+	int ret;
+
+	vpif_dbg(2, debug, "%s\n", __func__);
+
+	if (vb2_is_busy(&common->buffer_queue))
+		return -EBUSY;
+
+	ret = vpif_try_fmt_vid_cap(file, priv, fmt);
+	if (ret)
+		return ret;
+
+	/* store the format in the channel object */
+	common->fmt = *fmt;
+	return 0;
+}
+
+/**
+ * vpif_querycap() - QUERYCAP handler
+ * @file: file ptr
+ * @priv: file handle
+ * @cap: ptr to v4l2_capability structure
+ */
+static int vpif_querycap(struct file *file, void  *priv,
+				struct v4l2_capability *cap)
+{
+	struct vpif_capture_config *config = vpif_dev->platform_data;
+
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	strlcpy(cap->driver, VPIF_DRIVER_NAME, sizeof(cap->driver));
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
+		 dev_name(vpif_dev));
+	strlcpy(cap->card, config->card_name, sizeof(cap->card));
+
+	return 0;
+}
+
+/**
+ * vpif_enum_dv_timings() - ENUM_DV_TIMINGS handler
+ * @file: file ptr
+ * @priv: file handle
+ * @timings: input timings
+ */
+static int
+vpif_enum_dv_timings(struct file *file, void *priv,
+		     struct v4l2_enum_dv_timings *timings)
+{
+	struct vpif_capture_config *config = vpif_dev->platform_data;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
+	struct vpif_capture_chan_config *chan_cfg;
+	struct v4l2_input input;
+	int ret;
+
+	if (config->chan_config[ch->channel_id].inputs == NULL)
+		return -ENODATA;
+
+	chan_cfg = &config->chan_config[ch->channel_id];
+	input = chan_cfg->inputs[ch->input_idx].input;
+	if (input.capabilities != V4L2_IN_CAP_DV_TIMINGS)
+		return -ENODATA;
+
+	timings->pad = 0;
+
+	ret = v4l2_subdev_call(ch->sd, pad, enum_dv_timings, timings);
+	if (ret == -ENOIOCTLCMD || ret == -ENODEV)
+		return -EINVAL;
+
+	return ret;
+}
+
+/**
+ * vpif_query_dv_timings() - QUERY_DV_TIMINGS handler
+ * @file: file ptr
+ * @priv: file handle
+ * @timings: input timings
+ */
+static int
+vpif_query_dv_timings(struct file *file, void *priv,
+		      struct v4l2_dv_timings *timings)
+{
+	struct vpif_capture_config *config = vpif_dev->platform_data;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
+	struct vpif_capture_chan_config *chan_cfg;
+	struct v4l2_input input;
+	int ret;
+
+	if (config->chan_config[ch->channel_id].inputs == NULL)
+		return -ENODATA;
+
+	chan_cfg = &config->chan_config[ch->channel_id];
+	input = chan_cfg->inputs[ch->input_idx].input;
+	if (input.capabilities != V4L2_IN_CAP_DV_TIMINGS)
+		return -ENODATA;
+
+	ret = v4l2_subdev_call(ch->sd, video, query_dv_timings, timings);
+	if (ret == -ENOIOCTLCMD || ret == -ENODEV)
+		return -ENODATA;
+
+	return ret;
+}
+
+/**
+ * vpif_s_dv_timings() - S_DV_TIMINGS handler
+ * @file: file ptr
+ * @priv: file handle
+ * @timings: digital video timings
+ */
+static int vpif_s_dv_timings(struct file *file, void *priv,
+		struct v4l2_dv_timings *timings)
+{
+	struct vpif_capture_config *config = vpif_dev->platform_data;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
+	struct vpif_params *vpifparams = &ch->vpifparams;
+	struct vpif_channel_config_params *std_info = &vpifparams->std_info;
+	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
+	struct video_obj *vid_ch = &ch->video;
+	struct v4l2_bt_timings *bt = &vid_ch->dv_timings.bt;
+	struct vpif_capture_chan_config *chan_cfg;
+	struct v4l2_input input;
+	int ret;
+
+	if (config->chan_config[ch->channel_id].inputs == NULL)
+		return -ENODATA;
+
+	chan_cfg = &config->chan_config[ch->channel_id];
+	input = chan_cfg->inputs[ch->input_idx].input;
+	if (input.capabilities != V4L2_IN_CAP_DV_TIMINGS)
+		return -ENODATA;
+
+	if (timings->type != V4L2_DV_BT_656_1120) {
+		vpif_dbg(2, debug, "Timing type not defined\n");
+		return -EINVAL;
+	}
+
+	if (vb2_is_busy(&common->buffer_queue))
+		return -EBUSY;
+
+	/* Configure subdevice timings, if any */
+	ret = v4l2_subdev_call(ch->sd, video, s_dv_timings, timings);
+	if (ret == -ENOIOCTLCMD || ret == -ENODEV)
+		ret = 0;
+	if (ret < 0) {
+		vpif_dbg(2, debug, "Error setting custom DV timings\n");
+		return ret;
+	}
+
+	if (!(timings->bt.width && timings->bt.height &&
+				(timings->bt.hbackporch ||
+				 timings->bt.hfrontporch ||
+				 timings->bt.hsync) &&
+				timings->bt.vfrontporch &&
+				(timings->bt.vbackporch ||
+				 timings->bt.vsync))) {
+		vpif_dbg(2, debug, "Timings for width, height, "
+				"horizontal back porch, horizontal sync, "
+				"horizontal front porch, vertical back porch, "
+				"vertical sync and vertical back porch "
+				"must be defined\n");
+		return -EINVAL;
+	}
+
+	vid_ch->dv_timings = *timings;
+
+	/* Configure video port timings */
+
+	std_info->eav2sav = V4L2_DV_BT_BLANKING_WIDTH(bt) - 8;
+	std_info->sav2eav = bt->width;
+
+	std_info->l1 = 1;
+	std_info->l3 = bt->vsync + bt->vbackporch + 1;
+
+	std_info->vsize = V4L2_DV_BT_FRAME_HEIGHT(bt);
+	if (bt->interlaced) {
+		if (bt->il_vbackporch || bt->il_vfrontporch || bt->il_vsync) {
+			std_info->l5 = std_info->vsize/2 -
+				(bt->vfrontporch - 1);
+			std_info->l7 = std_info->vsize/2 + 1;
+			std_info->l9 = std_info->l7 + bt->il_vsync +
+				bt->il_vbackporch + 1;
+			std_info->l11 = std_info->vsize -
+				(bt->il_vfrontporch - 1);
+		} else {
+			vpif_dbg(2, debug, "Required timing values for "
+					"interlaced BT format missing\n");
+			return -EINVAL;
+		}
+	} else {
+		std_info->l5 = std_info->vsize - (bt->vfrontporch - 1);
+	}
+	strncpy(std_info->name, "Custom timings BT656/1120", VPIF_MAX_NAME);
+	std_info->width = bt->width;
+	std_info->height = bt->height;
+	std_info->frm_fmt = bt->interlaced ? 0 : 1;
+	std_info->ycmux_mode = 0;
+	std_info->capture_format = 0;
+	std_info->vbi_supported = 0;
+	std_info->hd_sd = 1;
+	std_info->stdid = 0;
+
+	vid_ch->stdid = 0;
+	return 0;
+}
+
+/**
+ * vpif_g_dv_timings() - G_DV_TIMINGS handler
+ * @file: file ptr
+ * @priv: file handle
+ * @timings: digital video timings
+ */
+static int vpif_g_dv_timings(struct file *file, void *priv,
+		struct v4l2_dv_timings *timings)
+{
+	struct vpif_capture_config *config = vpif_dev->platform_data;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
+	struct video_obj *vid_ch = &ch->video;
+	struct vpif_capture_chan_config *chan_cfg;
+	struct v4l2_input input;
+
+	if (config->chan_config[ch->channel_id].inputs == NULL)
+		return -ENODATA;
+
+	chan_cfg = &config->chan_config[ch->channel_id];
+	input = chan_cfg->inputs[ch->input_idx].input;
+	if (input.capabilities != V4L2_IN_CAP_DV_TIMINGS)
+		return -ENODATA;
+
+	*timings = vid_ch->dv_timings;
+
+	return 0;
+}
+
+/*
+ * vpif_log_status() - Status information
+ * @file: file ptr
+ * @priv: file handle
+ *
+ * Returns zero.
+ */
+static int vpif_log_status(struct file *filep, void *priv)
+{
+	/* status for sub devices */
+	v4l2_device_call_all(&vpif_obj.v4l2_dev, 0, core, log_status);
+
+	return 0;
+}
+
+/* vpif capture ioctl operations */
+static const struct v4l2_ioctl_ops vpif_ioctl_ops = {
+	.vidioc_querycap		= vpif_querycap,
+	.vidioc_enum_fmt_vid_cap	= vpif_enum_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap		= vpif_g_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap		= vpif_s_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap		= vpif_try_fmt_vid_cap,
+
+	.vidioc_enum_input		= vpif_enum_input,
+	.vidioc_s_input			= vpif_s_input,
+	.vidioc_g_input			= vpif_g_input,
+
+	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
+	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
+	.vidioc_querybuf		= vb2_ioctl_querybuf,
+	.vidioc_qbuf			= vb2_ioctl_qbuf,
+	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
+	.vidioc_expbuf			= vb2_ioctl_expbuf,
+	.vidioc_streamon		= vb2_ioctl_streamon,
+	.vidioc_streamoff		= vb2_ioctl_streamoff,
+
+	.vidioc_querystd		= vpif_querystd,
+	.vidioc_s_std			= vpif_s_std,
+	.vidioc_g_std			= vpif_g_std,
+
+	.vidioc_enum_dv_timings		= vpif_enum_dv_timings,
+	.vidioc_query_dv_timings	= vpif_query_dv_timings,
+	.vidioc_s_dv_timings		= vpif_s_dv_timings,
+	.vidioc_g_dv_timings		= vpif_g_dv_timings,
+
+	.vidioc_log_status		= vpif_log_status,
+};
+
+/* vpif file operations */
+static struct v4l2_file_operations vpif_fops = {
+	.owner = THIS_MODULE,
+	.open = v4l2_fh_open,
+	.release = vb2_fop_release,
+	.unlocked_ioctl = video_ioctl2,
+	.mmap = vb2_fop_mmap,
+	.poll = vb2_fop_poll
+};
+
+/**
+ * initialize_vpif() - Initialize vpif data structures
+ *
+ * Allocate memory for data structures and initialize them
+ */
+static int initialize_vpif(void)
+{
+	int err, i, j;
+	int free_channel_objects_index;
+
+	/* Allocate memory for six channel objects */
+	for (i = 0; i < VPIF_CAPTURE_MAX_DEVICES; i++) {
+		vpif_obj.dev[i] =
+		    kzalloc(sizeof(*vpif_obj.dev[i]), GFP_KERNEL);
+		/* If memory allocation fails, return error */
+		if (!vpif_obj.dev[i]) {
+			free_channel_objects_index = i;
+			err = -ENOMEM;
+			goto vpif_init_free_channel_objects;
+		}
+	}
+	return 0;
+
+vpif_init_free_channel_objects:
+	for (j = 0; j < free_channel_objects_index; j++)
+		kfree(vpif_obj.dev[j]);
+	return err;
+}
+
+static int vpif_async_bound(struct v4l2_async_notifier *notifier,
+			    struct v4l2_subdev *subdev,
+			    struct v4l2_async_subdev *asd)
+{
+	int i;
+
+	for (i = 0; i < vpif_obj.config->subdev_count; i++)
+		if (!strcmp(vpif_obj.config->subdev_info[i].name,
+			    subdev->name)) {
+			vpif_obj.sd[i] = subdev;
+			return 0;
+		}
+
+	return -EINVAL;
+}
+
+static int vpif_probe_complete(void)
+{
+	struct common_obj *common;
+	struct video_device *vdev;
+	struct channel_obj *ch;
+	struct vb2_queue *q;
+	int j, err, k;
+
+	for (j = 0; j < VPIF_CAPTURE_MAX_DEVICES; j++) {
+		ch = vpif_obj.dev[j];
+		ch->channel_id = j;
+		common = &(ch->common[VPIF_VIDEO_INDEX]);
+		spin_lock_init(&common->irqlock);
+		mutex_init(&common->lock);
+
+		/* select input 0 */
+		err = vpif_set_input(vpif_obj.config, ch, 0);
+		if (err)
+			goto probe_out;
+
+		/* set initial format */
+		ch->video.stdid = V4L2_STD_525_60;
+		memset(&ch->video.dv_timings, 0, sizeof(ch->video.dv_timings));
+		vpif_update_std_info(ch);
+
+		/* Initialize vb2 queue */
+		q = &common->buffer_queue;
+		q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
+		q->drv_priv = ch;
+		q->ops = &video_qops;
+		q->mem_ops = &vb2_dma_contig_memops;
+		q->buf_struct_size = sizeof(struct vpif_cap_buffer);
+		q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+		q->min_buffers_needed = 1;
+		q->lock = &common->lock;
+
+		err = vb2_queue_init(q);
+		if (err) {
+			vpif_err("vpif_capture: vb2_queue_init() failed\n");
+			goto probe_out;
+		}
+
+		common->alloc_ctx = vb2_dma_contig_init_ctx(vpif_dev);
+		if (IS_ERR(common->alloc_ctx)) {
+			vpif_err("Failed to get the context\n");
+			err = PTR_ERR(common->alloc_ctx);
+			goto probe_out;
+		}
+
+		INIT_LIST_HEAD(&common->dma_queue);
+
+		/* Initialize the video_device structure */
+		vdev = &ch->video_dev;
+		strlcpy(vdev->name, VPIF_DRIVER_NAME, sizeof(vdev->name));
+		vdev->release = video_device_release_empty;
+		vdev->fops = &vpif_fops;
+		vdev->ioctl_ops = &vpif_ioctl_ops;
+		vdev->v4l2_dev = &vpif_obj.v4l2_dev;
+		vdev->vfl_dir = VFL_DIR_RX;
+		vdev->queue = q;
+		vdev->lock = &common->lock;
+		video_set_drvdata(&ch->video_dev, ch);
+		err = video_register_device(vdev,
+					    VFL_TYPE_GRABBER, (j ? 1 : 0));
+		if (err)
+			goto probe_out;
+	}
+
+	v4l2_info(&vpif_obj.v4l2_dev, "VPIF capture driver initialized\n");
+	return 0;
+
+probe_out:
+	for (k = 0; k < j; k++) {
+		/* Get the pointer to the channel object */
+		ch = vpif_obj.dev[k];
+		common = &ch->common[k];
+		vb2_dma_contig_cleanup_ctx(common->alloc_ctx);
+		/* Unregister video device */
+		video_unregister_device(&ch->video_dev);
+	}
+	kfree(vpif_obj.sd);
+	v4l2_device_unregister(&vpif_obj.v4l2_dev);
+
+	return err;
+}
+
+static int vpif_async_complete(struct v4l2_async_notifier *notifier)
+{
+	return vpif_probe_complete();
+}
+
+/**
+ * vpif_probe : This function probes the vpif capture driver
+ * @pdev: platform device pointer
+ *
+ * This creates device entries by register itself to the V4L2 driver and
+ * initializes fields of each channel objects
+ */
+static __init int vpif_probe(struct platform_device *pdev)
+{
+	struct vpif_subdev_info *subdevdata;
+	struct i2c_adapter *i2c_adap;
+	struct resource *res;
+	int subdev_count;
+	int res_idx = 0;
+	int i, err;
+
+	vpif_dev = &pdev->dev;
+
+	err = initialize_vpif();
+	if (err) {
+		v4l2_err(vpif_dev->driver, "Error initializing vpif\n");
+		return err;
+	}
+
+	err = v4l2_device_register(vpif_dev, &vpif_obj.v4l2_dev);
+	if (err) {
+		v4l2_err(vpif_dev->driver, "Error registering v4l2 device\n");
+		return err;
+	}
+
+	while ((res = platform_get_resource(pdev, IORESOURCE_IRQ, res_idx))) {
+		err = devm_request_irq(&pdev->dev, res->start, vpif_channel_isr,
+					IRQF_SHARED, VPIF_DRIVER_NAME,
+					(void *)(&vpif_obj.dev[res_idx]->
+					channel_id));
+		if (err) {
+			err = -EINVAL;
+			goto vpif_unregister;
+		}
+		res_idx++;
+	}
+
+	vpif_obj.config = pdev->dev.platform_data;
+
+	subdev_count = vpif_obj.config->subdev_count;
+	vpif_obj.sd = kzalloc(sizeof(struct v4l2_subdev *) * subdev_count,
+				GFP_KERNEL);
+	if (vpif_obj.sd == NULL) {
+		vpif_err("unable to allocate memory for subdevice pointers\n");
+		err = -ENOMEM;
+		goto vpif_unregister;
+	}
+
+	if (!vpif_obj.config->asd_sizes) {
+		i2c_adap = i2c_get_adapter(1);
+		for (i = 0; i < subdev_count; i++) {
+			subdevdata = &vpif_obj.config->subdev_info[i];
+			vpif_obj.sd[i] =
+				v4l2_i2c_new_subdev_board(&vpif_obj.v4l2_dev,
+							  i2c_adap,
+							  &subdevdata->
+							  board_info,
+							  NULL);
+
+			if (!vpif_obj.sd[i]) {
+				vpif_err("Error registering v4l2 subdevice\n");
+				err = -ENODEV;
+				goto probe_subdev_out;
+			}
+			v4l2_info(&vpif_obj.v4l2_dev,
+				  "registered sub device %s\n",
+				   subdevdata->name);
+		}
+		vpif_probe_complete();
+	} else {
+		vpif_obj.notifier.subdevs = vpif_obj.config->asd;
+		vpif_obj.notifier.num_subdevs = vpif_obj.config->asd_sizes[0];
+		vpif_obj.notifier.bound = vpif_async_bound;
+		vpif_obj.notifier.complete = vpif_async_complete;
+		err = v4l2_async_notifier_register(&vpif_obj.v4l2_dev,
+						   &vpif_obj.notifier);
+		if (err) {
+			vpif_err("Error registering async notifier\n");
+			err = -EINVAL;
+			goto probe_subdev_out;
+		}
+	}
+
+	return 0;
+
+probe_subdev_out:
+	/* free sub devices memory */
+	kfree(vpif_obj.sd);
+vpif_unregister:
+	v4l2_device_unregister(&vpif_obj.v4l2_dev);
+
+	return err;
+}
+
+/**
+ * vpif_remove() - driver remove handler
+ * @device: ptr to platform device structure
+ *
+ * The vidoe device is unregistered
+ */
+static int vpif_remove(struct platform_device *device)
+{
+	struct common_obj *common;
+	struct channel_obj *ch;
+	int i;
+
+	v4l2_device_unregister(&vpif_obj.v4l2_dev);
+
+	kfree(vpif_obj.sd);
+	/* un-register device */
+	for (i = 0; i < VPIF_CAPTURE_MAX_DEVICES; i++) {
+		/* Get the pointer to the channel object */
+		ch = vpif_obj.dev[i];
+		common = &ch->common[VPIF_VIDEO_INDEX];
+		vb2_dma_contig_cleanup_ctx(common->alloc_ctx);
+		/* Unregister video device */
+		video_unregister_device(&ch->video_dev);
+		kfree(vpif_obj.dev[i]);
+	}
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+/**
+ * vpif_suspend: vpif device suspend
+ */
+static int vpif_suspend(struct device *dev)
+{
+
+	struct common_obj *common;
+	struct channel_obj *ch;
+	int i;
+
+	for (i = 0; i < VPIF_CAPTURE_MAX_DEVICES; i++) {
+		/* Get the pointer to the channel object */
+		ch = vpif_obj.dev[i];
+		common = &ch->common[VPIF_VIDEO_INDEX];
+
+		if (!vb2_start_streaming_called(&common->buffer_queue))
+			continue;
+
+		mutex_lock(&common->lock);
+		/* Disable channel */
+		if (ch->channel_id == VPIF_CHANNEL0_VIDEO) {
+			enable_channel0(0);
+			channel0_intr_enable(0);
+		}
+		if (ch->channel_id == VPIF_CHANNEL1_VIDEO ||
+			ycmux_mode == 2) {
+			enable_channel1(0);
+			channel1_intr_enable(0);
+		}
+		mutex_unlock(&common->lock);
+	}
+
+	return 0;
+}
+
+/*
+ * vpif_resume: vpif device suspend
+ */
+static int vpif_resume(struct device *dev)
+{
+	struct common_obj *common;
+	struct channel_obj *ch;
+	int i;
+
+	for (i = 0; i < VPIF_CAPTURE_MAX_DEVICES; i++) {
+		/* Get the pointer to the channel object */
+		ch = vpif_obj.dev[i];
+		common = &ch->common[VPIF_VIDEO_INDEX];
+
+		if (!vb2_start_streaming_called(&common->buffer_queue))
+			continue;
+
+		mutex_lock(&common->lock);
+		/* Enable channel */
+		if (ch->channel_id == VPIF_CHANNEL0_VIDEO) {
+			enable_channel0(1);
+			channel0_intr_enable(1);
+		}
+		if (ch->channel_id == VPIF_CHANNEL1_VIDEO ||
+			ycmux_mode == 2) {
+			enable_channel1(1);
+			channel1_intr_enable(1);
+		}
+		mutex_unlock(&common->lock);
+	}
+
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(vpif_pm_ops, vpif_suspend, vpif_resume);
+
+static __refdata struct platform_driver vpif_driver = {
+	.driver	= {
+		.name	= VPIF_DRIVER_NAME,
+		.pm	= &vpif_pm_ops,
+	},
+	.probe = vpif_probe,
+	.remove = vpif_remove,
+};
+
+module_platform_driver(vpif_driver);
diff --git a/drivers/media/platform/davinci/vpif_capture.h b/drivers/media/platform/davinci/vpif_capture.h
new file mode 100644
index 0000000..4a76009
--- /dev/null
+++ b/drivers/media/platform/davinci/vpif_capture.h
@@ -0,0 +1,122 @@
+/*
+ * Copyright (C) 2009 Texas Instruments Inc
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#ifndef VPIF_CAPTURE_H
+#define VPIF_CAPTURE_H
+
+/* Header files */
+#include <media/videobuf2-dma-contig.h>
+#include <media/v4l2-device.h>
+
+#include "vpif.h"
+
+/* Macros */
+#define VPIF_CAPTURE_VERSION		"0.0.2"
+
+#define VPIF_VALID_FIELD(field)		(((V4L2_FIELD_ANY == field) || \
+	(V4L2_FIELD_NONE == field)) || \
+	(((V4L2_FIELD_INTERLACED == field) || \
+	(V4L2_FIELD_SEQ_TB == field)) || \
+	(V4L2_FIELD_SEQ_BT == field)))
+
+#define VPIF_CAPTURE_MAX_DEVICES	2
+#define VPIF_VIDEO_INDEX		0
+#define VPIF_NUMBER_OF_OBJECTS		1
+
+/* Enumerated data type to give id to each device per channel */
+enum vpif_channel_id {
+	VPIF_CHANNEL0_VIDEO = 0,
+	VPIF_CHANNEL1_VIDEO,
+};
+
+struct video_obj {
+	enum v4l2_field buf_field;
+	/* Currently selected or default standard */
+	v4l2_std_id stdid;
+	struct v4l2_dv_timings dv_timings;
+};
+
+struct vpif_cap_buffer {
+	struct vb2_v4l2_buffer vb;
+	struct list_head list;
+};
+
+struct common_obj {
+	/* Pointer pointing to current v4l2_buffer */
+	struct vpif_cap_buffer *cur_frm;
+	/* Pointer pointing to current v4l2_buffer */
+	struct vpif_cap_buffer *next_frm;
+	/* Used to store pixel format */
+	struct v4l2_format fmt;
+	/* Buffer queue used in video-buf */
+	struct vb2_queue buffer_queue;
+	/* allocator-specific contexts for each plane */
+	struct vb2_alloc_ctx *alloc_ctx;
+	/* Queue of filled frames */
+	struct list_head dma_queue;
+	/* Used in video-buf */
+	spinlock_t irqlock;
+	/* lock used to access this structure */
+	struct mutex lock;
+	/* Function pointer to set the addresses */
+	void (*set_addr) (unsigned long, unsigned long, unsigned long,
+			  unsigned long);
+	/* offset where Y top starts from the starting of the buffer */
+	u32 ytop_off;
+	/* offset where Y bottom starts from the starting of the buffer */
+	u32 ybtm_off;
+	/* offset where C top starts from the starting of the buffer */
+	u32 ctop_off;
+	/* offset where C bottom starts from the starting of the buffer */
+	u32 cbtm_off;
+	/* Indicates width of the image data */
+	u32 width;
+	/* Indicates height of the image data */
+	u32 height;
+};
+
+struct channel_obj {
+	/* Identifies video device for this channel */
+	struct video_device video_dev;
+	/* Indicates id of the field which is being displayed */
+	u32 field_id;
+	/* flag to indicate whether decoder is initialized */
+	u8 initialized;
+	/* Identifies channel */
+	enum vpif_channel_id channel_id;
+	/* Current input */
+	u32 input_idx;
+	/* subdev corresponding to the current input, may be NULL */
+	struct v4l2_subdev *sd;
+	/* vpif configuration params */
+	struct vpif_params vpifparams;
+	/* common object array */
+	struct common_obj common[VPIF_NUMBER_OF_OBJECTS];
+	/* video object */
+	struct video_obj video;
+};
+
+struct vpif_device {
+	struct v4l2_device v4l2_dev;
+	struct channel_obj *dev[VPIF_CAPTURE_NUM_CHANNELS];
+	struct v4l2_subdev **sd;
+	struct v4l2_async_notifier notifier;
+	struct vpif_capture_config *config;
+};
+
+#endif				/* VPIF_CAPTURE_H */
diff --git a/drivers/media/platform/davinci/vpif_display.c b/drivers/media/platform/davinci/vpif_display.c
new file mode 100644
index 0000000..fd27803
--- /dev/null
+++ b/drivers/media/platform/davinci/vpif_display.c
@@ -0,0 +1,1443 @@
+/*
+ * vpif-display - VPIF display driver
+ * Display driver for TI DaVinci VPIF
+ *
+ * Copyright (C) 2009 Texas Instruments Incorporated - http://www.ti.com/
+ * Copyright (C) 2014 Lad, Prabhakar <prabhakar.csengg@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed .as is. WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#include <media/v4l2-ioctl.h>
+
+#include "vpif.h"
+#include "vpif_display.h"
+
+MODULE_DESCRIPTION("TI DaVinci VPIF Display driver");
+MODULE_LICENSE("GPL");
+MODULE_VERSION(VPIF_DISPLAY_VERSION);
+
+#define VPIF_V4L2_STD (V4L2_STD_525_60 | V4L2_STD_625_50)
+
+#define vpif_err(fmt, arg...)	v4l2_err(&vpif_obj.v4l2_dev, fmt, ## arg)
+#define vpif_dbg(level, debug, fmt, arg...)	\
+		v4l2_dbg(level, debug, &vpif_obj.v4l2_dev, fmt, ## arg)
+
+static int debug = 1;
+
+module_param(debug, int, 0644);
+
+MODULE_PARM_DESC(debug, "Debug level 0-1");
+
+#define VPIF_DRIVER_NAME	"vpif_display"
+
+/* Is set to 1 in case of SDTV formats, 2 in case of HDTV formats. */
+static int ycmux_mode;
+
+static u8 channel_first_int[VPIF_NUMOBJECTS][2] = { {1, 1} };
+
+static struct vpif_device vpif_obj = { {NULL} };
+static struct device *vpif_dev;
+static void vpif_calculate_offsets(struct channel_obj *ch);
+static void vpif_config_addr(struct channel_obj *ch, int muxmode);
+
+static inline
+struct vpif_disp_buffer *to_vpif_buffer(struct vb2_v4l2_buffer *vb)
+{
+	return container_of(vb, struct vpif_disp_buffer, vb);
+}
+
+/**
+ * vpif_buffer_prepare :  callback function for buffer prepare
+ * @vb: ptr to vb2_buffer
+ *
+ * This is the callback function for buffer prepare when vb2_qbuf()
+ * function is called. The buffer is prepared and user space virtual address
+ * or user address is converted into  physical address
+ */
+static int vpif_buffer_prepare(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct channel_obj *ch = vb2_get_drv_priv(vb->vb2_queue);
+	struct common_obj *common;
+
+	common = &ch->common[VPIF_VIDEO_INDEX];
+
+	vb2_set_plane_payload(vb, 0, common->fmt.fmt.pix.sizeimage);
+	if (vb2_get_plane_payload(vb, 0) > vb2_plane_size(vb, 0))
+		return -EINVAL;
+
+	vbuf->field = common->fmt.fmt.pix.field;
+
+	if (vb->vb2_queue->type != V4L2_BUF_TYPE_SLICED_VBI_OUTPUT) {
+		unsigned long addr = vb2_dma_contig_plane_dma_addr(vb, 0);
+
+		if (!ISALIGNED(addr + common->ytop_off) ||
+			!ISALIGNED(addr + common->ybtm_off) ||
+			!ISALIGNED(addr + common->ctop_off) ||
+			!ISALIGNED(addr + common->cbtm_off)) {
+			vpif_err("buffer offset not aligned to 8 bytes\n");
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+/**
+ * vpif_buffer_queue_setup : Callback function for buffer setup.
+ * @vq: vb2_queue ptr
+ * @fmt: v4l2 format
+ * @nbuffers: ptr to number of buffers requested by application
+ * @nplanes:: contains number of distinct video planes needed to hold a frame
+ * @sizes[]: contains the size (in bytes) of each plane.
+ * @alloc_ctxs: ptr to allocation context
+ *
+ * This callback function is called when reqbuf() is called to adjust
+ * the buffer count and buffer size
+ */
+static int vpif_buffer_queue_setup(struct vb2_queue *vq,
+				const void *parg,
+				unsigned int *nbuffers, unsigned int *nplanes,
+				unsigned int sizes[], void *alloc_ctxs[])
+{
+	const struct v4l2_format *fmt = parg;
+	struct channel_obj *ch = vb2_get_drv_priv(vq);
+	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
+
+	if (fmt && fmt->fmt.pix.sizeimage < common->fmt.fmt.pix.sizeimage)
+		return -EINVAL;
+
+	if (vq->num_buffers + *nbuffers < 3)
+		*nbuffers = 3 - vq->num_buffers;
+
+	*nplanes = 1;
+	sizes[0] = fmt ? fmt->fmt.pix.sizeimage : common->fmt.fmt.pix.sizeimage;
+	alloc_ctxs[0] = common->alloc_ctx;
+
+	/* Calculate the offset for Y and C data  in the buffer */
+	vpif_calculate_offsets(ch);
+
+	return 0;
+}
+
+/**
+ * vpif_buffer_queue : Callback function to add buffer to DMA queue
+ * @vb: ptr to vb2_buffer
+ *
+ * This callback fucntion queues the buffer to DMA engine
+ */
+static void vpif_buffer_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct vpif_disp_buffer *buf = to_vpif_buffer(vbuf);
+	struct channel_obj *ch = vb2_get_drv_priv(vb->vb2_queue);
+	struct common_obj *common;
+	unsigned long flags;
+
+	common = &ch->common[VPIF_VIDEO_INDEX];
+
+	/* add the buffer to the DMA queue */
+	spin_lock_irqsave(&common->irqlock, flags);
+	list_add_tail(&buf->list, &common->dma_queue);
+	spin_unlock_irqrestore(&common->irqlock, flags);
+}
+
+/**
+ * vpif_start_streaming : Starts the DMA engine for streaming
+ * @vb: ptr to vb2_buffer
+ * @count: number of buffers
+ */
+static int vpif_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct vpif_display_config *vpif_config_data =
+					vpif_dev->platform_data;
+	struct channel_obj *ch = vb2_get_drv_priv(vq);
+	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
+	struct vpif_params *vpif = &ch->vpifparams;
+	struct vpif_disp_buffer *buf, *tmp;
+	unsigned long addr, flags;
+	int ret;
+
+	spin_lock_irqsave(&common->irqlock, flags);
+
+	/* Initialize field_id */
+	ch->field_id = 0;
+
+	/* clock settings */
+	if (vpif_config_data->set_clock) {
+		ret = vpif_config_data->set_clock(ch->vpifparams.std_info.
+		ycmux_mode, ch->vpifparams.std_info.hd_sd);
+		if (ret < 0) {
+			vpif_err("can't set clock\n");
+			goto err;
+		}
+	}
+
+	/* set the parameters and addresses */
+	ret = vpif_set_video_params(vpif, ch->channel_id + 2);
+	if (ret < 0)
+		goto err;
+
+	ycmux_mode = ret;
+	vpif_config_addr(ch, ret);
+	/* Get the next frame from the buffer queue */
+	common->next_frm = common->cur_frm =
+			    list_entry(common->dma_queue.next,
+				       struct vpif_disp_buffer, list);
+
+	list_del(&common->cur_frm->list);
+	spin_unlock_irqrestore(&common->irqlock, flags);
+
+	addr = vb2_dma_contig_plane_dma_addr(&common->cur_frm->vb.vb2_buf, 0);
+	common->set_addr((addr + common->ytop_off),
+			    (addr + common->ybtm_off),
+			    (addr + common->ctop_off),
+			    (addr + common->cbtm_off));
+
+	/*
+	 * Set interrupt for both the fields in VPIF
+	 * Register enable channel in VPIF register
+	 */
+	channel_first_int[VPIF_VIDEO_INDEX][ch->channel_id] = 1;
+	if (VPIF_CHANNEL2_VIDEO == ch->channel_id) {
+		channel2_intr_assert();
+		channel2_intr_enable(1);
+		enable_channel2(1);
+		if (vpif_config_data->chan_config[VPIF_CHANNEL2_VIDEO].clip_en)
+			channel2_clipping_enable(1);
+	}
+
+	if (VPIF_CHANNEL3_VIDEO == ch->channel_id || ycmux_mode == 2) {
+		channel3_intr_assert();
+		channel3_intr_enable(1);
+		enable_channel3(1);
+		if (vpif_config_data->chan_config[VPIF_CHANNEL3_VIDEO].clip_en)
+			channel3_clipping_enable(1);
+	}
+
+	return 0;
+
+err:
+	list_for_each_entry_safe(buf, tmp, &common->dma_queue, list) {
+		list_del(&buf->list);
+		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_QUEUED);
+	}
+	spin_unlock_irqrestore(&common->irqlock, flags);
+
+	return ret;
+}
+
+/**
+ * vpif_stop_streaming : Stop the DMA engine
+ * @vq: ptr to vb2_queue
+ *
+ * This callback stops the DMA engine and any remaining buffers
+ * in the DMA queue are released.
+ */
+static void vpif_stop_streaming(struct vb2_queue *vq)
+{
+	struct channel_obj *ch = vb2_get_drv_priv(vq);
+	struct common_obj *common;
+	unsigned long flags;
+
+	common = &ch->common[VPIF_VIDEO_INDEX];
+
+	/* Disable channel */
+	if (VPIF_CHANNEL2_VIDEO == ch->channel_id) {
+		enable_channel2(0);
+		channel2_intr_enable(0);
+	}
+	if (VPIF_CHANNEL3_VIDEO == ch->channel_id || ycmux_mode == 2) {
+		enable_channel3(0);
+		channel3_intr_enable(0);
+	}
+
+	/* release all active buffers */
+	spin_lock_irqsave(&common->irqlock, flags);
+	if (common->cur_frm == common->next_frm) {
+		vb2_buffer_done(&common->cur_frm->vb.vb2_buf,
+				VB2_BUF_STATE_ERROR);
+	} else {
+		if (common->cur_frm != NULL)
+			vb2_buffer_done(&common->cur_frm->vb.vb2_buf,
+					VB2_BUF_STATE_ERROR);
+		if (common->next_frm != NULL)
+			vb2_buffer_done(&common->next_frm->vb.vb2_buf,
+					VB2_BUF_STATE_ERROR);
+	}
+
+	while (!list_empty(&common->dma_queue)) {
+		common->next_frm = list_entry(common->dma_queue.next,
+						struct vpif_disp_buffer, list);
+		list_del(&common->next_frm->list);
+		vb2_buffer_done(&common->next_frm->vb.vb2_buf,
+				VB2_BUF_STATE_ERROR);
+	}
+	spin_unlock_irqrestore(&common->irqlock, flags);
+}
+
+static struct vb2_ops video_qops = {
+	.queue_setup		= vpif_buffer_queue_setup,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
+	.buf_prepare		= vpif_buffer_prepare,
+	.start_streaming	= vpif_start_streaming,
+	.stop_streaming		= vpif_stop_streaming,
+	.buf_queue		= vpif_buffer_queue,
+};
+
+static void process_progressive_mode(struct common_obj *common)
+{
+	unsigned long addr = 0;
+
+	spin_lock(&common->irqlock);
+	/* Get the next buffer from buffer queue */
+	common->next_frm = list_entry(common->dma_queue.next,
+				struct vpif_disp_buffer, list);
+	/* Remove that buffer from the buffer queue */
+	list_del(&common->next_frm->list);
+	spin_unlock(&common->irqlock);
+
+	/* Set top and bottom field addrs in VPIF registers */
+	addr = vb2_dma_contig_plane_dma_addr(&common->next_frm->vb.vb2_buf, 0);
+	common->set_addr(addr + common->ytop_off,
+				 addr + common->ybtm_off,
+				 addr + common->ctop_off,
+				 addr + common->cbtm_off);
+}
+
+static void process_interlaced_mode(int fid, struct common_obj *common)
+{
+	/* device field id and local field id are in sync */
+	/* If this is even field */
+	if (0 == fid) {
+		if (common->cur_frm == common->next_frm)
+			return;
+
+		/* one frame is displayed If next frame is
+		 *  available, release cur_frm and move on */
+		/* Copy frame display time */
+		v4l2_get_timestamp(&common->cur_frm->vb.timestamp);
+		/* Change status of the cur_frm */
+		vb2_buffer_done(&common->cur_frm->vb.vb2_buf,
+					VB2_BUF_STATE_DONE);
+		/* Make cur_frm pointing to next_frm */
+		common->cur_frm = common->next_frm;
+
+	} else if (1 == fid) {	/* odd field */
+		spin_lock(&common->irqlock);
+		if (list_empty(&common->dma_queue)
+		    || (common->cur_frm != common->next_frm)) {
+			spin_unlock(&common->irqlock);
+			return;
+		}
+		spin_unlock(&common->irqlock);
+		/* one field is displayed configure the next
+		 * frame if it is available else hold on current
+		 * frame */
+		/* Get next from the buffer queue */
+		process_progressive_mode(common);
+	}
+}
+
+/*
+ * vpif_channel_isr: It changes status of the displayed buffer, takes next
+ * buffer from the queue and sets its address in VPIF registers
+ */
+static irqreturn_t vpif_channel_isr(int irq, void *dev_id)
+{
+	struct vpif_device *dev = &vpif_obj;
+	struct channel_obj *ch;
+	struct common_obj *common;
+	int fid = -1, i;
+	int channel_id = 0;
+
+	channel_id = *(int *)(dev_id);
+	if (!vpif_intr_status(channel_id + 2))
+		return IRQ_NONE;
+
+	ch = dev->dev[channel_id];
+	for (i = 0; i < VPIF_NUMOBJECTS; i++) {
+		common = &ch->common[i];
+		/* If streaming is started in this channel */
+
+		if (1 == ch->vpifparams.std_info.frm_fmt) {
+			spin_lock(&common->irqlock);
+			if (list_empty(&common->dma_queue)) {
+				spin_unlock(&common->irqlock);
+				continue;
+			}
+			spin_unlock(&common->irqlock);
+
+			/* Progressive mode */
+			if (!channel_first_int[i][channel_id]) {
+				/* Mark status of the cur_frm to
+				 * done and unlock semaphore on it */
+				v4l2_get_timestamp(
+					&common->cur_frm->vb.timestamp);
+				vb2_buffer_done(&common->cur_frm->vb.vb2_buf,
+						VB2_BUF_STATE_DONE);
+				/* Make cur_frm pointing to next_frm */
+				common->cur_frm = common->next_frm;
+			}
+
+			channel_first_int[i][channel_id] = 0;
+			process_progressive_mode(common);
+		} else {
+			/* Interlaced mode */
+			/* If it is first interrupt, ignore it */
+
+			if (channel_first_int[i][channel_id]) {
+				channel_first_int[i][channel_id] = 0;
+				continue;
+			}
+
+			if (0 == i) {
+				ch->field_id ^= 1;
+				/* Get field id from VPIF registers */
+				fid = vpif_channel_getfid(ch->channel_id + 2);
+				/* If fid does not match with stored field id */
+				if (fid != ch->field_id) {
+					/* Make them in sync */
+					if (0 == fid)
+						ch->field_id = fid;
+
+					return IRQ_HANDLED;
+				}
+			}
+			process_interlaced_mode(fid, common);
+		}
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int vpif_update_std_info(struct channel_obj *ch)
+{
+	struct video_obj *vid_ch = &ch->video;
+	struct vpif_params *vpifparams = &ch->vpifparams;
+	struct vpif_channel_config_params *std_info = &vpifparams->std_info;
+	const struct vpif_channel_config_params *config;
+
+	int i;
+
+	for (i = 0; i < vpif_ch_params_count; i++) {
+		config = &vpif_ch_params[i];
+		if (config->hd_sd == 0) {
+			vpif_dbg(2, debug, "SD format\n");
+			if (config->stdid & vid_ch->stdid) {
+				memcpy(std_info, config, sizeof(*config));
+				break;
+			}
+		}
+	}
+
+	if (i == vpif_ch_params_count) {
+		vpif_dbg(1, debug, "Format not found\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int vpif_update_resolution(struct channel_obj *ch)
+{
+	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
+	struct video_obj *vid_ch = &ch->video;
+	struct vpif_params *vpifparams = &ch->vpifparams;
+	struct vpif_channel_config_params *std_info = &vpifparams->std_info;
+
+	if (!vid_ch->stdid && !vid_ch->dv_timings.bt.height)
+		return -EINVAL;
+
+	if (vid_ch->stdid) {
+		if (vpif_update_std_info(ch))
+			return -EINVAL;
+	}
+
+	common->fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUV422P;
+	common->fmt.fmt.pix.width = std_info->width;
+	common->fmt.fmt.pix.height = std_info->height;
+	vpif_dbg(1, debug, "Pixel details: Width = %d,Height = %d\n",
+			common->fmt.fmt.pix.width, common->fmt.fmt.pix.height);
+
+	/* Set height and width paramateres */
+	common->height = std_info->height;
+	common->width = std_info->width;
+	common->fmt.fmt.pix.sizeimage = common->height * common->width * 2;
+
+	if (vid_ch->stdid)
+		common->fmt.fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
+	else
+		common->fmt.fmt.pix.colorspace = V4L2_COLORSPACE_REC709;
+
+	if (ch->vpifparams.std_info.frm_fmt)
+		common->fmt.fmt.pix.field = V4L2_FIELD_NONE;
+	else
+		common->fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
+
+	return 0;
+}
+
+/*
+ * vpif_calculate_offsets: This function calculates buffers offset for Y and C
+ * in the top and bottom field
+ */
+static void vpif_calculate_offsets(struct channel_obj *ch)
+{
+	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
+	struct vpif_params *vpifparams = &ch->vpifparams;
+	enum v4l2_field field = common->fmt.fmt.pix.field;
+	struct video_obj *vid_ch = &ch->video;
+	unsigned int hpitch, sizeimage;
+
+	if (V4L2_FIELD_ANY == common->fmt.fmt.pix.field) {
+		if (ch->vpifparams.std_info.frm_fmt)
+			vid_ch->buf_field = V4L2_FIELD_NONE;
+		else
+			vid_ch->buf_field = V4L2_FIELD_INTERLACED;
+	} else {
+		vid_ch->buf_field = common->fmt.fmt.pix.field;
+	}
+
+	sizeimage = common->fmt.fmt.pix.sizeimage;
+
+	hpitch = common->fmt.fmt.pix.bytesperline;
+	if ((V4L2_FIELD_NONE == vid_ch->buf_field) ||
+	    (V4L2_FIELD_INTERLACED == vid_ch->buf_field)) {
+		common->ytop_off = 0;
+		common->ybtm_off = hpitch;
+		common->ctop_off = sizeimage / 2;
+		common->cbtm_off = sizeimage / 2 + hpitch;
+	} else if (V4L2_FIELD_SEQ_TB == vid_ch->buf_field) {
+		common->ytop_off = 0;
+		common->ybtm_off = sizeimage / 4;
+		common->ctop_off = sizeimage / 2;
+		common->cbtm_off = common->ctop_off + sizeimage / 4;
+	} else if (V4L2_FIELD_SEQ_BT == vid_ch->buf_field) {
+		common->ybtm_off = 0;
+		common->ytop_off = sizeimage / 4;
+		common->cbtm_off = sizeimage / 2;
+		common->ctop_off = common->cbtm_off + sizeimage / 4;
+	}
+
+	if ((V4L2_FIELD_NONE == vid_ch->buf_field) ||
+	    (V4L2_FIELD_INTERLACED == vid_ch->buf_field)) {
+		vpifparams->video_params.storage_mode = 1;
+	} else {
+		vpifparams->video_params.storage_mode = 0;
+	}
+
+	if (ch->vpifparams.std_info.frm_fmt == 1) {
+		vpifparams->video_params.hpitch =
+		    common->fmt.fmt.pix.bytesperline;
+	} else {
+		if ((field == V4L2_FIELD_ANY) ||
+			(field == V4L2_FIELD_INTERLACED))
+			vpifparams->video_params.hpitch =
+			    common->fmt.fmt.pix.bytesperline * 2;
+		else
+			vpifparams->video_params.hpitch =
+			    common->fmt.fmt.pix.bytesperline;
+	}
+
+	ch->vpifparams.video_params.stdid = ch->vpifparams.std_info.stdid;
+}
+
+static void vpif_config_addr(struct channel_obj *ch, int muxmode)
+{
+	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
+
+	if (VPIF_CHANNEL3_VIDEO == ch->channel_id) {
+		common->set_addr = ch3_set_videobuf_addr;
+	} else {
+		if (2 == muxmode)
+			common->set_addr = ch2_set_videobuf_addr_yc_nmux;
+		else
+			common->set_addr = ch2_set_videobuf_addr;
+	}
+}
+
+/* functions implementing ioctls */
+/**
+ * vpif_querycap() - QUERYCAP handler
+ * @file: file ptr
+ * @priv: file handle
+ * @cap: ptr to v4l2_capability structure
+ */
+static int vpif_querycap(struct file *file, void  *priv,
+				struct v4l2_capability *cap)
+{
+	struct vpif_display_config *config = vpif_dev->platform_data;
+
+	cap->device_caps = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	strlcpy(cap->driver, VPIF_DRIVER_NAME, sizeof(cap->driver));
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
+		 dev_name(vpif_dev));
+	strlcpy(cap->card, config->card_name, sizeof(cap->card));
+
+	return 0;
+}
+
+static int vpif_enum_fmt_vid_out(struct file *file, void  *priv,
+					struct v4l2_fmtdesc *fmt)
+{
+	if (fmt->index != 0)
+		return -EINVAL;
+
+	/* Fill in the information about format */
+	fmt->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	strcpy(fmt->description, "YCbCr4:2:2 YC Planar");
+	fmt->pixelformat = V4L2_PIX_FMT_YUV422P;
+	fmt->flags = 0;
+	return 0;
+}
+
+static int vpif_g_fmt_vid_out(struct file *file, void *priv,
+				struct v4l2_format *fmt)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
+	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
+
+	/* Check the validity of the buffer type */
+	if (common->fmt.type != fmt->type)
+		return -EINVAL;
+
+	if (vpif_update_resolution(ch))
+		return -EINVAL;
+	*fmt = common->fmt;
+	return 0;
+}
+
+static int vpif_try_fmt_vid_out(struct file *file, void *priv,
+				struct v4l2_format *fmt)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
+	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
+	struct v4l2_pix_format *pixfmt = &fmt->fmt.pix;
+
+	/*
+	 * to supress v4l-compliance warnings silently correct
+	 * the pixelformat
+	 */
+	if (pixfmt->pixelformat != V4L2_PIX_FMT_YUV422P)
+		pixfmt->pixelformat = common->fmt.fmt.pix.pixelformat;
+
+	if (vpif_update_resolution(ch))
+		return -EINVAL;
+
+	pixfmt->colorspace = common->fmt.fmt.pix.colorspace;
+	pixfmt->field = common->fmt.fmt.pix.field;
+	pixfmt->bytesperline = common->fmt.fmt.pix.width;
+	pixfmt->width = common->fmt.fmt.pix.width;
+	pixfmt->height = common->fmt.fmt.pix.height;
+	pixfmt->sizeimage = pixfmt->bytesperline * pixfmt->height * 2;
+
+	return 0;
+}
+
+static int vpif_s_fmt_vid_out(struct file *file, void *priv,
+				struct v4l2_format *fmt)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
+	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
+	struct v4l2_pix_format *pixfmt = &fmt->fmt.pix;
+	int ret;
+
+	if (vb2_is_busy(&common->buffer_queue))
+		return -EBUSY;
+
+	ret = vpif_try_fmt_vid_out(file, priv, fmt);
+	if (ret)
+		return ret;
+
+	/* store the pix format in the channel object */
+	common->fmt.fmt.pix = *pixfmt;
+
+	/* store the format in the channel object */
+	common->fmt = *fmt;
+	return 0;
+}
+
+static int vpif_s_std(struct file *file, void *priv, v4l2_std_id std_id)
+{
+	struct vpif_display_config *config = vpif_dev->platform_data;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
+	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
+	struct vpif_display_chan_config *chan_cfg;
+	struct v4l2_output output;
+	int ret;
+
+	if (config->chan_config[ch->channel_id].outputs == NULL)
+		return -ENODATA;
+
+	chan_cfg = &config->chan_config[ch->channel_id];
+	output = chan_cfg->outputs[ch->output_idx].output;
+	if (output.capabilities != V4L2_OUT_CAP_STD)
+		return -ENODATA;
+
+	if (vb2_is_busy(&common->buffer_queue))
+		return -EBUSY;
+
+
+	if (!(std_id & VPIF_V4L2_STD))
+		return -EINVAL;
+
+	/* Call encoder subdevice function to set the standard */
+	ch->video.stdid = std_id;
+	memset(&ch->video.dv_timings, 0, sizeof(ch->video.dv_timings));
+	/* Get the information about the standard */
+	if (vpif_update_resolution(ch))
+		return -EINVAL;
+
+	common->fmt.fmt.pix.bytesperline = common->fmt.fmt.pix.width;
+
+	ret = v4l2_device_call_until_err(&vpif_obj.v4l2_dev, 1, video,
+						s_std_output, std_id);
+	if (ret < 0) {
+		vpif_err("Failed to set output standard\n");
+		return ret;
+	}
+
+	ret = v4l2_device_call_until_err(&vpif_obj.v4l2_dev, 1, video,
+							s_std, std_id);
+	if (ret < 0)
+		vpif_err("Failed to set standard for sub devices\n");
+	return ret;
+}
+
+static int vpif_g_std(struct file *file, void *priv, v4l2_std_id *std)
+{
+	struct vpif_display_config *config = vpif_dev->platform_data;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
+	struct vpif_display_chan_config *chan_cfg;
+	struct v4l2_output output;
+
+	if (config->chan_config[ch->channel_id].outputs == NULL)
+		return -ENODATA;
+
+	chan_cfg = &config->chan_config[ch->channel_id];
+	output = chan_cfg->outputs[ch->output_idx].output;
+	if (output.capabilities != V4L2_OUT_CAP_STD)
+		return -ENODATA;
+
+	*std = ch->video.stdid;
+	return 0;
+}
+
+static int vpif_enum_output(struct file *file, void *fh,
+				struct v4l2_output *output)
+{
+
+	struct vpif_display_config *config = vpif_dev->platform_data;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
+	struct vpif_display_chan_config *chan_cfg;
+
+	chan_cfg = &config->chan_config[ch->channel_id];
+	if (output->index >= chan_cfg->output_count) {
+		vpif_dbg(1, debug, "Invalid output index\n");
+		return -EINVAL;
+	}
+
+	*output = chan_cfg->outputs[output->index].output;
+	return 0;
+}
+
+/**
+ * vpif_output_to_subdev() - Maps output to sub device
+ * @vpif_cfg - global config ptr
+ * @chan_cfg - channel config ptr
+ * @index - Given output index from application
+ *
+ * lookup the sub device information for a given output index.
+ * we report all the output to application. output table also
+ * has sub device name for the each output
+ */
+static int
+vpif_output_to_subdev(struct vpif_display_config *vpif_cfg,
+		      struct vpif_display_chan_config *chan_cfg, int index)
+{
+	struct vpif_subdev_info *subdev_info;
+	const char *subdev_name;
+	int i;
+
+	vpif_dbg(2, debug, "vpif_output_to_subdev\n");
+
+	if (chan_cfg->outputs == NULL)
+		return -1;
+
+	subdev_name = chan_cfg->outputs[index].subdev_name;
+	if (subdev_name == NULL)
+		return -1;
+
+	/* loop through the sub device list to get the sub device info */
+	for (i = 0; i < vpif_cfg->subdev_count; i++) {
+		subdev_info = &vpif_cfg->subdevinfo[i];
+		if (!strcmp(subdev_info->name, subdev_name))
+			return i;
+	}
+	return -1;
+}
+
+/**
+ * vpif_set_output() - Select an output
+ * @vpif_cfg - global config ptr
+ * @ch - channel
+ * @index - Given output index from application
+ *
+ * Select the given output.
+ */
+static int vpif_set_output(struct vpif_display_config *vpif_cfg,
+		      struct channel_obj *ch, int index)
+{
+	struct vpif_display_chan_config *chan_cfg =
+		&vpif_cfg->chan_config[ch->channel_id];
+	struct v4l2_subdev *sd = NULL;
+	u32 input = 0, output = 0;
+	int sd_index;
+	int ret;
+
+	sd_index = vpif_output_to_subdev(vpif_cfg, chan_cfg, index);
+	if (sd_index >= 0)
+		sd = vpif_obj.sd[sd_index];
+
+	if (sd) {
+		input = chan_cfg->outputs[index].input_route;
+		output = chan_cfg->outputs[index].output_route;
+		ret = v4l2_subdev_call(sd, video, s_routing, input, output, 0);
+		if (ret < 0 && ret != -ENOIOCTLCMD) {
+			vpif_err("Failed to set output\n");
+			return ret;
+		}
+
+	}
+	ch->output_idx = index;
+	ch->sd = sd;
+	if (chan_cfg->outputs != NULL)
+		/* update tvnorms from the sub device output info */
+		ch->video_dev.tvnorms = chan_cfg->outputs[index].output.std;
+	return 0;
+}
+
+static int vpif_s_output(struct file *file, void *priv, unsigned int i)
+{
+	struct vpif_display_config *config = vpif_dev->platform_data;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
+	struct vpif_display_chan_config *chan_cfg;
+	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
+
+	if (vb2_is_busy(&common->buffer_queue))
+		return -EBUSY;
+
+	chan_cfg = &config->chan_config[ch->channel_id];
+
+	if (i >= chan_cfg->output_count)
+		return -EINVAL;
+
+	return vpif_set_output(config, ch, i);
+}
+
+static int vpif_g_output(struct file *file, void *priv, unsigned int *i)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
+
+	*i = ch->output_idx;
+
+	return 0;
+}
+
+/**
+ * vpif_enum_dv_timings() - ENUM_DV_TIMINGS handler
+ * @file: file ptr
+ * @priv: file handle
+ * @timings: input timings
+ */
+static int
+vpif_enum_dv_timings(struct file *file, void *priv,
+		     struct v4l2_enum_dv_timings *timings)
+{
+	struct vpif_display_config *config = vpif_dev->platform_data;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
+	struct vpif_display_chan_config *chan_cfg;
+	struct v4l2_output output;
+	int ret;
+
+	if (config->chan_config[ch->channel_id].outputs == NULL)
+		return -ENODATA;
+
+	chan_cfg = &config->chan_config[ch->channel_id];
+	output = chan_cfg->outputs[ch->output_idx].output;
+	if (output.capabilities != V4L2_OUT_CAP_DV_TIMINGS)
+		return -ENODATA;
+
+	timings->pad = 0;
+
+	ret = v4l2_subdev_call(ch->sd, pad, enum_dv_timings, timings);
+	if (ret == -ENOIOCTLCMD || ret == -ENODEV)
+		return -EINVAL;
+	return ret;
+}
+
+/**
+ * vpif_s_dv_timings() - S_DV_TIMINGS handler
+ * @file: file ptr
+ * @priv: file handle
+ * @timings: digital video timings
+ */
+static int vpif_s_dv_timings(struct file *file, void *priv,
+		struct v4l2_dv_timings *timings)
+{
+	struct vpif_display_config *config = vpif_dev->platform_data;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
+	struct vpif_params *vpifparams = &ch->vpifparams;
+	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
+	struct vpif_channel_config_params *std_info = &vpifparams->std_info;
+	struct video_obj *vid_ch = &ch->video;
+	struct v4l2_bt_timings *bt = &vid_ch->dv_timings.bt;
+	struct vpif_display_chan_config *chan_cfg;
+	struct v4l2_output output;
+	int ret;
+
+	if (config->chan_config[ch->channel_id].outputs == NULL)
+		return -ENODATA;
+
+	chan_cfg = &config->chan_config[ch->channel_id];
+	output = chan_cfg->outputs[ch->output_idx].output;
+	if (output.capabilities != V4L2_OUT_CAP_DV_TIMINGS)
+		return -ENODATA;
+
+	if (vb2_is_busy(&common->buffer_queue))
+		return -EBUSY;
+
+	if (timings->type != V4L2_DV_BT_656_1120) {
+		vpif_dbg(2, debug, "Timing type not defined\n");
+		return -EINVAL;
+	}
+
+	/* Configure subdevice timings, if any */
+	ret = v4l2_subdev_call(ch->sd, video, s_dv_timings, timings);
+	if (ret == -ENOIOCTLCMD || ret == -ENODEV)
+		ret = 0;
+	if (ret < 0) {
+		vpif_dbg(2, debug, "Error setting custom DV timings\n");
+		return ret;
+	}
+
+	if (!(timings->bt.width && timings->bt.height &&
+				(timings->bt.hbackporch ||
+				 timings->bt.hfrontporch ||
+				 timings->bt.hsync) &&
+				timings->bt.vfrontporch &&
+				(timings->bt.vbackporch ||
+				 timings->bt.vsync))) {
+		vpif_dbg(2, debug, "Timings for width, height, "
+				"horizontal back porch, horizontal sync, "
+				"horizontal front porch, vertical back porch, "
+				"vertical sync and vertical back porch "
+				"must be defined\n");
+		return -EINVAL;
+	}
+
+	vid_ch->dv_timings = *timings;
+
+	/* Configure video port timings */
+
+	std_info->eav2sav = V4L2_DV_BT_BLANKING_WIDTH(bt) - 8;
+	std_info->sav2eav = bt->width;
+
+	std_info->l1 = 1;
+	std_info->l3 = bt->vsync + bt->vbackporch + 1;
+
+	std_info->vsize = V4L2_DV_BT_FRAME_HEIGHT(bt);
+	if (bt->interlaced) {
+		if (bt->il_vbackporch || bt->il_vfrontporch || bt->il_vsync) {
+			std_info->l5 = std_info->vsize/2 -
+				(bt->vfrontporch - 1);
+			std_info->l7 = std_info->vsize/2 + 1;
+			std_info->l9 = std_info->l7 + bt->il_vsync +
+				bt->il_vbackporch + 1;
+			std_info->l11 = std_info->vsize -
+				(bt->il_vfrontporch - 1);
+		} else {
+			vpif_dbg(2, debug, "Required timing values for "
+					"interlaced BT format missing\n");
+			return -EINVAL;
+		}
+	} else {
+		std_info->l5 = std_info->vsize - (bt->vfrontporch - 1);
+	}
+	strncpy(std_info->name, "Custom timings BT656/1120",
+			VPIF_MAX_NAME);
+	std_info->width = bt->width;
+	std_info->height = bt->height;
+	std_info->frm_fmt = bt->interlaced ? 0 : 1;
+	std_info->ycmux_mode = 0;
+	std_info->capture_format = 0;
+	std_info->vbi_supported = 0;
+	std_info->hd_sd = 1;
+	std_info->stdid = 0;
+	vid_ch->stdid = 0;
+
+	return 0;
+}
+
+/**
+ * vpif_g_dv_timings() - G_DV_TIMINGS handler
+ * @file: file ptr
+ * @priv: file handle
+ * @timings: digital video timings
+ */
+static int vpif_g_dv_timings(struct file *file, void *priv,
+		struct v4l2_dv_timings *timings)
+{
+	struct vpif_display_config *config = vpif_dev->platform_data;
+	struct video_device *vdev = video_devdata(file);
+	struct channel_obj *ch = video_get_drvdata(vdev);
+	struct vpif_display_chan_config *chan_cfg;
+	struct video_obj *vid_ch = &ch->video;
+	struct v4l2_output output;
+
+	if (config->chan_config[ch->channel_id].outputs == NULL)
+		goto error;
+
+	chan_cfg = &config->chan_config[ch->channel_id];
+	output = chan_cfg->outputs[ch->output_idx].output;
+
+	if (output.capabilities != V4L2_OUT_CAP_DV_TIMINGS)
+		goto error;
+
+	*timings = vid_ch->dv_timings;
+
+	return 0;
+error:
+	return -ENODATA;
+}
+
+/*
+ * vpif_log_status() - Status information
+ * @file: file ptr
+ * @priv: file handle
+ *
+ * Returns zero.
+ */
+static int vpif_log_status(struct file *filep, void *priv)
+{
+	/* status for sub devices */
+	v4l2_device_call_all(&vpif_obj.v4l2_dev, 0, core, log_status);
+
+	return 0;
+}
+
+/* vpif display ioctl operations */
+static const struct v4l2_ioctl_ops vpif_ioctl_ops = {
+	.vidioc_querycap		= vpif_querycap,
+	.vidioc_enum_fmt_vid_out	= vpif_enum_fmt_vid_out,
+	.vidioc_g_fmt_vid_out		= vpif_g_fmt_vid_out,
+	.vidioc_s_fmt_vid_out		= vpif_s_fmt_vid_out,
+	.vidioc_try_fmt_vid_out		= vpif_try_fmt_vid_out,
+
+	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
+	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
+	.vidioc_querybuf		= vb2_ioctl_querybuf,
+	.vidioc_qbuf			= vb2_ioctl_qbuf,
+	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
+	.vidioc_expbuf			= vb2_ioctl_expbuf,
+	.vidioc_streamon		= vb2_ioctl_streamon,
+	.vidioc_streamoff		= vb2_ioctl_streamoff,
+
+	.vidioc_s_std			= vpif_s_std,
+	.vidioc_g_std			= vpif_g_std,
+
+	.vidioc_enum_output		= vpif_enum_output,
+	.vidioc_s_output		= vpif_s_output,
+	.vidioc_g_output		= vpif_g_output,
+
+	.vidioc_enum_dv_timings		= vpif_enum_dv_timings,
+	.vidioc_s_dv_timings		= vpif_s_dv_timings,
+	.vidioc_g_dv_timings		= vpif_g_dv_timings,
+
+	.vidioc_log_status		= vpif_log_status,
+};
+
+static const struct v4l2_file_operations vpif_fops = {
+	.owner		= THIS_MODULE,
+	.open		= v4l2_fh_open,
+	.release	= vb2_fop_release,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap		= vb2_fop_mmap,
+	.poll		= vb2_fop_poll
+};
+
+/*Configure the channels, buffer sizei, request irq */
+static int initialize_vpif(void)
+{
+	int free_channel_objects_index;
+	int err, i, j;
+
+	/* Allocate memory for six channel objects */
+	for (i = 0; i < VPIF_DISPLAY_MAX_DEVICES; i++) {
+		vpif_obj.dev[i] =
+		    kzalloc(sizeof(struct channel_obj), GFP_KERNEL);
+		/* If memory allocation fails, return error */
+		if (!vpif_obj.dev[i]) {
+			free_channel_objects_index = i;
+			err = -ENOMEM;
+			goto vpif_init_free_channel_objects;
+		}
+	}
+
+	return 0;
+
+vpif_init_free_channel_objects:
+	for (j = 0; j < free_channel_objects_index; j++)
+		kfree(vpif_obj.dev[j]);
+	return err;
+}
+
+static int vpif_async_bound(struct v4l2_async_notifier *notifier,
+			    struct v4l2_subdev *subdev,
+			    struct v4l2_async_subdev *asd)
+{
+	int i;
+
+	for (i = 0; i < vpif_obj.config->subdev_count; i++)
+		if (!strcmp(vpif_obj.config->subdevinfo[i].name,
+			    subdev->name)) {
+			vpif_obj.sd[i] = subdev;
+			vpif_obj.sd[i]->grp_id = 1 << i;
+			return 0;
+		}
+
+	return -EINVAL;
+}
+
+static int vpif_probe_complete(void)
+{
+	struct common_obj *common;
+	struct video_device *vdev;
+	struct channel_obj *ch;
+	struct vb2_queue *q;
+	int j, err, k;
+
+	for (j = 0; j < VPIF_DISPLAY_MAX_DEVICES; j++) {
+		ch = vpif_obj.dev[j];
+		/* Initialize field of the channel objects */
+		for (k = 0; k < VPIF_NUMOBJECTS; k++) {
+			common = &ch->common[k];
+			spin_lock_init(&common->irqlock);
+			mutex_init(&common->lock);
+			common->set_addr = NULL;
+			common->ytop_off = 0;
+			common->ybtm_off = 0;
+			common->ctop_off = 0;
+			common->cbtm_off = 0;
+			common->cur_frm = NULL;
+			common->next_frm = NULL;
+			memset(&common->fmt, 0, sizeof(common->fmt));
+		}
+		ch->initialized = 0;
+		if (vpif_obj.config->subdev_count)
+			ch->sd = vpif_obj.sd[0];
+		ch->channel_id = j;
+
+		memset(&ch->vpifparams, 0, sizeof(ch->vpifparams));
+
+		ch->common[VPIF_VIDEO_INDEX].fmt.type =
+						V4L2_BUF_TYPE_VIDEO_OUTPUT;
+
+		/* select output 0 */
+		err = vpif_set_output(vpif_obj.config, ch, 0);
+		if (err)
+			goto probe_out;
+
+		/* set initial format */
+		ch->video.stdid = V4L2_STD_525_60;
+		memset(&ch->video.dv_timings, 0, sizeof(ch->video.dv_timings));
+		vpif_update_resolution(ch);
+
+		/* Initialize vb2 queue */
+		q = &common->buffer_queue;
+		q->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+		q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
+		q->drv_priv = ch;
+		q->ops = &video_qops;
+		q->mem_ops = &vb2_dma_contig_memops;
+		q->buf_struct_size = sizeof(struct vpif_disp_buffer);
+		q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+		q->min_buffers_needed = 1;
+		q->lock = &common->lock;
+		err = vb2_queue_init(q);
+		if (err) {
+			vpif_err("vpif_display: vb2_queue_init() failed\n");
+			goto probe_out;
+		}
+
+		common->alloc_ctx = vb2_dma_contig_init_ctx(vpif_dev);
+		if (IS_ERR(common->alloc_ctx)) {
+			vpif_err("Failed to get the context\n");
+			err = PTR_ERR(common->alloc_ctx);
+			goto probe_out;
+		}
+
+		INIT_LIST_HEAD(&common->dma_queue);
+
+		/* register video device */
+		vpif_dbg(1, debug, "channel=%p,channel->video_dev=%p\n",
+			 ch, &ch->video_dev);
+
+		/* Initialize the video_device structure */
+		vdev = &ch->video_dev;
+		strlcpy(vdev->name, VPIF_DRIVER_NAME, sizeof(vdev->name));
+		vdev->release = video_device_release_empty;
+		vdev->fops = &vpif_fops;
+		vdev->ioctl_ops = &vpif_ioctl_ops;
+		vdev->v4l2_dev = &vpif_obj.v4l2_dev;
+		vdev->vfl_dir = VFL_DIR_TX;
+		vdev->queue = q;
+		vdev->lock = &common->lock;
+		video_set_drvdata(&ch->video_dev, ch);
+		err = video_register_device(vdev, VFL_TYPE_GRABBER,
+					    (j ? 3 : 2));
+		if (err < 0)
+			goto probe_out;
+	}
+
+	return 0;
+
+probe_out:
+	for (k = 0; k < j; k++) {
+		ch = vpif_obj.dev[k];
+		common = &ch->common[k];
+		vb2_dma_contig_cleanup_ctx(common->alloc_ctx);
+		video_unregister_device(&ch->video_dev);
+	}
+	return err;
+}
+
+static int vpif_async_complete(struct v4l2_async_notifier *notifier)
+{
+	return vpif_probe_complete();
+}
+
+/*
+ * vpif_probe: This function creates device entries by register itself to the
+ * V4L2 driver and initializes fields of each channel objects
+ */
+static __init int vpif_probe(struct platform_device *pdev)
+{
+	struct vpif_subdev_info *subdevdata;
+	struct i2c_adapter *i2c_adap;
+	struct resource *res;
+	int subdev_count;
+	int res_idx = 0;
+	int i, err;
+
+	vpif_dev = &pdev->dev;
+	err = initialize_vpif();
+
+	if (err) {
+		v4l2_err(vpif_dev->driver, "Error initializing vpif\n");
+		return err;
+	}
+
+	err = v4l2_device_register(vpif_dev, &vpif_obj.v4l2_dev);
+	if (err) {
+		v4l2_err(vpif_dev->driver, "Error registering v4l2 device\n");
+		return err;
+	}
+
+	while ((res = platform_get_resource(pdev, IORESOURCE_IRQ, res_idx))) {
+		err = devm_request_irq(&pdev->dev, res->start, vpif_channel_isr,
+					IRQF_SHARED, VPIF_DRIVER_NAME,
+					(void *)(&vpif_obj.dev[res_idx]->
+					channel_id));
+		if (err) {
+			err = -EINVAL;
+			vpif_err("VPIF IRQ request failed\n");
+			goto vpif_unregister;
+		}
+		res_idx++;
+	}
+
+	vpif_obj.config = pdev->dev.platform_data;
+	subdev_count = vpif_obj.config->subdev_count;
+	subdevdata = vpif_obj.config->subdevinfo;
+	vpif_obj.sd = kzalloc(sizeof(struct v4l2_subdev *) * subdev_count,
+								GFP_KERNEL);
+	if (vpif_obj.sd == NULL) {
+		vpif_err("unable to allocate memory for subdevice pointers\n");
+		err = -ENOMEM;
+		goto vpif_unregister;
+	}
+
+	if (!vpif_obj.config->asd_sizes) {
+		i2c_adap = i2c_get_adapter(1);
+		for (i = 0; i < subdev_count; i++) {
+			vpif_obj.sd[i] =
+				v4l2_i2c_new_subdev_board(&vpif_obj.v4l2_dev,
+							  i2c_adap,
+							  &subdevdata[i].
+							  board_info,
+							  NULL);
+			if (!vpif_obj.sd[i]) {
+				vpif_err("Error registering v4l2 subdevice\n");
+				err = -ENODEV;
+				goto probe_subdev_out;
+			}
+
+			if (vpif_obj.sd[i])
+				vpif_obj.sd[i]->grp_id = 1 << i;
+		}
+		vpif_probe_complete();
+	} else {
+		vpif_obj.notifier.subdevs = vpif_obj.config->asd;
+		vpif_obj.notifier.num_subdevs = vpif_obj.config->asd_sizes[0];
+		vpif_obj.notifier.bound = vpif_async_bound;
+		vpif_obj.notifier.complete = vpif_async_complete;
+		err = v4l2_async_notifier_register(&vpif_obj.v4l2_dev,
+						   &vpif_obj.notifier);
+		if (err) {
+			vpif_err("Error registering async notifier\n");
+			err = -EINVAL;
+			goto probe_subdev_out;
+		}
+	}
+
+	return 0;
+
+probe_subdev_out:
+	kfree(vpif_obj.sd);
+vpif_unregister:
+	v4l2_device_unregister(&vpif_obj.v4l2_dev);
+
+	return err;
+}
+
+/*
+ * vpif_remove: It un-register channels from V4L2 driver
+ */
+static int vpif_remove(struct platform_device *device)
+{
+	struct common_obj *common;
+	struct channel_obj *ch;
+	int i;
+
+	v4l2_device_unregister(&vpif_obj.v4l2_dev);
+
+	kfree(vpif_obj.sd);
+	/* un-register device */
+	for (i = 0; i < VPIF_DISPLAY_MAX_DEVICES; i++) {
+		/* Get the pointer to the channel object */
+		ch = vpif_obj.dev[i];
+		common = &ch->common[VPIF_VIDEO_INDEX];
+		vb2_dma_contig_cleanup_ctx(common->alloc_ctx);
+		/* Unregister video device */
+		video_unregister_device(&ch->video_dev);
+		kfree(vpif_obj.dev[i]);
+	}
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int vpif_suspend(struct device *dev)
+{
+	struct common_obj *common;
+	struct channel_obj *ch;
+	int i;
+
+	for (i = 0; i < VPIF_DISPLAY_MAX_DEVICES; i++) {
+		/* Get the pointer to the channel object */
+		ch = vpif_obj.dev[i];
+		common = &ch->common[VPIF_VIDEO_INDEX];
+
+		if (!vb2_start_streaming_called(&common->buffer_queue))
+			continue;
+
+		mutex_lock(&common->lock);
+		/* Disable channel */
+		if (ch->channel_id == VPIF_CHANNEL2_VIDEO) {
+			enable_channel2(0);
+			channel2_intr_enable(0);
+		}
+		if (ch->channel_id == VPIF_CHANNEL3_VIDEO ||
+			ycmux_mode == 2) {
+			enable_channel3(0);
+			channel3_intr_enable(0);
+		}
+		mutex_unlock(&common->lock);
+	}
+
+	return 0;
+}
+
+static int vpif_resume(struct device *dev)
+{
+
+	struct common_obj *common;
+	struct channel_obj *ch;
+	int i;
+
+	for (i = 0; i < VPIF_DISPLAY_MAX_DEVICES; i++) {
+		/* Get the pointer to the channel object */
+		ch = vpif_obj.dev[i];
+		common = &ch->common[VPIF_VIDEO_INDEX];
+
+		if (!vb2_start_streaming_called(&common->buffer_queue))
+			continue;
+
+		mutex_lock(&common->lock);
+		/* Enable channel */
+		if (ch->channel_id == VPIF_CHANNEL2_VIDEO) {
+			enable_channel2(1);
+			channel2_intr_enable(1);
+		}
+		if (ch->channel_id == VPIF_CHANNEL3_VIDEO ||
+				ycmux_mode == 2) {
+			enable_channel3(1);
+			channel3_intr_enable(1);
+		}
+		mutex_unlock(&common->lock);
+	}
+
+	return 0;
+}
+
+#endif
+
+static SIMPLE_DEV_PM_OPS(vpif_pm_ops, vpif_suspend, vpif_resume);
+
+static __refdata struct platform_driver vpif_driver = {
+	.driver	= {
+			.name	= VPIF_DRIVER_NAME,
+			.pm	= &vpif_pm_ops,
+	},
+	.probe	= vpif_probe,
+	.remove	= vpif_remove,
+};
+
+module_platform_driver(vpif_driver);
diff --git a/drivers/media/platform/davinci/vpif_display.h b/drivers/media/platform/davinci/vpif_display.h
new file mode 100644
index 0000000..e7a1723
--- /dev/null
+++ b/drivers/media/platform/davinci/vpif_display.h
@@ -0,0 +1,127 @@
+/*
+ * VPIF display header file
+ *
+ * Copyright (C) 2009 Texas Instruments Incorporated - http://www.ti.com/
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed .as is. WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef VPIF_DISPLAY_H
+#define VPIF_DISPLAY_H
+
+/* Header files */
+#include <media/videobuf2-dma-contig.h>
+#include <media/v4l2-device.h>
+
+#include "vpif.h"
+
+/* Macros */
+#define VPIF_DISPLAY_VERSION	"0.0.2"
+
+#define VPIF_VALID_FIELD(field) \
+	(((V4L2_FIELD_ANY == field) || (V4L2_FIELD_NONE == field)) || \
+	(((V4L2_FIELD_INTERLACED == field) || (V4L2_FIELD_SEQ_TB == field)) || \
+	(V4L2_FIELD_SEQ_BT == field)))
+
+#define VPIF_DISPLAY_MAX_DEVICES	(2)
+#define VPIF_SLICED_BUF_SIZE		(256)
+#define VPIF_SLICED_MAX_SERVICES	(3)
+#define VPIF_VIDEO_INDEX		(0)
+#define VPIF_VBI_INDEX			(1)
+#define VPIF_HBI_INDEX			(2)
+
+/* Setting it to 1 as HBI/VBI support yet to be added , else 3*/
+#define VPIF_NUMOBJECTS	(1)
+
+/* Macros */
+#define ISALIGNED(a)    (0 == ((a) & 7))
+
+/* enumerated data types */
+/* Enumerated data type to give id to each device per channel */
+enum vpif_channel_id {
+	VPIF_CHANNEL2_VIDEO = 0,	/* Channel2 Video */
+	VPIF_CHANNEL3_VIDEO,		/* Channel3 Video */
+};
+
+/* structures */
+
+struct video_obj {
+	enum v4l2_field buf_field;
+	u32 latest_only;		/* indicate whether to return
+					 * most recent displayed frame only */
+	v4l2_std_id stdid;		/* Currently selected or default
+					 * standard */
+	struct v4l2_dv_timings dv_timings;
+};
+
+struct vpif_disp_buffer {
+	struct vb2_v4l2_buffer vb;
+	struct list_head list;
+};
+
+struct common_obj {
+	struct vpif_disp_buffer *cur_frm;	/* Pointer pointing to current
+						 * vb2_buffer */
+	struct vpif_disp_buffer *next_frm;	/* Pointer pointing to next
+						 * vb2_buffer */
+	struct v4l2_format fmt;			/* Used to store the format */
+	struct vb2_queue buffer_queue;		/* Buffer queue used in
+						 * video-buf */
+	/* allocator-specific contexts for each plane */
+	struct vb2_alloc_ctx *alloc_ctx;
+
+	struct list_head dma_queue;		/* Queue of filled frames */
+	spinlock_t irqlock;			/* Used in video-buf */
+
+	/* channel specific parameters */
+	struct mutex lock;			/* lock used to access this
+						 * structure */
+	u32 ytop_off;				/* offset of Y top from the
+						 * starting of the buffer */
+	u32 ybtm_off;				/* offset of Y bottom from the
+						 * starting of the buffer */
+	u32 ctop_off;				/* offset of C top from the
+						 * starting of the buffer */
+	u32 cbtm_off;				/* offset of C bottom from the
+						 * starting of the buffer */
+	/* Function pointer to set the addresses */
+	void (*set_addr)(unsigned long, unsigned long,
+				unsigned long, unsigned long);
+	u32 height;
+	u32 width;
+};
+
+struct channel_obj {
+	/* V4l2 specific parameters */
+	struct video_device video_dev;	/* Identifies video device for
+					 * this channel */
+	u32 field_id;			/* Indicates id of the field
+					 * which is being displayed */
+	u8 initialized;			/* flag to indicate whether
+					 * encoder is initialized */
+	u32 output_idx;			/* Current output index */
+	struct v4l2_subdev *sd;		/* Current output subdev(may be NULL) */
+
+	enum vpif_channel_id channel_id;/* Identifies channel */
+	struct vpif_params vpifparams;
+	struct common_obj common[VPIF_NUMOBJECTS];
+	struct video_obj video;
+};
+
+/* vpif device structure */
+struct vpif_device {
+	struct v4l2_device v4l2_dev;
+	struct channel_obj *dev[VPIF_DISPLAY_NUM_CHANNELS];
+	struct v4l2_subdev **sd;
+	struct v4l2_async_notifier notifier;
+	struct vpif_display_config *config;
+};
+
+#endif				/* VPIF_DISPLAY_H */
diff --git a/drivers/media/platform/davinci/vpss.c b/drivers/media/platform/davinci/vpss.c
new file mode 100644
index 0000000..fce86f1
--- /dev/null
+++ b/drivers/media/platform/davinci/vpss.c
@@ -0,0 +1,532 @@
+/*
+ * Copyright (C) 2009 Texas Instruments.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ * common vpss system module platform driver for all video drivers.
+ */
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/io.h>
+#include <linux/pm_runtime.h>
+#include <linux/err.h>
+
+#include <media/davinci/vpss.h>
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("VPSS Driver");
+MODULE_AUTHOR("Texas Instruments");
+
+/* DM644x defines */
+#define DM644X_SBL_PCR_VPSS		(4)
+
+#define DM355_VPSSBL_INTSEL		0x10
+#define DM355_VPSSBL_EVTSEL		0x14
+/* vpss BL register offsets */
+#define DM355_VPSSBL_CCDCMUX		0x1c
+/* vpss CLK register offsets */
+#define DM355_VPSSCLK_CLKCTRL		0x04
+/* masks and shifts */
+#define VPSS_HSSISEL_SHIFT		4
+/*
+ * VDINT0 - vpss_int0, VDINT1 - vpss_int1, H3A - vpss_int4,
+ * IPIPE_INT1_SDR - vpss_int5
+ */
+#define DM355_VPSSBL_INTSEL_DEFAULT	0xff83ff10
+/* VENCINT - vpss_int8 */
+#define DM355_VPSSBL_EVTSEL_DEFAULT	0x4
+
+#define DM365_ISP5_PCCR				0x04
+#define DM365_ISP5_PCCR_BL_CLK_ENABLE		BIT(0)
+#define DM365_ISP5_PCCR_ISIF_CLK_ENABLE		BIT(1)
+#define DM365_ISP5_PCCR_H3A_CLK_ENABLE		BIT(2)
+#define DM365_ISP5_PCCR_RSZ_CLK_ENABLE		BIT(3)
+#define DM365_ISP5_PCCR_IPIPE_CLK_ENABLE	BIT(4)
+#define DM365_ISP5_PCCR_IPIPEIF_CLK_ENABLE	BIT(5)
+#define DM365_ISP5_PCCR_RSV			BIT(6)
+
+#define DM365_ISP5_BCR			0x08
+#define DM365_ISP5_BCR_ISIF_OUT_ENABLE	BIT(1)
+
+#define DM365_ISP5_INTSEL1		0x10
+#define DM365_ISP5_INTSEL2		0x14
+#define DM365_ISP5_INTSEL3		0x18
+#define DM365_ISP5_CCDCMUX 		0x20
+#define DM365_ISP5_PG_FRAME_SIZE 	0x28
+#define DM365_VPBE_CLK_CTRL 		0x00
+
+#define VPSS_CLK_CTRL			0x01c40044
+#define VPSS_CLK_CTRL_VENCCLKEN		BIT(3)
+#define VPSS_CLK_CTRL_DACCLKEN		BIT(4)
+
+/*
+ * vpss interrupts. VDINT0 - vpss_int0, VDINT1 - vpss_int1,
+ * AF - vpss_int3
+ */
+#define DM365_ISP5_INTSEL1_DEFAULT	0x0b1f0100
+/* AEW - vpss_int6, RSZ_INT_DMA - vpss_int5 */
+#define DM365_ISP5_INTSEL2_DEFAULT	0x1f0a0f1f
+/* VENC - vpss_int8 */
+#define DM365_ISP5_INTSEL3_DEFAULT	0x00000015
+
+/* masks and shifts for DM365*/
+#define DM365_CCDC_PG_VD_POL_SHIFT 	0
+#define DM365_CCDC_PG_HD_POL_SHIFT 	1
+
+#define CCD_SRC_SEL_MASK		(BIT_MASK(5) | BIT_MASK(4))
+#define CCD_SRC_SEL_SHIFT		4
+
+/* Different SoC platforms supported by this driver */
+enum vpss_platform_type {
+	DM644X,
+	DM355,
+	DM365,
+};
+
+/*
+ * vpss operations. Depends on platform. Not all functions are available
+ * on all platforms. The api, first check if a function is available before
+ * invoking it. In the probe, the function ptrs are initialized based on
+ * vpss name. vpss name can be "dm355_vpss", "dm644x_vpss" etc.
+ */
+struct vpss_hw_ops {
+	/* enable clock */
+	int (*enable_clock)(enum vpss_clock_sel clock_sel, int en);
+	/* select input to ccdc */
+	void (*select_ccdc_source)(enum vpss_ccdc_source_sel src_sel);
+	/* clear wbl overflow bit */
+	int (*clear_wbl_overflow)(enum vpss_wbl_sel wbl_sel);
+	/* set sync polarity */
+	void (*set_sync_pol)(struct vpss_sync_pol);
+	/* set the PG_FRAME_SIZE register*/
+	void (*set_pg_frame_size)(struct vpss_pg_frame_size);
+	/* check and clear interrupt if occurred */
+	int (*dma_complete_interrupt)(void);
+};
+
+/* vpss configuration */
+struct vpss_oper_config {
+	__iomem void *vpss_regs_base0;
+	__iomem void *vpss_regs_base1;
+	resource_size_t *vpss_regs_base2;
+	enum vpss_platform_type platform;
+	spinlock_t vpss_lock;
+	struct vpss_hw_ops hw_ops;
+};
+
+static struct vpss_oper_config oper_cfg;
+
+/* register access routines */
+static inline u32 bl_regr(u32 offset)
+{
+	return __raw_readl(oper_cfg.vpss_regs_base0 + offset);
+}
+
+static inline void bl_regw(u32 val, u32 offset)
+{
+	__raw_writel(val, oper_cfg.vpss_regs_base0 + offset);
+}
+
+static inline u32 vpss_regr(u32 offset)
+{
+	return __raw_readl(oper_cfg.vpss_regs_base1 + offset);
+}
+
+static inline void vpss_regw(u32 val, u32 offset)
+{
+	__raw_writel(val, oper_cfg.vpss_regs_base1 + offset);
+}
+
+/* For DM365 only */
+static inline u32 isp5_read(u32 offset)
+{
+	return __raw_readl(oper_cfg.vpss_regs_base0 + offset);
+}
+
+/* For DM365 only */
+static inline void isp5_write(u32 val, u32 offset)
+{
+	__raw_writel(val, oper_cfg.vpss_regs_base0 + offset);
+}
+
+static void dm365_select_ccdc_source(enum vpss_ccdc_source_sel src_sel)
+{
+	u32 temp = isp5_read(DM365_ISP5_CCDCMUX) & ~CCD_SRC_SEL_MASK;
+
+	/* if we are using pattern generator, enable it */
+	if (src_sel == VPSS_PGLPBK || src_sel == VPSS_CCDCPG)
+		temp |= 0x08;
+
+	temp |= (src_sel << CCD_SRC_SEL_SHIFT);
+	isp5_write(temp, DM365_ISP5_CCDCMUX);
+}
+
+static void dm355_select_ccdc_source(enum vpss_ccdc_source_sel src_sel)
+{
+	bl_regw(src_sel << VPSS_HSSISEL_SHIFT, DM355_VPSSBL_CCDCMUX);
+}
+
+int vpss_dma_complete_interrupt(void)
+{
+	if (!oper_cfg.hw_ops.dma_complete_interrupt)
+		return 2;
+	return oper_cfg.hw_ops.dma_complete_interrupt();
+}
+EXPORT_SYMBOL(vpss_dma_complete_interrupt);
+
+int vpss_select_ccdc_source(enum vpss_ccdc_source_sel src_sel)
+{
+	if (!oper_cfg.hw_ops.select_ccdc_source)
+		return -EINVAL;
+
+	oper_cfg.hw_ops.select_ccdc_source(src_sel);
+	return 0;
+}
+EXPORT_SYMBOL(vpss_select_ccdc_source);
+
+static int dm644x_clear_wbl_overflow(enum vpss_wbl_sel wbl_sel)
+{
+	u32 mask = 1, val;
+
+	if (wbl_sel < VPSS_PCR_AEW_WBL_0 ||
+	    wbl_sel > VPSS_PCR_CCDC_WBL_O)
+		return -EINVAL;
+
+	/* writing a 0 clear the overflow */
+	mask = ~(mask << wbl_sel);
+	val = bl_regr(DM644X_SBL_PCR_VPSS) & mask;
+	bl_regw(val, DM644X_SBL_PCR_VPSS);
+	return 0;
+}
+
+void vpss_set_sync_pol(struct vpss_sync_pol sync)
+{
+	if (!oper_cfg.hw_ops.set_sync_pol)
+		return;
+
+	oper_cfg.hw_ops.set_sync_pol(sync);
+}
+EXPORT_SYMBOL(vpss_set_sync_pol);
+
+int vpss_clear_wbl_overflow(enum vpss_wbl_sel wbl_sel)
+{
+	if (!oper_cfg.hw_ops.clear_wbl_overflow)
+		return -EINVAL;
+
+	return oper_cfg.hw_ops.clear_wbl_overflow(wbl_sel);
+}
+EXPORT_SYMBOL(vpss_clear_wbl_overflow);
+
+/*
+ *  dm355_enable_clock - Enable VPSS Clock
+ *  @clock_sel: Clock to be enabled/disabled
+ *  @en: enable/disable flag
+ *
+ *  This is called to enable or disable a vpss clock
+ */
+static int dm355_enable_clock(enum vpss_clock_sel clock_sel, int en)
+{
+	unsigned long flags;
+	u32 utemp, mask = 0x1, shift = 0;
+
+	switch (clock_sel) {
+	case VPSS_VPBE_CLOCK:
+		/* nothing since lsb */
+		break;
+	case VPSS_VENC_CLOCK_SEL:
+		shift = 2;
+		break;
+	case VPSS_CFALD_CLOCK:
+		shift = 3;
+		break;
+	case VPSS_H3A_CLOCK:
+		shift = 4;
+		break;
+	case VPSS_IPIPE_CLOCK:
+		shift = 5;
+		break;
+	case VPSS_CCDC_CLOCK:
+		shift = 6;
+		break;
+	default:
+		printk(KERN_ERR "dm355_enable_clock:"
+				" Invalid selector: %d\n", clock_sel);
+		return -EINVAL;
+	}
+
+	spin_lock_irqsave(&oper_cfg.vpss_lock, flags);
+	utemp = vpss_regr(DM355_VPSSCLK_CLKCTRL);
+	if (!en)
+		utemp &= ~(mask << shift);
+	else
+		utemp |= (mask << shift);
+
+	vpss_regw(utemp, DM355_VPSSCLK_CLKCTRL);
+	spin_unlock_irqrestore(&oper_cfg.vpss_lock, flags);
+	return 0;
+}
+
+static int dm365_enable_clock(enum vpss_clock_sel clock_sel, int en)
+{
+	unsigned long flags;
+	u32 utemp, mask = 0x1, shift = 0, offset = DM365_ISP5_PCCR;
+	u32 (*read)(u32 offset) = isp5_read;
+	void(*write)(u32 val, u32 offset) = isp5_write;
+
+	switch (clock_sel) {
+	case VPSS_BL_CLOCK:
+		break;
+	case VPSS_CCDC_CLOCK:
+		shift = 1;
+		break;
+	case VPSS_H3A_CLOCK:
+		shift = 2;
+		break;
+	case VPSS_RSZ_CLOCK:
+		shift = 3;
+		break;
+	case VPSS_IPIPE_CLOCK:
+		shift = 4;
+		break;
+	case VPSS_IPIPEIF_CLOCK:
+		shift = 5;
+		break;
+	case VPSS_PCLK_INTERNAL:
+		shift = 6;
+		break;
+	case VPSS_PSYNC_CLOCK_SEL:
+		shift = 7;
+		break;
+	case VPSS_VPBE_CLOCK:
+		read = vpss_regr;
+		write = vpss_regw;
+		offset = DM365_VPBE_CLK_CTRL;
+		break;
+	case VPSS_VENC_CLOCK_SEL:
+		shift = 2;
+		read = vpss_regr;
+		write = vpss_regw;
+		offset = DM365_VPBE_CLK_CTRL;
+		break;
+	case VPSS_LDC_CLOCK:
+		shift = 3;
+		read = vpss_regr;
+		write = vpss_regw;
+		offset = DM365_VPBE_CLK_CTRL;
+		break;
+	case VPSS_FDIF_CLOCK:
+		shift = 4;
+		read = vpss_regr;
+		write = vpss_regw;
+		offset = DM365_VPBE_CLK_CTRL;
+		break;
+	case VPSS_OSD_CLOCK_SEL:
+		shift = 6;
+		read = vpss_regr;
+		write = vpss_regw;
+		offset = DM365_VPBE_CLK_CTRL;
+		break;
+	case VPSS_LDC_CLOCK_SEL:
+		shift = 7;
+		read = vpss_regr;
+		write = vpss_regw;
+		offset = DM365_VPBE_CLK_CTRL;
+		break;
+	default:
+		printk(KERN_ERR "dm365_enable_clock: Invalid selector: %d\n",
+		       clock_sel);
+		return -1;
+	}
+
+	spin_lock_irqsave(&oper_cfg.vpss_lock, flags);
+	utemp = read(offset);
+	if (!en) {
+		mask = ~mask;
+		utemp &= (mask << shift);
+	} else
+		utemp |= (mask << shift);
+
+	write(utemp, offset);
+	spin_unlock_irqrestore(&oper_cfg.vpss_lock, flags);
+
+	return 0;
+}
+
+int vpss_enable_clock(enum vpss_clock_sel clock_sel, int en)
+{
+	if (!oper_cfg.hw_ops.enable_clock)
+		return -EINVAL;
+
+	return oper_cfg.hw_ops.enable_clock(clock_sel, en);
+}
+EXPORT_SYMBOL(vpss_enable_clock);
+
+void dm365_vpss_set_sync_pol(struct vpss_sync_pol sync)
+{
+	int val = 0;
+	val = isp5_read(DM365_ISP5_CCDCMUX);
+
+	val |= (sync.ccdpg_hdpol << DM365_CCDC_PG_HD_POL_SHIFT);
+	val |= (sync.ccdpg_vdpol << DM365_CCDC_PG_VD_POL_SHIFT);
+
+	isp5_write(val, DM365_ISP5_CCDCMUX);
+}
+EXPORT_SYMBOL(dm365_vpss_set_sync_pol);
+
+void vpss_set_pg_frame_size(struct vpss_pg_frame_size frame_size)
+{
+	if (!oper_cfg.hw_ops.set_pg_frame_size)
+		return;
+
+	oper_cfg.hw_ops.set_pg_frame_size(frame_size);
+}
+EXPORT_SYMBOL(vpss_set_pg_frame_size);
+
+void dm365_vpss_set_pg_frame_size(struct vpss_pg_frame_size frame_size)
+{
+	int current_reg = ((frame_size.hlpfr >> 1) - 1) << 16;
+
+	current_reg |= (frame_size.pplen - 1);
+	isp5_write(current_reg, DM365_ISP5_PG_FRAME_SIZE);
+}
+EXPORT_SYMBOL(dm365_vpss_set_pg_frame_size);
+
+static int vpss_probe(struct platform_device *pdev)
+{
+	struct resource *res;
+	char *platform_name;
+
+	if (!pdev->dev.platform_data) {
+		dev_err(&pdev->dev, "no platform data\n");
+		return -ENOENT;
+	}
+
+	platform_name = pdev->dev.platform_data;
+	if (!strcmp(platform_name, "dm355_vpss"))
+		oper_cfg.platform = DM355;
+	else if (!strcmp(platform_name, "dm365_vpss"))
+		oper_cfg.platform = DM365;
+	else if (!strcmp(platform_name, "dm644x_vpss"))
+		oper_cfg.platform = DM644X;
+	else {
+		dev_err(&pdev->dev, "vpss driver not supported on"
+			" this platform\n");
+		return -ENODEV;
+	}
+
+	dev_info(&pdev->dev, "%s vpss probed\n", platform_name);
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	oper_cfg.vpss_regs_base0 = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(oper_cfg.vpss_regs_base0))
+		return PTR_ERR(oper_cfg.vpss_regs_base0);
+
+	if (oper_cfg.platform == DM355 || oper_cfg.platform == DM365) {
+		res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+
+		oper_cfg.vpss_regs_base1 = devm_ioremap_resource(&pdev->dev,
+								 res);
+		if (IS_ERR(oper_cfg.vpss_regs_base1))
+			return PTR_ERR(oper_cfg.vpss_regs_base1);
+	}
+
+	if (oper_cfg.platform == DM355) {
+		oper_cfg.hw_ops.enable_clock = dm355_enable_clock;
+		oper_cfg.hw_ops.select_ccdc_source = dm355_select_ccdc_source;
+		/* Setup vpss interrupts */
+		bl_regw(DM355_VPSSBL_INTSEL_DEFAULT, DM355_VPSSBL_INTSEL);
+		bl_regw(DM355_VPSSBL_EVTSEL_DEFAULT, DM355_VPSSBL_EVTSEL);
+	} else if (oper_cfg.platform == DM365) {
+		oper_cfg.hw_ops.enable_clock = dm365_enable_clock;
+		oper_cfg.hw_ops.select_ccdc_source = dm365_select_ccdc_source;
+		/* Setup vpss interrupts */
+		isp5_write((isp5_read(DM365_ISP5_PCCR) |
+				      DM365_ISP5_PCCR_BL_CLK_ENABLE |
+				      DM365_ISP5_PCCR_ISIF_CLK_ENABLE |
+				      DM365_ISP5_PCCR_H3A_CLK_ENABLE |
+				      DM365_ISP5_PCCR_RSZ_CLK_ENABLE |
+				      DM365_ISP5_PCCR_IPIPE_CLK_ENABLE |
+				      DM365_ISP5_PCCR_IPIPEIF_CLK_ENABLE |
+				      DM365_ISP5_PCCR_RSV), DM365_ISP5_PCCR);
+		isp5_write((isp5_read(DM365_ISP5_BCR) |
+			    DM365_ISP5_BCR_ISIF_OUT_ENABLE), DM365_ISP5_BCR);
+		isp5_write(DM365_ISP5_INTSEL1_DEFAULT, DM365_ISP5_INTSEL1);
+		isp5_write(DM365_ISP5_INTSEL2_DEFAULT, DM365_ISP5_INTSEL2);
+		isp5_write(DM365_ISP5_INTSEL3_DEFAULT, DM365_ISP5_INTSEL3);
+	} else
+		oper_cfg.hw_ops.clear_wbl_overflow = dm644x_clear_wbl_overflow;
+
+	pm_runtime_enable(&pdev->dev);
+
+	pm_runtime_get(&pdev->dev);
+
+	spin_lock_init(&oper_cfg.vpss_lock);
+	dev_info(&pdev->dev, "%s vpss probe success\n", platform_name);
+
+	return 0;
+}
+
+static int vpss_remove(struct platform_device *pdev)
+{
+	pm_runtime_disable(&pdev->dev);
+	return 0;
+}
+
+static int vpss_suspend(struct device *dev)
+{
+	pm_runtime_put(dev);
+	return 0;
+}
+
+static int vpss_resume(struct device *dev)
+{
+	pm_runtime_get(dev);
+	return 0;
+}
+
+static const struct dev_pm_ops vpss_pm_ops = {
+	.suspend = vpss_suspend,
+	.resume = vpss_resume,
+};
+
+static struct platform_driver vpss_driver = {
+	.driver = {
+		.name	= "vpss",
+		.pm = &vpss_pm_ops,
+	},
+	.remove = vpss_remove,
+	.probe = vpss_probe,
+};
+
+static void vpss_exit(void)
+{
+	iounmap(oper_cfg.vpss_regs_base2);
+	release_mem_region(VPSS_CLK_CTRL, 4);
+	platform_driver_unregister(&vpss_driver);
+}
+
+static int __init vpss_init(void)
+{
+	if (!request_mem_region(VPSS_CLK_CTRL, 4, "vpss_clock_control"))
+		return -EBUSY;
+
+	oper_cfg.vpss_regs_base2 = ioremap(VPSS_CLK_CTRL, 4);
+	writel(VPSS_CLK_CTRL_VENCCLKEN |
+		     VPSS_CLK_CTRL_DACCLKEN, oper_cfg.vpss_regs_base2);
+
+	return platform_driver_register(&vpss_driver);
+}
+subsys_initcall(vpss_init);
+module_exit(vpss_exit);
diff --git a/drivers/media/platform/exynos-gsc/Makefile b/drivers/media/platform/exynos-gsc/Makefile
new file mode 100644
index 0000000..6d1411c
--- /dev/null
+++ b/drivers/media/platform/exynos-gsc/Makefile
@@ -0,0 +1,3 @@
+exynos-gsc-objs := gsc-core.o gsc-m2m.o gsc-regs.o
+
+obj-$(CONFIG_VIDEO_SAMSUNG_EXYNOS_GSC)	+= exynos-gsc.o
diff --git a/drivers/media/platform/exynos-gsc/gsc-core.c b/drivers/media/platform/exynos-gsc/gsc-core.c
new file mode 100644
index 0000000..15c543d
--- /dev/null
+++ b/drivers/media/platform/exynos-gsc/gsc-core.c
@@ -0,0 +1,1266 @@
+/*
+ * Copyright (c) 2011 - 2012 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ *
+ * Samsung EXYNOS5 SoC series G-Scaler driver
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published
+ * by the Free Software Foundation, either version 2 of the License,
+ * or (at your option) any later version.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/bug.h>
+#include <linux/interrupt.h>
+#include <linux/workqueue.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/list.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/clk.h>
+#include <linux/of.h>
+#include <media/v4l2-ioctl.h>
+
+#include "gsc-core.h"
+
+#define GSC_CLOCK_GATE_NAME	"gscl"
+
+static const struct gsc_fmt gsc_formats[] = {
+	{
+		.name		= "RGB565",
+		.pixelformat	= V4L2_PIX_FMT_RGB565X,
+		.depth		= { 16 },
+		.color		= GSC_RGB,
+		.num_planes	= 1,
+		.num_comp	= 1,
+	}, {
+		.name		= "XRGB-8-8-8-8, 32 bpp",
+		.pixelformat	= V4L2_PIX_FMT_RGB32,
+		.depth		= { 32 },
+		.color		= GSC_RGB,
+		.num_planes	= 1,
+		.num_comp	= 1,
+	}, {
+		.name		= "YUV 4:2:2 packed, YCbYCr",
+		.pixelformat	= V4L2_PIX_FMT_YUYV,
+		.depth		= { 16 },
+		.color		= GSC_YUV422,
+		.yorder		= GSC_LSB_Y,
+		.corder		= GSC_CBCR,
+		.num_planes	= 1,
+		.num_comp	= 1,
+		.mbus_code	= MEDIA_BUS_FMT_YUYV8_2X8,
+	}, {
+		.name		= "YUV 4:2:2 packed, CbYCrY",
+		.pixelformat	= V4L2_PIX_FMT_UYVY,
+		.depth		= { 16 },
+		.color		= GSC_YUV422,
+		.yorder		= GSC_LSB_C,
+		.corder		= GSC_CBCR,
+		.num_planes	= 1,
+		.num_comp	= 1,
+		.mbus_code	= MEDIA_BUS_FMT_UYVY8_2X8,
+	}, {
+		.name		= "YUV 4:2:2 packed, CrYCbY",
+		.pixelformat	= V4L2_PIX_FMT_VYUY,
+		.depth		= { 16 },
+		.color		= GSC_YUV422,
+		.yorder		= GSC_LSB_C,
+		.corder		= GSC_CRCB,
+		.num_planes	= 1,
+		.num_comp	= 1,
+		.mbus_code	= MEDIA_BUS_FMT_VYUY8_2X8,
+	}, {
+		.name		= "YUV 4:2:2 packed, YCrYCb",
+		.pixelformat	= V4L2_PIX_FMT_YVYU,
+		.depth		= { 16 },
+		.color		= GSC_YUV422,
+		.yorder		= GSC_LSB_Y,
+		.corder		= GSC_CRCB,
+		.num_planes	= 1,
+		.num_comp	= 1,
+		.mbus_code	= MEDIA_BUS_FMT_YVYU8_2X8,
+	}, {
+		.name		= "YUV 4:4:4 planar, YCbYCr",
+		.pixelformat	= V4L2_PIX_FMT_YUV32,
+		.depth		= { 32 },
+		.color		= GSC_YUV444,
+		.yorder		= GSC_LSB_Y,
+		.corder		= GSC_CBCR,
+		.num_planes	= 1,
+		.num_comp	= 1,
+	}, {
+		.name		= "YUV 4:2:2 planar, Y/Cb/Cr",
+		.pixelformat	= V4L2_PIX_FMT_YUV422P,
+		.depth		= { 16 },
+		.color		= GSC_YUV422,
+		.yorder		= GSC_LSB_Y,
+		.corder		= GSC_CBCR,
+		.num_planes	= 1,
+		.num_comp	= 3,
+	}, {
+		.name		= "YUV 4:2:2 planar, Y/CbCr",
+		.pixelformat	= V4L2_PIX_FMT_NV16,
+		.depth		= { 16 },
+		.color		= GSC_YUV422,
+		.yorder		= GSC_LSB_Y,
+		.corder		= GSC_CBCR,
+		.num_planes	= 1,
+		.num_comp	= 2,
+	}, {
+		.name		= "YUV 4:2:2 planar, Y/CrCb",
+		.pixelformat	= V4L2_PIX_FMT_NV61,
+		.depth		= { 16 },
+		.color		= GSC_YUV422,
+		.yorder		= GSC_LSB_Y,
+		.corder		= GSC_CRCB,
+		.num_planes	= 1,
+		.num_comp	= 2,
+	}, {
+		.name		= "YUV 4:2:0 planar, YCbCr",
+		.pixelformat	= V4L2_PIX_FMT_YUV420,
+		.depth		= { 12 },
+		.color		= GSC_YUV420,
+		.yorder		= GSC_LSB_Y,
+		.corder		= GSC_CBCR,
+		.num_planes	= 1,
+		.num_comp	= 3,
+	}, {
+		.name		= "YUV 4:2:0 planar, YCrCb",
+		.pixelformat	= V4L2_PIX_FMT_YVU420,
+		.depth		= { 12 },
+		.color		= GSC_YUV420,
+		.yorder		= GSC_LSB_Y,
+		.corder		= GSC_CRCB,
+		.num_planes	= 1,
+		.num_comp	= 3,
+
+	}, {
+		.name		= "YUV 4:2:0 planar, Y/CbCr",
+		.pixelformat	= V4L2_PIX_FMT_NV12,
+		.depth		= { 12 },
+		.color		= GSC_YUV420,
+		.yorder		= GSC_LSB_Y,
+		.corder		= GSC_CBCR,
+		.num_planes	= 1,
+		.num_comp	= 2,
+	}, {
+		.name		= "YUV 4:2:0 planar, Y/CrCb",
+		.pixelformat	= V4L2_PIX_FMT_NV21,
+		.depth		= { 12 },
+		.color		= GSC_YUV420,
+		.yorder		= GSC_LSB_Y,
+		.corder		= GSC_CRCB,
+		.num_planes	= 1,
+		.num_comp	= 2,
+	}, {
+		.name		= "YUV 4:2:0 non-contig. 2p, Y/CbCr",
+		.pixelformat	= V4L2_PIX_FMT_NV12M,
+		.depth		= { 8, 4 },
+		.color		= GSC_YUV420,
+		.yorder		= GSC_LSB_Y,
+		.corder		= GSC_CBCR,
+		.num_planes	= 2,
+		.num_comp	= 2,
+	}, {
+		.name		= "YUV 4:2:0 non-contig. 3p, Y/Cb/Cr",
+		.pixelformat	= V4L2_PIX_FMT_YUV420M,
+		.depth		= { 8, 2, 2 },
+		.color		= GSC_YUV420,
+		.yorder		= GSC_LSB_Y,
+		.corder		= GSC_CBCR,
+		.num_planes	= 3,
+		.num_comp	= 3,
+	}, {
+		.name		= "YUV 4:2:0 non-contig. 3p, Y/Cr/Cb",
+		.pixelformat	= V4L2_PIX_FMT_YVU420M,
+		.depth		= { 8, 2, 2 },
+		.color		= GSC_YUV420,
+		.yorder		= GSC_LSB_Y,
+		.corder		= GSC_CRCB,
+		.num_planes	= 3,
+		.num_comp	= 3,
+	}, {
+		.name		= "YUV 4:2:0 n.c. 2p, Y/CbCr tiled",
+		.pixelformat	= V4L2_PIX_FMT_NV12MT_16X16,
+		.depth		= { 8, 4 },
+		.color		= GSC_YUV420,
+		.yorder		= GSC_LSB_Y,
+		.corder		= GSC_CBCR,
+		.num_planes	= 2,
+		.num_comp	= 2,
+	}
+};
+
+const struct gsc_fmt *get_format(int index)
+{
+	if (index >= ARRAY_SIZE(gsc_formats))
+		return NULL;
+
+	return (struct gsc_fmt *)&gsc_formats[index];
+}
+
+const struct gsc_fmt *find_fmt(u32 *pixelformat, u32 *mbus_code, u32 index)
+{
+	const struct gsc_fmt *fmt, *def_fmt = NULL;
+	unsigned int i;
+
+	if (index >= ARRAY_SIZE(gsc_formats))
+		return NULL;
+
+	for (i = 0; i < ARRAY_SIZE(gsc_formats); ++i) {
+		fmt = get_format(i);
+		if (pixelformat && fmt->pixelformat == *pixelformat)
+			return fmt;
+		if (mbus_code && fmt->mbus_code == *mbus_code)
+			return fmt;
+		if (index == i)
+			def_fmt = fmt;
+	}
+	return def_fmt;
+
+}
+
+void gsc_set_frame_size(struct gsc_frame *frame, int width, int height)
+{
+	frame->f_width	= width;
+	frame->f_height	= height;
+	frame->crop.width = width;
+	frame->crop.height = height;
+	frame->crop.left = 0;
+	frame->crop.top = 0;
+}
+
+int gsc_cal_prescaler_ratio(struct gsc_variant *var, u32 src, u32 dst,
+								u32 *ratio)
+{
+	if ((dst > src) || (dst >= src / var->poly_sc_down_max)) {
+		*ratio = 1;
+		return 0;
+	}
+
+	if ((src / var->poly_sc_down_max / var->pre_sc_down_max) > dst) {
+		pr_err("Exceeded maximum downscaling ratio (1/16))");
+		return -EINVAL;
+	}
+
+	*ratio = (dst > (src / 8)) ? 2 : 4;
+
+	return 0;
+}
+
+void gsc_get_prescaler_shfactor(u32 hratio, u32 vratio, u32 *sh)
+{
+	if (hratio == 4 && vratio == 4)
+		*sh = 4;
+	else if ((hratio == 4 && vratio == 2) ||
+		 (hratio == 2 && vratio == 4))
+		*sh = 3;
+	else if ((hratio == 4 && vratio == 1) ||
+		 (hratio == 1 && vratio == 4) ||
+		 (hratio == 2 && vratio == 2))
+		*sh = 2;
+	else if (hratio == 1 && vratio == 1)
+		*sh = 0;
+	else
+		*sh = 1;
+}
+
+void gsc_check_src_scale_info(struct gsc_variant *var,
+				struct gsc_frame *s_frame, u32 *wratio,
+				 u32 tx, u32 ty, u32 *hratio)
+{
+	int remainder = 0, walign, halign;
+
+	if (is_yuv420(s_frame->fmt->color)) {
+		walign = GSC_SC_ALIGN_4;
+		halign = GSC_SC_ALIGN_4;
+	} else if (is_yuv422(s_frame->fmt->color)) {
+		walign = GSC_SC_ALIGN_4;
+		halign = GSC_SC_ALIGN_2;
+	} else {
+		walign = GSC_SC_ALIGN_2;
+		halign = GSC_SC_ALIGN_2;
+	}
+
+	remainder = s_frame->crop.width % (*wratio * walign);
+	if (remainder) {
+		s_frame->crop.width -= remainder;
+		gsc_cal_prescaler_ratio(var, s_frame->crop.width, tx, wratio);
+		pr_info("cropped src width size is recalculated from %d to %d",
+			s_frame->crop.width + remainder, s_frame->crop.width);
+	}
+
+	remainder = s_frame->crop.height % (*hratio * halign);
+	if (remainder) {
+		s_frame->crop.height -= remainder;
+		gsc_cal_prescaler_ratio(var, s_frame->crop.height, ty, hratio);
+		pr_info("cropped src height size is recalculated from %d to %d",
+			s_frame->crop.height + remainder, s_frame->crop.height);
+	}
+}
+
+int gsc_enum_fmt_mplane(struct v4l2_fmtdesc *f)
+{
+	const struct gsc_fmt *fmt;
+
+	fmt = find_fmt(NULL, NULL, f->index);
+	if (!fmt)
+		return -EINVAL;
+
+	strlcpy(f->description, fmt->name, sizeof(f->description));
+	f->pixelformat = fmt->pixelformat;
+
+	return 0;
+}
+
+static int get_plane_info(struct gsc_frame *frm, u32 addr, u32 *index, u32 *ret_addr)
+{
+	if (frm->addr.y == addr) {
+		*index = 0;
+		*ret_addr = frm->addr.y;
+	} else if (frm->addr.cb == addr) {
+		*index = 1;
+		*ret_addr = frm->addr.cb;
+	} else if (frm->addr.cr == addr) {
+		*index = 2;
+		*ret_addr = frm->addr.cr;
+	} else {
+		pr_err("Plane address is wrong");
+		return -EINVAL;
+	}
+	return 0;
+}
+
+void gsc_set_prefbuf(struct gsc_dev *gsc, struct gsc_frame *frm)
+{
+	u32 f_chk_addr, f_chk_len, s_chk_addr, s_chk_len;
+	f_chk_addr = f_chk_len = s_chk_addr = s_chk_len = 0;
+
+	f_chk_addr = frm->addr.y;
+	f_chk_len = frm->payload[0];
+	if (frm->fmt->num_planes == 2) {
+		s_chk_addr = frm->addr.cb;
+		s_chk_len = frm->payload[1];
+	} else if (frm->fmt->num_planes == 3) {
+		u32 low_addr, low_plane, mid_addr, mid_plane;
+		u32 high_addr, high_plane;
+		u32 t_min, t_max;
+
+		t_min = min3(frm->addr.y, frm->addr.cb, frm->addr.cr);
+		if (get_plane_info(frm, t_min, &low_plane, &low_addr))
+			return;
+		t_max = max3(frm->addr.y, frm->addr.cb, frm->addr.cr);
+		if (get_plane_info(frm, t_max, &high_plane, &high_addr))
+			return;
+
+		mid_plane = 3 - (low_plane + high_plane);
+		if (mid_plane == 0)
+			mid_addr = frm->addr.y;
+		else if (mid_plane == 1)
+			mid_addr = frm->addr.cb;
+		else if (mid_plane == 2)
+			mid_addr = frm->addr.cr;
+		else
+			return;
+
+		f_chk_addr = low_addr;
+		if (mid_addr + frm->payload[mid_plane] - low_addr >
+		    high_addr + frm->payload[high_plane] - mid_addr) {
+			f_chk_len = frm->payload[low_plane];
+			s_chk_addr = mid_addr;
+			s_chk_len = high_addr +
+					frm->payload[high_plane] - mid_addr;
+		} else {
+			f_chk_len = mid_addr +
+					frm->payload[mid_plane] - low_addr;
+			s_chk_addr = high_addr;
+			s_chk_len = frm->payload[high_plane];
+		}
+	}
+	pr_debug("f_addr = 0x%08x, f_len = %d, s_addr = 0x%08x, s_len = %d\n",
+			f_chk_addr, f_chk_len, s_chk_addr, s_chk_len);
+}
+
+int gsc_try_fmt_mplane(struct gsc_ctx *ctx, struct v4l2_format *f)
+{
+	struct gsc_dev *gsc = ctx->gsc_dev;
+	struct gsc_variant *variant = gsc->variant;
+	struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
+	const struct gsc_fmt *fmt;
+	u32 max_w, max_h, mod_x, mod_y;
+	u32 min_w, min_h, tmp_w, tmp_h;
+	int i;
+
+	pr_debug("user put w: %d, h: %d", pix_mp->width, pix_mp->height);
+
+	fmt = find_fmt(&pix_mp->pixelformat, NULL, 0);
+	if (!fmt) {
+		pr_err("pixelformat format (0x%X) invalid\n",
+						pix_mp->pixelformat);
+		return -EINVAL;
+	}
+
+	if (pix_mp->field == V4L2_FIELD_ANY)
+		pix_mp->field = V4L2_FIELD_NONE;
+	else if (pix_mp->field != V4L2_FIELD_NONE) {
+		pr_err("Not supported field order(%d)\n", pix_mp->field);
+		return -EINVAL;
+	}
+
+	max_w = variant->pix_max->target_rot_dis_w;
+	max_h = variant->pix_max->target_rot_dis_h;
+
+	mod_x = ffs(variant->pix_align->org_w) - 1;
+	if (is_yuv420(fmt->color))
+		mod_y = ffs(variant->pix_align->org_h) - 1;
+	else
+		mod_y = ffs(variant->pix_align->org_h) - 2;
+
+	if (V4L2_TYPE_IS_OUTPUT(f->type)) {
+		min_w = variant->pix_min->org_w;
+		min_h = variant->pix_min->org_h;
+	} else {
+		min_w = variant->pix_min->target_rot_dis_w;
+		min_h = variant->pix_min->target_rot_dis_h;
+	}
+
+	pr_debug("mod_x: %d, mod_y: %d, max_w: %d, max_h = %d",
+			mod_x, mod_y, max_w, max_h);
+
+	/* To check if image size is modified to adjust parameter against
+	   hardware abilities */
+	tmp_w = pix_mp->width;
+	tmp_h = pix_mp->height;
+
+	v4l_bound_align_image(&pix_mp->width, min_w, max_w, mod_x,
+		&pix_mp->height, min_h, max_h, mod_y, 0);
+	if (tmp_w != pix_mp->width || tmp_h != pix_mp->height)
+		pr_info("Image size has been modified from %dx%d to %dx%d",
+			 tmp_w, tmp_h, pix_mp->width, pix_mp->height);
+
+	pix_mp->num_planes = fmt->num_planes;
+
+	if (pix_mp->width >= 1280) /* HD */
+		pix_mp->colorspace = V4L2_COLORSPACE_REC709;
+	else /* SD */
+		pix_mp->colorspace = V4L2_COLORSPACE_SMPTE170M;
+
+
+	for (i = 0; i < pix_mp->num_planes; ++i) {
+		int bpl = (pix_mp->width * fmt->depth[i]) >> 3;
+		pix_mp->plane_fmt[i].bytesperline = bpl;
+		pix_mp->plane_fmt[i].sizeimage = bpl * pix_mp->height;
+
+		pr_debug("[%d]: bpl: %d, sizeimage: %d",
+				i, bpl, pix_mp->plane_fmt[i].sizeimage);
+	}
+
+	return 0;
+}
+
+int gsc_g_fmt_mplane(struct gsc_ctx *ctx, struct v4l2_format *f)
+{
+	struct gsc_frame *frame;
+	struct v4l2_pix_format_mplane *pix_mp;
+	int i;
+
+	frame = ctx_get_frame(ctx, f->type);
+	if (IS_ERR(frame))
+		return PTR_ERR(frame);
+
+	pix_mp = &f->fmt.pix_mp;
+
+	pix_mp->width		= frame->f_width;
+	pix_mp->height		= frame->f_height;
+	pix_mp->field		= V4L2_FIELD_NONE;
+	pix_mp->pixelformat	= frame->fmt->pixelformat;
+	pix_mp->colorspace	= V4L2_COLORSPACE_REC709;
+	pix_mp->num_planes	= frame->fmt->num_planes;
+
+	for (i = 0; i < pix_mp->num_planes; ++i) {
+		pix_mp->plane_fmt[i].bytesperline = (frame->f_width *
+			frame->fmt->depth[i]) / 8;
+		pix_mp->plane_fmt[i].sizeimage =
+			 pix_mp->plane_fmt[i].bytesperline * frame->f_height;
+	}
+
+	return 0;
+}
+
+void gsc_check_crop_change(u32 tmp_w, u32 tmp_h, u32 *w, u32 *h)
+{
+	if (tmp_w != *w || tmp_h != *h) {
+		pr_info("Cropped size has been modified from %dx%d to %dx%d",
+							*w, *h, tmp_w, tmp_h);
+		*w = tmp_w;
+		*h = tmp_h;
+	}
+}
+
+int gsc_g_crop(struct gsc_ctx *ctx, struct v4l2_crop *cr)
+{
+	struct gsc_frame *frame;
+
+	frame = ctx_get_frame(ctx, cr->type);
+	if (IS_ERR(frame))
+		return PTR_ERR(frame);
+
+	cr->c = frame->crop;
+
+	return 0;
+}
+
+int gsc_try_crop(struct gsc_ctx *ctx, struct v4l2_crop *cr)
+{
+	struct gsc_frame *f;
+	struct gsc_dev *gsc = ctx->gsc_dev;
+	struct gsc_variant *variant = gsc->variant;
+	u32 mod_x = 0, mod_y = 0, tmp_w, tmp_h;
+	u32 min_w, min_h, max_w, max_h;
+
+	if (cr->c.top < 0 || cr->c.left < 0) {
+		pr_err("doesn't support negative values for top & left\n");
+		return -EINVAL;
+	}
+	pr_debug("user put w: %d, h: %d", cr->c.width, cr->c.height);
+
+	if (cr->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		f = &ctx->d_frame;
+	else if (cr->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		f = &ctx->s_frame;
+	else
+		return -EINVAL;
+
+	max_w = f->f_width;
+	max_h = f->f_height;
+	tmp_w = cr->c.width;
+	tmp_h = cr->c.height;
+
+	if (V4L2_TYPE_IS_OUTPUT(cr->type)) {
+		if ((is_yuv422(f->fmt->color) && f->fmt->num_comp == 1) ||
+		    is_rgb(f->fmt->color))
+			min_w = 32;
+		else
+			min_w = 64;
+		if ((is_yuv422(f->fmt->color) && f->fmt->num_comp == 3) ||
+		    is_yuv420(f->fmt->color))
+			min_h = 32;
+		else
+			min_h = 16;
+	} else {
+		if (is_yuv420(f->fmt->color) || is_yuv422(f->fmt->color))
+			mod_x = ffs(variant->pix_align->target_w) - 1;
+		if (is_yuv420(f->fmt->color))
+			mod_y = ffs(variant->pix_align->target_h) - 1;
+		if (ctx->gsc_ctrls.rotate->val == 90 ||
+		    ctx->gsc_ctrls.rotate->val == 270) {
+			max_w = f->f_height;
+			max_h = f->f_width;
+			min_w = variant->pix_min->target_rot_en_w;
+			min_h = variant->pix_min->target_rot_en_h;
+			tmp_w = cr->c.height;
+			tmp_h = cr->c.width;
+		} else {
+			min_w = variant->pix_min->target_rot_dis_w;
+			min_h = variant->pix_min->target_rot_dis_h;
+		}
+	}
+	pr_debug("mod_x: %d, mod_y: %d, min_w: %d, min_h = %d",
+					mod_x, mod_y, min_w, min_h);
+	pr_debug("tmp_w : %d, tmp_h : %d", tmp_w, tmp_h);
+
+	v4l_bound_align_image(&tmp_w, min_w, max_w, mod_x,
+			      &tmp_h, min_h, max_h, mod_y, 0);
+
+	if (!V4L2_TYPE_IS_OUTPUT(cr->type) &&
+		(ctx->gsc_ctrls.rotate->val == 90 ||
+		ctx->gsc_ctrls.rotate->val == 270))
+		gsc_check_crop_change(tmp_h, tmp_w,
+					&cr->c.width, &cr->c.height);
+	else
+		gsc_check_crop_change(tmp_w, tmp_h,
+					&cr->c.width, &cr->c.height);
+
+
+	/* adjust left/top if cropping rectangle is out of bounds */
+	/* Need to add code to algin left value with 2's multiple */
+	if (cr->c.left + tmp_w > max_w)
+		cr->c.left = max_w - tmp_w;
+	if (cr->c.top + tmp_h > max_h)
+		cr->c.top = max_h - tmp_h;
+
+	if ((is_yuv420(f->fmt->color) || is_yuv422(f->fmt->color)) &&
+		cr->c.left & 1)
+			cr->c.left -= 1;
+
+	pr_debug("Aligned l:%d, t:%d, w:%d, h:%d, f_w: %d, f_h: %d",
+	    cr->c.left, cr->c.top, cr->c.width, cr->c.height, max_w, max_h);
+
+	return 0;
+}
+
+int gsc_check_scaler_ratio(struct gsc_variant *var, int sw, int sh, int dw,
+			   int dh, int rot, int out_path)
+{
+	int tmp_w, tmp_h, sc_down_max;
+
+	if (out_path == GSC_DMA)
+		sc_down_max = var->sc_down_max;
+	else
+		sc_down_max = var->local_sc_down;
+
+	if (rot == 90 || rot == 270) {
+		tmp_w = dh;
+		tmp_h = dw;
+	} else {
+		tmp_w = dw;
+		tmp_h = dh;
+	}
+
+	if ((sw / tmp_w) > sc_down_max ||
+	    (sh / tmp_h) > sc_down_max ||
+	    (tmp_w / sw) > var->sc_up_max ||
+	    (tmp_h / sh) > var->sc_up_max)
+		return -EINVAL;
+
+	return 0;
+}
+
+int gsc_set_scaler_info(struct gsc_ctx *ctx)
+{
+	struct gsc_scaler *sc = &ctx->scaler;
+	struct gsc_frame *s_frame = &ctx->s_frame;
+	struct gsc_frame *d_frame = &ctx->d_frame;
+	struct gsc_variant *variant = ctx->gsc_dev->variant;
+	struct device *dev = &ctx->gsc_dev->pdev->dev;
+	int tx, ty;
+	int ret;
+
+	ret = gsc_check_scaler_ratio(variant, s_frame->crop.width,
+		s_frame->crop.height, d_frame->crop.width, d_frame->crop.height,
+		ctx->gsc_ctrls.rotate->val, ctx->out_path);
+	if (ret) {
+		pr_err("out of scaler range");
+		return ret;
+	}
+
+	if (ctx->gsc_ctrls.rotate->val == 90 ||
+	    ctx->gsc_ctrls.rotate->val == 270) {
+		ty = d_frame->crop.width;
+		tx = d_frame->crop.height;
+	} else {
+		tx = d_frame->crop.width;
+		ty = d_frame->crop.height;
+	}
+
+	if (tx <= 0 || ty <= 0) {
+		dev_err(dev, "Invalid target size: %dx%d", tx, ty);
+		return -EINVAL;
+	}
+
+	ret = gsc_cal_prescaler_ratio(variant, s_frame->crop.width,
+				      tx, &sc->pre_hratio);
+	if (ret) {
+		pr_err("Horizontal scale ratio is out of range");
+		return ret;
+	}
+
+	ret = gsc_cal_prescaler_ratio(variant, s_frame->crop.height,
+				      ty, &sc->pre_vratio);
+	if (ret) {
+		pr_err("Vertical scale ratio is out of range");
+		return ret;
+	}
+
+	gsc_check_src_scale_info(variant, s_frame, &sc->pre_hratio,
+				 tx, ty, &sc->pre_vratio);
+
+	gsc_get_prescaler_shfactor(sc->pre_hratio, sc->pre_vratio,
+				   &sc->pre_shfactor);
+
+	sc->main_hratio = (s_frame->crop.width << 16) / tx;
+	sc->main_vratio = (s_frame->crop.height << 16) / ty;
+
+	pr_debug("scaler input/output size : sx = %d, sy = %d, tx = %d, ty = %d",
+			s_frame->crop.width, s_frame->crop.height, tx, ty);
+	pr_debug("scaler ratio info : pre_shfactor : %d, pre_h : %d",
+			sc->pre_shfactor, sc->pre_hratio);
+	pr_debug("pre_v :%d, main_h : %d, main_v : %d",
+			sc->pre_vratio, sc->main_hratio, sc->main_vratio);
+
+	return 0;
+}
+
+static int __gsc_s_ctrl(struct gsc_ctx *ctx, struct v4l2_ctrl *ctrl)
+{
+	struct gsc_dev *gsc = ctx->gsc_dev;
+	struct gsc_variant *variant = gsc->variant;
+	unsigned int flags = GSC_DST_FMT | GSC_SRC_FMT;
+	int ret = 0;
+
+	if (ctrl->flags & V4L2_CTRL_FLAG_INACTIVE)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_HFLIP:
+		ctx->hflip = ctrl->val;
+		break;
+
+	case V4L2_CID_VFLIP:
+		ctx->vflip = ctrl->val;
+		break;
+
+	case V4L2_CID_ROTATE:
+		if ((ctx->state & flags) == flags) {
+			ret = gsc_check_scaler_ratio(variant,
+					ctx->s_frame.crop.width,
+					ctx->s_frame.crop.height,
+					ctx->d_frame.crop.width,
+					ctx->d_frame.crop.height,
+					ctx->gsc_ctrls.rotate->val,
+					ctx->out_path);
+
+			if (ret)
+				return -EINVAL;
+		}
+
+		ctx->rotation = ctrl->val;
+		break;
+
+	case V4L2_CID_ALPHA_COMPONENT:
+		ctx->d_frame.alpha = ctrl->val;
+		break;
+	}
+
+	ctx->state |= GSC_PARAMS;
+	return 0;
+}
+
+static int gsc_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct gsc_ctx *ctx = ctrl_to_ctx(ctrl);
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&ctx->gsc_dev->slock, flags);
+	ret = __gsc_s_ctrl(ctx, ctrl);
+	spin_unlock_irqrestore(&ctx->gsc_dev->slock, flags);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops gsc_ctrl_ops = {
+	.s_ctrl = gsc_s_ctrl,
+};
+
+int gsc_ctrls_create(struct gsc_ctx *ctx)
+{
+	if (ctx->ctrls_rdy) {
+		pr_err("Control handler of this context was created already");
+		return 0;
+	}
+
+	v4l2_ctrl_handler_init(&ctx->ctrl_handler, GSC_MAX_CTRL_NUM);
+
+	ctx->gsc_ctrls.rotate = v4l2_ctrl_new_std(&ctx->ctrl_handler,
+				&gsc_ctrl_ops, V4L2_CID_ROTATE, 0, 270, 90, 0);
+	ctx->gsc_ctrls.hflip = v4l2_ctrl_new_std(&ctx->ctrl_handler,
+				&gsc_ctrl_ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
+	ctx->gsc_ctrls.vflip = v4l2_ctrl_new_std(&ctx->ctrl_handler,
+				&gsc_ctrl_ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
+	ctx->gsc_ctrls.global_alpha = v4l2_ctrl_new_std(&ctx->ctrl_handler,
+			&gsc_ctrl_ops, V4L2_CID_ALPHA_COMPONENT, 0, 255, 1, 0);
+
+	ctx->ctrls_rdy = ctx->ctrl_handler.error == 0;
+
+	if (ctx->ctrl_handler.error) {
+		int err = ctx->ctrl_handler.error;
+		v4l2_ctrl_handler_free(&ctx->ctrl_handler);
+		pr_err("Failed to create G-Scaler control handlers");
+		return err;
+	}
+
+	return 0;
+}
+
+void gsc_ctrls_delete(struct gsc_ctx *ctx)
+{
+	if (ctx->ctrls_rdy) {
+		v4l2_ctrl_handler_free(&ctx->ctrl_handler);
+		ctx->ctrls_rdy = false;
+	}
+}
+
+/* The color format (num_comp, num_planes) must be already configured. */
+int gsc_prepare_addr(struct gsc_ctx *ctx, struct vb2_buffer *vb,
+			struct gsc_frame *frame, struct gsc_addr *addr)
+{
+	int ret = 0;
+	u32 pix_size;
+
+	if ((vb == NULL) || (frame == NULL))
+		return -EINVAL;
+
+	pix_size = frame->f_width * frame->f_height;
+
+	pr_debug("num_planes= %d, num_comp= %d, pix_size= %d",
+		frame->fmt->num_planes, frame->fmt->num_comp, pix_size);
+
+	addr->y = vb2_dma_contig_plane_dma_addr(vb, 0);
+
+	if (frame->fmt->num_planes == 1) {
+		switch (frame->fmt->num_comp) {
+		case 1:
+			addr->cb = 0;
+			addr->cr = 0;
+			break;
+		case 2:
+			/* decompose Y into Y/Cb */
+			addr->cb = (dma_addr_t)(addr->y + pix_size);
+			addr->cr = 0;
+			break;
+		case 3:
+			/* decompose Y into Y/Cb/Cr */
+			addr->cb = (dma_addr_t)(addr->y + pix_size);
+			if (GSC_YUV420 == frame->fmt->color)
+				addr->cr = (dma_addr_t)(addr->cb
+						+ (pix_size >> 2));
+			else /* 422 */
+				addr->cr = (dma_addr_t)(addr->cb
+						+ (pix_size >> 1));
+			break;
+		default:
+			pr_err("Invalid the number of color planes");
+			return -EINVAL;
+		}
+	} else {
+		if (frame->fmt->num_planes >= 2)
+			addr->cb = vb2_dma_contig_plane_dma_addr(vb, 1);
+
+		if (frame->fmt->num_planes == 3)
+			addr->cr = vb2_dma_contig_plane_dma_addr(vb, 2);
+	}
+
+	if ((frame->fmt->pixelformat == V4L2_PIX_FMT_VYUY) ||
+		(frame->fmt->pixelformat == V4L2_PIX_FMT_YVYU) ||
+		(frame->fmt->pixelformat == V4L2_PIX_FMT_YVU420) ||
+		(frame->fmt->pixelformat == V4L2_PIX_FMT_YVU420M))
+		swap(addr->cb, addr->cr);
+
+	pr_debug("ADDR: y= %pad  cb= %pad cr= %pad ret= %d",
+		&addr->y, &addr->cb, &addr->cr, ret);
+
+	return ret;
+}
+
+static irqreturn_t gsc_irq_handler(int irq, void *priv)
+{
+	struct gsc_dev *gsc = priv;
+	struct gsc_ctx *ctx;
+	int gsc_irq;
+
+	gsc_irq = gsc_hw_get_irq_status(gsc);
+	gsc_hw_clear_irq(gsc, gsc_irq);
+
+	if (gsc_irq == GSC_IRQ_OVERRUN) {
+		pr_err("Local path input over-run interrupt has occurred!\n");
+		return IRQ_HANDLED;
+	}
+
+	spin_lock(&gsc->slock);
+
+	if (test_and_clear_bit(ST_M2M_PEND, &gsc->state)) {
+
+		gsc_hw_enable_control(gsc, false);
+
+		if (test_and_clear_bit(ST_M2M_SUSPENDING, &gsc->state)) {
+			set_bit(ST_M2M_SUSPENDED, &gsc->state);
+			wake_up(&gsc->irq_queue);
+			goto isr_unlock;
+		}
+		ctx = v4l2_m2m_get_curr_priv(gsc->m2m.m2m_dev);
+
+		if (!ctx || !ctx->m2m_ctx)
+			goto isr_unlock;
+
+		spin_unlock(&gsc->slock);
+		gsc_m2m_job_finish(ctx, VB2_BUF_STATE_DONE);
+
+		/* wake_up job_abort, stop_streaming */
+		if (ctx->state & GSC_CTX_STOP_REQ) {
+			ctx->state &= ~GSC_CTX_STOP_REQ;
+			wake_up(&gsc->irq_queue);
+		}
+		return IRQ_HANDLED;
+	}
+
+isr_unlock:
+	spin_unlock(&gsc->slock);
+	return IRQ_HANDLED;
+}
+
+static struct gsc_pix_max gsc_v_100_max = {
+	.org_scaler_bypass_w	= 8192,
+	.org_scaler_bypass_h	= 8192,
+	.org_scaler_input_w	= 4800,
+	.org_scaler_input_h	= 3344,
+	.real_rot_dis_w		= 4800,
+	.real_rot_dis_h		= 3344,
+	.real_rot_en_w		= 2047,
+	.real_rot_en_h		= 2047,
+	.target_rot_dis_w	= 4800,
+	.target_rot_dis_h	= 3344,
+	.target_rot_en_w	= 2016,
+	.target_rot_en_h	= 2016,
+};
+
+static struct gsc_pix_min gsc_v_100_min = {
+	.org_w			= 64,
+	.org_h			= 32,
+	.real_w			= 64,
+	.real_h			= 32,
+	.target_rot_dis_w	= 64,
+	.target_rot_dis_h	= 32,
+	.target_rot_en_w	= 32,
+	.target_rot_en_h	= 16,
+};
+
+static struct gsc_pix_align gsc_v_100_align = {
+	.org_h			= 16,
+	.org_w			= 16, /* yuv420 : 16, others : 8 */
+	.offset_h		= 2,  /* yuv420/422 : 2, others : 1 */
+	.real_w			= 16, /* yuv420/422 : 4~16, others : 2~8 */
+	.real_h			= 16, /* yuv420 : 4~16, others : 1 */
+	.target_w		= 2,  /* yuv420/422 : 2, others : 1 */
+	.target_h		= 2,  /* yuv420 : 2, others : 1 */
+};
+
+static struct gsc_variant gsc_v_100_variant = {
+	.pix_max		= &gsc_v_100_max,
+	.pix_min		= &gsc_v_100_min,
+	.pix_align		= &gsc_v_100_align,
+	.in_buf_cnt		= 32,
+	.out_buf_cnt		= 32,
+	.sc_up_max		= 8,
+	.sc_down_max		= 16,
+	.poly_sc_down_max	= 4,
+	.pre_sc_down_max	= 4,
+	.local_sc_down		= 2,
+};
+
+static struct gsc_driverdata gsc_v_100_drvdata = {
+	.variant = {
+		[0] = &gsc_v_100_variant,
+		[1] = &gsc_v_100_variant,
+		[2] = &gsc_v_100_variant,
+		[3] = &gsc_v_100_variant,
+	},
+	.num_entities = 4,
+	.lclk_frequency = 266000000UL,
+};
+
+static const struct platform_device_id gsc_driver_ids[] = {
+	{
+		.name		= "exynos-gsc",
+		.driver_data	= (unsigned long)&gsc_v_100_drvdata,
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(platform, gsc_driver_ids);
+
+static const struct of_device_id exynos_gsc_match[] = {
+	{
+		.compatible = "samsung,exynos5-gsc",
+		.data = &gsc_v_100_drvdata,
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(of, exynos_gsc_match);
+
+static void *gsc_get_drv_data(struct platform_device *pdev)
+{
+	struct gsc_driverdata *driver_data = NULL;
+
+	if (pdev->dev.of_node) {
+		const struct of_device_id *match;
+		match = of_match_node(exynos_gsc_match,
+					pdev->dev.of_node);
+		if (match)
+			driver_data = (struct gsc_driverdata *)match->data;
+	} else {
+		driver_data = (struct gsc_driverdata *)
+			platform_get_device_id(pdev)->driver_data;
+	}
+
+	return driver_data;
+}
+
+static void gsc_clk_put(struct gsc_dev *gsc)
+{
+	if (!IS_ERR(gsc->clock))
+		clk_unprepare(gsc->clock);
+}
+
+static int gsc_clk_get(struct gsc_dev *gsc)
+{
+	int ret;
+
+	dev_dbg(&gsc->pdev->dev, "gsc_clk_get Called\n");
+
+	gsc->clock = devm_clk_get(&gsc->pdev->dev, GSC_CLOCK_GATE_NAME);
+	if (IS_ERR(gsc->clock)) {
+		dev_err(&gsc->pdev->dev, "failed to get clock~~~: %s\n",
+			GSC_CLOCK_GATE_NAME);
+		return PTR_ERR(gsc->clock);
+	}
+
+	ret = clk_prepare(gsc->clock);
+	if (ret < 0) {
+		dev_err(&gsc->pdev->dev, "clock prepare failed for clock: %s\n",
+			GSC_CLOCK_GATE_NAME);
+		gsc->clock = ERR_PTR(-EINVAL);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int gsc_m2m_suspend(struct gsc_dev *gsc)
+{
+	unsigned long flags;
+	int timeout;
+
+	spin_lock_irqsave(&gsc->slock, flags);
+	if (!gsc_m2m_pending(gsc)) {
+		spin_unlock_irqrestore(&gsc->slock, flags);
+		return 0;
+	}
+	clear_bit(ST_M2M_SUSPENDED, &gsc->state);
+	set_bit(ST_M2M_SUSPENDING, &gsc->state);
+	spin_unlock_irqrestore(&gsc->slock, flags);
+
+	timeout = wait_event_timeout(gsc->irq_queue,
+			     test_bit(ST_M2M_SUSPENDED, &gsc->state),
+			     GSC_SHUTDOWN_TIMEOUT);
+
+	clear_bit(ST_M2M_SUSPENDING, &gsc->state);
+	return timeout == 0 ? -EAGAIN : 0;
+}
+
+static int gsc_m2m_resume(struct gsc_dev *gsc)
+{
+	struct gsc_ctx *ctx;
+	unsigned long flags;
+
+	spin_lock_irqsave(&gsc->slock, flags);
+	/* Clear for full H/W setup in first run after resume */
+	ctx = gsc->m2m.ctx;
+	gsc->m2m.ctx = NULL;
+	spin_unlock_irqrestore(&gsc->slock, flags);
+
+	if (test_and_clear_bit(ST_M2M_SUSPENDED, &gsc->state))
+		gsc_m2m_job_finish(ctx, VB2_BUF_STATE_ERROR);
+
+	return 0;
+}
+
+static int gsc_probe(struct platform_device *pdev)
+{
+	struct gsc_dev *gsc;
+	struct resource *res;
+	struct gsc_driverdata *drv_data = gsc_get_drv_data(pdev);
+	struct device *dev = &pdev->dev;
+	int ret = 0;
+
+	gsc = devm_kzalloc(dev, sizeof(struct gsc_dev), GFP_KERNEL);
+	if (!gsc)
+		return -ENOMEM;
+
+	if (dev->of_node)
+		gsc->id = of_alias_get_id(pdev->dev.of_node, "gsc");
+	else
+		gsc->id = pdev->id;
+
+	if (gsc->id >= drv_data->num_entities) {
+		dev_err(dev, "Invalid platform device id: %d\n", gsc->id);
+		return -EINVAL;
+	}
+
+	gsc->variant = drv_data->variant[gsc->id];
+	gsc->pdev = pdev;
+	gsc->pdata = dev->platform_data;
+
+	init_waitqueue_head(&gsc->irq_queue);
+	spin_lock_init(&gsc->slock);
+	mutex_init(&gsc->lock);
+	gsc->clock = ERR_PTR(-EINVAL);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	gsc->regs = devm_ioremap_resource(dev, res);
+	if (IS_ERR(gsc->regs))
+		return PTR_ERR(gsc->regs);
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res) {
+		dev_err(dev, "failed to get IRQ resource\n");
+		return -ENXIO;
+	}
+
+	ret = gsc_clk_get(gsc);
+	if (ret)
+		return ret;
+
+	ret = devm_request_irq(dev, res->start, gsc_irq_handler,
+				0, pdev->name, gsc);
+	if (ret) {
+		dev_err(dev, "failed to install irq (%d)\n", ret);
+		goto err_clk;
+	}
+
+	ret = v4l2_device_register(dev, &gsc->v4l2_dev);
+	if (ret)
+		goto err_clk;
+
+	ret = gsc_register_m2m_device(gsc);
+	if (ret)
+		goto err_v4l2;
+
+	platform_set_drvdata(pdev, gsc);
+	pm_runtime_enable(dev);
+	ret = pm_runtime_get_sync(&pdev->dev);
+	if (ret < 0)
+		goto err_m2m;
+
+	/* Initialize continious memory allocator */
+	gsc->alloc_ctx = vb2_dma_contig_init_ctx(dev);
+	if (IS_ERR(gsc->alloc_ctx)) {
+		ret = PTR_ERR(gsc->alloc_ctx);
+		goto err_pm;
+	}
+
+	dev_dbg(dev, "gsc-%d registered successfully\n", gsc->id);
+
+	pm_runtime_put(dev);
+	return 0;
+err_pm:
+	pm_runtime_put(dev);
+err_m2m:
+	gsc_unregister_m2m_device(gsc);
+err_v4l2:
+	v4l2_device_unregister(&gsc->v4l2_dev);
+err_clk:
+	gsc_clk_put(gsc);
+	return ret;
+}
+
+static int gsc_remove(struct platform_device *pdev)
+{
+	struct gsc_dev *gsc = platform_get_drvdata(pdev);
+
+	gsc_unregister_m2m_device(gsc);
+	v4l2_device_unregister(&gsc->v4l2_dev);
+
+	vb2_dma_contig_cleanup_ctx(gsc->alloc_ctx);
+	pm_runtime_disable(&pdev->dev);
+	gsc_clk_put(gsc);
+
+	dev_dbg(&pdev->dev, "%s driver unloaded\n", pdev->name);
+	return 0;
+}
+
+static int gsc_runtime_resume(struct device *dev)
+{
+	struct gsc_dev *gsc = dev_get_drvdata(dev);
+	int ret = 0;
+
+	pr_debug("gsc%d: state: 0x%lx", gsc->id, gsc->state);
+
+	ret = clk_enable(gsc->clock);
+	if (ret)
+		return ret;
+
+	gsc_hw_set_sw_reset(gsc);
+	gsc_wait_reset(gsc);
+
+	return gsc_m2m_resume(gsc);
+}
+
+static int gsc_runtime_suspend(struct device *dev)
+{
+	struct gsc_dev *gsc = dev_get_drvdata(dev);
+	int ret = 0;
+
+	ret = gsc_m2m_suspend(gsc);
+	if (!ret)
+		clk_disable(gsc->clock);
+
+	pr_debug("gsc%d: state: 0x%lx", gsc->id, gsc->state);
+	return ret;
+}
+
+static int gsc_resume(struct device *dev)
+{
+	struct gsc_dev *gsc = dev_get_drvdata(dev);
+	unsigned long flags;
+
+	pr_debug("gsc%d: state: 0x%lx", gsc->id, gsc->state);
+
+	/* Do not resume if the device was idle before system suspend */
+	spin_lock_irqsave(&gsc->slock, flags);
+	if (!test_and_clear_bit(ST_SUSPEND, &gsc->state) ||
+	    !gsc_m2m_opened(gsc)) {
+		spin_unlock_irqrestore(&gsc->slock, flags);
+		return 0;
+	}
+	spin_unlock_irqrestore(&gsc->slock, flags);
+
+	if (!pm_runtime_suspended(dev))
+		return gsc_runtime_resume(dev);
+
+	return 0;
+}
+
+static int gsc_suspend(struct device *dev)
+{
+	struct gsc_dev *gsc = dev_get_drvdata(dev);
+
+	pr_debug("gsc%d: state: 0x%lx", gsc->id, gsc->state);
+
+	if (test_and_set_bit(ST_SUSPEND, &gsc->state))
+		return 0;
+
+	if (!pm_runtime_suspended(dev))
+		return gsc_runtime_suspend(dev);
+
+	return 0;
+}
+
+static const struct dev_pm_ops gsc_pm_ops = {
+	.suspend		= gsc_suspend,
+	.resume			= gsc_resume,
+	.runtime_suspend	= gsc_runtime_suspend,
+	.runtime_resume		= gsc_runtime_resume,
+};
+
+static struct platform_driver gsc_driver = {
+	.probe		= gsc_probe,
+	.remove		= gsc_remove,
+	.id_table	= gsc_driver_ids,
+	.driver = {
+		.name	= GSC_MODULE_NAME,
+		.pm	= &gsc_pm_ops,
+		.of_match_table = exynos_gsc_match,
+	}
+};
+
+module_platform_driver(gsc_driver);
+
+MODULE_AUTHOR("Hyunwong Kim <khw0178.kim@samsung.com>");
+MODULE_DESCRIPTION("Samsung EXYNOS5 Soc series G-Scaler driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/exynos-gsc/gsc-core.h b/drivers/media/platform/exynos-gsc/gsc-core.h
new file mode 100644
index 0000000..e93a233
--- /dev/null
+++ b/drivers/media/platform/exynos-gsc/gsc-core.h
@@ -0,0 +1,522 @@
+/*
+ * Copyright (c) 2011 - 2012 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ *
+ * header file for Samsung EXYNOS5 SoC series G-Scaler driver
+
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef GSC_CORE_H_
+#define GSC_CORE_H_
+
+#include <linux/delay.h>
+#include <linux/sched.h>
+#include <linux/spinlock.h>
+#include <linux/types.h>
+#include <linux/videodev2.h>
+#include <linux/io.h>
+#include <linux/pm_runtime.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/v4l2-mediabus.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "gsc-regs.h"
+
+#define CONFIG_VB2_GSC_DMA_CONTIG	1
+#define GSC_MODULE_NAME			"exynos-gsc"
+
+#define GSC_SHUTDOWN_TIMEOUT		((100*HZ)/1000)
+#define GSC_MAX_DEVS			4
+#define GSC_M2M_BUF_NUM			0
+#define GSC_MAX_CTRL_NUM		10
+#define GSC_SC_ALIGN_4			4
+#define GSC_SC_ALIGN_2			2
+#define DEFAULT_CSC_EQ			1
+#define DEFAULT_CSC_RANGE		1
+
+#define GSC_PARAMS			(1 << 0)
+#define GSC_SRC_FMT			(1 << 1)
+#define GSC_DST_FMT			(1 << 2)
+#define GSC_CTX_M2M			(1 << 3)
+#define GSC_CTX_STOP_REQ		(1 << 6)
+#define	GSC_CTX_ABORT			(1 << 7)
+
+enum gsc_dev_flags {
+	/* for global */
+	ST_SUSPEND,
+
+	/* for m2m node */
+	ST_M2M_OPEN,
+	ST_M2M_RUN,
+	ST_M2M_PEND,
+	ST_M2M_SUSPENDED,
+	ST_M2M_SUSPENDING,
+};
+
+enum gsc_irq {
+	GSC_IRQ_DONE,
+	GSC_IRQ_OVERRUN
+};
+
+/**
+ * enum gsc_datapath - the path of data used for G-Scaler
+ * @GSC_CAMERA: from camera
+ * @GSC_DMA: from/to DMA
+ * @GSC_LOCAL: to local path
+ * @GSC_WRITEBACK: from FIMD
+ */
+enum gsc_datapath {
+	GSC_CAMERA = 0x1,
+	GSC_DMA,
+	GSC_MIXER,
+	GSC_FIMD,
+	GSC_WRITEBACK,
+};
+
+enum gsc_color_fmt {
+	GSC_RGB = 0x1,
+	GSC_YUV420 = 0x2,
+	GSC_YUV422 = 0x4,
+	GSC_YUV444 = 0x8,
+};
+
+enum gsc_yuv_fmt {
+	GSC_LSB_Y = 0x10,
+	GSC_LSB_C,
+	GSC_CBCR = 0x20,
+	GSC_CRCB,
+};
+
+#define fh_to_ctx(__fh) container_of(__fh, struct gsc_ctx, fh)
+#define is_rgb(x) (!!((x) & 0x1))
+#define is_yuv420(x) (!!((x) & 0x2))
+#define is_yuv422(x) (!!((x) & 0x4))
+
+#define gsc_m2m_active(dev)	test_bit(ST_M2M_RUN, &(dev)->state)
+#define gsc_m2m_pending(dev)	test_bit(ST_M2M_PEND, &(dev)->state)
+#define gsc_m2m_opened(dev)	test_bit(ST_M2M_OPEN, &(dev)->state)
+
+#define ctrl_to_ctx(__ctrl) \
+	container_of((__ctrl)->handler, struct gsc_ctx, ctrl_handler)
+/**
+ * struct gsc_fmt - the driver's internal color format data
+ * @mbus_code: Media Bus pixel code, -1 if not applicable
+ * @name: format description
+ * @pixelformat: the fourcc code for this format, 0 if not applicable
+ * @yorder: Y/C order
+ * @corder: Chrominance order control
+ * @num_planes: number of physically non-contiguous data planes
+ * @nr_comp: number of physically contiguous data planes
+ * @depth: per plane driver's private 'number of bits per pixel'
+ * @flags: flags indicating which operation mode format applies to
+ */
+struct gsc_fmt {
+	u32 mbus_code;
+	char	*name;
+	u32	pixelformat;
+	u32	color;
+	u32	yorder;
+	u32	corder;
+	u16	num_planes;
+	u16	num_comp;
+	u8	depth[VIDEO_MAX_PLANES];
+	u32	flags;
+};
+
+/**
+ * struct gsc_input_buf - the driver's video buffer
+ * @vb:	videobuf2 buffer
+ * @list : linked list structure for buffer queue
+ * @idx : index of G-Scaler input buffer
+ */
+struct gsc_input_buf {
+	struct vb2_v4l2_buffer vb;
+	struct list_head	list;
+	int			idx;
+};
+
+/**
+ * struct gsc_addr - the G-Scaler physical address set
+ * @y:	 luminance plane address
+ * @cb:	 Cb plane address
+ * @cr:	 Cr plane address
+ */
+struct gsc_addr {
+	dma_addr_t y;
+	dma_addr_t cb;
+	dma_addr_t cr;
+};
+
+/* struct gsc_ctrls - the G-Scaler control set
+ * @rotate: rotation degree
+ * @hflip: horizontal flip
+ * @vflip: vertical flip
+ * @global_alpha: the alpha value of current frame
+ */
+struct gsc_ctrls {
+	struct v4l2_ctrl *rotate;
+	struct v4l2_ctrl *hflip;
+	struct v4l2_ctrl *vflip;
+	struct v4l2_ctrl *global_alpha;
+};
+
+/**
+ * struct gsc_scaler - the configuration data for G-Scaler inetrnal scaler
+ * @pre_shfactor:	pre sclaer shift factor
+ * @pre_hratio:		horizontal ratio of the prescaler
+ * @pre_vratio:		vertical ratio of the prescaler
+ * @main_hratio:	the main scaler's horizontal ratio
+ * @main_vratio:	the main scaler's vertical ratio
+ */
+struct gsc_scaler {
+	u32 pre_shfactor;
+	u32 pre_hratio;
+	u32 pre_vratio;
+	u32 main_hratio;
+	u32 main_vratio;
+};
+
+struct gsc_dev;
+
+struct gsc_ctx;
+
+/**
+ * struct gsc_frame - source/target frame properties
+ * @f_width:	SRC : SRCIMG_WIDTH, DST : OUTPUTDMA_WHOLE_IMG_WIDTH
+ * @f_height:	SRC : SRCIMG_HEIGHT, DST : OUTPUTDMA_WHOLE_IMG_HEIGHT
+ * @crop:	cropped(source)/scaled(destination) size
+ * @payload:	image size in bytes (w x h x bpp)
+ * @addr:	image frame buffer physical addresses
+ * @fmt:	G-Scaler color format pointer
+ * @colorspace: value indicating v4l2_colorspace
+ * @alpha:	frame's alpha value
+ */
+struct gsc_frame {
+	u32 f_width;
+	u32 f_height;
+	struct v4l2_rect crop;
+	unsigned long payload[VIDEO_MAX_PLANES];
+	struct gsc_addr	addr;
+	const struct gsc_fmt *fmt;
+	u32 colorspace;
+	u8 alpha;
+};
+
+/**
+ * struct gsc_m2m_device - v4l2 memory-to-memory device data
+ * @vfd: the video device node for v4l2 m2m mode
+ * @m2m_dev: v4l2 memory-to-memory device data
+ * @ctx: hardware context data
+ * @refcnt: the reference counter
+ */
+struct gsc_m2m_device {
+	struct video_device	*vfd;
+	struct v4l2_m2m_dev	*m2m_dev;
+	struct gsc_ctx		*ctx;
+	int			refcnt;
+};
+
+/**
+ *  struct gsc_pix_max - image pixel size limits in various IP configurations
+ *
+ *  @org_scaler_bypass_w: max pixel width when the scaler is disabled
+ *  @org_scaler_bypass_h: max pixel height when the scaler is disabled
+ *  @org_scaler_input_w: max pixel width when the scaler is enabled
+ *  @org_scaler_input_h: max pixel height when the scaler is enabled
+ *  @real_rot_dis_w: max pixel src cropped height with the rotator is off
+ *  @real_rot_dis_h: max pixel src croppped width with the rotator is off
+ *  @real_rot_en_w: max pixel src cropped width with the rotator is on
+ *  @real_rot_en_h: max pixel src cropped height with the rotator is on
+ *  @target_rot_dis_w: max pixel dst scaled width with the rotator is off
+ *  @target_rot_dis_h: max pixel dst scaled height with the rotator is off
+ *  @target_rot_en_w: max pixel dst scaled width with the rotator is on
+ *  @target_rot_en_h: max pixel dst scaled height with the rotator is on
+ */
+struct gsc_pix_max {
+	u16 org_scaler_bypass_w;
+	u16 org_scaler_bypass_h;
+	u16 org_scaler_input_w;
+	u16 org_scaler_input_h;
+	u16 real_rot_dis_w;
+	u16 real_rot_dis_h;
+	u16 real_rot_en_w;
+	u16 real_rot_en_h;
+	u16 target_rot_dis_w;
+	u16 target_rot_dis_h;
+	u16 target_rot_en_w;
+	u16 target_rot_en_h;
+};
+
+/**
+ *  struct gsc_pix_min - image pixel size limits in various IP configurations
+ *
+ *  @org_w: minimum source pixel width
+ *  @org_h: minimum source pixel height
+ *  @real_w: minimum input crop pixel width
+ *  @real_h: minimum input crop pixel height
+ *  @target_rot_dis_w: minimum output scaled pixel height when rotator is off
+ *  @target_rot_dis_h: minimum output scaled pixel height when rotator is off
+ *  @target_rot_en_w: minimum output scaled pixel height when rotator is on
+ *  @target_rot_en_h: minimum output scaled pixel height when rotator is on
+ */
+struct gsc_pix_min {
+	u16 org_w;
+	u16 org_h;
+	u16 real_w;
+	u16 real_h;
+	u16 target_rot_dis_w;
+	u16 target_rot_dis_h;
+	u16 target_rot_en_w;
+	u16 target_rot_en_h;
+};
+
+struct gsc_pix_align {
+	u16 org_h;
+	u16 org_w;
+	u16 offset_h;
+	u16 real_w;
+	u16 real_h;
+	u16 target_w;
+	u16 target_h;
+};
+
+/**
+ * struct gsc_variant - G-Scaler variant information
+ */
+struct gsc_variant {
+	struct gsc_pix_max *pix_max;
+	struct gsc_pix_min *pix_min;
+	struct gsc_pix_align *pix_align;
+	u16		in_buf_cnt;
+	u16		out_buf_cnt;
+	u16		sc_up_max;
+	u16		sc_down_max;
+	u16		poly_sc_down_max;
+	u16		pre_sc_down_max;
+	u16		local_sc_down;
+};
+
+/**
+ * struct gsc_driverdata - per device type driver data for init time.
+ *
+ * @variant: the variant information for this driver.
+ * @lclk_frequency: G-Scaler clock frequency
+ * @num_entities: the number of g-scalers
+ */
+struct gsc_driverdata {
+	struct gsc_variant *variant[GSC_MAX_DEVS];
+	unsigned long	lclk_frequency;
+	int		num_entities;
+};
+
+/**
+ * struct gsc_dev - abstraction for G-Scaler entity
+ * @slock:	the spinlock protecting this data structure
+ * @lock:	the mutex protecting this data structure
+ * @pdev:	pointer to the G-Scaler platform device
+ * @variant:	the IP variant information
+ * @id:		G-Scaler device index (0..GSC_MAX_DEVS)
+ * @clock:	clocks required for G-Scaler operation
+ * @regs:	the mapped hardware registers
+ * @irq_queue:	interrupt handler waitqueue
+ * @m2m:	memory-to-memory V4L2 device information
+ * @state:	flags used to synchronize m2m and capture mode operation
+ * @alloc_ctx:	videobuf2 memory allocator context
+ * @vdev:	video device for G-Scaler instance
+ */
+struct gsc_dev {
+	spinlock_t			slock;
+	struct mutex			lock;
+	struct platform_device		*pdev;
+	struct gsc_variant		*variant;
+	u16				id;
+	struct clk			*clock;
+	void __iomem			*regs;
+	wait_queue_head_t		irq_queue;
+	struct gsc_m2m_device		m2m;
+	struct exynos_platform_gscaler	*pdata;
+	unsigned long			state;
+	struct vb2_alloc_ctx		*alloc_ctx;
+	struct video_device		vdev;
+	struct v4l2_device		v4l2_dev;
+};
+
+/**
+ * gsc_ctx - the device context data
+ * @s_frame:		source frame properties
+ * @d_frame:		destination frame properties
+ * @in_path:		input mode (DMA or camera)
+ * @out_path:		output mode (DMA or FIFO)
+ * @scaler:		image scaler properties
+ * @flags:		additional flags for image conversion
+ * @state:		flags to keep track of user configuration
+ * @gsc_dev:		the G-Scaler device this context applies to
+ * @m2m_ctx:		memory-to-memory device context
+ * @fh:                 v4l2 file handle
+ * @ctrl_handler:       v4l2 controls handler
+ * @gsc_ctrls		G-Scaler control set
+ * @ctrls_rdy:          true if the control handler is initialized
+ */
+struct gsc_ctx {
+	struct gsc_frame	s_frame;
+	struct gsc_frame	d_frame;
+	enum gsc_datapath	in_path;
+	enum gsc_datapath	out_path;
+	struct gsc_scaler	scaler;
+	u32			flags;
+	u32			state;
+	int			rotation;
+	unsigned int		hflip:1;
+	unsigned int		vflip:1;
+	struct gsc_dev		*gsc_dev;
+	struct v4l2_m2m_ctx	*m2m_ctx;
+	struct v4l2_fh		fh;
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct gsc_ctrls	gsc_ctrls;
+	bool			ctrls_rdy;
+};
+
+void gsc_set_prefbuf(struct gsc_dev *gsc, struct gsc_frame *frm);
+int gsc_register_m2m_device(struct gsc_dev *gsc);
+void gsc_unregister_m2m_device(struct gsc_dev *gsc);
+void gsc_m2m_job_finish(struct gsc_ctx *ctx, int vb_state);
+
+u32 get_plane_size(struct gsc_frame *fr, unsigned int plane);
+const struct gsc_fmt *get_format(int index);
+const struct gsc_fmt *find_fmt(u32 *pixelformat, u32 *mbus_code, u32 index);
+int gsc_enum_fmt_mplane(struct v4l2_fmtdesc *f);
+int gsc_try_fmt_mplane(struct gsc_ctx *ctx, struct v4l2_format *f);
+void gsc_set_frame_size(struct gsc_frame *frame, int width, int height);
+int gsc_g_fmt_mplane(struct gsc_ctx *ctx, struct v4l2_format *f);
+void gsc_check_crop_change(u32 tmp_w, u32 tmp_h, u32 *w, u32 *h);
+int gsc_g_crop(struct gsc_ctx *ctx, struct v4l2_crop *cr);
+int gsc_try_crop(struct gsc_ctx *ctx, struct v4l2_crop *cr);
+int gsc_cal_prescaler_ratio(struct gsc_variant *var, u32 src, u32 dst,
+							u32 *ratio);
+void gsc_get_prescaler_shfactor(u32 hratio, u32 vratio, u32 *sh);
+void gsc_check_src_scale_info(struct gsc_variant *var,
+				struct gsc_frame *s_frame,
+				u32 *wratio, u32 tx, u32 ty, u32 *hratio);
+int gsc_check_scaler_ratio(struct gsc_variant *var, int sw, int sh, int dw,
+			   int dh, int rot, int out_path);
+int gsc_set_scaler_info(struct gsc_ctx *ctx);
+int gsc_ctrls_create(struct gsc_ctx *ctx);
+void gsc_ctrls_delete(struct gsc_ctx *ctx);
+int gsc_prepare_addr(struct gsc_ctx *ctx, struct vb2_buffer *vb,
+		     struct gsc_frame *frame, struct gsc_addr *addr);
+
+static inline void gsc_ctx_state_lock_set(u32 state, struct gsc_ctx *ctx)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&ctx->gsc_dev->slock, flags);
+	ctx->state |= state;
+	spin_unlock_irqrestore(&ctx->gsc_dev->slock, flags);
+}
+
+static inline void gsc_ctx_state_lock_clear(u32 state, struct gsc_ctx *ctx)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&ctx->gsc_dev->slock, flags);
+	ctx->state &= ~state;
+	spin_unlock_irqrestore(&ctx->gsc_dev->slock, flags);
+}
+
+static inline int is_tiled(const struct gsc_fmt *fmt)
+{
+	return fmt->pixelformat == V4L2_PIX_FMT_NV12MT_16X16;
+}
+
+static inline void gsc_hw_enable_control(struct gsc_dev *dev, bool on)
+{
+	u32 cfg = readl(dev->regs + GSC_ENABLE);
+
+	if (on)
+		cfg |= GSC_ENABLE_ON;
+	else
+		cfg &= ~GSC_ENABLE_ON;
+
+	writel(cfg, dev->regs + GSC_ENABLE);
+}
+
+static inline int gsc_hw_get_irq_status(struct gsc_dev *dev)
+{
+	u32 cfg = readl(dev->regs + GSC_IRQ);
+	if (cfg & GSC_IRQ_STATUS_OR_IRQ)
+		return GSC_IRQ_OVERRUN;
+	else
+		return GSC_IRQ_DONE;
+
+}
+
+static inline void gsc_hw_clear_irq(struct gsc_dev *dev, int irq)
+{
+	u32 cfg = readl(dev->regs + GSC_IRQ);
+	if (irq == GSC_IRQ_OVERRUN)
+		cfg |= GSC_IRQ_STATUS_OR_IRQ;
+	else if (irq == GSC_IRQ_DONE)
+		cfg |= GSC_IRQ_STATUS_FRM_DONE_IRQ;
+	writel(cfg, dev->regs + GSC_IRQ);
+}
+
+static inline bool gsc_ctx_state_is_set(u32 mask, struct gsc_ctx *ctx)
+{
+	unsigned long flags;
+	bool ret;
+
+	spin_lock_irqsave(&ctx->gsc_dev->slock, flags);
+	ret = (ctx->state & mask) == mask;
+	spin_unlock_irqrestore(&ctx->gsc_dev->slock, flags);
+	return ret;
+}
+
+static inline struct gsc_frame *ctx_get_frame(struct gsc_ctx *ctx,
+					      enum v4l2_buf_type type)
+{
+	struct gsc_frame *frame;
+
+	if (V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE == type) {
+		frame = &ctx->s_frame;
+	} else if (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE == type) {
+		frame = &ctx->d_frame;
+	} else {
+		pr_err("Wrong buffer/video queue type (%d)", type);
+		return ERR_PTR(-EINVAL);
+	}
+
+	return frame;
+}
+
+void gsc_hw_set_sw_reset(struct gsc_dev *dev);
+int gsc_wait_reset(struct gsc_dev *dev);
+
+void gsc_hw_set_frm_done_irq_mask(struct gsc_dev *dev, bool mask);
+void gsc_hw_set_gsc_irq_enable(struct gsc_dev *dev, bool mask);
+void gsc_hw_set_input_buf_masking(struct gsc_dev *dev, u32 shift, bool enable);
+void gsc_hw_set_output_buf_masking(struct gsc_dev *dev, u32 shift, bool enable);
+void gsc_hw_set_input_addr(struct gsc_dev *dev, struct gsc_addr *addr,
+							int index);
+void gsc_hw_set_output_addr(struct gsc_dev *dev, struct gsc_addr *addr,
+							int index);
+void gsc_hw_set_input_path(struct gsc_ctx *ctx);
+void gsc_hw_set_in_size(struct gsc_ctx *ctx);
+void gsc_hw_set_in_image_rgb(struct gsc_ctx *ctx);
+void gsc_hw_set_in_image_format(struct gsc_ctx *ctx);
+void gsc_hw_set_output_path(struct gsc_ctx *ctx);
+void gsc_hw_set_out_size(struct gsc_ctx *ctx);
+void gsc_hw_set_out_image_rgb(struct gsc_ctx *ctx);
+void gsc_hw_set_out_image_format(struct gsc_ctx *ctx);
+void gsc_hw_set_prescaler(struct gsc_ctx *ctx);
+void gsc_hw_set_mainscaler(struct gsc_ctx *ctx);
+void gsc_hw_set_rotation(struct gsc_ctx *ctx);
+void gsc_hw_set_global_alpha(struct gsc_ctx *ctx);
+void gsc_hw_set_sfr_update(struct gsc_ctx *ctx);
+
+#endif /* GSC_CORE_H_ */
diff --git a/drivers/media/platform/exynos-gsc/gsc-m2m.c b/drivers/media/platform/exynos-gsc/gsc-m2m.c
new file mode 100644
index 0000000..d82e717
--- /dev/null
+++ b/drivers/media/platform/exynos-gsc/gsc-m2m.c
@@ -0,0 +1,795 @@
+/*
+ * Copyright (c) 2011 - 2012 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ *
+ * Samsung EXYNOS5 SoC series G-Scaler driver
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published
+ * by the Free Software Foundation, either version 2 of the License,
+ * or (at your option) any later version.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/bug.h>
+#include <linux/interrupt.h>
+#include <linux/workqueue.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/list.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/clk.h>
+
+#include <media/v4l2-ioctl.h>
+
+#include "gsc-core.h"
+
+static int gsc_m2m_ctx_stop_req(struct gsc_ctx *ctx)
+{
+	struct gsc_ctx *curr_ctx;
+	struct gsc_dev *gsc = ctx->gsc_dev;
+	int ret;
+
+	curr_ctx = v4l2_m2m_get_curr_priv(gsc->m2m.m2m_dev);
+	if (!gsc_m2m_pending(gsc) || (curr_ctx != ctx))
+		return 0;
+
+	gsc_ctx_state_lock_set(GSC_CTX_STOP_REQ, ctx);
+	ret = wait_event_timeout(gsc->irq_queue,
+			!gsc_ctx_state_is_set(GSC_CTX_STOP_REQ, ctx),
+			GSC_SHUTDOWN_TIMEOUT);
+
+	return ret == 0 ? -ETIMEDOUT : ret;
+}
+
+static void __gsc_m2m_job_abort(struct gsc_ctx *ctx)
+{
+	int ret;
+
+	ret = gsc_m2m_ctx_stop_req(ctx);
+	if ((ret == -ETIMEDOUT) || (ctx->state & GSC_CTX_ABORT)) {
+		gsc_ctx_state_lock_clear(GSC_CTX_STOP_REQ | GSC_CTX_ABORT, ctx);
+		gsc_m2m_job_finish(ctx, VB2_BUF_STATE_ERROR);
+	}
+}
+
+static int gsc_m2m_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct gsc_ctx *ctx = q->drv_priv;
+	int ret;
+
+	ret = pm_runtime_get_sync(&ctx->gsc_dev->pdev->dev);
+	return ret > 0 ? 0 : ret;
+}
+
+static void gsc_m2m_stop_streaming(struct vb2_queue *q)
+{
+	struct gsc_ctx *ctx = q->drv_priv;
+
+	__gsc_m2m_job_abort(ctx);
+
+	pm_runtime_put(&ctx->gsc_dev->pdev->dev);
+}
+
+void gsc_m2m_job_finish(struct gsc_ctx *ctx, int vb_state)
+{
+	struct vb2_v4l2_buffer *src_vb, *dst_vb;
+
+	if (!ctx || !ctx->m2m_ctx)
+		return;
+
+	src_vb = v4l2_m2m_src_buf_remove(ctx->m2m_ctx);
+	dst_vb = v4l2_m2m_dst_buf_remove(ctx->m2m_ctx);
+
+	if (src_vb && dst_vb) {
+		dst_vb->timestamp = src_vb->timestamp;
+		dst_vb->timecode = src_vb->timecode;
+		dst_vb->flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+		dst_vb->flags |=
+			src_vb->flags
+			& V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+
+		v4l2_m2m_buf_done(src_vb, vb_state);
+		v4l2_m2m_buf_done(dst_vb, vb_state);
+
+		v4l2_m2m_job_finish(ctx->gsc_dev->m2m.m2m_dev,
+				    ctx->m2m_ctx);
+	}
+}
+
+static void gsc_m2m_job_abort(void *priv)
+{
+	__gsc_m2m_job_abort((struct gsc_ctx *)priv);
+}
+
+static int gsc_get_bufs(struct gsc_ctx *ctx)
+{
+	struct gsc_frame *s_frame, *d_frame;
+	struct vb2_v4l2_buffer *src_vb, *dst_vb;
+	int ret;
+
+	s_frame = &ctx->s_frame;
+	d_frame = &ctx->d_frame;
+
+	src_vb = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
+	ret = gsc_prepare_addr(ctx, &src_vb->vb2_buf, s_frame, &s_frame->addr);
+	if (ret)
+		return ret;
+
+	dst_vb = v4l2_m2m_next_dst_buf(ctx->m2m_ctx);
+	ret = gsc_prepare_addr(ctx, &dst_vb->vb2_buf, d_frame, &d_frame->addr);
+	if (ret)
+		return ret;
+
+	dst_vb->timestamp = src_vb->timestamp;
+
+	return 0;
+}
+
+static void gsc_m2m_device_run(void *priv)
+{
+	struct gsc_ctx *ctx = priv;
+	struct gsc_dev *gsc;
+	unsigned long flags;
+	int ret;
+	bool is_set = false;
+
+	if (WARN(!ctx, "null hardware context\n"))
+		return;
+
+	gsc = ctx->gsc_dev;
+	spin_lock_irqsave(&gsc->slock, flags);
+
+	set_bit(ST_M2M_PEND, &gsc->state);
+
+	/* Reconfigure hardware if the context has changed. */
+	if (gsc->m2m.ctx != ctx) {
+		pr_debug("gsc->m2m.ctx = 0x%p, current_ctx = 0x%p",
+				gsc->m2m.ctx, ctx);
+		ctx->state |= GSC_PARAMS;
+		gsc->m2m.ctx = ctx;
+	}
+
+	is_set = ctx->state & GSC_CTX_STOP_REQ;
+	if (is_set) {
+		ctx->state &= ~GSC_CTX_STOP_REQ;
+		ctx->state |= GSC_CTX_ABORT;
+		wake_up(&gsc->irq_queue);
+		goto put_device;
+	}
+
+	ret = gsc_get_bufs(ctx);
+	if (ret) {
+		pr_err("Wrong address");
+		goto put_device;
+	}
+
+	gsc_set_prefbuf(gsc, &ctx->s_frame);
+	gsc_hw_set_input_addr(gsc, &ctx->s_frame.addr, GSC_M2M_BUF_NUM);
+	gsc_hw_set_output_addr(gsc, &ctx->d_frame.addr, GSC_M2M_BUF_NUM);
+
+	if (ctx->state & GSC_PARAMS) {
+		gsc_hw_set_input_buf_masking(gsc, GSC_M2M_BUF_NUM, false);
+		gsc_hw_set_output_buf_masking(gsc, GSC_M2M_BUF_NUM, false);
+		gsc_hw_set_frm_done_irq_mask(gsc, false);
+		gsc_hw_set_gsc_irq_enable(gsc, true);
+
+		if (gsc_set_scaler_info(ctx)) {
+			pr_err("Scaler setup error");
+			goto put_device;
+		}
+
+		gsc_hw_set_input_path(ctx);
+		gsc_hw_set_in_size(ctx);
+		gsc_hw_set_in_image_format(ctx);
+
+		gsc_hw_set_output_path(ctx);
+		gsc_hw_set_out_size(ctx);
+		gsc_hw_set_out_image_format(ctx);
+
+		gsc_hw_set_prescaler(ctx);
+		gsc_hw_set_mainscaler(ctx);
+		gsc_hw_set_rotation(ctx);
+		gsc_hw_set_global_alpha(ctx);
+	}
+
+	/* update shadow registers */
+	gsc_hw_set_sfr_update(ctx);
+
+	ctx->state &= ~GSC_PARAMS;
+	gsc_hw_enable_control(gsc, true);
+
+	spin_unlock_irqrestore(&gsc->slock, flags);
+	return;
+
+put_device:
+	ctx->state &= ~GSC_PARAMS;
+	spin_unlock_irqrestore(&gsc->slock, flags);
+}
+
+static int gsc_m2m_queue_setup(struct vb2_queue *vq,
+			const void *parg,
+			unsigned int *num_buffers, unsigned int *num_planes,
+			unsigned int sizes[], void *allocators[])
+{
+	struct gsc_ctx *ctx = vb2_get_drv_priv(vq);
+	struct gsc_frame *frame;
+	int i;
+
+	frame = ctx_get_frame(ctx, vq->type);
+	if (IS_ERR(frame))
+		return PTR_ERR(frame);
+
+	if (!frame->fmt)
+		return -EINVAL;
+
+	*num_planes = frame->fmt->num_planes;
+	for (i = 0; i < frame->fmt->num_planes; i++) {
+		sizes[i] = frame->payload[i];
+		allocators[i] = ctx->gsc_dev->alloc_ctx;
+	}
+	return 0;
+}
+
+static int gsc_m2m_buf_prepare(struct vb2_buffer *vb)
+{
+	struct gsc_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct gsc_frame *frame;
+	int i;
+
+	frame = ctx_get_frame(ctx, vb->vb2_queue->type);
+	if (IS_ERR(frame))
+		return PTR_ERR(frame);
+
+	if (!V4L2_TYPE_IS_OUTPUT(vb->vb2_queue->type)) {
+		for (i = 0; i < frame->fmt->num_planes; i++)
+			vb2_set_plane_payload(vb, i, frame->payload[i]);
+	}
+
+	return 0;
+}
+
+static void gsc_m2m_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct gsc_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+
+	pr_debug("ctx: %p, ctx->state: 0x%x", ctx, ctx->state);
+
+	if (ctx->m2m_ctx)
+		v4l2_m2m_buf_queue(ctx->m2m_ctx, vbuf);
+}
+
+static struct vb2_ops gsc_m2m_qops = {
+	.queue_setup	 = gsc_m2m_queue_setup,
+	.buf_prepare	 = gsc_m2m_buf_prepare,
+	.buf_queue	 = gsc_m2m_buf_queue,
+	.wait_prepare	 = vb2_ops_wait_prepare,
+	.wait_finish	 = vb2_ops_wait_finish,
+	.stop_streaming	 = gsc_m2m_stop_streaming,
+	.start_streaming = gsc_m2m_start_streaming,
+};
+
+static int gsc_m2m_querycap(struct file *file, void *fh,
+			   struct v4l2_capability *cap)
+{
+	struct gsc_ctx *ctx = fh_to_ctx(fh);
+	struct gsc_dev *gsc = ctx->gsc_dev;
+
+	strlcpy(cap->driver, gsc->pdev->name, sizeof(cap->driver));
+	strlcpy(cap->card, gsc->pdev->name, sizeof(cap->card));
+	strlcpy(cap->bus_info, "platform", sizeof(cap->bus_info));
+	cap->device_caps = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_M2M_MPLANE |
+		V4L2_CAP_VIDEO_CAPTURE_MPLANE |	V4L2_CAP_VIDEO_OUTPUT_MPLANE;
+
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	return 0;
+}
+
+static int gsc_m2m_enum_fmt_mplane(struct file *file, void *priv,
+				struct v4l2_fmtdesc *f)
+{
+	return gsc_enum_fmt_mplane(f);
+}
+
+static int gsc_m2m_g_fmt_mplane(struct file *file, void *fh,
+			     struct v4l2_format *f)
+{
+	struct gsc_ctx *ctx = fh_to_ctx(fh);
+
+	return gsc_g_fmt_mplane(ctx, f);
+}
+
+static int gsc_m2m_try_fmt_mplane(struct file *file, void *fh,
+				  struct v4l2_format *f)
+{
+	struct gsc_ctx *ctx = fh_to_ctx(fh);
+
+	return gsc_try_fmt_mplane(ctx, f);
+}
+
+static int gsc_m2m_s_fmt_mplane(struct file *file, void *fh,
+				 struct v4l2_format *f)
+{
+	struct gsc_ctx *ctx = fh_to_ctx(fh);
+	struct vb2_queue *vq;
+	struct gsc_frame *frame;
+	struct v4l2_pix_format_mplane *pix;
+	int i, ret = 0;
+
+	ret = gsc_m2m_try_fmt_mplane(file, fh, f);
+	if (ret)
+		return ret;
+
+	vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
+
+	if (vb2_is_streaming(vq)) {
+		pr_err("queue (%d) busy", f->type);
+		return -EBUSY;
+	}
+
+	if (V4L2_TYPE_IS_OUTPUT(f->type))
+		frame = &ctx->s_frame;
+	else
+		frame = &ctx->d_frame;
+
+	pix = &f->fmt.pix_mp;
+	frame->fmt = find_fmt(&pix->pixelformat, NULL, 0);
+	frame->colorspace = pix->colorspace;
+	if (!frame->fmt)
+		return -EINVAL;
+
+	for (i = 0; i < frame->fmt->num_planes; i++)
+		frame->payload[i] = pix->plane_fmt[i].sizeimage;
+
+	gsc_set_frame_size(frame, pix->width, pix->height);
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		gsc_ctx_state_lock_set(GSC_PARAMS | GSC_DST_FMT, ctx);
+	else
+		gsc_ctx_state_lock_set(GSC_PARAMS | GSC_SRC_FMT, ctx);
+
+	pr_debug("f_w: %d, f_h: %d", frame->f_width, frame->f_height);
+
+	return 0;
+}
+
+static int gsc_m2m_reqbufs(struct file *file, void *fh,
+			  struct v4l2_requestbuffers *reqbufs)
+{
+	struct gsc_ctx *ctx = fh_to_ctx(fh);
+	struct gsc_dev *gsc = ctx->gsc_dev;
+	u32 max_cnt;
+
+	max_cnt = (reqbufs->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) ?
+		gsc->variant->in_buf_cnt : gsc->variant->out_buf_cnt;
+	if (reqbufs->count > max_cnt) {
+		return -EINVAL;
+	} else if (reqbufs->count == 0) {
+		if (reqbufs->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+			gsc_ctx_state_lock_clear(GSC_SRC_FMT, ctx);
+		else
+			gsc_ctx_state_lock_clear(GSC_DST_FMT, ctx);
+	}
+
+	return v4l2_m2m_reqbufs(file, ctx->m2m_ctx, reqbufs);
+}
+
+static int gsc_m2m_expbuf(struct file *file, void *fh,
+				struct v4l2_exportbuffer *eb)
+{
+	struct gsc_ctx *ctx = fh_to_ctx(fh);
+	return v4l2_m2m_expbuf(file, ctx->m2m_ctx, eb);
+}
+
+static int gsc_m2m_querybuf(struct file *file, void *fh,
+					struct v4l2_buffer *buf)
+{
+	struct gsc_ctx *ctx = fh_to_ctx(fh);
+	return v4l2_m2m_querybuf(file, ctx->m2m_ctx, buf);
+}
+
+static int gsc_m2m_qbuf(struct file *file, void *fh,
+			  struct v4l2_buffer *buf)
+{
+	struct gsc_ctx *ctx = fh_to_ctx(fh);
+	return v4l2_m2m_qbuf(file, ctx->m2m_ctx, buf);
+}
+
+static int gsc_m2m_dqbuf(struct file *file, void *fh,
+			   struct v4l2_buffer *buf)
+{
+	struct gsc_ctx *ctx = fh_to_ctx(fh);
+	return v4l2_m2m_dqbuf(file, ctx->m2m_ctx, buf);
+}
+
+static int gsc_m2m_streamon(struct file *file, void *fh,
+			   enum v4l2_buf_type type)
+{
+	struct gsc_ctx *ctx = fh_to_ctx(fh);
+
+	/* The source and target color format need to be set */
+	if (V4L2_TYPE_IS_OUTPUT(type)) {
+		if (!gsc_ctx_state_is_set(GSC_SRC_FMT, ctx))
+			return -EINVAL;
+	} else if (!gsc_ctx_state_is_set(GSC_DST_FMT, ctx)) {
+		return -EINVAL;
+	}
+
+	return v4l2_m2m_streamon(file, ctx->m2m_ctx, type);
+}
+
+static int gsc_m2m_streamoff(struct file *file, void *fh,
+			    enum v4l2_buf_type type)
+{
+	struct gsc_ctx *ctx = fh_to_ctx(fh);
+	return v4l2_m2m_streamoff(file, ctx->m2m_ctx, type);
+}
+
+/* Return 1 if rectangle a is enclosed in rectangle b, or 0 otherwise. */
+static int is_rectangle_enclosed(struct v4l2_rect *a, struct v4l2_rect *b)
+{
+	if (a->left < b->left || a->top < b->top)
+		return 0;
+
+	if (a->left + a->width > b->left + b->width)
+		return 0;
+
+	if (a->top + a->height > b->top + b->height)
+		return 0;
+
+	return 1;
+}
+
+static int gsc_m2m_g_selection(struct file *file, void *fh,
+			struct v4l2_selection *s)
+{
+	struct gsc_frame *frame;
+	struct gsc_ctx *ctx = fh_to_ctx(fh);
+
+	if ((s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) &&
+	    (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE))
+		return -EINVAL;
+
+	frame = ctx_get_frame(ctx, s->type);
+	if (IS_ERR(frame))
+		return PTR_ERR(frame);
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		s->r.left = 0;
+		s->r.top = 0;
+		s->r.width = frame->f_width;
+		s->r.height = frame->f_height;
+		return 0;
+
+	case V4L2_SEL_TGT_COMPOSE:
+	case V4L2_SEL_TGT_CROP:
+		s->r.left = frame->crop.left;
+		s->r.top = frame->crop.top;
+		s->r.width = frame->crop.width;
+		s->r.height = frame->crop.height;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int gsc_m2m_s_selection(struct file *file, void *fh,
+				struct v4l2_selection *s)
+{
+	struct gsc_frame *frame;
+	struct gsc_ctx *ctx = fh_to_ctx(fh);
+	struct v4l2_crop cr;
+	struct gsc_variant *variant = ctx->gsc_dev->variant;
+	int ret;
+
+	cr.type = s->type;
+	cr.c = s->r;
+
+	if ((s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) &&
+	    (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE))
+		return -EINVAL;
+
+	ret = gsc_try_crop(ctx, &cr);
+	if (ret)
+		return ret;
+
+	if (s->flags & V4L2_SEL_FLAG_LE &&
+	    !is_rectangle_enclosed(&cr.c, &s->r))
+		return -ERANGE;
+
+	if (s->flags & V4L2_SEL_FLAG_GE &&
+	    !is_rectangle_enclosed(&s->r, &cr.c))
+		return -ERANGE;
+
+	s->r = cr.c;
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+	case V4L2_SEL_TGT_COMPOSE:
+		frame = &ctx->s_frame;
+		break;
+
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		frame = &ctx->d_frame;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	/* Check to see if scaling ratio is within supported range */
+	if (gsc_ctx_state_is_set(GSC_DST_FMT | GSC_SRC_FMT, ctx)) {
+		if (s->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+			ret = gsc_check_scaler_ratio(variant, cr.c.width,
+				cr.c.height, ctx->d_frame.crop.width,
+				ctx->d_frame.crop.height,
+				ctx->gsc_ctrls.rotate->val, ctx->out_path);
+		} else {
+			ret = gsc_check_scaler_ratio(variant,
+				ctx->s_frame.crop.width,
+				ctx->s_frame.crop.height, cr.c.width,
+				cr.c.height, ctx->gsc_ctrls.rotate->val,
+				ctx->out_path);
+		}
+
+		if (ret) {
+			pr_err("Out of scaler range");
+			return -EINVAL;
+		}
+	}
+
+	frame->crop = cr.c;
+
+	gsc_ctx_state_lock_set(GSC_PARAMS, ctx);
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops gsc_m2m_ioctl_ops = {
+	.vidioc_querycap		= gsc_m2m_querycap,
+	.vidioc_enum_fmt_vid_cap_mplane	= gsc_m2m_enum_fmt_mplane,
+	.vidioc_enum_fmt_vid_out_mplane	= gsc_m2m_enum_fmt_mplane,
+	.vidioc_g_fmt_vid_cap_mplane	= gsc_m2m_g_fmt_mplane,
+	.vidioc_g_fmt_vid_out_mplane	= gsc_m2m_g_fmt_mplane,
+	.vidioc_try_fmt_vid_cap_mplane	= gsc_m2m_try_fmt_mplane,
+	.vidioc_try_fmt_vid_out_mplane	= gsc_m2m_try_fmt_mplane,
+	.vidioc_s_fmt_vid_cap_mplane	= gsc_m2m_s_fmt_mplane,
+	.vidioc_s_fmt_vid_out_mplane	= gsc_m2m_s_fmt_mplane,
+	.vidioc_reqbufs			= gsc_m2m_reqbufs,
+	.vidioc_expbuf                  = gsc_m2m_expbuf,
+	.vidioc_querybuf		= gsc_m2m_querybuf,
+	.vidioc_qbuf			= gsc_m2m_qbuf,
+	.vidioc_dqbuf			= gsc_m2m_dqbuf,
+	.vidioc_streamon		= gsc_m2m_streamon,
+	.vidioc_streamoff		= gsc_m2m_streamoff,
+	.vidioc_g_selection		= gsc_m2m_g_selection,
+	.vidioc_s_selection		= gsc_m2m_s_selection
+};
+
+static int queue_init(void *priv, struct vb2_queue *src_vq,
+			struct vb2_queue *dst_vq)
+{
+	struct gsc_ctx *ctx = priv;
+	int ret;
+
+	memset(src_vq, 0, sizeof(*src_vq));
+	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+	src_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
+	src_vq->drv_priv = ctx;
+	src_vq->ops = &gsc_m2m_qops;
+	src_vq->mem_ops = &vb2_dma_contig_memops;
+	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	src_vq->lock = &ctx->gsc_dev->lock;
+
+	ret = vb2_queue_init(src_vq);
+	if (ret)
+		return ret;
+
+	memset(dst_vq, 0, sizeof(*dst_vq));
+	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	dst_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
+	dst_vq->drv_priv = ctx;
+	dst_vq->ops = &gsc_m2m_qops;
+	dst_vq->mem_ops = &vb2_dma_contig_memops;
+	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	dst_vq->lock = &ctx->gsc_dev->lock;
+
+	return vb2_queue_init(dst_vq);
+}
+
+static int gsc_m2m_open(struct file *file)
+{
+	struct gsc_dev *gsc = video_drvdata(file);
+	struct gsc_ctx *ctx = NULL;
+	int ret;
+
+	pr_debug("pid: %d, state: 0x%lx", task_pid_nr(current), gsc->state);
+
+	if (mutex_lock_interruptible(&gsc->lock))
+		return -ERESTARTSYS;
+
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx) {
+		ret = -ENOMEM;
+		goto unlock;
+	}
+
+	v4l2_fh_init(&ctx->fh, gsc->m2m.vfd);
+	ret = gsc_ctrls_create(ctx);
+	if (ret)
+		goto error_fh;
+
+	/* Use separate control handler per file handle */
+	ctx->fh.ctrl_handler = &ctx->ctrl_handler;
+	file->private_data = &ctx->fh;
+	v4l2_fh_add(&ctx->fh);
+
+	ctx->gsc_dev = gsc;
+	/* Default color format */
+	ctx->s_frame.fmt = get_format(0);
+	ctx->d_frame.fmt = get_format(0);
+	/* Setup the device context for mem2mem mode. */
+	ctx->state = GSC_CTX_M2M;
+	ctx->flags = 0;
+	ctx->in_path = GSC_DMA;
+	ctx->out_path = GSC_DMA;
+
+	ctx->m2m_ctx = v4l2_m2m_ctx_init(gsc->m2m.m2m_dev, ctx, queue_init);
+	if (IS_ERR(ctx->m2m_ctx)) {
+		pr_err("Failed to initialize m2m context");
+		ret = PTR_ERR(ctx->m2m_ctx);
+		goto error_ctrls;
+	}
+
+	if (gsc->m2m.refcnt++ == 0)
+		set_bit(ST_M2M_OPEN, &gsc->state);
+
+	pr_debug("gsc m2m driver is opened, ctx(0x%p)", ctx);
+
+	mutex_unlock(&gsc->lock);
+	return 0;
+
+error_ctrls:
+	gsc_ctrls_delete(ctx);
+error_fh:
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	kfree(ctx);
+unlock:
+	mutex_unlock(&gsc->lock);
+	return ret;
+}
+
+static int gsc_m2m_release(struct file *file)
+{
+	struct gsc_ctx *ctx = fh_to_ctx(file->private_data);
+	struct gsc_dev *gsc = ctx->gsc_dev;
+
+	pr_debug("pid: %d, state: 0x%lx, refcnt= %d",
+		task_pid_nr(current), gsc->state, gsc->m2m.refcnt);
+
+	mutex_lock(&gsc->lock);
+
+	v4l2_m2m_ctx_release(ctx->m2m_ctx);
+	gsc_ctrls_delete(ctx);
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+
+	if (--gsc->m2m.refcnt <= 0)
+		clear_bit(ST_M2M_OPEN, &gsc->state);
+	kfree(ctx);
+
+	mutex_unlock(&gsc->lock);
+	return 0;
+}
+
+static unsigned int gsc_m2m_poll(struct file *file,
+					struct poll_table_struct *wait)
+{
+	struct gsc_ctx *ctx = fh_to_ctx(file->private_data);
+	struct gsc_dev *gsc = ctx->gsc_dev;
+	int ret;
+
+	if (mutex_lock_interruptible(&gsc->lock))
+		return -ERESTARTSYS;
+
+	ret = v4l2_m2m_poll(file, ctx->m2m_ctx, wait);
+	mutex_unlock(&gsc->lock);
+
+	return ret;
+}
+
+static int gsc_m2m_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct gsc_ctx *ctx = fh_to_ctx(file->private_data);
+	struct gsc_dev *gsc = ctx->gsc_dev;
+	int ret;
+
+	if (mutex_lock_interruptible(&gsc->lock))
+		return -ERESTARTSYS;
+
+	ret = v4l2_m2m_mmap(file, ctx->m2m_ctx, vma);
+	mutex_unlock(&gsc->lock);
+
+	return ret;
+}
+
+static const struct v4l2_file_operations gsc_m2m_fops = {
+	.owner		= THIS_MODULE,
+	.open		= gsc_m2m_open,
+	.release	= gsc_m2m_release,
+	.poll		= gsc_m2m_poll,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap		= gsc_m2m_mmap,
+};
+
+static struct v4l2_m2m_ops gsc_m2m_ops = {
+	.device_run	= gsc_m2m_device_run,
+	.job_abort	= gsc_m2m_job_abort,
+};
+
+int gsc_register_m2m_device(struct gsc_dev *gsc)
+{
+	struct platform_device *pdev;
+	int ret;
+
+	if (!gsc)
+		return -ENODEV;
+
+	pdev = gsc->pdev;
+
+	gsc->vdev.fops		= &gsc_m2m_fops;
+	gsc->vdev.ioctl_ops	= &gsc_m2m_ioctl_ops;
+	gsc->vdev.release	= video_device_release_empty;
+	gsc->vdev.lock		= &gsc->lock;
+	gsc->vdev.vfl_dir	= VFL_DIR_M2M;
+	gsc->vdev.v4l2_dev	= &gsc->v4l2_dev;
+	snprintf(gsc->vdev.name, sizeof(gsc->vdev.name), "%s.%d:m2m",
+					GSC_MODULE_NAME, gsc->id);
+
+	video_set_drvdata(&gsc->vdev, gsc);
+
+	gsc->m2m.vfd = &gsc->vdev;
+	gsc->m2m.m2m_dev = v4l2_m2m_init(&gsc_m2m_ops);
+	if (IS_ERR(gsc->m2m.m2m_dev)) {
+		dev_err(&pdev->dev, "failed to initialize v4l2-m2m device\n");
+		ret = PTR_ERR(gsc->m2m.m2m_dev);
+		goto err_m2m_r1;
+	}
+
+	ret = video_register_device(&gsc->vdev, VFL_TYPE_GRABBER, -1);
+	if (ret) {
+		dev_err(&pdev->dev,
+			 "%s(): failed to register video device\n", __func__);
+		goto err_m2m_r2;
+	}
+
+	pr_debug("gsc m2m driver registered as /dev/video%d", gsc->vdev.num);
+	return 0;
+
+err_m2m_r2:
+	v4l2_m2m_release(gsc->m2m.m2m_dev);
+err_m2m_r1:
+	video_device_release(gsc->m2m.vfd);
+
+	return ret;
+}
+
+void gsc_unregister_m2m_device(struct gsc_dev *gsc)
+{
+	if (gsc)
+		v4l2_m2m_release(gsc->m2m.m2m_dev);
+}
diff --git a/drivers/media/platform/exynos-gsc/gsc-regs.c b/drivers/media/platform/exynos-gsc/gsc-regs.c
new file mode 100644
index 0000000..ce12a11
--- /dev/null
+++ b/drivers/media/platform/exynos-gsc/gsc-regs.c
@@ -0,0 +1,430 @@
+/*
+ * Copyright (c) 2011 - 2012 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ *
+ * Samsung EXYNOS5 SoC series G-Scaler driver
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published
+ * by the Free Software Foundation, either version 2 of the License,
+ * or (at your option) any later version.
+ */
+
+#include <linux/io.h>
+#include <linux/delay.h>
+
+#include "gsc-core.h"
+
+void gsc_hw_set_sw_reset(struct gsc_dev *dev)
+{
+	writel(GSC_SW_RESET_SRESET, dev->regs + GSC_SW_RESET);
+}
+
+int gsc_wait_reset(struct gsc_dev *dev)
+{
+	unsigned long end = jiffies + msecs_to_jiffies(50);
+	u32 cfg;
+
+	while (time_before(jiffies, end)) {
+		cfg = readl(dev->regs + GSC_SW_RESET);
+		if (!cfg)
+			return 0;
+		usleep_range(10, 20);
+	}
+
+	return -EBUSY;
+}
+
+void gsc_hw_set_frm_done_irq_mask(struct gsc_dev *dev, bool mask)
+{
+	u32 cfg;
+
+	cfg = readl(dev->regs + GSC_IRQ);
+	if (mask)
+		cfg |= GSC_IRQ_FRMDONE_MASK;
+	else
+		cfg &= ~GSC_IRQ_FRMDONE_MASK;
+	writel(cfg, dev->regs + GSC_IRQ);
+}
+
+void gsc_hw_set_gsc_irq_enable(struct gsc_dev *dev, bool mask)
+{
+	u32 cfg;
+
+	cfg = readl(dev->regs + GSC_IRQ);
+	if (mask)
+		cfg |= GSC_IRQ_ENABLE;
+	else
+		cfg &= ~GSC_IRQ_ENABLE;
+	writel(cfg, dev->regs + GSC_IRQ);
+}
+
+void gsc_hw_set_input_buf_masking(struct gsc_dev *dev, u32 shift,
+				bool enable)
+{
+	u32 cfg = readl(dev->regs + GSC_IN_BASE_ADDR_Y_MASK);
+	u32 mask = 1 << shift;
+
+	cfg &= ~mask;
+	cfg |= enable << shift;
+
+	writel(cfg, dev->regs + GSC_IN_BASE_ADDR_Y_MASK);
+	writel(cfg, dev->regs + GSC_IN_BASE_ADDR_CB_MASK);
+	writel(cfg, dev->regs + GSC_IN_BASE_ADDR_CR_MASK);
+}
+
+void gsc_hw_set_output_buf_masking(struct gsc_dev *dev, u32 shift,
+				bool enable)
+{
+	u32 cfg = readl(dev->regs + GSC_OUT_BASE_ADDR_Y_MASK);
+	u32 mask = 1 << shift;
+
+	cfg &= ~mask;
+	cfg |= enable << shift;
+
+	writel(cfg, dev->regs + GSC_OUT_BASE_ADDR_Y_MASK);
+	writel(cfg, dev->regs + GSC_OUT_BASE_ADDR_CB_MASK);
+	writel(cfg, dev->regs + GSC_OUT_BASE_ADDR_CR_MASK);
+}
+
+void gsc_hw_set_input_addr(struct gsc_dev *dev, struct gsc_addr *addr,
+				int index)
+{
+	pr_debug("src_buf[%d]: %pad, cb: %pad, cr: %pad", index,
+			&addr->y, &addr->cb, &addr->cr);
+	writel(addr->y, dev->regs + GSC_IN_BASE_ADDR_Y(index));
+	writel(addr->cb, dev->regs + GSC_IN_BASE_ADDR_CB(index));
+	writel(addr->cr, dev->regs + GSC_IN_BASE_ADDR_CR(index));
+
+}
+
+void gsc_hw_set_output_addr(struct gsc_dev *dev,
+			     struct gsc_addr *addr, int index)
+{
+	pr_debug("dst_buf[%d]: %pad, cb: %pad, cr: %pad",
+			index, &addr->y, &addr->cb, &addr->cr);
+	writel(addr->y, dev->regs + GSC_OUT_BASE_ADDR_Y(index));
+	writel(addr->cb, dev->regs + GSC_OUT_BASE_ADDR_CB(index));
+	writel(addr->cr, dev->regs + GSC_OUT_BASE_ADDR_CR(index));
+}
+
+void gsc_hw_set_input_path(struct gsc_ctx *ctx)
+{
+	struct gsc_dev *dev = ctx->gsc_dev;
+
+	u32 cfg = readl(dev->regs + GSC_IN_CON);
+	cfg &= ~(GSC_IN_PATH_MASK | GSC_IN_LOCAL_SEL_MASK);
+
+	if (ctx->in_path == GSC_DMA)
+		cfg |= GSC_IN_PATH_MEMORY;
+
+	writel(cfg, dev->regs + GSC_IN_CON);
+}
+
+void gsc_hw_set_in_size(struct gsc_ctx *ctx)
+{
+	struct gsc_dev *dev = ctx->gsc_dev;
+	struct gsc_frame *frame = &ctx->s_frame;
+	u32 cfg;
+
+	/* Set input pixel offset */
+	cfg = GSC_SRCIMG_OFFSET_X(frame->crop.left);
+	cfg |= GSC_SRCIMG_OFFSET_Y(frame->crop.top);
+	writel(cfg, dev->regs + GSC_SRCIMG_OFFSET);
+
+	/* Set input original size */
+	cfg = GSC_SRCIMG_WIDTH(frame->f_width);
+	cfg |= GSC_SRCIMG_HEIGHT(frame->f_height);
+	writel(cfg, dev->regs + GSC_SRCIMG_SIZE);
+
+	/* Set input cropped size */
+	cfg = GSC_CROPPED_WIDTH(frame->crop.width);
+	cfg |= GSC_CROPPED_HEIGHT(frame->crop.height);
+	writel(cfg, dev->regs + GSC_CROPPED_SIZE);
+}
+
+void gsc_hw_set_in_image_rgb(struct gsc_ctx *ctx)
+{
+	struct gsc_dev *dev = ctx->gsc_dev;
+	struct gsc_frame *frame = &ctx->s_frame;
+	u32 cfg;
+
+	cfg = readl(dev->regs + GSC_IN_CON);
+	if (frame->colorspace == V4L2_COLORSPACE_REC709)
+		cfg |= GSC_IN_RGB_HD_WIDE;
+	else
+		cfg |= GSC_IN_RGB_SD_WIDE;
+
+	if (frame->fmt->pixelformat == V4L2_PIX_FMT_RGB565X)
+		cfg |= GSC_IN_RGB565;
+	else if (frame->fmt->pixelformat == V4L2_PIX_FMT_RGB32)
+		cfg |= GSC_IN_XRGB8888;
+
+	writel(cfg, dev->regs + GSC_IN_CON);
+}
+
+void gsc_hw_set_in_image_format(struct gsc_ctx *ctx)
+{
+	struct gsc_dev *dev = ctx->gsc_dev;
+	struct gsc_frame *frame = &ctx->s_frame;
+	u32 i, depth = 0;
+	u32 cfg;
+
+	cfg = readl(dev->regs + GSC_IN_CON);
+	cfg &= ~(GSC_IN_RGB_TYPE_MASK | GSC_IN_YUV422_1P_ORDER_MASK |
+		 GSC_IN_CHROMA_ORDER_MASK | GSC_IN_FORMAT_MASK |
+		 GSC_IN_TILE_TYPE_MASK | GSC_IN_TILE_MODE);
+	writel(cfg, dev->regs + GSC_IN_CON);
+
+	if (is_rgb(frame->fmt->color)) {
+		gsc_hw_set_in_image_rgb(ctx);
+		return;
+	}
+	for (i = 0; i < frame->fmt->num_planes; i++)
+		depth += frame->fmt->depth[i];
+
+	switch (frame->fmt->num_comp) {
+	case 1:
+		cfg |= GSC_IN_YUV422_1P;
+		if (frame->fmt->yorder == GSC_LSB_Y)
+			cfg |= GSC_IN_YUV422_1P_ORDER_LSB_Y;
+		else
+			cfg |= GSC_IN_YUV422_1P_OEDER_LSB_C;
+		if (frame->fmt->corder == GSC_CBCR)
+			cfg |= GSC_IN_CHROMA_ORDER_CBCR;
+		else
+			cfg |= GSC_IN_CHROMA_ORDER_CRCB;
+		break;
+	case 2:
+		if (depth == 12)
+			cfg |= GSC_IN_YUV420_2P;
+		else
+			cfg |= GSC_IN_YUV422_2P;
+		if (frame->fmt->corder == GSC_CBCR)
+			cfg |= GSC_IN_CHROMA_ORDER_CBCR;
+		else
+			cfg |= GSC_IN_CHROMA_ORDER_CRCB;
+		break;
+	case 3:
+		if (depth == 12)
+			cfg |= GSC_IN_YUV420_3P;
+		else
+			cfg |= GSC_IN_YUV422_3P;
+		break;
+	}
+
+	if (is_tiled(frame->fmt))
+		cfg |= GSC_IN_TILE_C_16x8 | GSC_IN_TILE_MODE;
+
+	writel(cfg, dev->regs + GSC_IN_CON);
+}
+
+void gsc_hw_set_output_path(struct gsc_ctx *ctx)
+{
+	struct gsc_dev *dev = ctx->gsc_dev;
+
+	u32 cfg = readl(dev->regs + GSC_OUT_CON);
+	cfg &= ~GSC_OUT_PATH_MASK;
+
+	if (ctx->out_path == GSC_DMA)
+		cfg |= GSC_OUT_PATH_MEMORY;
+	else
+		cfg |= GSC_OUT_PATH_LOCAL;
+
+	writel(cfg, dev->regs + GSC_OUT_CON);
+}
+
+void gsc_hw_set_out_size(struct gsc_ctx *ctx)
+{
+	struct gsc_dev *dev = ctx->gsc_dev;
+	struct gsc_frame *frame = &ctx->d_frame;
+	u32 cfg;
+
+	/* Set output original size */
+	if (ctx->out_path == GSC_DMA) {
+		cfg = GSC_DSTIMG_OFFSET_X(frame->crop.left);
+		cfg |= GSC_DSTIMG_OFFSET_Y(frame->crop.top);
+		writel(cfg, dev->regs + GSC_DSTIMG_OFFSET);
+
+		cfg = GSC_DSTIMG_WIDTH(frame->f_width);
+		cfg |= GSC_DSTIMG_HEIGHT(frame->f_height);
+		writel(cfg, dev->regs + GSC_DSTIMG_SIZE);
+	}
+
+	/* Set output scaled size */
+	if (ctx->gsc_ctrls.rotate->val == 90 ||
+	    ctx->gsc_ctrls.rotate->val == 270) {
+		cfg = GSC_SCALED_WIDTH(frame->crop.height);
+		cfg |= GSC_SCALED_HEIGHT(frame->crop.width);
+	} else {
+		cfg = GSC_SCALED_WIDTH(frame->crop.width);
+		cfg |= GSC_SCALED_HEIGHT(frame->crop.height);
+	}
+	writel(cfg, dev->regs + GSC_SCALED_SIZE);
+}
+
+void gsc_hw_set_out_image_rgb(struct gsc_ctx *ctx)
+{
+	struct gsc_dev *dev = ctx->gsc_dev;
+	struct gsc_frame *frame = &ctx->d_frame;
+	u32 cfg;
+
+	cfg = readl(dev->regs + GSC_OUT_CON);
+	if (frame->colorspace == V4L2_COLORSPACE_REC709)
+		cfg |= GSC_OUT_RGB_HD_WIDE;
+	else
+		cfg |= GSC_OUT_RGB_SD_WIDE;
+
+	if (frame->fmt->pixelformat == V4L2_PIX_FMT_RGB565X)
+		cfg |= GSC_OUT_RGB565;
+	else if (frame->fmt->pixelformat == V4L2_PIX_FMT_RGB32)
+		cfg |= GSC_OUT_XRGB8888;
+
+	writel(cfg, dev->regs + GSC_OUT_CON);
+}
+
+void gsc_hw_set_out_image_format(struct gsc_ctx *ctx)
+{
+	struct gsc_dev *dev = ctx->gsc_dev;
+	struct gsc_frame *frame = &ctx->d_frame;
+	u32 i, depth = 0;
+	u32 cfg;
+
+	cfg = readl(dev->regs + GSC_OUT_CON);
+	cfg &= ~(GSC_OUT_RGB_TYPE_MASK | GSC_OUT_YUV422_1P_ORDER_MASK |
+		 GSC_OUT_CHROMA_ORDER_MASK | GSC_OUT_FORMAT_MASK |
+		 GSC_OUT_TILE_TYPE_MASK | GSC_OUT_TILE_MODE);
+	writel(cfg, dev->regs + GSC_OUT_CON);
+
+	if (is_rgb(frame->fmt->color)) {
+		gsc_hw_set_out_image_rgb(ctx);
+		return;
+	}
+
+	if (ctx->out_path != GSC_DMA) {
+		cfg |= GSC_OUT_YUV444;
+		goto end_set;
+	}
+
+	for (i = 0; i < frame->fmt->num_planes; i++)
+		depth += frame->fmt->depth[i];
+
+	switch (frame->fmt->num_comp) {
+	case 1:
+		cfg |= GSC_OUT_YUV422_1P;
+		if (frame->fmt->yorder == GSC_LSB_Y)
+			cfg |= GSC_OUT_YUV422_1P_ORDER_LSB_Y;
+		else
+			cfg |= GSC_OUT_YUV422_1P_OEDER_LSB_C;
+		if (frame->fmt->corder == GSC_CBCR)
+			cfg |= GSC_OUT_CHROMA_ORDER_CBCR;
+		else
+			cfg |= GSC_OUT_CHROMA_ORDER_CRCB;
+		break;
+	case 2:
+		if (depth == 12)
+			cfg |= GSC_OUT_YUV420_2P;
+		else
+			cfg |= GSC_OUT_YUV422_2P;
+		if (frame->fmt->corder == GSC_CBCR)
+			cfg |= GSC_OUT_CHROMA_ORDER_CBCR;
+		else
+			cfg |= GSC_OUT_CHROMA_ORDER_CRCB;
+		break;
+	case 3:
+		cfg |= GSC_OUT_YUV420_3P;
+		break;
+	}
+
+	if (is_tiled(frame->fmt))
+		cfg |= GSC_OUT_TILE_C_16x8 | GSC_OUT_TILE_MODE;
+
+end_set:
+	writel(cfg, dev->regs + GSC_OUT_CON);
+}
+
+void gsc_hw_set_prescaler(struct gsc_ctx *ctx)
+{
+	struct gsc_dev *dev = ctx->gsc_dev;
+	struct gsc_scaler *sc = &ctx->scaler;
+	u32 cfg;
+
+	cfg = GSC_PRESC_SHFACTOR(sc->pre_shfactor);
+	cfg |= GSC_PRESC_H_RATIO(sc->pre_hratio);
+	cfg |= GSC_PRESC_V_RATIO(sc->pre_vratio);
+	writel(cfg, dev->regs + GSC_PRE_SCALE_RATIO);
+}
+
+void gsc_hw_set_mainscaler(struct gsc_ctx *ctx)
+{
+	struct gsc_dev *dev = ctx->gsc_dev;
+	struct gsc_scaler *sc = &ctx->scaler;
+	u32 cfg;
+
+	cfg = GSC_MAIN_H_RATIO_VALUE(sc->main_hratio);
+	writel(cfg, dev->regs + GSC_MAIN_H_RATIO);
+
+	cfg = GSC_MAIN_V_RATIO_VALUE(sc->main_vratio);
+	writel(cfg, dev->regs + GSC_MAIN_V_RATIO);
+}
+
+void gsc_hw_set_rotation(struct gsc_ctx *ctx)
+{
+	struct gsc_dev *dev = ctx->gsc_dev;
+	u32 cfg;
+
+	cfg = readl(dev->regs + GSC_IN_CON);
+	cfg &= ~GSC_IN_ROT_MASK;
+
+	switch (ctx->gsc_ctrls.rotate->val) {
+	case 270:
+		cfg |= GSC_IN_ROT_270;
+		break;
+	case 180:
+		cfg |= GSC_IN_ROT_180;
+		break;
+	case 90:
+		if (ctx->gsc_ctrls.hflip->val)
+			cfg |= GSC_IN_ROT_90_XFLIP;
+		else if (ctx->gsc_ctrls.vflip->val)
+			cfg |= GSC_IN_ROT_90_YFLIP;
+		else
+			cfg |= GSC_IN_ROT_90;
+		break;
+	case 0:
+		if (ctx->gsc_ctrls.hflip->val)
+			cfg |= GSC_IN_ROT_XFLIP;
+		else if (ctx->gsc_ctrls.vflip->val)
+			cfg |= GSC_IN_ROT_YFLIP;
+	}
+
+	writel(cfg, dev->regs + GSC_IN_CON);
+}
+
+void gsc_hw_set_global_alpha(struct gsc_ctx *ctx)
+{
+	struct gsc_dev *dev = ctx->gsc_dev;
+	struct gsc_frame *frame = &ctx->d_frame;
+	u32 cfg;
+
+	if (!is_rgb(frame->fmt->color)) {
+		pr_debug("Not a RGB format");
+		return;
+	}
+
+	cfg = readl(dev->regs + GSC_OUT_CON);
+	cfg &= ~GSC_OUT_GLOBAL_ALPHA_MASK;
+
+	cfg |= GSC_OUT_GLOBAL_ALPHA(ctx->gsc_ctrls.global_alpha->val);
+	writel(cfg, dev->regs + GSC_OUT_CON);
+}
+
+void gsc_hw_set_sfr_update(struct gsc_ctx *ctx)
+{
+	struct gsc_dev *dev = ctx->gsc_dev;
+	u32 cfg;
+
+	cfg = readl(dev->regs + GSC_ENABLE);
+	cfg |= GSC_ENABLE_SFR_UPDATE;
+	writel(cfg, dev->regs + GSC_ENABLE);
+}
diff --git a/drivers/media/platform/exynos-gsc/gsc-regs.h b/drivers/media/platform/exynos-gsc/gsc-regs.h
new file mode 100644
index 0000000..4678f9a
--- /dev/null
+++ b/drivers/media/platform/exynos-gsc/gsc-regs.h
@@ -0,0 +1,172 @@
+/*
+ * Copyright (c) 2011 - 2012 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ *
+ * Register definition file for Samsung G-Scaler driver
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef REGS_GSC_H_
+#define REGS_GSC_H_
+
+/* G-Scaler enable */
+#define GSC_ENABLE			0x00
+#define GSC_ENABLE_OP_STATUS		(1 << 2)
+#define GSC_ENABLE_SFR_UPDATE		(1 << 1)
+#define GSC_ENABLE_ON			(1 << 0)
+
+/* G-Scaler S/W reset */
+#define GSC_SW_RESET			0x04
+#define GSC_SW_RESET_SRESET		(1 << 0)
+
+/* G-Scaler IRQ */
+#define GSC_IRQ				0x08
+#define GSC_IRQ_STATUS_OR_IRQ		(1 << 17)
+#define GSC_IRQ_STATUS_FRM_DONE_IRQ	(1 << 16)
+#define GSC_IRQ_FRMDONE_MASK		(1 << 1)
+#define GSC_IRQ_ENABLE			(1 << 0)
+
+/* G-Scaler input control */
+#define GSC_IN_CON			0x10
+#define GSC_IN_ROT_MASK			(7 << 16)
+#define GSC_IN_ROT_270			(7 << 16)
+#define GSC_IN_ROT_90_YFLIP		(6 << 16)
+#define GSC_IN_ROT_90_XFLIP		(5 << 16)
+#define GSC_IN_ROT_90			(4 << 16)
+#define GSC_IN_ROT_180			(3 << 16)
+#define GSC_IN_ROT_YFLIP		(2 << 16)
+#define GSC_IN_ROT_XFLIP		(1 << 16)
+#define GSC_IN_RGB_TYPE_MASK		(3 << 14)
+#define GSC_IN_RGB_HD_NARROW		(3 << 14)
+#define GSC_IN_RGB_HD_WIDE		(2 << 14)
+#define GSC_IN_RGB_SD_NARROW		(1 << 14)
+#define GSC_IN_RGB_SD_WIDE		(0 << 14)
+#define GSC_IN_YUV422_1P_ORDER_MASK	(1 << 13)
+#define GSC_IN_YUV422_1P_ORDER_LSB_Y	(0 << 13)
+#define GSC_IN_YUV422_1P_OEDER_LSB_C	(1 << 13)
+#define GSC_IN_CHROMA_ORDER_MASK	(1 << 12)
+#define GSC_IN_CHROMA_ORDER_CBCR	(0 << 12)
+#define GSC_IN_CHROMA_ORDER_CRCB	(1 << 12)
+#define GSC_IN_FORMAT_MASK		(7 << 8)
+#define GSC_IN_XRGB8888			(0 << 8)
+#define GSC_IN_RGB565			(1 << 8)
+#define GSC_IN_YUV420_2P		(2 << 8)
+#define GSC_IN_YUV420_3P		(3 << 8)
+#define GSC_IN_YUV422_1P		(4 << 8)
+#define GSC_IN_YUV422_2P		(5 << 8)
+#define GSC_IN_YUV422_3P		(6 << 8)
+#define GSC_IN_TILE_TYPE_MASK		(1 << 4)
+#define GSC_IN_TILE_C_16x8		(0 << 4)
+#define GSC_IN_TILE_MODE		(1 << 3)
+#define GSC_IN_LOCAL_SEL_MASK		(3 << 1)
+#define GSC_IN_PATH_MASK		(1 << 0)
+#define GSC_IN_PATH_MEMORY		(0 << 0)
+
+/* G-Scaler source image size */
+#define GSC_SRCIMG_SIZE			0x14
+#define GSC_SRCIMG_HEIGHT(x)		((x) << 16)
+#define GSC_SRCIMG_WIDTH(x)		((x) << 0)
+
+/* G-Scaler source image offset */
+#define GSC_SRCIMG_OFFSET		0x18
+#define GSC_SRCIMG_OFFSET_Y(x)		((x) << 16)
+#define GSC_SRCIMG_OFFSET_X(x)		((x) << 0)
+
+/* G-Scaler cropped source image size */
+#define GSC_CROPPED_SIZE		0x1c
+#define GSC_CROPPED_HEIGHT(x)		((x) << 16)
+#define GSC_CROPPED_WIDTH(x)		((x) << 0)
+
+/* G-Scaler output control */
+#define GSC_OUT_CON			0x20
+#define GSC_OUT_GLOBAL_ALPHA_MASK	(0xff << 24)
+#define GSC_OUT_GLOBAL_ALPHA(x)		((x) << 24)
+#define GSC_OUT_RGB_TYPE_MASK		(3 << 10)
+#define GSC_OUT_RGB_HD_WIDE		(3 << 10)
+#define GSC_OUT_RGB_HD_NARROW		(2 << 10)
+#define GSC_OUT_RGB_SD_WIDE		(1 << 10)
+#define GSC_OUT_RGB_SD_NARROW		(0 << 10)
+#define GSC_OUT_YUV422_1P_ORDER_MASK	(1 << 9)
+#define GSC_OUT_YUV422_1P_ORDER_LSB_Y	(0 << 9)
+#define GSC_OUT_YUV422_1P_OEDER_LSB_C	(1 << 9)
+#define GSC_OUT_CHROMA_ORDER_MASK	(1 << 8)
+#define GSC_OUT_CHROMA_ORDER_CBCR	(0 << 8)
+#define GSC_OUT_CHROMA_ORDER_CRCB	(1 << 8)
+#define GSC_OUT_FORMAT_MASK		(7 << 4)
+#define GSC_OUT_XRGB8888		(0 << 4)
+#define GSC_OUT_RGB565			(1 << 4)
+#define GSC_OUT_YUV420_2P		(2 << 4)
+#define GSC_OUT_YUV420_3P		(3 << 4)
+#define GSC_OUT_YUV422_1P		(4 << 4)
+#define GSC_OUT_YUV422_2P		(5 << 4)
+#define GSC_OUT_YUV444			(7 << 4)
+#define GSC_OUT_TILE_TYPE_MASK		(1 << 2)
+#define GSC_OUT_TILE_C_16x8		(0 << 2)
+#define GSC_OUT_TILE_MODE		(1 << 1)
+#define GSC_OUT_PATH_MASK		(1 << 0)
+#define GSC_OUT_PATH_LOCAL		(1 << 0)
+#define GSC_OUT_PATH_MEMORY		(0 << 0)
+
+/* G-Scaler scaled destination image size */
+#define GSC_SCALED_SIZE			0x24
+#define GSC_SCALED_HEIGHT(x)		((x) << 16)
+#define GSC_SCALED_WIDTH(x)		((x) << 0)
+
+/* G-Scaler pre scale ratio */
+#define GSC_PRE_SCALE_RATIO		0x28
+#define GSC_PRESC_SHFACTOR(x)		((x) << 28)
+#define GSC_PRESC_V_RATIO(x)		((x) << 16)
+#define GSC_PRESC_H_RATIO(x)		((x) << 0)
+
+/* G-Scaler main scale horizontal ratio */
+#define GSC_MAIN_H_RATIO		0x2c
+#define GSC_MAIN_H_RATIO_VALUE(x)	((x) << 0)
+
+/* G-Scaler main scale vertical ratio */
+#define GSC_MAIN_V_RATIO		0x30
+#define GSC_MAIN_V_RATIO_VALUE(x)	((x) << 0)
+
+/* G-Scaler destination image size */
+#define GSC_DSTIMG_SIZE			0x40
+#define GSC_DSTIMG_HEIGHT(x)		((x) << 16)
+#define GSC_DSTIMG_WIDTH(x)		((x) << 0)
+
+/* G-Scaler destination image offset */
+#define GSC_DSTIMG_OFFSET		0x44
+#define GSC_DSTIMG_OFFSET_Y(x)		((x) << 16)
+#define GSC_DSTIMG_OFFSET_X(x)		((x) << 0)
+
+/* G-Scaler input y address mask */
+#define GSC_IN_BASE_ADDR_Y_MASK		0x4c
+/* G-Scaler input y base address */
+#define GSC_IN_BASE_ADDR_Y(n)		(0x50 + (n) * 0x4)
+
+/* G-Scaler input cb address mask */
+#define GSC_IN_BASE_ADDR_CB_MASK	0x7c
+/* G-Scaler input cb base address */
+#define GSC_IN_BASE_ADDR_CB(n)		(0x80 + (n) * 0x4)
+
+/* G-Scaler input cr address mask */
+#define GSC_IN_BASE_ADDR_CR_MASK	0xac
+/* G-Scaler input cr base address */
+#define GSC_IN_BASE_ADDR_CR(n)		(0xb0 + (n) * 0x4)
+
+/* G-Scaler output y address mask */
+#define GSC_OUT_BASE_ADDR_Y_MASK	0x10c
+/* G-Scaler output y base address */
+#define GSC_OUT_BASE_ADDR_Y(n)		(0x110 + (n) * 0x4)
+
+/* G-Scaler output cb address mask */
+#define GSC_OUT_BASE_ADDR_CB_MASK	0x15c
+/* G-Scaler output cb base address */
+#define GSC_OUT_BASE_ADDR_CB(n)		(0x160 + (n) * 0x4)
+
+/* G-Scaler output cr address mask */
+#define GSC_OUT_BASE_ADDR_CR_MASK	0x1ac
+/* G-Scaler output cr base address */
+#define GSC_OUT_BASE_ADDR_CR(n)		(0x1b0 + (n) * 0x4)
+
+#endif /* REGS_GSC_H_ */
diff --git a/drivers/media/platform/exynos4-is/Kconfig b/drivers/media/platform/exynos4-is/Kconfig
new file mode 100644
index 0000000..40423c6
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/Kconfig
@@ -0,0 +1,81 @@
+
+config VIDEO_SAMSUNG_EXYNOS4_IS
+	bool "Samsung S5P/EXYNOS4 SoC series Camera Subsystem driver"
+	depends on VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
+	depends on ARCH_S5PV210 || ARCH_EXYNOS || COMPILE_TEST
+	depends on OF && COMMON_CLK
+	help
+	  Say Y here to enable camera host interface devices for
+	  Samsung S5P and EXYNOS SoC series.
+
+if VIDEO_SAMSUNG_EXYNOS4_IS
+
+config VIDEO_EXYNOS4_IS_COMMON
+       tristate
+
+config VIDEO_S5P_FIMC
+	tristate "S5P/EXYNOS4 FIMC/CAMIF camera interface driver"
+	depends on I2C
+	depends on HAS_DMA
+	select VIDEOBUF2_DMA_CONTIG
+	select V4L2_MEM2MEM_DEV
+	select MFD_SYSCON
+	select VIDEO_EXYNOS4_IS_COMMON
+	help
+	  This is a V4L2 driver for Samsung S5P and EXYNOS4 SoC camera host
+	  interface and video postprocessor (FIMC) devices.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called s5p-fimc.
+
+config VIDEO_S5P_MIPI_CSIS
+	tristate "S5P/EXYNOS MIPI-CSI2 receiver (MIPI-CSIS) driver"
+	depends on REGULATOR
+	select GENERIC_PHY
+	help
+	  This is a V4L2 driver for Samsung S5P and EXYNOS4 SoC MIPI-CSI2
+	  receiver (MIPI-CSIS) devices.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called s5p-csis.
+
+if SOC_EXYNOS4212 || SOC_EXYNOS4412 || SOC_EXYNOS5250
+
+config VIDEO_EXYNOS_FIMC_LITE
+	tristate "EXYNOS FIMC-LITE camera interface driver"
+	depends on I2C
+	depends on HAS_DMA
+	select VIDEOBUF2_DMA_CONTIG
+	select VIDEO_EXYNOS4_IS_COMMON
+	help
+	  This is a V4L2 driver for Samsung EXYNOS4/5 SoC FIMC-LITE camera
+	  host interface.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called exynos-fimc-lite.
+endif
+
+config VIDEO_EXYNOS4_FIMC_IS
+	tristate "EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver"
+	depends on I2C
+	depends on HAS_DMA
+	select VIDEOBUF2_DMA_CONTIG
+	depends on OF
+	select FW_LOADER
+	help
+	  This is a V4L2 driver for Samsung EXYNOS4x12 SoC series
+	  FIMC-IS (Imaging Subsystem).
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called exynos4-fimc-is.
+
+config VIDEO_EXYNOS4_ISP_DMA_CAPTURE
+	bool "EXYNOS4x12 FIMC-IS ISP Direct DMA capture support"
+	depends on VIDEO_EXYNOS4_FIMC_IS
+	select VIDEO_EXYNOS4_IS_COMMON
+	default y
+	  help
+	  This option enables an additional video device node exposing a V4L2
+	  video capture interface for the FIMC-IS ISP raw (Bayer) capture DMA.
+
+endif # VIDEO_SAMSUNG_EXYNOS4_IS
diff --git a/drivers/media/platform/exynos4-is/Makefile b/drivers/media/platform/exynos4-is/Makefile
new file mode 100644
index 0000000..eed1b18
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/Makefile
@@ -0,0 +1,17 @@
+s5p-fimc-objs := fimc-core.o fimc-reg.o fimc-m2m.o fimc-capture.o media-dev.o
+exynos-fimc-lite-objs += fimc-lite-reg.o fimc-lite.o
+s5p-csis-objs := mipi-csis.o
+exynos4-is-common-objs := common.o
+
+exynos-fimc-is-objs := fimc-is.o fimc-isp.o fimc-is-sensor.o fimc-is-regs.o
+exynos-fimc-is-objs += fimc-is-param.o fimc-is-errno.o fimc-is-i2c.o
+
+ifeq ($(CONFIG_VIDEO_EXYNOS4_ISP_DMA_CAPTURE),y)
+exynos-fimc-is-objs += fimc-isp-video.o
+endif
+
+obj-$(CONFIG_VIDEO_S5P_MIPI_CSIS)	+= s5p-csis.o
+obj-$(CONFIG_VIDEO_EXYNOS_FIMC_LITE)	+= exynos-fimc-lite.o
+obj-$(CONFIG_VIDEO_EXYNOS4_FIMC_IS)	+= exynos-fimc-is.o
+obj-$(CONFIG_VIDEO_S5P_FIMC)		+= s5p-fimc.o
+obj-$(CONFIG_VIDEO_EXYNOS4_IS_COMMON)	+= exynos4-is-common.o
diff --git a/drivers/media/platform/exynos4-is/common.c b/drivers/media/platform/exynos4-is/common.c
new file mode 100644
index 0000000..0eb34ec
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/common.c
@@ -0,0 +1,53 @@
+/*
+ * Samsung S5P/EXYNOS4 SoC Camera Subsystem driver
+ *
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ * Author: Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <media/exynos-fimc.h>
+#include "common.h"
+
+/* Called with the media graph mutex held or entity->stream_count > 0. */
+struct v4l2_subdev *fimc_find_remote_sensor(struct media_entity *entity)
+{
+	struct media_pad *pad = &entity->pads[0];
+	struct v4l2_subdev *sd;
+
+	while (pad->flags & MEDIA_PAD_FL_SINK) {
+		/* source pad */
+		pad = media_entity_remote_pad(pad);
+		if (pad == NULL ||
+		    media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+			break;
+
+		sd = media_entity_to_v4l2_subdev(pad->entity);
+
+		if (sd->grp_id == GRP_ID_FIMC_IS_SENSOR ||
+		    sd->grp_id == GRP_ID_SENSOR)
+			return sd;
+		/* sink pad */
+		pad = &sd->entity.pads[0];
+	}
+	return NULL;
+}
+EXPORT_SYMBOL(fimc_find_remote_sensor);
+
+void __fimc_vidioc_querycap(struct device *dev, struct v4l2_capability *cap,
+						unsigned int caps)
+{
+	strlcpy(cap->driver, dev->driver->name, sizeof(cap->driver));
+	strlcpy(cap->card, dev->driver->name, sizeof(cap->card));
+	snprintf(cap->bus_info, sizeof(cap->bus_info),
+				"platform:%s", dev_name(dev));
+	cap->device_caps = caps;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+}
+EXPORT_SYMBOL(__fimc_vidioc_querycap);
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/exynos4-is/common.h b/drivers/media/platform/exynos4-is/common.h
new file mode 100644
index 0000000..75b9c71
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/common.h
@@ -0,0 +1,16 @@
+/*
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/device.h>
+#include <linux/videodev2.h>
+#include <media/media-entity.h>
+#include <media/v4l2-subdev.h>
+
+struct v4l2_subdev *fimc_find_remote_sensor(struct media_entity *entity);
+void __fimc_vidioc_querycap(struct device *dev, struct v4l2_capability *cap,
+			    unsigned int caps);
diff --git a/drivers/media/platform/exynos4-is/fimc-capture.c b/drivers/media/platform/exynos4-is/fimc-capture.c
new file mode 100644
index 0000000..99e5732
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-capture.c
@@ -0,0 +1,1914 @@
+/*
+ * Samsung S5P/EXYNOS4 SoC series camera interface (camera capture) driver
+ *
+ * Copyright (C) 2010 - 2012 Samsung Electronics Co., Ltd.
+ * Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/bug.h>
+#include <linux/interrupt.h>
+#include <linux/device.h>
+#include <linux/pm_runtime.h>
+#include <linux/list.h>
+#include <linux/slab.h>
+
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "common.h"
+#include "fimc-core.h"
+#include "fimc-reg.h"
+#include "media-dev.h"
+
+static int fimc_capture_hw_init(struct fimc_dev *fimc)
+{
+	struct fimc_source_info *si = &fimc->vid_cap.source_config;
+	struct fimc_ctx *ctx = fimc->vid_cap.ctx;
+	int ret;
+	unsigned long flags;
+
+	if (ctx == NULL || ctx->s_frame.fmt == NULL)
+		return -EINVAL;
+
+	if (si->fimc_bus_type == FIMC_BUS_TYPE_ISP_WRITEBACK) {
+		ret = fimc_hw_camblk_cfg_writeback(fimc);
+		if (ret < 0)
+			return ret;
+	}
+
+	spin_lock_irqsave(&fimc->slock, flags);
+	fimc_prepare_dma_offset(ctx, &ctx->d_frame);
+	fimc_set_yuv_order(ctx);
+
+	fimc_hw_set_camera_polarity(fimc, si);
+	fimc_hw_set_camera_type(fimc, si);
+	fimc_hw_set_camera_source(fimc, si);
+	fimc_hw_set_camera_offset(fimc, &ctx->s_frame);
+
+	ret = fimc_set_scaler_info(ctx);
+	if (!ret) {
+		fimc_hw_set_input_path(ctx);
+		fimc_hw_set_prescaler(ctx);
+		fimc_hw_set_mainscaler(ctx);
+		fimc_hw_set_target_format(ctx);
+		fimc_hw_set_rotation(ctx);
+		fimc_hw_set_effect(ctx);
+		fimc_hw_set_output_path(ctx);
+		fimc_hw_set_out_dma(ctx);
+		if (fimc->drv_data->alpha_color)
+			fimc_hw_set_rgb_alpha(ctx);
+		clear_bit(ST_CAPT_APPLY_CFG, &fimc->state);
+	}
+	spin_unlock_irqrestore(&fimc->slock, flags);
+	return ret;
+}
+
+/*
+ * Reinitialize the driver so it is ready to start the streaming again.
+ * Set fimc->state to indicate stream off and the hardware shut down state.
+ * If not suspending (@suspend is false), return any buffers to videobuf2.
+ * Otherwise put any owned buffers onto the pending buffers queue, so they
+ * can be re-spun when the device is being resumed. Also perform FIMC
+ * software reset and disable streaming on the whole pipeline if required.
+ */
+static int fimc_capture_state_cleanup(struct fimc_dev *fimc, bool suspend)
+{
+	struct fimc_vid_cap *cap = &fimc->vid_cap;
+	struct fimc_vid_buffer *buf;
+	unsigned long flags;
+	bool streaming;
+
+	spin_lock_irqsave(&fimc->slock, flags);
+	streaming = fimc->state & (1 << ST_CAPT_ISP_STREAM);
+
+	fimc->state &= ~(1 << ST_CAPT_RUN | 1 << ST_CAPT_SHUT |
+			 1 << ST_CAPT_STREAM | 1 << ST_CAPT_ISP_STREAM);
+	if (suspend)
+		fimc->state |= (1 << ST_CAPT_SUSPENDED);
+	else
+		fimc->state &= ~(1 << ST_CAPT_PEND | 1 << ST_CAPT_SUSPENDED);
+
+	/* Release unused buffers */
+	while (!suspend && !list_empty(&cap->pending_buf_q)) {
+		buf = fimc_pending_queue_pop(cap);
+		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+	}
+	/* If suspending put unused buffers onto pending queue */
+	while (!list_empty(&cap->active_buf_q)) {
+		buf = fimc_active_queue_pop(cap);
+		if (suspend)
+			fimc_pending_queue_add(cap, buf);
+		else
+			vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+	}
+
+	fimc_hw_reset(fimc);
+	cap->buf_index = 0;
+
+	spin_unlock_irqrestore(&fimc->slock, flags);
+
+	if (streaming)
+		return fimc_pipeline_call(&cap->ve, set_stream, 0);
+	else
+		return 0;
+}
+
+static int fimc_stop_capture(struct fimc_dev *fimc, bool suspend)
+{
+	unsigned long flags;
+
+	if (!fimc_capture_active(fimc))
+		return 0;
+
+	spin_lock_irqsave(&fimc->slock, flags);
+	set_bit(ST_CAPT_SHUT, &fimc->state);
+	fimc_deactivate_capture(fimc);
+	spin_unlock_irqrestore(&fimc->slock, flags);
+
+	wait_event_timeout(fimc->irq_queue,
+			   !test_bit(ST_CAPT_SHUT, &fimc->state),
+			   (2*HZ/10)); /* 200 ms */
+
+	return fimc_capture_state_cleanup(fimc, suspend);
+}
+
+/**
+ * fimc_capture_config_update - apply the camera interface configuration
+ *
+ * To be called from within the interrupt handler with fimc.slock
+ * spinlock held. It updates the camera pixel crop, rotation and
+ * image flip in H/W.
+ */
+static int fimc_capture_config_update(struct fimc_ctx *ctx)
+{
+	struct fimc_dev *fimc = ctx->fimc_dev;
+	int ret;
+
+	fimc_hw_set_camera_offset(fimc, &ctx->s_frame);
+
+	ret = fimc_set_scaler_info(ctx);
+	if (ret)
+		return ret;
+
+	fimc_hw_set_prescaler(ctx);
+	fimc_hw_set_mainscaler(ctx);
+	fimc_hw_set_target_format(ctx);
+	fimc_hw_set_rotation(ctx);
+	fimc_hw_set_effect(ctx);
+	fimc_prepare_dma_offset(ctx, &ctx->d_frame);
+	fimc_hw_set_out_dma(ctx);
+	if (fimc->drv_data->alpha_color)
+		fimc_hw_set_rgb_alpha(ctx);
+
+	clear_bit(ST_CAPT_APPLY_CFG, &fimc->state);
+	return ret;
+}
+
+void fimc_capture_irq_handler(struct fimc_dev *fimc, int deq_buf)
+{
+	struct fimc_vid_cap *cap = &fimc->vid_cap;
+	struct fimc_pipeline *p = to_fimc_pipeline(cap->ve.pipe);
+	struct v4l2_subdev *csis = p->subdevs[IDX_CSIS];
+	struct fimc_frame *f = &cap->ctx->d_frame;
+	struct fimc_vid_buffer *v_buf;
+
+	if (test_and_clear_bit(ST_CAPT_SHUT, &fimc->state)) {
+		wake_up(&fimc->irq_queue);
+		goto done;
+	}
+
+	if (!list_empty(&cap->active_buf_q) &&
+	    test_bit(ST_CAPT_RUN, &fimc->state) && deq_buf) {
+		v_buf = fimc_active_queue_pop(cap);
+
+		v4l2_get_timestamp(&v_buf->vb.timestamp);
+		v_buf->vb.sequence = cap->frame_count++;
+
+		vb2_buffer_done(&v_buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
+	}
+
+	if (!list_empty(&cap->pending_buf_q)) {
+
+		v_buf = fimc_pending_queue_pop(cap);
+		fimc_hw_set_output_addr(fimc, &v_buf->paddr, cap->buf_index);
+		v_buf->index = cap->buf_index;
+
+		/* Move the buffer to the capture active queue */
+		fimc_active_queue_add(cap, v_buf);
+
+		dbg("next frame: %d, done frame: %d",
+		    fimc_hw_get_frame_index(fimc), v_buf->index);
+
+		if (++cap->buf_index >= FIMC_MAX_OUT_BUFS)
+			cap->buf_index = 0;
+	}
+	/*
+	 * Set up a buffer at MIPI-CSIS if current image format
+	 * requires the frame embedded data capture.
+	 */
+	if (f->fmt->mdataplanes && !list_empty(&cap->active_buf_q)) {
+		unsigned int plane = ffs(f->fmt->mdataplanes) - 1;
+		unsigned int size = f->payload[plane];
+		s32 index = fimc_hw_get_frame_index(fimc);
+		void *vaddr;
+
+		list_for_each_entry(v_buf, &cap->active_buf_q, list) {
+			if (v_buf->index != index)
+				continue;
+			vaddr = vb2_plane_vaddr(&v_buf->vb.vb2_buf, plane);
+			v4l2_subdev_call(csis, video, s_rx_buffer,
+					 vaddr, &size);
+			break;
+		}
+	}
+
+	if (cap->active_buf_cnt == 0) {
+		if (deq_buf)
+			clear_bit(ST_CAPT_RUN, &fimc->state);
+
+		if (++cap->buf_index >= FIMC_MAX_OUT_BUFS)
+			cap->buf_index = 0;
+	} else {
+		set_bit(ST_CAPT_RUN, &fimc->state);
+	}
+
+	if (test_bit(ST_CAPT_APPLY_CFG, &fimc->state))
+		fimc_capture_config_update(cap->ctx);
+done:
+	if (cap->active_buf_cnt == 1) {
+		fimc_deactivate_capture(fimc);
+		clear_bit(ST_CAPT_STREAM, &fimc->state);
+	}
+
+	dbg("frame: %d, active_buf_cnt: %d",
+	    fimc_hw_get_frame_index(fimc), cap->active_buf_cnt);
+}
+
+
+static int start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct fimc_ctx *ctx = q->drv_priv;
+	struct fimc_dev *fimc = ctx->fimc_dev;
+	struct fimc_vid_cap *vid_cap = &fimc->vid_cap;
+	int min_bufs;
+	int ret;
+
+	vid_cap->frame_count = 0;
+
+	ret = fimc_capture_hw_init(fimc);
+	if (ret) {
+		fimc_capture_state_cleanup(fimc, false);
+		return ret;
+	}
+
+	set_bit(ST_CAPT_PEND, &fimc->state);
+
+	min_bufs = fimc->vid_cap.reqbufs_count > 1 ? 2 : 1;
+
+	if (vid_cap->active_buf_cnt >= min_bufs &&
+	    !test_and_set_bit(ST_CAPT_STREAM, &fimc->state)) {
+		fimc_activate_capture(ctx);
+
+		if (!test_and_set_bit(ST_CAPT_ISP_STREAM, &fimc->state))
+			return fimc_pipeline_call(&vid_cap->ve, set_stream, 1);
+	}
+
+	return 0;
+}
+
+static void stop_streaming(struct vb2_queue *q)
+{
+	struct fimc_ctx *ctx = q->drv_priv;
+	struct fimc_dev *fimc = ctx->fimc_dev;
+
+	if (!fimc_capture_active(fimc))
+		return;
+
+	fimc_stop_capture(fimc, false);
+}
+
+int fimc_capture_suspend(struct fimc_dev *fimc)
+{
+	bool suspend = fimc_capture_busy(fimc);
+
+	int ret = fimc_stop_capture(fimc, suspend);
+	if (ret)
+		return ret;
+	return fimc_pipeline_call(&fimc->vid_cap.ve, close);
+}
+
+static void buffer_queue(struct vb2_buffer *vb);
+
+int fimc_capture_resume(struct fimc_dev *fimc)
+{
+	struct fimc_vid_cap *vid_cap = &fimc->vid_cap;
+	struct exynos_video_entity *ve = &vid_cap->ve;
+	struct fimc_vid_buffer *buf;
+	int i;
+
+	if (!test_and_clear_bit(ST_CAPT_SUSPENDED, &fimc->state))
+		return 0;
+
+	INIT_LIST_HEAD(&fimc->vid_cap.active_buf_q);
+	vid_cap->buf_index = 0;
+	fimc_pipeline_call(ve, open, &ve->vdev.entity, false);
+	fimc_capture_hw_init(fimc);
+
+	clear_bit(ST_CAPT_SUSPENDED, &fimc->state);
+
+	for (i = 0; i < vid_cap->reqbufs_count; i++) {
+		if (list_empty(&vid_cap->pending_buf_q))
+			break;
+		buf = fimc_pending_queue_pop(vid_cap);
+		buffer_queue(&buf->vb.vb2_buf);
+	}
+	return 0;
+
+}
+
+static int queue_setup(struct vb2_queue *vq, const void *parg,
+		       unsigned int *num_buffers, unsigned int *num_planes,
+		       unsigned int sizes[], void *allocators[])
+{
+	const struct v4l2_format *pfmt = parg;
+	const struct v4l2_pix_format_mplane *pixm = NULL;
+	struct fimc_ctx *ctx = vq->drv_priv;
+	struct fimc_frame *frame = &ctx->d_frame;
+	struct fimc_fmt *fmt = frame->fmt;
+	unsigned long wh;
+	int i;
+
+	if (pfmt) {
+		pixm = &pfmt->fmt.pix_mp;
+		fmt = fimc_find_format(&pixm->pixelformat, NULL,
+				       FMT_FLAGS_CAM | FMT_FLAGS_M2M, -1);
+		wh = pixm->width * pixm->height;
+	} else {
+		wh = frame->f_width * frame->f_height;
+	}
+
+	if (fmt == NULL)
+		return -EINVAL;
+
+	*num_planes = fmt->memplanes;
+
+	for (i = 0; i < fmt->memplanes; i++) {
+		unsigned int size = (wh * fmt->depth[i]) / 8;
+		if (pixm)
+			sizes[i] = max(size, pixm->plane_fmt[i].sizeimage);
+		else if (fimc_fmt_is_user_defined(fmt->color))
+			sizes[i] = frame->payload[i];
+		else
+			sizes[i] = max_t(u32, size, frame->payload[i]);
+
+		allocators[i] = ctx->fimc_dev->alloc_ctx;
+	}
+
+	return 0;
+}
+
+static int buffer_prepare(struct vb2_buffer *vb)
+{
+	struct vb2_queue *vq = vb->vb2_queue;
+	struct fimc_ctx *ctx = vq->drv_priv;
+	int i;
+
+	if (ctx->d_frame.fmt == NULL)
+		return -EINVAL;
+
+	for (i = 0; i < ctx->d_frame.fmt->memplanes; i++) {
+		unsigned long size = ctx->d_frame.payload[i];
+
+		if (vb2_plane_size(vb, i) < size) {
+			v4l2_err(&ctx->fimc_dev->vid_cap.ve.vdev,
+				 "User buffer too small (%ld < %ld)\n",
+				 vb2_plane_size(vb, i), size);
+			return -EINVAL;
+		}
+		vb2_set_plane_payload(vb, i, size);
+	}
+
+	return 0;
+}
+
+static void buffer_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct fimc_vid_buffer *buf
+		= container_of(vbuf, struct fimc_vid_buffer, vb);
+	struct fimc_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct fimc_dev *fimc = ctx->fimc_dev;
+	struct fimc_vid_cap *vid_cap = &fimc->vid_cap;
+	struct exynos_video_entity *ve = &vid_cap->ve;
+	unsigned long flags;
+	int min_bufs;
+
+	spin_lock_irqsave(&fimc->slock, flags);
+	fimc_prepare_addr(ctx, &buf->vb.vb2_buf, &ctx->d_frame, &buf->paddr);
+
+	if (!test_bit(ST_CAPT_SUSPENDED, &fimc->state) &&
+	    !test_bit(ST_CAPT_STREAM, &fimc->state) &&
+	    vid_cap->active_buf_cnt < FIMC_MAX_OUT_BUFS) {
+		/* Setup the buffer directly for processing. */
+		int buf_id = (vid_cap->reqbufs_count == 1) ? -1 :
+				vid_cap->buf_index;
+
+		fimc_hw_set_output_addr(fimc, &buf->paddr, buf_id);
+		buf->index = vid_cap->buf_index;
+		fimc_active_queue_add(vid_cap, buf);
+
+		if (++vid_cap->buf_index >= FIMC_MAX_OUT_BUFS)
+			vid_cap->buf_index = 0;
+	} else {
+		fimc_pending_queue_add(vid_cap, buf);
+	}
+
+	min_bufs = vid_cap->reqbufs_count > 1 ? 2 : 1;
+
+
+	if (vb2_is_streaming(&vid_cap->vbq) &&
+	    vid_cap->active_buf_cnt >= min_bufs &&
+	    !test_and_set_bit(ST_CAPT_STREAM, &fimc->state)) {
+		int ret;
+
+		fimc_activate_capture(ctx);
+		spin_unlock_irqrestore(&fimc->slock, flags);
+
+		if (test_and_set_bit(ST_CAPT_ISP_STREAM, &fimc->state))
+			return;
+
+		ret = fimc_pipeline_call(ve, set_stream, 1);
+		if (ret < 0)
+			v4l2_err(&ve->vdev, "stream on failed: %d\n", ret);
+		return;
+	}
+	spin_unlock_irqrestore(&fimc->slock, flags);
+}
+
+static struct vb2_ops fimc_capture_qops = {
+	.queue_setup		= queue_setup,
+	.buf_prepare		= buffer_prepare,
+	.buf_queue		= buffer_queue,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
+	.start_streaming	= start_streaming,
+	.stop_streaming		= stop_streaming,
+};
+
+static int fimc_capture_set_default_format(struct fimc_dev *fimc);
+
+static int fimc_capture_open(struct file *file)
+{
+	struct fimc_dev *fimc = video_drvdata(file);
+	struct fimc_vid_cap *vc = &fimc->vid_cap;
+	struct exynos_video_entity *ve = &vc->ve;
+	int ret = -EBUSY;
+
+	dbg("pid: %d, state: 0x%lx", task_pid_nr(current), fimc->state);
+
+	mutex_lock(&fimc->lock);
+
+	if (fimc_m2m_active(fimc))
+		goto unlock;
+
+	set_bit(ST_CAPT_BUSY, &fimc->state);
+	ret = pm_runtime_get_sync(&fimc->pdev->dev);
+	if (ret < 0)
+		goto unlock;
+
+	ret = v4l2_fh_open(file);
+	if (ret) {
+		pm_runtime_put_sync(&fimc->pdev->dev);
+		goto unlock;
+	}
+
+	if (v4l2_fh_is_singular_file(file)) {
+		fimc_md_graph_lock(ve);
+
+		ret = fimc_pipeline_call(ve, open, &ve->vdev.entity, true);
+
+		if (ret == 0 && vc->user_subdev_api && vc->inh_sensor_ctrls) {
+			/*
+			 * Recreate controls of the the video node to drop
+			 * any controls inherited from the sensor subdev.
+			 */
+			fimc_ctrls_delete(vc->ctx);
+
+			ret = fimc_ctrls_create(vc->ctx);
+			if (ret == 0)
+				vc->inh_sensor_ctrls = false;
+		}
+		if (ret == 0)
+			ve->vdev.entity.use_count++;
+
+		fimc_md_graph_unlock(ve);
+
+		if (ret == 0)
+			ret = fimc_capture_set_default_format(fimc);
+
+		if (ret < 0) {
+			clear_bit(ST_CAPT_BUSY, &fimc->state);
+			pm_runtime_put_sync(&fimc->pdev->dev);
+			v4l2_fh_release(file);
+		}
+	}
+unlock:
+	mutex_unlock(&fimc->lock);
+	return ret;
+}
+
+static int fimc_capture_release(struct file *file)
+{
+	struct fimc_dev *fimc = video_drvdata(file);
+	struct fimc_vid_cap *vc = &fimc->vid_cap;
+	bool close = v4l2_fh_is_singular_file(file);
+	int ret;
+
+	dbg("pid: %d, state: 0x%lx", task_pid_nr(current), fimc->state);
+
+	mutex_lock(&fimc->lock);
+
+	if (close && vc->streaming) {
+		media_entity_pipeline_stop(&vc->ve.vdev.entity);
+		vc->streaming = false;
+	}
+
+	ret = _vb2_fop_release(file, NULL);
+
+	if (close) {
+		clear_bit(ST_CAPT_BUSY, &fimc->state);
+		fimc_pipeline_call(&vc->ve, close);
+		clear_bit(ST_CAPT_SUSPENDED, &fimc->state);
+
+		fimc_md_graph_lock(&vc->ve);
+		vc->ve.vdev.entity.use_count--;
+		fimc_md_graph_unlock(&vc->ve);
+	}
+
+	pm_runtime_put_sync(&fimc->pdev->dev);
+	mutex_unlock(&fimc->lock);
+
+	return ret;
+}
+
+static const struct v4l2_file_operations fimc_capture_fops = {
+	.owner		= THIS_MODULE,
+	.open		= fimc_capture_open,
+	.release	= fimc_capture_release,
+	.poll		= vb2_fop_poll,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap		= vb2_fop_mmap,
+};
+
+/*
+ * Format and crop negotiation helpers
+ */
+
+static struct fimc_fmt *fimc_capture_try_format(struct fimc_ctx *ctx,
+						u32 *width, u32 *height,
+						u32 *code, u32 *fourcc, int pad)
+{
+	bool rotation = ctx->rotation == 90 || ctx->rotation == 270;
+	struct fimc_dev *fimc = ctx->fimc_dev;
+	const struct fimc_variant *var = fimc->variant;
+	const struct fimc_pix_limit *pl = var->pix_limit;
+	struct fimc_frame *dst = &ctx->d_frame;
+	u32 depth, min_w, max_w, min_h, align_h = 3;
+	u32 mask = FMT_FLAGS_CAM;
+	struct fimc_fmt *ffmt;
+
+	/* Conversion from/to JPEG or User Defined format is not supported */
+	if (code && ctx->s_frame.fmt && pad == FIMC_SD_PAD_SOURCE &&
+	    fimc_fmt_is_user_defined(ctx->s_frame.fmt->color))
+		*code = ctx->s_frame.fmt->mbus_code;
+
+	if (fourcc && *fourcc != V4L2_PIX_FMT_JPEG && pad == FIMC_SD_PAD_SOURCE)
+		mask |= FMT_FLAGS_M2M;
+
+	if (pad == FIMC_SD_PAD_SINK_FIFO)
+		mask = FMT_FLAGS_WRITEBACK;
+
+	ffmt = fimc_find_format(fourcc, code, mask, 0);
+	if (WARN_ON(!ffmt))
+		return NULL;
+
+	if (code)
+		*code = ffmt->mbus_code;
+	if (fourcc)
+		*fourcc = ffmt->fourcc;
+
+	if (pad != FIMC_SD_PAD_SOURCE) {
+		max_w = fimc_fmt_is_user_defined(ffmt->color) ?
+			pl->scaler_dis_w : pl->scaler_en_w;
+		/* Apply the camera input interface pixel constraints */
+		v4l_bound_align_image(width, max_t(u32, *width, 32), max_w, 4,
+				      height, max_t(u32, *height, 32),
+				      FIMC_CAMIF_MAX_HEIGHT,
+				      fimc_fmt_is_user_defined(ffmt->color) ?
+				      3 : 1,
+				      0);
+		return ffmt;
+	}
+	/* Can't scale or crop in transparent (JPEG) transfer mode */
+	if (fimc_fmt_is_user_defined(ffmt->color)) {
+		*width  = ctx->s_frame.f_width;
+		*height = ctx->s_frame.f_height;
+		return ffmt;
+	}
+	/* Apply the scaler and the output DMA constraints */
+	max_w = rotation ? pl->out_rot_en_w : pl->out_rot_dis_w;
+	if (ctx->state & FIMC_COMPOSE) {
+		min_w = dst->offs_h + dst->width;
+		min_h = dst->offs_v + dst->height;
+	} else {
+		min_w = var->min_out_pixsize;
+		min_h = var->min_out_pixsize;
+	}
+	if (var->min_vsize_align == 1 && !rotation)
+		align_h = fimc_fmt_is_rgb(ffmt->color) ? 0 : 1;
+
+	depth = fimc_get_format_depth(ffmt);
+	v4l_bound_align_image(width, min_w, max_w,
+			      ffs(var->min_out_pixsize) - 1,
+			      height, min_h, FIMC_CAMIF_MAX_HEIGHT,
+			      align_h,
+			      64/(ALIGN(depth, 8)));
+
+	dbg("pad%d: code: 0x%x, %dx%d. dst fmt: %dx%d",
+	    pad, code ? *code : 0, *width, *height,
+	    dst->f_width, dst->f_height);
+
+	return ffmt;
+}
+
+static void fimc_capture_try_selection(struct fimc_ctx *ctx,
+				       struct v4l2_rect *r,
+				       int target)
+{
+	bool rotate = ctx->rotation == 90 || ctx->rotation == 270;
+	struct fimc_dev *fimc = ctx->fimc_dev;
+	const struct fimc_variant *var = fimc->variant;
+	const struct fimc_pix_limit *pl = var->pix_limit;
+	struct fimc_frame *sink = &ctx->s_frame;
+	u32 max_w, max_h, min_w = 0, min_h = 0, min_sz;
+	u32 align_sz = 0, align_h = 4;
+	u32 max_sc_h, max_sc_v;
+
+	/* In JPEG transparent transfer mode cropping is not supported */
+	if (fimc_fmt_is_user_defined(ctx->d_frame.fmt->color)) {
+		r->width  = sink->f_width;
+		r->height = sink->f_height;
+		r->left   = r->top = 0;
+		return;
+	}
+	if (target == V4L2_SEL_TGT_COMPOSE) {
+		if (ctx->rotation != 90 && ctx->rotation != 270)
+			align_h = 1;
+		max_sc_h = min(SCALER_MAX_HRATIO, 1 << (ffs(sink->width) - 3));
+		max_sc_v = min(SCALER_MAX_VRATIO, 1 << (ffs(sink->height) - 1));
+		min_sz = var->min_out_pixsize;
+	} else {
+		u32 depth = fimc_get_format_depth(sink->fmt);
+		align_sz = 64/ALIGN(depth, 8);
+		min_sz = var->min_inp_pixsize;
+		min_w = min_h = min_sz;
+		max_sc_h = max_sc_v = 1;
+	}
+	/*
+	 * For the compose rectangle the following constraints must be met:
+	 * - it must fit in the sink pad format rectangle (f_width/f_height);
+	 * - maximum downscaling ratio is 64;
+	 * - maximum crop size depends if the rotator is used or not;
+	 * - the sink pad format width/height must be 4 multiple of the
+	 *   prescaler ratios determined by sink pad size and source pad crop,
+	 *   the prescaler ratio is returned by fimc_get_scaler_factor().
+	 */
+	max_w = min_t(u32,
+		      rotate ? pl->out_rot_en_w : pl->out_rot_dis_w,
+		      rotate ? sink->f_height : sink->f_width);
+	max_h = min_t(u32, FIMC_CAMIF_MAX_HEIGHT, sink->f_height);
+
+	if (target == V4L2_SEL_TGT_COMPOSE) {
+		min_w = min_t(u32, max_w, sink->f_width / max_sc_h);
+		min_h = min_t(u32, max_h, sink->f_height / max_sc_v);
+		if (rotate) {
+			swap(max_sc_h, max_sc_v);
+			swap(min_w, min_h);
+		}
+	}
+	v4l_bound_align_image(&r->width, min_w, max_w, ffs(min_sz) - 1,
+			      &r->height, min_h, max_h, align_h,
+			      align_sz);
+	/* Adjust left/top if crop/compose rectangle is out of bounds */
+	r->left = clamp_t(u32, r->left, 0, sink->f_width - r->width);
+	r->top  = clamp_t(u32, r->top, 0, sink->f_height - r->height);
+	r->left = round_down(r->left, var->hor_offs_align);
+
+	dbg("target %#x: (%d,%d)/%dx%d, sink fmt: %dx%d",
+	    target, r->left, r->top, r->width, r->height,
+	    sink->f_width, sink->f_height);
+}
+
+/*
+ * The video node ioctl operations
+ */
+static int fimc_cap_querycap(struct file *file, void *priv,
+					struct v4l2_capability *cap)
+{
+	struct fimc_dev *fimc = video_drvdata(file);
+
+	__fimc_vidioc_querycap(&fimc->pdev->dev, cap, V4L2_CAP_STREAMING |
+					V4L2_CAP_VIDEO_CAPTURE_MPLANE);
+	return 0;
+}
+
+static int fimc_cap_enum_fmt_mplane(struct file *file, void *priv,
+				    struct v4l2_fmtdesc *f)
+{
+	struct fimc_fmt *fmt;
+
+	fmt = fimc_find_format(NULL, NULL, FMT_FLAGS_CAM | FMT_FLAGS_M2M,
+			       f->index);
+	if (!fmt)
+		return -EINVAL;
+	strncpy(f->description, fmt->name, sizeof(f->description) - 1);
+	f->pixelformat = fmt->fourcc;
+	if (fmt->fourcc == MEDIA_BUS_FMT_JPEG_1X8)
+		f->flags |= V4L2_FMT_FLAG_COMPRESSED;
+	return 0;
+}
+
+static struct media_entity *fimc_pipeline_get_head(struct media_entity *me)
+{
+	struct media_pad *pad = &me->pads[0];
+
+	while (!(pad->flags & MEDIA_PAD_FL_SOURCE)) {
+		pad = media_entity_remote_pad(pad);
+		if (!pad)
+			break;
+		me = pad->entity;
+		pad = &me->pads[0];
+	}
+
+	return me;
+}
+
+/**
+ * fimc_pipeline_try_format - negotiate and/or set formats at pipeline
+ *                            elements
+ * @ctx: FIMC capture context
+ * @tfmt: media bus format to try/set on subdevs
+ * @fmt_id: fimc pixel format id corresponding to returned @tfmt (output)
+ * @set: true to set format on subdevs, false to try only
+ */
+static int fimc_pipeline_try_format(struct fimc_ctx *ctx,
+				    struct v4l2_mbus_framefmt *tfmt,
+				    struct fimc_fmt **fmt_id,
+				    bool set)
+{
+	struct fimc_dev *fimc = ctx->fimc_dev;
+	struct fimc_pipeline *p = to_fimc_pipeline(fimc->vid_cap.ve.pipe);
+	struct v4l2_subdev *sd = p->subdevs[IDX_SENSOR];
+	struct v4l2_subdev_format sfmt;
+	struct v4l2_mbus_framefmt *mf = &sfmt.format;
+	struct media_entity *me;
+	struct fimc_fmt *ffmt;
+	struct media_pad *pad;
+	int ret, i = 1;
+	u32 fcc;
+
+	if (WARN_ON(!sd || !tfmt))
+		return -EINVAL;
+
+	memset(&sfmt, 0, sizeof(sfmt));
+	sfmt.format = *tfmt;
+	sfmt.which = set ? V4L2_SUBDEV_FORMAT_ACTIVE : V4L2_SUBDEV_FORMAT_TRY;
+
+	me = fimc_pipeline_get_head(&sd->entity);
+
+	while (1) {
+		ffmt = fimc_find_format(NULL, mf->code != 0 ? &mf->code : NULL,
+					FMT_FLAGS_CAM, i++);
+		if (ffmt == NULL) {
+			/*
+			 * Notify user-space if common pixel code for
+			 * host and sensor does not exist.
+			 */
+			return -EINVAL;
+		}
+		mf->code = tfmt->code = ffmt->mbus_code;
+
+		/* set format on all pipeline subdevs */
+		while (me != &fimc->vid_cap.subdev.entity) {
+			sd = media_entity_to_v4l2_subdev(me);
+
+			sfmt.pad = 0;
+			ret = v4l2_subdev_call(sd, pad, set_fmt, NULL, &sfmt);
+			if (ret)
+				return ret;
+
+			if (me->pads[0].flags & MEDIA_PAD_FL_SINK) {
+				sfmt.pad = me->num_pads - 1;
+				mf->code = tfmt->code;
+				ret = v4l2_subdev_call(sd, pad, set_fmt, NULL,
+									&sfmt);
+				if (ret)
+					return ret;
+			}
+
+			pad = media_entity_remote_pad(&me->pads[sfmt.pad]);
+			if (!pad)
+				return -EINVAL;
+			me = pad->entity;
+		}
+
+		if (mf->code != tfmt->code)
+			continue;
+
+		fcc = ffmt->fourcc;
+		tfmt->width  = mf->width;
+		tfmt->height = mf->height;
+		ffmt = fimc_capture_try_format(ctx, &tfmt->width, &tfmt->height,
+					NULL, &fcc, FIMC_SD_PAD_SINK_CAM);
+		ffmt = fimc_capture_try_format(ctx, &tfmt->width, &tfmt->height,
+					NULL, &fcc, FIMC_SD_PAD_SOURCE);
+		if (ffmt && ffmt->mbus_code)
+			mf->code = ffmt->mbus_code;
+		if (mf->width != tfmt->width || mf->height != tfmt->height)
+			continue;
+		tfmt->code = mf->code;
+		break;
+	}
+
+	if (fmt_id && ffmt)
+		*fmt_id = ffmt;
+	*tfmt = *mf;
+
+	return 0;
+}
+
+/**
+ * fimc_get_sensor_frame_desc - query the sensor for media bus frame parameters
+ * @sensor: pointer to the sensor subdev
+ * @plane_fmt: provides plane sizes corresponding to the frame layout entries
+ * @try: true to set the frame parameters, false to query only
+ *
+ * This function is used by this driver only for compressed/blob data formats.
+ */
+static int fimc_get_sensor_frame_desc(struct v4l2_subdev *sensor,
+				      struct v4l2_plane_pix_format *plane_fmt,
+				      unsigned int num_planes, bool try)
+{
+	struct v4l2_mbus_frame_desc fd;
+	int i, ret;
+	int pad;
+
+	for (i = 0; i < num_planes; i++)
+		fd.entry[i].length = plane_fmt[i].sizeimage;
+
+	pad = sensor->entity.num_pads - 1;
+	if (try)
+		ret = v4l2_subdev_call(sensor, pad, set_frame_desc, pad, &fd);
+	else
+		ret = v4l2_subdev_call(sensor, pad, get_frame_desc, pad, &fd);
+
+	if (ret < 0)
+		return ret;
+
+	if (num_planes != fd.num_entries)
+		return -EINVAL;
+
+	for (i = 0; i < num_planes; i++)
+		plane_fmt[i].sizeimage = fd.entry[i].length;
+
+	if (fd.entry[0].length > FIMC_MAX_JPEG_BUF_SIZE) {
+		v4l2_err(sensor->v4l2_dev,  "Unsupported buffer size: %u\n",
+			 fd.entry[0].length);
+
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int fimc_cap_g_fmt_mplane(struct file *file, void *fh,
+				 struct v4l2_format *f)
+{
+	struct fimc_dev *fimc = video_drvdata(file);
+
+	__fimc_get_format(&fimc->vid_cap.ctx->d_frame, f);
+	return 0;
+}
+
+/*
+ * Try or set format on the fimc.X.capture video node and additionally
+ * on the whole pipeline if @try is false.
+ * Locking: the caller must _not_ hold the graph mutex.
+ */
+static int __video_try_or_set_format(struct fimc_dev *fimc,
+				     struct v4l2_format *f, bool try,
+				     struct fimc_fmt **inp_fmt,
+				     struct fimc_fmt **out_fmt)
+{
+	struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp;
+	struct fimc_vid_cap *vc = &fimc->vid_cap;
+	struct exynos_video_entity *ve = &vc->ve;
+	struct fimc_ctx *ctx = vc->ctx;
+	unsigned int width = 0, height = 0;
+	int ret = 0;
+
+	/* Pre-configure format at the camera input interface, for JPEG only */
+	if (fimc_jpeg_fourcc(pix->pixelformat)) {
+		fimc_capture_try_format(ctx, &pix->width, &pix->height,
+					NULL, &pix->pixelformat,
+					FIMC_SD_PAD_SINK_CAM);
+		if (try) {
+			width = pix->width;
+			height = pix->height;
+		} else {
+			ctx->s_frame.f_width = pix->width;
+			ctx->s_frame.f_height = pix->height;
+		}
+	}
+
+	/* Try the format at the scaler and the DMA output */
+	*out_fmt = fimc_capture_try_format(ctx, &pix->width, &pix->height,
+					  NULL, &pix->pixelformat,
+					  FIMC_SD_PAD_SOURCE);
+	if (*out_fmt == NULL)
+		return -EINVAL;
+
+	/* Restore image width/height for JPEG (no resizing supported). */
+	if (try && fimc_jpeg_fourcc(pix->pixelformat)) {
+		pix->width = width;
+		pix->height = height;
+	}
+
+	/* Try to match format at the host and the sensor */
+	if (!vc->user_subdev_api) {
+		struct v4l2_mbus_framefmt mbus_fmt;
+		struct v4l2_mbus_framefmt *mf;
+
+		mf = try ? &mbus_fmt : &fimc->vid_cap.ci_fmt;
+
+		mf->code = (*out_fmt)->mbus_code;
+		mf->width = pix->width;
+		mf->height = pix->height;
+
+		fimc_md_graph_lock(ve);
+		ret = fimc_pipeline_try_format(ctx, mf, inp_fmt, try);
+		fimc_md_graph_unlock(ve);
+
+		if (ret < 0)
+			return ret;
+
+		pix->width = mf->width;
+		pix->height = mf->height;
+	}
+
+	fimc_adjust_mplane_format(*out_fmt, pix->width, pix->height, pix);
+
+	if ((*out_fmt)->flags & FMT_FLAGS_COMPRESSED) {
+		struct v4l2_subdev *sensor;
+
+		fimc_md_graph_lock(ve);
+
+		sensor = __fimc_md_get_subdev(ve->pipe, IDX_SENSOR);
+		if (sensor)
+			fimc_get_sensor_frame_desc(sensor, pix->plane_fmt,
+						   (*out_fmt)->memplanes, try);
+		else
+			ret = -EPIPE;
+
+		fimc_md_graph_unlock(ve);
+	}
+
+	return ret;
+}
+
+static int fimc_cap_try_fmt_mplane(struct file *file, void *fh,
+				   struct v4l2_format *f)
+{
+	struct fimc_dev *fimc = video_drvdata(file);
+	struct fimc_fmt *out_fmt = NULL, *inp_fmt = NULL;
+
+	return __video_try_or_set_format(fimc, f, true, &inp_fmt, &out_fmt);
+}
+
+static void fimc_capture_mark_jpeg_xfer(struct fimc_ctx *ctx,
+					enum fimc_color_fmt color)
+{
+	bool jpeg = fimc_fmt_is_user_defined(color);
+
+	ctx->scaler.enabled = !jpeg;
+	fimc_ctrls_activate(ctx, !jpeg);
+
+	if (jpeg)
+		set_bit(ST_CAPT_JPEG, &ctx->fimc_dev->state);
+	else
+		clear_bit(ST_CAPT_JPEG, &ctx->fimc_dev->state);
+}
+
+static int __fimc_capture_set_format(struct fimc_dev *fimc,
+				     struct v4l2_format *f)
+{
+	struct fimc_vid_cap *vc = &fimc->vid_cap;
+	struct fimc_ctx *ctx = vc->ctx;
+	struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp;
+	struct fimc_frame *ff = &ctx->d_frame;
+	struct fimc_fmt *inp_fmt = NULL;
+	int ret, i;
+
+	if (vb2_is_busy(&fimc->vid_cap.vbq))
+		return -EBUSY;
+
+	ret = __video_try_or_set_format(fimc, f, false, &inp_fmt, &ff->fmt);
+	if (ret < 0)
+		return ret;
+
+	/* Update RGB Alpha control state and value range */
+	fimc_alpha_ctrl_update(ctx);
+
+	for (i = 0; i < ff->fmt->memplanes; i++) {
+		ff->bytesperline[i] = pix->plane_fmt[i].bytesperline;
+		ff->payload[i] = pix->plane_fmt[i].sizeimage;
+	}
+
+	set_frame_bounds(ff, pix->width, pix->height);
+	/* Reset the composition rectangle if not yet configured */
+	if (!(ctx->state & FIMC_COMPOSE))
+		set_frame_crop(ff, 0, 0, pix->width, pix->height);
+
+	fimc_capture_mark_jpeg_xfer(ctx, ff->fmt->color);
+
+	/* Reset cropping and set format at the camera interface input */
+	if (!vc->user_subdev_api) {
+		ctx->s_frame.fmt = inp_fmt;
+		set_frame_bounds(&ctx->s_frame, pix->width, pix->height);
+		set_frame_crop(&ctx->s_frame, 0, 0, pix->width, pix->height);
+	}
+
+	return ret;
+}
+
+static int fimc_cap_s_fmt_mplane(struct file *file, void *priv,
+				 struct v4l2_format *f)
+{
+	struct fimc_dev *fimc = video_drvdata(file);
+
+	return __fimc_capture_set_format(fimc, f);
+}
+
+static int fimc_cap_enum_input(struct file *file, void *priv,
+			       struct v4l2_input *i)
+{
+	struct fimc_dev *fimc = video_drvdata(file);
+	struct exynos_video_entity *ve = &fimc->vid_cap.ve;
+	struct v4l2_subdev *sd;
+
+	if (i->index != 0)
+		return -EINVAL;
+
+	i->type = V4L2_INPUT_TYPE_CAMERA;
+	fimc_md_graph_lock(ve);
+	sd = __fimc_md_get_subdev(ve->pipe, IDX_SENSOR);
+	fimc_md_graph_unlock(ve);
+
+	if (sd)
+		strlcpy(i->name, sd->name, sizeof(i->name));
+
+	return 0;
+}
+
+static int fimc_cap_s_input(struct file *file, void *priv, unsigned int i)
+{
+	return i == 0 ? i : -EINVAL;
+}
+
+static int fimc_cap_g_input(struct file *file, void *priv, unsigned int *i)
+{
+	*i = 0;
+	return 0;
+}
+
+/**
+ * fimc_pipeline_validate - check for formats inconsistencies
+ *                          between source and sink pad of each link
+ *
+ * Return 0 if all formats match or -EPIPE otherwise.
+ */
+static int fimc_pipeline_validate(struct fimc_dev *fimc)
+{
+	struct v4l2_subdev_format sink_fmt, src_fmt;
+	struct fimc_vid_cap *vc = &fimc->vid_cap;
+	struct v4l2_subdev *sd = &vc->subdev;
+	struct fimc_pipeline *p = to_fimc_pipeline(vc->ve.pipe);
+	struct media_pad *sink_pad, *src_pad;
+	int i, ret;
+
+	while (1) {
+		/*
+		 * Find current entity sink pad and any remote sink pad linked
+		 * to it. We stop if there is no sink pad in current entity or
+		 * it is not linked to any other remote entity.
+		 */
+		src_pad = NULL;
+
+		for (i = 0; i < sd->entity.num_pads; i++) {
+			struct media_pad *p = &sd->entity.pads[i];
+
+			if (p->flags & MEDIA_PAD_FL_SINK) {
+				sink_pad = p;
+				src_pad = media_entity_remote_pad(sink_pad);
+				if (src_pad)
+					break;
+			}
+		}
+
+		if (src_pad == NULL ||
+		    media_entity_type(src_pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+			break;
+
+		/* Don't call FIMC subdev operation to avoid nested locking */
+		if (sd == &vc->subdev) {
+			struct fimc_frame *ff = &vc->ctx->s_frame;
+			sink_fmt.format.width = ff->f_width;
+			sink_fmt.format.height = ff->f_height;
+			sink_fmt.format.code = ff->fmt ? ff->fmt->mbus_code : 0;
+		} else {
+			sink_fmt.pad = sink_pad->index;
+			sink_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+			ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &sink_fmt);
+			if (ret < 0 && ret != -ENOIOCTLCMD)
+				return -EPIPE;
+		}
+
+		/* Retrieve format at the source pad */
+		sd = media_entity_to_v4l2_subdev(src_pad->entity);
+		src_fmt.pad = src_pad->index;
+		src_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+		ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &src_fmt);
+		if (ret < 0 && ret != -ENOIOCTLCMD)
+			return -EPIPE;
+
+		if (src_fmt.format.width != sink_fmt.format.width ||
+		    src_fmt.format.height != sink_fmt.format.height ||
+		    src_fmt.format.code != sink_fmt.format.code)
+			return -EPIPE;
+
+		if (sd == p->subdevs[IDX_SENSOR] &&
+		    fimc_user_defined_mbus_fmt(src_fmt.format.code)) {
+			struct v4l2_plane_pix_format plane_fmt[FIMC_MAX_PLANES];
+			struct fimc_frame *frame = &vc->ctx->d_frame;
+			unsigned int i;
+
+			ret = fimc_get_sensor_frame_desc(sd, plane_fmt,
+							 frame->fmt->memplanes,
+							 false);
+			if (ret < 0)
+				return -EPIPE;
+
+			for (i = 0; i < frame->fmt->memplanes; i++)
+				if (frame->payload[i] < plane_fmt[i].sizeimage)
+					return -EPIPE;
+		}
+	}
+	return 0;
+}
+
+static int fimc_cap_streamon(struct file *file, void *priv,
+			     enum v4l2_buf_type type)
+{
+	struct fimc_dev *fimc = video_drvdata(file);
+	struct fimc_vid_cap *vc = &fimc->vid_cap;
+	struct media_entity *entity = &vc->ve.vdev.entity;
+	struct fimc_source_info *si = NULL;
+	struct v4l2_subdev *sd;
+	int ret;
+
+	if (fimc_capture_active(fimc))
+		return -EBUSY;
+
+	ret = media_entity_pipeline_start(entity, &vc->ve.pipe->mp);
+	if (ret < 0)
+		return ret;
+
+	sd = __fimc_md_get_subdev(vc->ve.pipe, IDX_SENSOR);
+	if (sd)
+		si = v4l2_get_subdev_hostdata(sd);
+
+	if (si == NULL) {
+		ret = -EPIPE;
+		goto err_p_stop;
+	}
+	/*
+	 * Save configuration data related to currently attached image
+	 * sensor or other data source, e.g. FIMC-IS.
+	 */
+	vc->source_config = *si;
+
+	if (vc->input == GRP_ID_FIMC_IS)
+		vc->source_config.fimc_bus_type = FIMC_BUS_TYPE_ISP_WRITEBACK;
+
+	if (vc->user_subdev_api) {
+		ret = fimc_pipeline_validate(fimc);
+		if (ret < 0)
+			goto err_p_stop;
+	}
+
+	ret = vb2_ioctl_streamon(file, priv, type);
+	if (!ret) {
+		vc->streaming = true;
+		return ret;
+	}
+
+err_p_stop:
+	media_entity_pipeline_stop(entity);
+	return ret;
+}
+
+static int fimc_cap_streamoff(struct file *file, void *priv,
+			    enum v4l2_buf_type type)
+{
+	struct fimc_dev *fimc = video_drvdata(file);
+	struct fimc_vid_cap *vc = &fimc->vid_cap;
+	int ret;
+
+	ret = vb2_ioctl_streamoff(file, priv, type);
+	if (ret < 0)
+		return ret;
+
+	media_entity_pipeline_stop(&vc->ve.vdev.entity);
+	vc->streaming = false;
+	return 0;
+}
+
+static int fimc_cap_reqbufs(struct file *file, void *priv,
+			    struct v4l2_requestbuffers *reqbufs)
+{
+	struct fimc_dev *fimc = video_drvdata(file);
+	int ret;
+
+	ret = vb2_ioctl_reqbufs(file, priv, reqbufs);
+
+	if (!ret)
+		fimc->vid_cap.reqbufs_count = reqbufs->count;
+
+	return ret;
+}
+
+static int fimc_cap_g_selection(struct file *file, void *fh,
+				struct v4l2_selection *s)
+{
+	struct fimc_dev *fimc = video_drvdata(file);
+	struct fimc_ctx *ctx = fimc->vid_cap.ctx;
+	struct fimc_frame *f = &ctx->s_frame;
+
+	if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		return -EINVAL;
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+		f = &ctx->d_frame;
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		s->r.left = 0;
+		s->r.top = 0;
+		s->r.width = f->o_width;
+		s->r.height = f->o_height;
+		return 0;
+
+	case V4L2_SEL_TGT_COMPOSE:
+		f = &ctx->d_frame;
+	case V4L2_SEL_TGT_CROP:
+		s->r.left = f->offs_h;
+		s->r.top = f->offs_v;
+		s->r.width = f->width;
+		s->r.height = f->height;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+/* Return 1 if rectangle a is enclosed in rectangle b, or 0 otherwise. */
+static int enclosed_rectangle(struct v4l2_rect *a, struct v4l2_rect *b)
+{
+	if (a->left < b->left || a->top < b->top)
+		return 0;
+	if (a->left + a->width > b->left + b->width)
+		return 0;
+	if (a->top + a->height > b->top + b->height)
+		return 0;
+
+	return 1;
+}
+
+static int fimc_cap_s_selection(struct file *file, void *fh,
+				struct v4l2_selection *s)
+{
+	struct fimc_dev *fimc = video_drvdata(file);
+	struct fimc_ctx *ctx = fimc->vid_cap.ctx;
+	struct v4l2_rect rect = s->r;
+	struct fimc_frame *f;
+	unsigned long flags;
+
+	if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		return -EINVAL;
+
+	if (s->target == V4L2_SEL_TGT_COMPOSE)
+		f = &ctx->d_frame;
+	else if (s->target == V4L2_SEL_TGT_CROP)
+		f = &ctx->s_frame;
+	else
+		return -EINVAL;
+
+	fimc_capture_try_selection(ctx, &rect, s->target);
+
+	if (s->flags & V4L2_SEL_FLAG_LE &&
+	    !enclosed_rectangle(&rect, &s->r))
+		return -ERANGE;
+
+	if (s->flags & V4L2_SEL_FLAG_GE &&
+	    !enclosed_rectangle(&s->r, &rect))
+		return -ERANGE;
+
+	s->r = rect;
+	spin_lock_irqsave(&fimc->slock, flags);
+	set_frame_crop(f, s->r.left, s->r.top, s->r.width,
+		       s->r.height);
+	spin_unlock_irqrestore(&fimc->slock, flags);
+
+	set_bit(ST_CAPT_APPLY_CFG, &fimc->state);
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops fimc_capture_ioctl_ops = {
+	.vidioc_querycap		= fimc_cap_querycap,
+
+	.vidioc_enum_fmt_vid_cap_mplane	= fimc_cap_enum_fmt_mplane,
+	.vidioc_try_fmt_vid_cap_mplane	= fimc_cap_try_fmt_mplane,
+	.vidioc_s_fmt_vid_cap_mplane	= fimc_cap_s_fmt_mplane,
+	.vidioc_g_fmt_vid_cap_mplane	= fimc_cap_g_fmt_mplane,
+
+	.vidioc_reqbufs			= fimc_cap_reqbufs,
+	.vidioc_querybuf		= vb2_ioctl_querybuf,
+	.vidioc_qbuf			= vb2_ioctl_qbuf,
+	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
+	.vidioc_expbuf			= vb2_ioctl_expbuf,
+	.vidioc_prepare_buf		= vb2_ioctl_prepare_buf,
+	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
+
+	.vidioc_streamon		= fimc_cap_streamon,
+	.vidioc_streamoff		= fimc_cap_streamoff,
+
+	.vidioc_g_selection		= fimc_cap_g_selection,
+	.vidioc_s_selection		= fimc_cap_s_selection,
+
+	.vidioc_enum_input		= fimc_cap_enum_input,
+	.vidioc_s_input			= fimc_cap_s_input,
+	.vidioc_g_input			= fimc_cap_g_input,
+};
+
+/* Capture subdev media entity operations */
+static int fimc_link_setup(struct media_entity *entity,
+			   const struct media_pad *local,
+			   const struct media_pad *remote, u32 flags)
+{
+	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+	struct fimc_dev *fimc = v4l2_get_subdevdata(sd);
+	struct fimc_vid_cap *vc = &fimc->vid_cap;
+	struct v4l2_subdev *sensor;
+
+	if (media_entity_type(remote->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+		return -EINVAL;
+
+	if (WARN_ON(fimc == NULL))
+		return 0;
+
+	dbg("%s --> %s, flags: 0x%x. input: 0x%x",
+	    local->entity->name, remote->entity->name, flags,
+	    fimc->vid_cap.input);
+
+	if (!(flags & MEDIA_LNK_FL_ENABLED)) {
+		fimc->vid_cap.input = 0;
+		return 0;
+	}
+
+	if (vc->input != 0)
+		return -EBUSY;
+
+	vc->input = sd->grp_id;
+
+	if (vc->user_subdev_api || vc->inh_sensor_ctrls)
+		return 0;
+
+	/* Inherit V4L2 controls from the image sensor subdev. */
+	sensor = fimc_find_remote_sensor(&vc->subdev.entity);
+	if (sensor == NULL)
+		return 0;
+
+	return v4l2_ctrl_add_handler(&vc->ctx->ctrls.handler,
+				     sensor->ctrl_handler, NULL);
+}
+
+static const struct media_entity_operations fimc_sd_media_ops = {
+	.link_setup = fimc_link_setup,
+};
+
+/**
+ * fimc_sensor_notify - v4l2_device notification from a sensor subdev
+ * @sd: pointer to a subdev generating the notification
+ * @notification: the notification type, must be S5P_FIMC_TX_END_NOTIFY
+ * @arg: pointer to an u32 type integer that stores the frame payload value
+ *
+ * The End Of Frame notification sent by sensor subdev in its still capture
+ * mode. If there is only a single VSYNC generated by the sensor at the
+ * beginning of a frame transmission, FIMC does not issue the LastIrq
+ * (end of frame) interrupt. And this notification is used to complete the
+ * frame capture and returning a buffer to user-space. Subdev drivers should
+ * call this notification from their last 'End of frame capture' interrupt.
+ */
+void fimc_sensor_notify(struct v4l2_subdev *sd, unsigned int notification,
+			void *arg)
+{
+	struct fimc_source_info	*si;
+	struct fimc_vid_buffer *buf;
+	struct fimc_md *fmd;
+	struct fimc_dev *fimc;
+	unsigned long flags;
+
+	if (sd == NULL)
+		return;
+
+	si = v4l2_get_subdev_hostdata(sd);
+	fmd = entity_to_fimc_mdev(&sd->entity);
+
+	spin_lock_irqsave(&fmd->slock, flags);
+
+	fimc = si ? source_to_sensor_info(si)->host : NULL;
+
+	if (fimc && arg && notification == S5P_FIMC_TX_END_NOTIFY &&
+	    test_bit(ST_CAPT_PEND, &fimc->state)) {
+		unsigned long irq_flags;
+		spin_lock_irqsave(&fimc->slock, irq_flags);
+		if (!list_empty(&fimc->vid_cap.active_buf_q)) {
+			buf = list_entry(fimc->vid_cap.active_buf_q.next,
+					 struct fimc_vid_buffer, list);
+			vb2_set_plane_payload(&buf->vb.vb2_buf, 0,
+					      *((u32 *)arg));
+		}
+		fimc_capture_irq_handler(fimc, 1);
+		fimc_deactivate_capture(fimc);
+		spin_unlock_irqrestore(&fimc->slock, irq_flags);
+	}
+	spin_unlock_irqrestore(&fmd->slock, flags);
+}
+
+static int fimc_subdev_enum_mbus_code(struct v4l2_subdev *sd,
+				      struct v4l2_subdev_pad_config *cfg,
+				      struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct fimc_fmt *fmt;
+
+	fmt = fimc_find_format(NULL, NULL, FMT_FLAGS_CAM, code->index);
+	if (!fmt)
+		return -EINVAL;
+	code->code = fmt->mbus_code;
+	return 0;
+}
+
+static int fimc_subdev_get_fmt(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_pad_config *cfg,
+			       struct v4l2_subdev_format *fmt)
+{
+	struct fimc_dev *fimc = v4l2_get_subdevdata(sd);
+	struct fimc_ctx *ctx = fimc->vid_cap.ctx;
+	struct fimc_frame *ff = &ctx->s_frame;
+	struct v4l2_mbus_framefmt *mf;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		mf = v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
+		fmt->format = *mf;
+		return 0;
+	}
+
+	mf = &fmt->format;
+	mutex_lock(&fimc->lock);
+
+	switch (fmt->pad) {
+	case FIMC_SD_PAD_SOURCE:
+		if (!WARN_ON(ff->fmt == NULL))
+			mf->code = ff->fmt->mbus_code;
+		/* Sink pads crop rectangle size */
+		mf->width = ff->width;
+		mf->height = ff->height;
+		break;
+	case FIMC_SD_PAD_SINK_FIFO:
+		*mf = fimc->vid_cap.wb_fmt;
+		break;
+	case FIMC_SD_PAD_SINK_CAM:
+	default:
+		*mf = fimc->vid_cap.ci_fmt;
+		break;
+	}
+
+	mutex_unlock(&fimc->lock);
+	mf->colorspace = V4L2_COLORSPACE_JPEG;
+
+	return 0;
+}
+
+static int fimc_subdev_set_fmt(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_pad_config *cfg,
+			       struct v4l2_subdev_format *fmt)
+{
+	struct fimc_dev *fimc = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *mf = &fmt->format;
+	struct fimc_vid_cap *vc = &fimc->vid_cap;
+	struct fimc_ctx *ctx = vc->ctx;
+	struct fimc_frame *ff;
+	struct fimc_fmt *ffmt;
+
+	dbg("pad%d: code: 0x%x, %dx%d",
+	    fmt->pad, mf->code, mf->width, mf->height);
+
+	if (fmt->pad == FIMC_SD_PAD_SOURCE && vb2_is_busy(&vc->vbq))
+		return -EBUSY;
+
+	mutex_lock(&fimc->lock);
+	ffmt = fimc_capture_try_format(ctx, &mf->width, &mf->height,
+				       &mf->code, NULL, fmt->pad);
+	mutex_unlock(&fimc->lock);
+	mf->colorspace = V4L2_COLORSPACE_JPEG;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		mf = v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
+		*mf = fmt->format;
+		return 0;
+	}
+	/* There must be a bug in the driver if this happens */
+	if (WARN_ON(ffmt == NULL))
+		return -EINVAL;
+
+	/* Update RGB Alpha control state and value range */
+	fimc_alpha_ctrl_update(ctx);
+
+	fimc_capture_mark_jpeg_xfer(ctx, ffmt->color);
+	if (fmt->pad == FIMC_SD_PAD_SOURCE) {
+		ff = &ctx->d_frame;
+		/* Sink pads crop rectangle size */
+		mf->width = ctx->s_frame.width;
+		mf->height = ctx->s_frame.height;
+	} else {
+		ff = &ctx->s_frame;
+	}
+
+	mutex_lock(&fimc->lock);
+	set_frame_bounds(ff, mf->width, mf->height);
+
+	if (fmt->pad == FIMC_SD_PAD_SINK_FIFO)
+		vc->wb_fmt = *mf;
+	else if (fmt->pad == FIMC_SD_PAD_SINK_CAM)
+		vc->ci_fmt = *mf;
+
+	ff->fmt = ffmt;
+
+	/* Reset the crop rectangle if required. */
+	if (!(fmt->pad == FIMC_SD_PAD_SOURCE && (ctx->state & FIMC_COMPOSE)))
+		set_frame_crop(ff, 0, 0, mf->width, mf->height);
+
+	if (fmt->pad != FIMC_SD_PAD_SOURCE)
+		ctx->state &= ~FIMC_COMPOSE;
+
+	mutex_unlock(&fimc->lock);
+	return 0;
+}
+
+static int fimc_subdev_get_selection(struct v4l2_subdev *sd,
+				     struct v4l2_subdev_pad_config *cfg,
+				     struct v4l2_subdev_selection *sel)
+{
+	struct fimc_dev *fimc = v4l2_get_subdevdata(sd);
+	struct fimc_ctx *ctx = fimc->vid_cap.ctx;
+	struct fimc_frame *f = &ctx->s_frame;
+	struct v4l2_rect *r = &sel->r;
+	struct v4l2_rect *try_sel;
+
+	if (sel->pad == FIMC_SD_PAD_SOURCE)
+		return -EINVAL;
+
+	mutex_lock(&fimc->lock);
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+		f = &ctx->d_frame;
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		r->width = f->o_width;
+		r->height = f->o_height;
+		r->left = 0;
+		r->top = 0;
+		mutex_unlock(&fimc->lock);
+		return 0;
+
+	case V4L2_SEL_TGT_CROP:
+		try_sel = v4l2_subdev_get_try_crop(sd, cfg, sel->pad);
+		break;
+	case V4L2_SEL_TGT_COMPOSE:
+		try_sel = v4l2_subdev_get_try_compose(sd, cfg, sel->pad);
+		f = &ctx->d_frame;
+		break;
+	default:
+		mutex_unlock(&fimc->lock);
+		return -EINVAL;
+	}
+
+	if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
+		sel->r = *try_sel;
+	} else {
+		r->left = f->offs_h;
+		r->top = f->offs_v;
+		r->width = f->width;
+		r->height = f->height;
+	}
+
+	dbg("target %#x: l:%d, t:%d, %dx%d, f_w: %d, f_h: %d",
+	    sel->pad, r->left, r->top, r->width, r->height,
+	    f->f_width, f->f_height);
+
+	mutex_unlock(&fimc->lock);
+	return 0;
+}
+
+static int fimc_subdev_set_selection(struct v4l2_subdev *sd,
+				     struct v4l2_subdev_pad_config *cfg,
+				     struct v4l2_subdev_selection *sel)
+{
+	struct fimc_dev *fimc = v4l2_get_subdevdata(sd);
+	struct fimc_ctx *ctx = fimc->vid_cap.ctx;
+	struct fimc_frame *f = &ctx->s_frame;
+	struct v4l2_rect *r = &sel->r;
+	struct v4l2_rect *try_sel;
+	unsigned long flags;
+
+	if (sel->pad == FIMC_SD_PAD_SOURCE)
+		return -EINVAL;
+
+	mutex_lock(&fimc->lock);
+	fimc_capture_try_selection(ctx, r, V4L2_SEL_TGT_CROP);
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		try_sel = v4l2_subdev_get_try_crop(sd, cfg, sel->pad);
+		break;
+	case V4L2_SEL_TGT_COMPOSE:
+		try_sel = v4l2_subdev_get_try_compose(sd, cfg, sel->pad);
+		f = &ctx->d_frame;
+		break;
+	default:
+		mutex_unlock(&fimc->lock);
+		return -EINVAL;
+	}
+
+	if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
+		*try_sel = sel->r;
+	} else {
+		spin_lock_irqsave(&fimc->slock, flags);
+		set_frame_crop(f, r->left, r->top, r->width, r->height);
+		set_bit(ST_CAPT_APPLY_CFG, &fimc->state);
+		if (sel->target == V4L2_SEL_TGT_COMPOSE)
+			ctx->state |= FIMC_COMPOSE;
+		spin_unlock_irqrestore(&fimc->slock, flags);
+	}
+
+	dbg("target %#x: (%d,%d)/%dx%d", sel->target, r->left, r->top,
+	    r->width, r->height);
+
+	mutex_unlock(&fimc->lock);
+	return 0;
+}
+
+static struct v4l2_subdev_pad_ops fimc_subdev_pad_ops = {
+	.enum_mbus_code = fimc_subdev_enum_mbus_code,
+	.get_selection = fimc_subdev_get_selection,
+	.set_selection = fimc_subdev_set_selection,
+	.get_fmt = fimc_subdev_get_fmt,
+	.set_fmt = fimc_subdev_set_fmt,
+};
+
+static struct v4l2_subdev_ops fimc_subdev_ops = {
+	.pad = &fimc_subdev_pad_ops,
+};
+
+/* Set default format at the sensor and host interface */
+static int fimc_capture_set_default_format(struct fimc_dev *fimc)
+{
+	struct v4l2_format fmt = {
+		.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE,
+		.fmt.pix_mp = {
+			.width		= FIMC_DEFAULT_WIDTH,
+			.height		= FIMC_DEFAULT_HEIGHT,
+			.pixelformat	= V4L2_PIX_FMT_YUYV,
+			.field		= V4L2_FIELD_NONE,
+			.colorspace	= V4L2_COLORSPACE_JPEG,
+		},
+	};
+
+	return __fimc_capture_set_format(fimc, &fmt);
+}
+
+/* fimc->lock must be already initialized */
+static int fimc_register_capture_device(struct fimc_dev *fimc,
+				 struct v4l2_device *v4l2_dev)
+{
+	struct video_device *vfd = &fimc->vid_cap.ve.vdev;
+	struct vb2_queue *q = &fimc->vid_cap.vbq;
+	struct fimc_ctx *ctx;
+	struct fimc_vid_cap *vid_cap;
+	struct fimc_fmt *fmt;
+	int ret = -ENOMEM;
+
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx)
+		return -ENOMEM;
+
+	ctx->fimc_dev	 = fimc;
+	ctx->in_path	 = FIMC_IO_CAMERA;
+	ctx->out_path	 = FIMC_IO_DMA;
+	ctx->state	 = FIMC_CTX_CAP;
+	ctx->s_frame.fmt = fimc_find_format(NULL, NULL, FMT_FLAGS_CAM, 0);
+	ctx->d_frame.fmt = ctx->s_frame.fmt;
+
+	memset(vfd, 0, sizeof(*vfd));
+	snprintf(vfd->name, sizeof(vfd->name), "fimc.%d.capture", fimc->id);
+
+	vfd->fops	= &fimc_capture_fops;
+	vfd->ioctl_ops	= &fimc_capture_ioctl_ops;
+	vfd->v4l2_dev	= v4l2_dev;
+	vfd->minor	= -1;
+	vfd->release	= video_device_release_empty;
+	vfd->queue	= q;
+	vfd->lock	= &fimc->lock;
+
+	video_set_drvdata(vfd, fimc);
+	vid_cap = &fimc->vid_cap;
+	vid_cap->active_buf_cnt = 0;
+	vid_cap->reqbufs_count = 0;
+	vid_cap->ctx = ctx;
+
+	INIT_LIST_HEAD(&vid_cap->pending_buf_q);
+	INIT_LIST_HEAD(&vid_cap->active_buf_q);
+
+	memset(q, 0, sizeof(*q));
+	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
+	q->drv_priv = ctx;
+	q->ops = &fimc_capture_qops;
+	q->mem_ops = &vb2_dma_contig_memops;
+	q->buf_struct_size = sizeof(struct fimc_vid_buffer);
+	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	q->lock = &fimc->lock;
+
+	ret = vb2_queue_init(q);
+	if (ret)
+		goto err_free_ctx;
+
+	/* Default format configuration */
+	fmt = fimc_find_format(NULL, NULL, FMT_FLAGS_CAM, 0);
+	vid_cap->ci_fmt.width = FIMC_DEFAULT_WIDTH;
+	vid_cap->ci_fmt.height = FIMC_DEFAULT_HEIGHT;
+	vid_cap->ci_fmt.code = fmt->mbus_code;
+
+	ctx->s_frame.width = FIMC_DEFAULT_WIDTH;
+	ctx->s_frame.height = FIMC_DEFAULT_HEIGHT;
+	ctx->s_frame.fmt = fmt;
+
+	fmt = fimc_find_format(NULL, NULL, FMT_FLAGS_WRITEBACK, 0);
+	vid_cap->wb_fmt = vid_cap->ci_fmt;
+	vid_cap->wb_fmt.code = fmt->mbus_code;
+
+	vid_cap->vd_pad.flags = MEDIA_PAD_FL_SINK;
+	ret = media_entity_init(&vfd->entity, 1, &vid_cap->vd_pad, 0);
+	if (ret)
+		goto err_free_ctx;
+
+	ret = fimc_ctrls_create(ctx);
+	if (ret)
+		goto err_me_cleanup;
+
+	ret = video_register_device(vfd, VFL_TYPE_GRABBER, -1);
+	if (ret)
+		goto err_ctrl_free;
+
+	v4l2_info(v4l2_dev, "Registered %s as /dev/%s\n",
+		  vfd->name, video_device_node_name(vfd));
+
+	vfd->ctrl_handler = &ctx->ctrls.handler;
+	return 0;
+
+err_ctrl_free:
+	fimc_ctrls_delete(ctx);
+err_me_cleanup:
+	media_entity_cleanup(&vfd->entity);
+err_free_ctx:
+	kfree(ctx);
+	return ret;
+}
+
+static int fimc_capture_subdev_registered(struct v4l2_subdev *sd)
+{
+	struct fimc_dev *fimc = v4l2_get_subdevdata(sd);
+	int ret;
+
+	if (fimc == NULL)
+		return -ENXIO;
+
+	ret = fimc_register_m2m_device(fimc, sd->v4l2_dev);
+	if (ret)
+		return ret;
+
+	fimc->vid_cap.ve.pipe = v4l2_get_subdev_hostdata(sd);
+
+	ret = fimc_register_capture_device(fimc, sd->v4l2_dev);
+	if (ret) {
+		fimc_unregister_m2m_device(fimc);
+		fimc->vid_cap.ve.pipe = NULL;
+	}
+
+	return ret;
+}
+
+static void fimc_capture_subdev_unregistered(struct v4l2_subdev *sd)
+{
+	struct fimc_dev *fimc = v4l2_get_subdevdata(sd);
+	struct video_device *vdev;
+
+	if (fimc == NULL)
+		return;
+
+	mutex_lock(&fimc->lock);
+
+	fimc_unregister_m2m_device(fimc);
+	vdev = &fimc->vid_cap.ve.vdev;
+
+	if (video_is_registered(vdev)) {
+		video_unregister_device(vdev);
+		media_entity_cleanup(&vdev->entity);
+		fimc_ctrls_delete(fimc->vid_cap.ctx);
+		fimc->vid_cap.ve.pipe = NULL;
+	}
+	kfree(fimc->vid_cap.ctx);
+	fimc->vid_cap.ctx = NULL;
+
+	mutex_unlock(&fimc->lock);
+}
+
+static const struct v4l2_subdev_internal_ops fimc_capture_sd_internal_ops = {
+	.registered = fimc_capture_subdev_registered,
+	.unregistered = fimc_capture_subdev_unregistered,
+};
+
+int fimc_initialize_capture_subdev(struct fimc_dev *fimc)
+{
+	struct v4l2_subdev *sd = &fimc->vid_cap.subdev;
+	int ret;
+
+	v4l2_subdev_init(sd, &fimc_subdev_ops);
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	snprintf(sd->name, sizeof(sd->name), "FIMC.%d", fimc->id);
+
+	fimc->vid_cap.sd_pads[FIMC_SD_PAD_SINK_CAM].flags = MEDIA_PAD_FL_SINK;
+	fimc->vid_cap.sd_pads[FIMC_SD_PAD_SINK_FIFO].flags = MEDIA_PAD_FL_SINK;
+	fimc->vid_cap.sd_pads[FIMC_SD_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_init(&sd->entity, FIMC_SD_PADS_NUM,
+				fimc->vid_cap.sd_pads, 0);
+	if (ret)
+		return ret;
+
+	sd->entity.ops = &fimc_sd_media_ops;
+	sd->internal_ops = &fimc_capture_sd_internal_ops;
+	v4l2_set_subdevdata(sd, fimc);
+	return 0;
+}
+
+void fimc_unregister_capture_subdev(struct fimc_dev *fimc)
+{
+	struct v4l2_subdev *sd = &fimc->vid_cap.subdev;
+
+	v4l2_device_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	v4l2_set_subdevdata(sd, NULL);
+}
diff --git a/drivers/media/platform/exynos4-is/fimc-core.c b/drivers/media/platform/exynos4-is/fimc-core.c
new file mode 100644
index 0000000..cef2a7f
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-core.c
@@ -0,0 +1,1309 @@
+/*
+ * Samsung S5P/EXYNOS4 SoC series FIMC (CAMIF) driver
+ *
+ * Copyright (C) 2010-2012 Samsung Electronics Co., Ltd.
+ * Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published
+ * by the Free Software Foundation, either version 2 of the License,
+ * or (at your option) any later version.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/bug.h>
+#include <linux/interrupt.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/list.h>
+#include <linux/mfd/syscon.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/slab.h>
+#include <linux/clk.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "fimc-core.h"
+#include "fimc-reg.h"
+#include "media-dev.h"
+
+static char *fimc_clocks[MAX_FIMC_CLOCKS] = {
+	"sclk_fimc", "fimc"
+};
+
+static struct fimc_fmt fimc_formats[] = {
+	{
+		.name		= "RGB565",
+		.fourcc		= V4L2_PIX_FMT_RGB565,
+		.depth		= { 16 },
+		.color		= FIMC_FMT_RGB565,
+		.memplanes	= 1,
+		.colplanes	= 1,
+		.flags		= FMT_FLAGS_M2M,
+	}, {
+		.name		= "BGR666",
+		.fourcc		= V4L2_PIX_FMT_BGR666,
+		.depth		= { 32 },
+		.color		= FIMC_FMT_RGB666,
+		.memplanes	= 1,
+		.colplanes	= 1,
+		.flags		= FMT_FLAGS_M2M,
+	}, {
+		.name		= "BGRA8888, 32 bpp",
+		.fourcc		= V4L2_PIX_FMT_BGR32,
+		.depth		= { 32 },
+		.color		= FIMC_FMT_RGB888,
+		.memplanes	= 1,
+		.colplanes	= 1,
+		.flags		= FMT_FLAGS_M2M | FMT_HAS_ALPHA,
+	}, {
+		.name		= "ARGB1555",
+		.fourcc		= V4L2_PIX_FMT_RGB555,
+		.depth		= { 16 },
+		.color		= FIMC_FMT_RGB555,
+		.memplanes	= 1,
+		.colplanes	= 1,
+		.flags		= FMT_FLAGS_M2M_OUT | FMT_HAS_ALPHA,
+	}, {
+		.name		= "ARGB4444",
+		.fourcc		= V4L2_PIX_FMT_RGB444,
+		.depth		= { 16 },
+		.color		= FIMC_FMT_RGB444,
+		.memplanes	= 1,
+		.colplanes	= 1,
+		.flags		= FMT_FLAGS_M2M_OUT | FMT_HAS_ALPHA,
+	}, {
+		.name		= "YUV 4:4:4",
+		.mbus_code	= MEDIA_BUS_FMT_YUV10_1X30,
+		.flags		= FMT_FLAGS_WRITEBACK,
+	}, {
+		.name		= "YUV 4:2:2 packed, YCbYCr",
+		.fourcc		= V4L2_PIX_FMT_YUYV,
+		.depth		= { 16 },
+		.color		= FIMC_FMT_YCBYCR422,
+		.memplanes	= 1,
+		.colplanes	= 1,
+		.mbus_code	= MEDIA_BUS_FMT_YUYV8_2X8,
+		.flags		= FMT_FLAGS_M2M | FMT_FLAGS_CAM,
+	}, {
+		.name		= "YUV 4:2:2 packed, CbYCrY",
+		.fourcc		= V4L2_PIX_FMT_UYVY,
+		.depth		= { 16 },
+		.color		= FIMC_FMT_CBYCRY422,
+		.memplanes	= 1,
+		.colplanes	= 1,
+		.mbus_code	= MEDIA_BUS_FMT_UYVY8_2X8,
+		.flags		= FMT_FLAGS_M2M | FMT_FLAGS_CAM,
+	}, {
+		.name		= "YUV 4:2:2 packed, CrYCbY",
+		.fourcc		= V4L2_PIX_FMT_VYUY,
+		.depth		= { 16 },
+		.color		= FIMC_FMT_CRYCBY422,
+		.memplanes	= 1,
+		.colplanes	= 1,
+		.mbus_code	= MEDIA_BUS_FMT_VYUY8_2X8,
+		.flags		= FMT_FLAGS_M2M | FMT_FLAGS_CAM,
+	}, {
+		.name		= "YUV 4:2:2 packed, YCrYCb",
+		.fourcc		= V4L2_PIX_FMT_YVYU,
+		.depth		= { 16 },
+		.color		= FIMC_FMT_YCRYCB422,
+		.memplanes	= 1,
+		.colplanes	= 1,
+		.mbus_code	= MEDIA_BUS_FMT_YVYU8_2X8,
+		.flags		= FMT_FLAGS_M2M | FMT_FLAGS_CAM,
+	}, {
+		.name		= "YUV 4:2:2 planar, Y/Cb/Cr",
+		.fourcc		= V4L2_PIX_FMT_YUV422P,
+		.depth		= { 16 },
+		.color		= FIMC_FMT_YCBYCR422,
+		.memplanes	= 1,
+		.colplanes	= 3,
+		.flags		= FMT_FLAGS_M2M,
+	}, {
+		.name		= "YUV 4:2:2 planar, Y/CbCr",
+		.fourcc		= V4L2_PIX_FMT_NV16,
+		.depth		= { 16 },
+		.color		= FIMC_FMT_YCBYCR422,
+		.memplanes	= 1,
+		.colplanes	= 2,
+		.flags		= FMT_FLAGS_M2M,
+	}, {
+		.name		= "YUV 4:2:2 planar, Y/CrCb",
+		.fourcc		= V4L2_PIX_FMT_NV61,
+		.depth		= { 16 },
+		.color		= FIMC_FMT_YCRYCB422,
+		.memplanes	= 1,
+		.colplanes	= 2,
+		.flags		= FMT_FLAGS_M2M,
+	}, {
+		.name		= "YUV 4:2:0 planar, YCbCr",
+		.fourcc		= V4L2_PIX_FMT_YUV420,
+		.depth		= { 12 },
+		.color		= FIMC_FMT_YCBCR420,
+		.memplanes	= 1,
+		.colplanes	= 3,
+		.flags		= FMT_FLAGS_M2M,
+	}, {
+		.name		= "YUV 4:2:0 planar, Y/CbCr",
+		.fourcc		= V4L2_PIX_FMT_NV12,
+		.depth		= { 12 },
+		.color		= FIMC_FMT_YCBCR420,
+		.memplanes	= 1,
+		.colplanes	= 2,
+		.flags		= FMT_FLAGS_M2M,
+	}, {
+		.name		= "YUV 4:2:0 non-contig. 2p, Y/CbCr",
+		.fourcc		= V4L2_PIX_FMT_NV12M,
+		.color		= FIMC_FMT_YCBCR420,
+		.depth		= { 8, 4 },
+		.memplanes	= 2,
+		.colplanes	= 2,
+		.flags		= FMT_FLAGS_M2M,
+	}, {
+		.name		= "YUV 4:2:0 non-contig. 3p, Y/Cb/Cr",
+		.fourcc		= V4L2_PIX_FMT_YUV420M,
+		.color		= FIMC_FMT_YCBCR420,
+		.depth		= { 8, 2, 2 },
+		.memplanes	= 3,
+		.colplanes	= 3,
+		.flags		= FMT_FLAGS_M2M,
+	}, {
+		.name		= "YUV 4:2:0 non-contig. 2p, tiled",
+		.fourcc		= V4L2_PIX_FMT_NV12MT,
+		.color		= FIMC_FMT_YCBCR420,
+		.depth		= { 8, 4 },
+		.memplanes	= 2,
+		.colplanes	= 2,
+		.flags		= FMT_FLAGS_M2M,
+	}, {
+		.name		= "JPEG encoded data",
+		.fourcc		= V4L2_PIX_FMT_JPEG,
+		.color		= FIMC_FMT_JPEG,
+		.depth		= { 8 },
+		.memplanes	= 1,
+		.colplanes	= 1,
+		.mbus_code	= MEDIA_BUS_FMT_JPEG_1X8,
+		.flags		= FMT_FLAGS_CAM | FMT_FLAGS_COMPRESSED,
+	}, {
+		.name		= "S5C73MX interleaved UYVY/JPEG",
+		.fourcc		= V4L2_PIX_FMT_S5C_UYVY_JPG,
+		.color		= FIMC_FMT_YUYV_JPEG,
+		.depth		= { 8 },
+		.memplanes	= 2,
+		.colplanes	= 1,
+		.mdataplanes	= 0x2, /* plane 1 holds frame meta data */
+		.mbus_code	= MEDIA_BUS_FMT_S5C_UYVY_JPEG_1X8,
+		.flags		= FMT_FLAGS_CAM | FMT_FLAGS_COMPRESSED,
+	},
+};
+
+struct fimc_fmt *fimc_get_format(unsigned int index)
+{
+	if (index >= ARRAY_SIZE(fimc_formats))
+		return NULL;
+
+	return &fimc_formats[index];
+}
+
+int fimc_check_scaler_ratio(struct fimc_ctx *ctx, int sw, int sh,
+			    int dw, int dh, int rotation)
+{
+	if (rotation == 90 || rotation == 270)
+		swap(dw, dh);
+
+	if (!ctx->scaler.enabled)
+		return (sw == dw && sh == dh) ? 0 : -EINVAL;
+
+	if ((sw >= SCALER_MAX_HRATIO * dw) || (sh >= SCALER_MAX_VRATIO * dh))
+		return -EINVAL;
+
+	return 0;
+}
+
+static int fimc_get_scaler_factor(u32 src, u32 tar, u32 *ratio, u32 *shift)
+{
+	u32 sh = 6;
+
+	if (src >= 64 * tar)
+		return -EINVAL;
+
+	while (sh--) {
+		u32 tmp = 1 << sh;
+		if (src >= tar * tmp) {
+			*shift = sh, *ratio = tmp;
+			return 0;
+		}
+	}
+	*shift = 0, *ratio = 1;
+	return 0;
+}
+
+int fimc_set_scaler_info(struct fimc_ctx *ctx)
+{
+	const struct fimc_variant *variant = ctx->fimc_dev->variant;
+	struct device *dev = &ctx->fimc_dev->pdev->dev;
+	struct fimc_scaler *sc = &ctx->scaler;
+	struct fimc_frame *s_frame = &ctx->s_frame;
+	struct fimc_frame *d_frame = &ctx->d_frame;
+	int tx, ty, sx, sy;
+	int ret;
+
+	if (ctx->rotation == 90 || ctx->rotation == 270) {
+		ty = d_frame->width;
+		tx = d_frame->height;
+	} else {
+		tx = d_frame->width;
+		ty = d_frame->height;
+	}
+	if (tx <= 0 || ty <= 0) {
+		dev_err(dev, "Invalid target size: %dx%d\n", tx, ty);
+		return -EINVAL;
+	}
+
+	sx = s_frame->width;
+	sy = s_frame->height;
+	if (sx <= 0 || sy <= 0) {
+		dev_err(dev, "Invalid source size: %dx%d\n", sx, sy);
+		return -EINVAL;
+	}
+	sc->real_width = sx;
+	sc->real_height = sy;
+
+	ret = fimc_get_scaler_factor(sx, tx, &sc->pre_hratio, &sc->hfactor);
+	if (ret)
+		return ret;
+
+	ret = fimc_get_scaler_factor(sy, ty,  &sc->pre_vratio, &sc->vfactor);
+	if (ret)
+		return ret;
+
+	sc->pre_dst_width = sx / sc->pre_hratio;
+	sc->pre_dst_height = sy / sc->pre_vratio;
+
+	if (variant->has_mainscaler_ext) {
+		sc->main_hratio = (sx << 14) / (tx << sc->hfactor);
+		sc->main_vratio = (sy << 14) / (ty << sc->vfactor);
+	} else {
+		sc->main_hratio = (sx << 8) / (tx << sc->hfactor);
+		sc->main_vratio = (sy << 8) / (ty << sc->vfactor);
+
+	}
+
+	sc->scaleup_h = (tx >= sx) ? 1 : 0;
+	sc->scaleup_v = (ty >= sy) ? 1 : 0;
+
+	/* check to see if input and output size/format differ */
+	if (s_frame->fmt->color == d_frame->fmt->color
+		&& s_frame->width == d_frame->width
+		&& s_frame->height == d_frame->height)
+		sc->copy_mode = 1;
+	else
+		sc->copy_mode = 0;
+
+	return 0;
+}
+
+static irqreturn_t fimc_irq_handler(int irq, void *priv)
+{
+	struct fimc_dev *fimc = priv;
+	struct fimc_ctx *ctx;
+
+	fimc_hw_clear_irq(fimc);
+
+	spin_lock(&fimc->slock);
+
+	if (test_and_clear_bit(ST_M2M_PEND, &fimc->state)) {
+		if (test_and_clear_bit(ST_M2M_SUSPENDING, &fimc->state)) {
+			set_bit(ST_M2M_SUSPENDED, &fimc->state);
+			wake_up(&fimc->irq_queue);
+			goto out;
+		}
+		ctx = v4l2_m2m_get_curr_priv(fimc->m2m.m2m_dev);
+		if (ctx != NULL) {
+			spin_unlock(&fimc->slock);
+			fimc_m2m_job_finish(ctx, VB2_BUF_STATE_DONE);
+
+			if (ctx->state & FIMC_CTX_SHUT) {
+				ctx->state &= ~FIMC_CTX_SHUT;
+				wake_up(&fimc->irq_queue);
+			}
+			return IRQ_HANDLED;
+		}
+	} else if (test_bit(ST_CAPT_PEND, &fimc->state)) {
+		int last_buf = test_bit(ST_CAPT_JPEG, &fimc->state) &&
+				fimc->vid_cap.reqbufs_count == 1;
+		fimc_capture_irq_handler(fimc, !last_buf);
+	}
+out:
+	spin_unlock(&fimc->slock);
+	return IRQ_HANDLED;
+}
+
+/* The color format (colplanes, memplanes) must be already configured. */
+int fimc_prepare_addr(struct fimc_ctx *ctx, struct vb2_buffer *vb,
+		      struct fimc_frame *frame, struct fimc_addr *paddr)
+{
+	int ret = 0;
+	u32 pix_size;
+
+	if (vb == NULL || frame == NULL)
+		return -EINVAL;
+
+	pix_size = frame->width * frame->height;
+
+	dbg("memplanes= %d, colplanes= %d, pix_size= %d",
+		frame->fmt->memplanes, frame->fmt->colplanes, pix_size);
+
+	paddr->y = vb2_dma_contig_plane_dma_addr(vb, 0);
+
+	if (frame->fmt->memplanes == 1) {
+		switch (frame->fmt->colplanes) {
+		case 1:
+			paddr->cb = 0;
+			paddr->cr = 0;
+			break;
+		case 2:
+			/* decompose Y into Y/Cb */
+			paddr->cb = (u32)(paddr->y + pix_size);
+			paddr->cr = 0;
+			break;
+		case 3:
+			paddr->cb = (u32)(paddr->y + pix_size);
+			/* decompose Y into Y/Cb/Cr */
+			if (FIMC_FMT_YCBCR420 == frame->fmt->color)
+				paddr->cr = (u32)(paddr->cb
+						+ (pix_size >> 2));
+			else /* 422 */
+				paddr->cr = (u32)(paddr->cb
+						+ (pix_size >> 1));
+			break;
+		default:
+			return -EINVAL;
+		}
+	} else if (!frame->fmt->mdataplanes) {
+		if (frame->fmt->memplanes >= 2)
+			paddr->cb = vb2_dma_contig_plane_dma_addr(vb, 1);
+
+		if (frame->fmt->memplanes == 3)
+			paddr->cr = vb2_dma_contig_plane_dma_addr(vb, 2);
+	}
+
+	dbg("PHYS_ADDR: y= 0x%X  cb= 0x%X cr= 0x%X ret= %d",
+	    paddr->y, paddr->cb, paddr->cr, ret);
+
+	return ret;
+}
+
+/* Set order for 1 and 2 plane YCBCR 4:2:2 formats. */
+void fimc_set_yuv_order(struct fimc_ctx *ctx)
+{
+	/* The one only mode supported in SoC. */
+	ctx->in_order_2p = FIMC_REG_CIOCTRL_ORDER422_2P_LSB_CRCB;
+	ctx->out_order_2p = FIMC_REG_CIOCTRL_ORDER422_2P_LSB_CRCB;
+
+	/* Set order for 1 plane input formats. */
+	switch (ctx->s_frame.fmt->color) {
+	case FIMC_FMT_YCRYCB422:
+		ctx->in_order_1p = FIMC_REG_MSCTRL_ORDER422_YCRYCB;
+		break;
+	case FIMC_FMT_CBYCRY422:
+		ctx->in_order_1p = FIMC_REG_MSCTRL_ORDER422_CBYCRY;
+		break;
+	case FIMC_FMT_CRYCBY422:
+		ctx->in_order_1p = FIMC_REG_MSCTRL_ORDER422_CRYCBY;
+		break;
+	case FIMC_FMT_YCBYCR422:
+	default:
+		ctx->in_order_1p = FIMC_REG_MSCTRL_ORDER422_YCBYCR;
+		break;
+	}
+	dbg("ctx->in_order_1p= %d", ctx->in_order_1p);
+
+	switch (ctx->d_frame.fmt->color) {
+	case FIMC_FMT_YCRYCB422:
+		ctx->out_order_1p = FIMC_REG_CIOCTRL_ORDER422_YCRYCB;
+		break;
+	case FIMC_FMT_CBYCRY422:
+		ctx->out_order_1p = FIMC_REG_CIOCTRL_ORDER422_CBYCRY;
+		break;
+	case FIMC_FMT_CRYCBY422:
+		ctx->out_order_1p = FIMC_REG_CIOCTRL_ORDER422_CRYCBY;
+		break;
+	case FIMC_FMT_YCBYCR422:
+	default:
+		ctx->out_order_1p = FIMC_REG_CIOCTRL_ORDER422_YCBYCR;
+		break;
+	}
+	dbg("ctx->out_order_1p= %d", ctx->out_order_1p);
+}
+
+void fimc_prepare_dma_offset(struct fimc_ctx *ctx, struct fimc_frame *f)
+{
+	bool pix_hoff = ctx->fimc_dev->drv_data->dma_pix_hoff;
+	u32 i, depth = 0;
+
+	for (i = 0; i < f->fmt->memplanes; i++)
+		depth += f->fmt->depth[i];
+
+	f->dma_offset.y_h = f->offs_h;
+	if (!pix_hoff)
+		f->dma_offset.y_h *= (depth >> 3);
+
+	f->dma_offset.y_v = f->offs_v;
+
+	f->dma_offset.cb_h = f->offs_h;
+	f->dma_offset.cb_v = f->offs_v;
+
+	f->dma_offset.cr_h = f->offs_h;
+	f->dma_offset.cr_v = f->offs_v;
+
+	if (!pix_hoff) {
+		if (f->fmt->colplanes == 3) {
+			f->dma_offset.cb_h >>= 1;
+			f->dma_offset.cr_h >>= 1;
+		}
+		if (f->fmt->color == FIMC_FMT_YCBCR420) {
+			f->dma_offset.cb_v >>= 1;
+			f->dma_offset.cr_v >>= 1;
+		}
+	}
+
+	dbg("in_offset: color= %d, y_h= %d, y_v= %d",
+	    f->fmt->color, f->dma_offset.y_h, f->dma_offset.y_v);
+}
+
+static int fimc_set_color_effect(struct fimc_ctx *ctx, enum v4l2_colorfx colorfx)
+{
+	struct fimc_effect *effect = &ctx->effect;
+
+	switch (colorfx) {
+	case V4L2_COLORFX_NONE:
+		effect->type = FIMC_REG_CIIMGEFF_FIN_BYPASS;
+		break;
+	case V4L2_COLORFX_BW:
+		effect->type = FIMC_REG_CIIMGEFF_FIN_ARBITRARY;
+		effect->pat_cb = 128;
+		effect->pat_cr = 128;
+		break;
+	case V4L2_COLORFX_SEPIA:
+		effect->type = FIMC_REG_CIIMGEFF_FIN_ARBITRARY;
+		effect->pat_cb = 115;
+		effect->pat_cr = 145;
+		break;
+	case V4L2_COLORFX_NEGATIVE:
+		effect->type = FIMC_REG_CIIMGEFF_FIN_NEGATIVE;
+		break;
+	case V4L2_COLORFX_EMBOSS:
+		effect->type = FIMC_REG_CIIMGEFF_FIN_EMBOSSING;
+		break;
+	case V4L2_COLORFX_ART_FREEZE:
+		effect->type = FIMC_REG_CIIMGEFF_FIN_ARTFREEZE;
+		break;
+	case V4L2_COLORFX_SILHOUETTE:
+		effect->type = FIMC_REG_CIIMGEFF_FIN_SILHOUETTE;
+		break;
+	case V4L2_COLORFX_SET_CBCR:
+		effect->type = FIMC_REG_CIIMGEFF_FIN_ARBITRARY;
+		effect->pat_cb = ctx->ctrls.colorfx_cbcr->val >> 8;
+		effect->pat_cr = ctx->ctrls.colorfx_cbcr->val & 0xff;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/*
+ * V4L2 controls handling
+ */
+#define ctrl_to_ctx(__ctrl) \
+	container_of((__ctrl)->handler, struct fimc_ctx, ctrls.handler)
+
+static int __fimc_s_ctrl(struct fimc_ctx *ctx, struct v4l2_ctrl *ctrl)
+{
+	struct fimc_dev *fimc = ctx->fimc_dev;
+	const struct fimc_variant *variant = fimc->variant;
+	int ret = 0;
+
+	if (ctrl->flags & V4L2_CTRL_FLAG_INACTIVE)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_HFLIP:
+		ctx->hflip = ctrl->val;
+		break;
+
+	case V4L2_CID_VFLIP:
+		ctx->vflip = ctrl->val;
+		break;
+
+	case V4L2_CID_ROTATE:
+		if (fimc_capture_pending(fimc)) {
+			ret = fimc_check_scaler_ratio(ctx, ctx->s_frame.width,
+					ctx->s_frame.height, ctx->d_frame.width,
+					ctx->d_frame.height, ctrl->val);
+			if (ret)
+				return -EINVAL;
+		}
+		if ((ctrl->val == 90 || ctrl->val == 270) &&
+		    !variant->has_out_rot)
+			return -EINVAL;
+
+		ctx->rotation = ctrl->val;
+		break;
+
+	case V4L2_CID_ALPHA_COMPONENT:
+		ctx->d_frame.alpha = ctrl->val;
+		break;
+
+	case V4L2_CID_COLORFX:
+		ret = fimc_set_color_effect(ctx, ctrl->val);
+		if (ret)
+			return ret;
+		break;
+	}
+
+	ctx->state |= FIMC_PARAMS;
+	set_bit(ST_CAPT_APPLY_CFG, &fimc->state);
+	return 0;
+}
+
+static int fimc_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct fimc_ctx *ctx = ctrl_to_ctx(ctrl);
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&ctx->fimc_dev->slock, flags);
+	ret = __fimc_s_ctrl(ctx, ctrl);
+	spin_unlock_irqrestore(&ctx->fimc_dev->slock, flags);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops fimc_ctrl_ops = {
+	.s_ctrl = fimc_s_ctrl,
+};
+
+int fimc_ctrls_create(struct fimc_ctx *ctx)
+{
+	unsigned int max_alpha = fimc_get_alpha_mask(ctx->d_frame.fmt);
+	struct fimc_ctrls *ctrls = &ctx->ctrls;
+	struct v4l2_ctrl_handler *handler = &ctrls->handler;
+
+	if (ctx->ctrls.ready)
+		return 0;
+
+	v4l2_ctrl_handler_init(handler, 6);
+
+	ctrls->rotate = v4l2_ctrl_new_std(handler, &fimc_ctrl_ops,
+					V4L2_CID_ROTATE, 0, 270, 90, 0);
+	ctrls->hflip = v4l2_ctrl_new_std(handler, &fimc_ctrl_ops,
+					V4L2_CID_HFLIP, 0, 1, 1, 0);
+	ctrls->vflip = v4l2_ctrl_new_std(handler, &fimc_ctrl_ops,
+					V4L2_CID_VFLIP, 0, 1, 1, 0);
+
+	if (ctx->fimc_dev->drv_data->alpha_color)
+		ctrls->alpha = v4l2_ctrl_new_std(handler, &fimc_ctrl_ops,
+					V4L2_CID_ALPHA_COMPONENT,
+					0, max_alpha, 1, 0);
+	else
+		ctrls->alpha = NULL;
+
+	ctrls->colorfx = v4l2_ctrl_new_std_menu(handler, &fimc_ctrl_ops,
+				V4L2_CID_COLORFX, V4L2_COLORFX_SET_CBCR,
+				~0x983f, V4L2_COLORFX_NONE);
+
+	ctrls->colorfx_cbcr = v4l2_ctrl_new_std(handler, &fimc_ctrl_ops,
+				V4L2_CID_COLORFX_CBCR, 0, 0xffff, 1, 0);
+
+	ctx->effect.type = FIMC_REG_CIIMGEFF_FIN_BYPASS;
+
+	if (!handler->error) {
+		v4l2_ctrl_cluster(2, &ctrls->colorfx);
+		ctrls->ready = true;
+	}
+
+	return handler->error;
+}
+
+void fimc_ctrls_delete(struct fimc_ctx *ctx)
+{
+	struct fimc_ctrls *ctrls = &ctx->ctrls;
+
+	if (ctrls->ready) {
+		v4l2_ctrl_handler_free(&ctrls->handler);
+		ctrls->ready = false;
+		ctrls->alpha = NULL;
+	}
+}
+
+void fimc_ctrls_activate(struct fimc_ctx *ctx, bool active)
+{
+	unsigned int has_alpha = ctx->d_frame.fmt->flags & FMT_HAS_ALPHA;
+	struct fimc_ctrls *ctrls = &ctx->ctrls;
+
+	if (!ctrls->ready)
+		return;
+
+	mutex_lock(ctrls->handler.lock);
+	v4l2_ctrl_activate(ctrls->rotate, active);
+	v4l2_ctrl_activate(ctrls->hflip, active);
+	v4l2_ctrl_activate(ctrls->vflip, active);
+	v4l2_ctrl_activate(ctrls->colorfx, active);
+	if (ctrls->alpha)
+		v4l2_ctrl_activate(ctrls->alpha, active && has_alpha);
+
+	if (active) {
+		fimc_set_color_effect(ctx, ctrls->colorfx->cur.val);
+		ctx->rotation = ctrls->rotate->val;
+		ctx->hflip    = ctrls->hflip->val;
+		ctx->vflip    = ctrls->vflip->val;
+	} else {
+		ctx->effect.type = FIMC_REG_CIIMGEFF_FIN_BYPASS;
+		ctx->rotation = 0;
+		ctx->hflip    = 0;
+		ctx->vflip    = 0;
+	}
+	mutex_unlock(ctrls->handler.lock);
+}
+
+/* Update maximum value of the alpha color control */
+void fimc_alpha_ctrl_update(struct fimc_ctx *ctx)
+{
+	struct fimc_dev *fimc = ctx->fimc_dev;
+	struct v4l2_ctrl *ctrl = ctx->ctrls.alpha;
+
+	if (ctrl == NULL || !fimc->drv_data->alpha_color)
+		return;
+
+	v4l2_ctrl_lock(ctrl);
+	ctrl->maximum = fimc_get_alpha_mask(ctx->d_frame.fmt);
+
+	if (ctrl->cur.val > ctrl->maximum)
+		ctrl->cur.val = ctrl->maximum;
+
+	v4l2_ctrl_unlock(ctrl);
+}
+
+void __fimc_get_format(struct fimc_frame *frame, struct v4l2_format *f)
+{
+	struct v4l2_pix_format_mplane *pixm = &f->fmt.pix_mp;
+	int i;
+
+	pixm->width = frame->o_width;
+	pixm->height = frame->o_height;
+	pixm->field = V4L2_FIELD_NONE;
+	pixm->pixelformat = frame->fmt->fourcc;
+	pixm->colorspace = V4L2_COLORSPACE_JPEG;
+	pixm->num_planes = frame->fmt->memplanes;
+
+	for (i = 0; i < pixm->num_planes; ++i) {
+		pixm->plane_fmt[i].bytesperline = frame->bytesperline[i];
+		pixm->plane_fmt[i].sizeimage = frame->payload[i];
+	}
+}
+
+/**
+ * fimc_adjust_mplane_format - adjust bytesperline/sizeimage for each plane
+ * @fmt: fimc pixel format description (input)
+ * @width: requested pixel width
+ * @height: requested pixel height
+ * @pix: multi-plane format to adjust
+ */
+void fimc_adjust_mplane_format(struct fimc_fmt *fmt, u32 width, u32 height,
+			       struct v4l2_pix_format_mplane *pix)
+{
+	u32 bytesperline = 0;
+	int i;
+
+	pix->colorspace	= V4L2_COLORSPACE_JPEG;
+	pix->field = V4L2_FIELD_NONE;
+	pix->num_planes = fmt->memplanes;
+	pix->pixelformat = fmt->fourcc;
+	pix->height = height;
+	pix->width = width;
+
+	for (i = 0; i < pix->num_planes; ++i) {
+		struct v4l2_plane_pix_format *plane_fmt = &pix->plane_fmt[i];
+		u32 bpl = plane_fmt->bytesperline;
+
+		if (fmt->colplanes > 1 && (bpl == 0 || bpl < pix->width))
+			bpl = pix->width; /* Planar */
+
+		if (fmt->colplanes == 1 && /* Packed */
+		    (bpl == 0 || ((bpl * 8) / fmt->depth[i]) < pix->width))
+			bpl = (pix->width * fmt->depth[0]) / 8;
+		/*
+		 * Currently bytesperline for each plane is same, except
+		 * V4L2_PIX_FMT_YUV420M format. This calculation may need
+		 * to be changed when other multi-planar formats are added
+		 * to the fimc_formats[] array.
+		 */
+		if (i == 0)
+			bytesperline = bpl;
+		else if (i == 1 && fmt->memplanes == 3)
+			bytesperline /= 2;
+
+		plane_fmt->bytesperline = bytesperline;
+		plane_fmt->sizeimage = max((pix->width * pix->height *
+				   fmt->depth[i]) / 8, plane_fmt->sizeimage);
+	}
+}
+
+/**
+ * fimc_find_format - lookup fimc color format by fourcc or media bus format
+ * @pixelformat: fourcc to match, ignored if null
+ * @mbus_code: media bus code to match, ignored if null
+ * @mask: the color flags to match
+ * @index: offset in the fimc_formats array, ignored if negative
+ */
+struct fimc_fmt *fimc_find_format(const u32 *pixelformat, const u32 *mbus_code,
+				  unsigned int mask, int index)
+{
+	struct fimc_fmt *fmt, *def_fmt = NULL;
+	unsigned int i;
+	int id = 0;
+
+	if (index >= (int)ARRAY_SIZE(fimc_formats))
+		return NULL;
+
+	for (i = 0; i < ARRAY_SIZE(fimc_formats); ++i) {
+		fmt = &fimc_formats[i];
+		if (!(fmt->flags & mask))
+			continue;
+		if (pixelformat && fmt->fourcc == *pixelformat)
+			return fmt;
+		if (mbus_code && fmt->mbus_code == *mbus_code)
+			return fmt;
+		if (index == id)
+			def_fmt = fmt;
+		id++;
+	}
+	return def_fmt;
+}
+
+static void fimc_clk_put(struct fimc_dev *fimc)
+{
+	int i;
+	for (i = 0; i < MAX_FIMC_CLOCKS; i++) {
+		if (IS_ERR(fimc->clock[i]))
+			continue;
+		clk_unprepare(fimc->clock[i]);
+		clk_put(fimc->clock[i]);
+		fimc->clock[i] = ERR_PTR(-EINVAL);
+	}
+}
+
+static int fimc_clk_get(struct fimc_dev *fimc)
+{
+	int i, ret;
+
+	for (i = 0; i < MAX_FIMC_CLOCKS; i++)
+		fimc->clock[i] = ERR_PTR(-EINVAL);
+
+	for (i = 0; i < MAX_FIMC_CLOCKS; i++) {
+		fimc->clock[i] = clk_get(&fimc->pdev->dev, fimc_clocks[i]);
+		if (IS_ERR(fimc->clock[i])) {
+			ret = PTR_ERR(fimc->clock[i]);
+			goto err;
+		}
+		ret = clk_prepare(fimc->clock[i]);
+		if (ret < 0) {
+			clk_put(fimc->clock[i]);
+			fimc->clock[i] = ERR_PTR(-EINVAL);
+			goto err;
+		}
+	}
+	return 0;
+err:
+	fimc_clk_put(fimc);
+	dev_err(&fimc->pdev->dev, "failed to get clock: %s\n",
+		fimc_clocks[i]);
+	return -ENXIO;
+}
+
+#ifdef CONFIG_PM
+static int fimc_m2m_suspend(struct fimc_dev *fimc)
+{
+	unsigned long flags;
+	int timeout;
+
+	spin_lock_irqsave(&fimc->slock, flags);
+	if (!fimc_m2m_pending(fimc)) {
+		spin_unlock_irqrestore(&fimc->slock, flags);
+		return 0;
+	}
+	clear_bit(ST_M2M_SUSPENDED, &fimc->state);
+	set_bit(ST_M2M_SUSPENDING, &fimc->state);
+	spin_unlock_irqrestore(&fimc->slock, flags);
+
+	timeout = wait_event_timeout(fimc->irq_queue,
+			     test_bit(ST_M2M_SUSPENDED, &fimc->state),
+			     FIMC_SHUTDOWN_TIMEOUT);
+
+	clear_bit(ST_M2M_SUSPENDING, &fimc->state);
+	return timeout == 0 ? -EAGAIN : 0;
+}
+
+static int fimc_m2m_resume(struct fimc_dev *fimc)
+{
+	struct fimc_ctx *ctx;
+	unsigned long flags;
+
+	spin_lock_irqsave(&fimc->slock, flags);
+	/* Clear for full H/W setup in first run after resume */
+	ctx = fimc->m2m.ctx;
+	fimc->m2m.ctx = NULL;
+	spin_unlock_irqrestore(&fimc->slock, flags);
+
+	if (test_and_clear_bit(ST_M2M_SUSPENDED, &fimc->state))
+		fimc_m2m_job_finish(ctx, VB2_BUF_STATE_ERROR);
+
+	return 0;
+}
+#endif /* CONFIG_PM */
+
+static const struct of_device_id fimc_of_match[];
+
+static int fimc_parse_dt(struct fimc_dev *fimc, u32 *clk_freq)
+{
+	struct device *dev = &fimc->pdev->dev;
+	struct device_node *node = dev->of_node;
+	const struct of_device_id *of_id;
+	struct fimc_variant *v;
+	struct fimc_pix_limit *lim;
+	u32 args[FIMC_PIX_LIMITS_MAX];
+	int ret;
+
+	if (of_property_read_bool(node, "samsung,lcd-wb"))
+		return -ENODEV;
+
+	v = devm_kzalloc(dev, sizeof(*v) + sizeof(*lim), GFP_KERNEL);
+	if (!v)
+		return -ENOMEM;
+
+	of_id = of_match_node(fimc_of_match, node);
+	if (!of_id)
+		return -EINVAL;
+	fimc->drv_data = of_id->data;
+	ret = of_property_read_u32_array(node, "samsung,pix-limits",
+					 args, FIMC_PIX_LIMITS_MAX);
+	if (ret < 0)
+		return ret;
+
+	lim = (struct fimc_pix_limit *)&v[1];
+
+	lim->scaler_en_w = args[0];
+	lim->scaler_dis_w = args[1];
+	lim->out_rot_en_w = args[2];
+	lim->out_rot_dis_w = args[3];
+	v->pix_limit = lim;
+
+	ret = of_property_read_u32_array(node, "samsung,min-pix-sizes",
+								args, 2);
+	v->min_inp_pixsize = ret ? FIMC_DEF_MIN_SIZE : args[0];
+	v->min_out_pixsize = ret ? FIMC_DEF_MIN_SIZE : args[1];
+	ret = of_property_read_u32_array(node, "samsung,min-pix-alignment",
+								args, 2);
+	v->min_vsize_align = ret ? FIMC_DEF_HEIGHT_ALIGN : args[0];
+	v->hor_offs_align = ret ? FIMC_DEF_HOR_OFFS_ALIGN : args[1];
+
+	ret = of_property_read_u32(node, "samsung,rotators", &args[1]);
+	v->has_inp_rot = ret ? 1 : args[1] & 0x01;
+	v->has_out_rot = ret ? 1 : args[1] & 0x10;
+	v->has_mainscaler_ext = of_property_read_bool(node,
+					"samsung,mainscaler-ext");
+
+	v->has_isp_wb = of_property_read_bool(node, "samsung,isp-wb");
+	v->has_cam_if = of_property_read_bool(node, "samsung,cam-if");
+	of_property_read_u32(node, "clock-frequency", clk_freq);
+	fimc->id = of_alias_get_id(node, "fimc");
+
+	fimc->variant = v;
+	return 0;
+}
+
+static int fimc_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	u32 lclk_freq = 0;
+	struct fimc_dev *fimc;
+	struct resource *res;
+	int ret = 0;
+
+	fimc = devm_kzalloc(dev, sizeof(*fimc), GFP_KERNEL);
+	if (!fimc)
+		return -ENOMEM;
+
+	fimc->pdev = pdev;
+
+	if (dev->of_node) {
+		ret = fimc_parse_dt(fimc, &lclk_freq);
+		if (ret < 0)
+			return ret;
+	} else {
+		fimc->drv_data = fimc_get_drvdata(pdev);
+		fimc->id = pdev->id;
+	}
+	if (!fimc->drv_data || fimc->id >= fimc->drv_data->num_entities ||
+	    fimc->id < 0) {
+		dev_err(dev, "Invalid driver data or device id (%d)\n",
+			fimc->id);
+		return -EINVAL;
+	}
+	if (!dev->of_node)
+		fimc->variant = fimc->drv_data->variant[fimc->id];
+
+	init_waitqueue_head(&fimc->irq_queue);
+	spin_lock_init(&fimc->slock);
+	mutex_init(&fimc->lock);
+
+	fimc->sysreg = fimc_get_sysreg_regmap(dev->of_node);
+	if (IS_ERR(fimc->sysreg))
+		return PTR_ERR(fimc->sysreg);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	fimc->regs = devm_ioremap_resource(dev, res);
+	if (IS_ERR(fimc->regs))
+		return PTR_ERR(fimc->regs);
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (res == NULL) {
+		dev_err(dev, "Failed to get IRQ resource\n");
+		return -ENXIO;
+	}
+
+	ret = fimc_clk_get(fimc);
+	if (ret)
+		return ret;
+
+	if (lclk_freq == 0)
+		lclk_freq = fimc->drv_data->lclk_frequency;
+
+	ret = clk_set_rate(fimc->clock[CLK_BUS], lclk_freq);
+	if (ret < 0)
+		return ret;
+
+	ret = clk_enable(fimc->clock[CLK_BUS]);
+	if (ret < 0)
+		return ret;
+
+	ret = devm_request_irq(dev, res->start, fimc_irq_handler,
+			       0, dev_name(dev), fimc);
+	if (ret < 0) {
+		dev_err(dev, "failed to install irq (%d)\n", ret);
+		goto err_sclk;
+	}
+
+	ret = fimc_initialize_capture_subdev(fimc);
+	if (ret < 0)
+		goto err_sclk;
+
+	platform_set_drvdata(pdev, fimc);
+	pm_runtime_enable(dev);
+
+	if (!pm_runtime_enabled(dev)) {
+		ret = clk_enable(fimc->clock[CLK_GATE]);
+		if (ret < 0)
+			goto err_sd;
+	}
+
+	/* Initialize contiguous memory allocator */
+	fimc->alloc_ctx = vb2_dma_contig_init_ctx(dev);
+	if (IS_ERR(fimc->alloc_ctx)) {
+		ret = PTR_ERR(fimc->alloc_ctx);
+		goto err_gclk;
+	}
+
+	dev_dbg(dev, "FIMC.%d registered successfully\n", fimc->id);
+	return 0;
+
+err_gclk:
+	if (!pm_runtime_enabled(dev))
+		clk_disable(fimc->clock[CLK_GATE]);
+err_sd:
+	fimc_unregister_capture_subdev(fimc);
+err_sclk:
+	clk_disable(fimc->clock[CLK_BUS]);
+	fimc_clk_put(fimc);
+	return ret;
+}
+
+#ifdef CONFIG_PM
+static int fimc_runtime_resume(struct device *dev)
+{
+	struct fimc_dev *fimc =	dev_get_drvdata(dev);
+
+	dbg("fimc%d: state: 0x%lx", fimc->id, fimc->state);
+
+	/* Enable clocks and perform basic initialization */
+	clk_enable(fimc->clock[CLK_GATE]);
+	fimc_hw_reset(fimc);
+
+	/* Resume the capture or mem-to-mem device */
+	if (fimc_capture_busy(fimc))
+		return fimc_capture_resume(fimc);
+
+	return fimc_m2m_resume(fimc);
+}
+
+static int fimc_runtime_suspend(struct device *dev)
+{
+	struct fimc_dev *fimc =	dev_get_drvdata(dev);
+	int ret = 0;
+
+	if (fimc_capture_busy(fimc))
+		ret = fimc_capture_suspend(fimc);
+	else
+		ret = fimc_m2m_suspend(fimc);
+	if (!ret)
+		clk_disable(fimc->clock[CLK_GATE]);
+
+	dbg("fimc%d: state: 0x%lx", fimc->id, fimc->state);
+	return ret;
+}
+#endif
+
+#ifdef CONFIG_PM_SLEEP
+static int fimc_resume(struct device *dev)
+{
+	struct fimc_dev *fimc =	dev_get_drvdata(dev);
+	unsigned long flags;
+
+	dbg("fimc%d: state: 0x%lx", fimc->id, fimc->state);
+
+	/* Do not resume if the device was idle before system suspend */
+	spin_lock_irqsave(&fimc->slock, flags);
+	if (!test_and_clear_bit(ST_LPM, &fimc->state) ||
+	    (!fimc_m2m_active(fimc) && !fimc_capture_busy(fimc))) {
+		spin_unlock_irqrestore(&fimc->slock, flags);
+		return 0;
+	}
+	fimc_hw_reset(fimc);
+	spin_unlock_irqrestore(&fimc->slock, flags);
+
+	if (fimc_capture_busy(fimc))
+		return fimc_capture_resume(fimc);
+
+	return fimc_m2m_resume(fimc);
+}
+
+static int fimc_suspend(struct device *dev)
+{
+	struct fimc_dev *fimc =	dev_get_drvdata(dev);
+
+	dbg("fimc%d: state: 0x%lx", fimc->id, fimc->state);
+
+	if (test_and_set_bit(ST_LPM, &fimc->state))
+		return 0;
+	if (fimc_capture_busy(fimc))
+		return fimc_capture_suspend(fimc);
+
+	return fimc_m2m_suspend(fimc);
+}
+#endif /* CONFIG_PM_SLEEP */
+
+static int fimc_remove(struct platform_device *pdev)
+{
+	struct fimc_dev *fimc = platform_get_drvdata(pdev);
+
+	pm_runtime_disable(&pdev->dev);
+	if (!pm_runtime_status_suspended(&pdev->dev))
+		clk_disable(fimc->clock[CLK_GATE]);
+	pm_runtime_set_suspended(&pdev->dev);
+
+	fimc_unregister_capture_subdev(fimc);
+	vb2_dma_contig_cleanup_ctx(fimc->alloc_ctx);
+
+	clk_disable(fimc->clock[CLK_BUS]);
+	fimc_clk_put(fimc);
+
+	dev_info(&pdev->dev, "driver unloaded\n");
+	return 0;
+}
+
+/* Image pixel limits, similar across several FIMC HW revisions. */
+static const struct fimc_pix_limit s5p_pix_limit[4] = {
+	[0] = {
+		.scaler_en_w	= 3264,
+		.scaler_dis_w	= 8192,
+		.out_rot_en_w	= 1920,
+		.out_rot_dis_w	= 4224,
+	},
+	[1] = {
+		.scaler_en_w	= 4224,
+		.scaler_dis_w	= 8192,
+		.out_rot_en_w	= 1920,
+		.out_rot_dis_w	= 4224,
+	},
+	[2] = {
+		.scaler_en_w	= 1920,
+		.scaler_dis_w	= 8192,
+		.out_rot_en_w	= 1280,
+		.out_rot_dis_w	= 1920,
+	},
+};
+
+static const struct fimc_variant fimc0_variant_s5p = {
+	.has_inp_rot	 = 1,
+	.has_out_rot	 = 1,
+	.has_cam_if	 = 1,
+	.min_inp_pixsize = 16,
+	.min_out_pixsize = 16,
+	.hor_offs_align	 = 8,
+	.min_vsize_align = 16,
+	.pix_limit	 = &s5p_pix_limit[0],
+};
+
+static const struct fimc_variant fimc2_variant_s5p = {
+	.has_cam_if	 = 1,
+	.min_inp_pixsize = 16,
+	.min_out_pixsize = 16,
+	.hor_offs_align	 = 8,
+	.min_vsize_align = 16,
+	.pix_limit	 = &s5p_pix_limit[1],
+};
+
+static const struct fimc_variant fimc0_variant_s5pv210 = {
+	.has_inp_rot	 = 1,
+	.has_out_rot	 = 1,
+	.has_cam_if	 = 1,
+	.min_inp_pixsize = 16,
+	.min_out_pixsize = 16,
+	.hor_offs_align	 = 8,
+	.min_vsize_align = 16,
+	.pix_limit	 = &s5p_pix_limit[1],
+};
+
+static const struct fimc_variant fimc1_variant_s5pv210 = {
+	.has_inp_rot	 = 1,
+	.has_out_rot	 = 1,
+	.has_cam_if	 = 1,
+	.has_mainscaler_ext = 1,
+	.min_inp_pixsize = 16,
+	.min_out_pixsize = 16,
+	.hor_offs_align	 = 1,
+	.min_vsize_align = 1,
+	.pix_limit	 = &s5p_pix_limit[2],
+};
+
+static const struct fimc_variant fimc2_variant_s5pv210 = {
+	.has_cam_if	 = 1,
+	.min_inp_pixsize = 16,
+	.min_out_pixsize = 16,
+	.hor_offs_align	 = 8,
+	.min_vsize_align = 16,
+	.pix_limit	 = &s5p_pix_limit[2],
+};
+
+/* S5PC100 */
+static const struct fimc_drvdata fimc_drvdata_s5p = {
+	.variant = {
+		[0] = &fimc0_variant_s5p,
+		[1] = &fimc0_variant_s5p,
+		[2] = &fimc2_variant_s5p,
+	},
+	.num_entities	= 3,
+	.lclk_frequency = 133000000UL,
+	.out_buf_count	= 4,
+};
+
+/* S5PV210, S5PC110 */
+static const struct fimc_drvdata fimc_drvdata_s5pv210 = {
+	.variant = {
+		[0] = &fimc0_variant_s5pv210,
+		[1] = &fimc1_variant_s5pv210,
+		[2] = &fimc2_variant_s5pv210,
+	},
+	.num_entities	= 3,
+	.lclk_frequency	= 166000000UL,
+	.out_buf_count	= 4,
+	.dma_pix_hoff	= 1,
+};
+
+/* EXYNOS4210, S5PV310, S5PC210 */
+static const struct fimc_drvdata fimc_drvdata_exynos4210 = {
+	.num_entities	= 4,
+	.lclk_frequency = 166000000UL,
+	.dma_pix_hoff	= 1,
+	.cistatus2	= 1,
+	.alpha_color	= 1,
+	.out_buf_count	= 32,
+};
+
+/* EXYNOS4212, EXYNOS4412 */
+static const struct fimc_drvdata fimc_drvdata_exynos4x12 = {
+	.num_entities	= 4,
+	.lclk_frequency	= 166000000UL,
+	.dma_pix_hoff	= 1,
+	.cistatus2	= 1,
+	.alpha_color	= 1,
+	.out_buf_count	= 32,
+};
+
+static const struct platform_device_id fimc_driver_ids[] = {
+	{
+		.name		= "s5p-fimc",
+		.driver_data	= (unsigned long)&fimc_drvdata_s5p,
+	}, {
+		.name		= "s5pv210-fimc",
+		.driver_data	= (unsigned long)&fimc_drvdata_s5pv210,
+	}, {
+		.name		= "exynos4-fimc",
+		.driver_data	= (unsigned long)&fimc_drvdata_exynos4210,
+	}, {
+		.name		= "exynos4x12-fimc",
+		.driver_data	= (unsigned long)&fimc_drvdata_exynos4x12,
+	},
+	{ },
+};
+
+static const struct of_device_id fimc_of_match[] = {
+	{
+		.compatible = "samsung,s5pv210-fimc",
+		.data = &fimc_drvdata_s5pv210,
+	}, {
+		.compatible = "samsung,exynos4210-fimc",
+		.data = &fimc_drvdata_exynos4210,
+	}, {
+		.compatible = "samsung,exynos4212-fimc",
+		.data = &fimc_drvdata_exynos4x12,
+	},
+	{ /* sentinel */ },
+};
+
+static const struct dev_pm_ops fimc_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(fimc_suspend, fimc_resume)
+	SET_RUNTIME_PM_OPS(fimc_runtime_suspend, fimc_runtime_resume, NULL)
+};
+
+static struct platform_driver fimc_driver = {
+	.probe		= fimc_probe,
+	.remove		= fimc_remove,
+	.id_table	= fimc_driver_ids,
+	.driver = {
+		.of_match_table = fimc_of_match,
+		.name		= FIMC_DRIVER_NAME,
+		.pm     	= &fimc_pm_ops,
+	}
+};
+
+int __init fimc_register_driver(void)
+{
+	return platform_driver_register(&fimc_driver);
+}
+
+void __exit fimc_unregister_driver(void)
+{
+	platform_driver_unregister(&fimc_driver);
+}
diff --git a/drivers/media/platform/exynos4-is/fimc-core.h b/drivers/media/platform/exynos4-is/fimc-core.h
new file mode 100644
index 0000000..d336fa2
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-core.h
@@ -0,0 +1,728 @@
+/*
+ * Copyright (C) 2010 - 2012 Samsung Electronics Co., Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef FIMC_CORE_H_
+#define FIMC_CORE_H_
+
+/*#define DEBUG*/
+
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/sched.h>
+#include <linux/spinlock.h>
+#include <linux/mfd/syscon.h>
+#include <linux/types.h>
+#include <linux/videodev2.h>
+#include <linux/io.h>
+#include <linux/sizes.h>
+
+#include <media/media-entity.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/v4l2-mediabus.h>
+#include <media/exynos-fimc.h>
+
+#define dbg(fmt, args...) \
+	pr_debug("%s:%d: " fmt "\n", __func__, __LINE__, ##args)
+
+/* Time to wait for next frame VSYNC interrupt while stopping operation. */
+#define FIMC_SHUTDOWN_TIMEOUT	((100*HZ)/1000)
+#define MAX_FIMC_CLOCKS		2
+#define FIMC_DRIVER_NAME	"exynos4-fimc"
+#define FIMC_MAX_DEVS		4
+#define FIMC_MAX_OUT_BUFS	4
+#define SCALER_MAX_HRATIO	64
+#define SCALER_MAX_VRATIO	64
+#define DMA_MIN_SIZE		8
+#define FIMC_CAMIF_MAX_HEIGHT	0x2000
+#define FIMC_MAX_JPEG_BUF_SIZE	(10 * SZ_1M)
+#define FIMC_MAX_PLANES		3
+#define FIMC_PIX_LIMITS_MAX	4
+#define FIMC_DEF_MIN_SIZE	16
+#define FIMC_DEF_HEIGHT_ALIGN	2
+#define FIMC_DEF_HOR_OFFS_ALIGN	1
+#define FIMC_DEFAULT_WIDTH	640
+#define FIMC_DEFAULT_HEIGHT	480
+
+/* indices to the clocks array */
+enum {
+	CLK_BUS,
+	CLK_GATE,
+};
+
+enum fimc_dev_flags {
+	ST_LPM,
+	/* m2m node */
+	ST_M2M_RUN,
+	ST_M2M_PEND,
+	ST_M2M_SUSPENDING,
+	ST_M2M_SUSPENDED,
+	/* capture node */
+	ST_CAPT_PEND,
+	ST_CAPT_RUN,
+	ST_CAPT_STREAM,
+	ST_CAPT_ISP_STREAM,
+	ST_CAPT_SUSPENDED,
+	ST_CAPT_SHUT,
+	ST_CAPT_BUSY,
+	ST_CAPT_APPLY_CFG,
+	ST_CAPT_JPEG,
+};
+
+#define fimc_m2m_active(dev) test_bit(ST_M2M_RUN, &(dev)->state)
+#define fimc_m2m_pending(dev) test_bit(ST_M2M_PEND, &(dev)->state)
+
+#define fimc_capture_running(dev) test_bit(ST_CAPT_RUN, &(dev)->state)
+#define fimc_capture_pending(dev) test_bit(ST_CAPT_PEND, &(dev)->state)
+#define fimc_capture_busy(dev) test_bit(ST_CAPT_BUSY, &(dev)->state)
+
+enum fimc_datapath {
+	FIMC_IO_NONE,
+	FIMC_IO_CAMERA,
+	FIMC_IO_DMA,
+	FIMC_IO_LCDFIFO,
+	FIMC_IO_WRITEBACK,
+	FIMC_IO_ISP,
+};
+
+enum fimc_color_fmt {
+	FIMC_FMT_RGB444	= 0x10,
+	FIMC_FMT_RGB555,
+	FIMC_FMT_RGB565,
+	FIMC_FMT_RGB666,
+	FIMC_FMT_RGB888,
+	FIMC_FMT_RGB30_LOCAL,
+	FIMC_FMT_YCBCR420 = 0x20,
+	FIMC_FMT_YCBYCR422,
+	FIMC_FMT_YCRYCB422,
+	FIMC_FMT_CBYCRY422,
+	FIMC_FMT_CRYCBY422,
+	FIMC_FMT_YCBCR444_LOCAL,
+	FIMC_FMT_RAW8 = 0x40,
+	FIMC_FMT_RAW10,
+	FIMC_FMT_RAW12,
+	FIMC_FMT_JPEG = 0x80,
+	FIMC_FMT_YUYV_JPEG = 0x100,
+};
+
+#define fimc_fmt_is_user_defined(x) (!!((x) & 0x180))
+#define fimc_fmt_is_rgb(x) (!!((x) & 0x10))
+
+#define IS_M2M(__strt) ((__strt) == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE || \
+			__strt == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+
+/* The hardware context state. */
+#define	FIMC_PARAMS		(1 << 0)
+#define	FIMC_COMPOSE		(1 << 1)
+#define	FIMC_CTX_M2M		(1 << 16)
+#define	FIMC_CTX_CAP		(1 << 17)
+#define	FIMC_CTX_SHUT		(1 << 18)
+
+/* Image conversion flags */
+#define	FIMC_IN_DMA_ACCESS_TILED	(1 << 0)
+#define	FIMC_IN_DMA_ACCESS_LINEAR	(0 << 0)
+#define	FIMC_OUT_DMA_ACCESS_TILED	(1 << 1)
+#define	FIMC_OUT_DMA_ACCESS_LINEAR	(0 << 1)
+#define	FIMC_SCAN_MODE_PROGRESSIVE	(0 << 2)
+#define	FIMC_SCAN_MODE_INTERLACED	(1 << 2)
+/*
+ * YCbCr data dynamic range for RGB-YUV color conversion.
+ * Y/Cb/Cr: (0 ~ 255) */
+#define	FIMC_COLOR_RANGE_WIDE		(0 << 3)
+/* Y (16 ~ 235), Cb/Cr (16 ~ 240) */
+#define	FIMC_COLOR_RANGE_NARROW		(1 << 3)
+
+/**
+ * struct fimc_dma_offset - pixel offset information for DMA
+ * @y_h:	y value horizontal offset
+ * @y_v:	y value vertical offset
+ * @cb_h:	cb value horizontal offset
+ * @cb_v:	cb value vertical offset
+ * @cr_h:	cr value horizontal offset
+ * @cr_v:	cr value vertical offset
+ */
+struct fimc_dma_offset {
+	int	y_h;
+	int	y_v;
+	int	cb_h;
+	int	cb_v;
+	int	cr_h;
+	int	cr_v;
+};
+
+/**
+ * struct fimc_effect - color effect information
+ * @type:	effect type
+ * @pat_cb:	cr value when type is "arbitrary"
+ * @pat_cr:	cr value when type is "arbitrary"
+ */
+struct fimc_effect {
+	u32	type;
+	u8	pat_cb;
+	u8	pat_cr;
+};
+
+/**
+ * struct fimc_scaler - the configuration data for FIMC inetrnal scaler
+ * @scaleup_h:		flag indicating scaling up horizontally
+ * @scaleup_v:		flag indicating scaling up vertically
+ * @copy_mode:		flag indicating transparent DMA transfer (no scaling
+ *			and color format conversion)
+ * @enabled:		flag indicating if the scaler is used
+ * @hfactor:		horizontal shift factor
+ * @vfactor:		vertical shift factor
+ * @pre_hratio:		horizontal ratio of the prescaler
+ * @pre_vratio:		vertical ratio of the prescaler
+ * @pre_dst_width:	the prescaler's destination width
+ * @pre_dst_height:	the prescaler's destination height
+ * @main_hratio:	the main scaler's horizontal ratio
+ * @main_vratio:	the main scaler's vertical ratio
+ * @real_width:		source pixel (width - offset)
+ * @real_height:	source pixel (height - offset)
+ */
+struct fimc_scaler {
+	unsigned int scaleup_h:1;
+	unsigned int scaleup_v:1;
+	unsigned int copy_mode:1;
+	unsigned int enabled:1;
+	u32	hfactor;
+	u32	vfactor;
+	u32	pre_hratio;
+	u32	pre_vratio;
+	u32	pre_dst_width;
+	u32	pre_dst_height;
+	u32	main_hratio;
+	u32	main_vratio;
+	u32	real_width;
+	u32	real_height;
+};
+
+/**
+ * struct fimc_addr - the FIMC physical address set for DMA
+ * @y:	 luminance plane physical address
+ * @cb:	 Cb plane physical address
+ * @cr:	 Cr plane physical address
+ */
+struct fimc_addr {
+	u32	y;
+	u32	cb;
+	u32	cr;
+};
+
+/**
+ * struct fimc_vid_buffer - the driver's video buffer
+ * @vb:    v4l videobuf buffer
+ * @list:  linked list structure for buffer queue
+ * @paddr: precalculated physical address set
+ * @index: buffer index for the output DMA engine
+ */
+struct fimc_vid_buffer {
+	struct vb2_v4l2_buffer vb;
+	struct list_head	list;
+	struct fimc_addr	paddr;
+	int			index;
+};
+
+/**
+ * struct fimc_frame - source/target frame properties
+ * @f_width:	image full width (virtual screen size)
+ * @f_height:	image full height (virtual screen size)
+ * @o_width:	original image width as set by S_FMT
+ * @o_height:	original image height as set by S_FMT
+ * @offs_h:	image horizontal pixel offset
+ * @offs_v:	image vertical pixel offset
+ * @width:	image pixel width
+ * @height:	image pixel weight
+ * @payload:	image size in bytes (w x h x bpp)
+ * @bytesperline: bytesperline value for each plane
+ * @paddr:	image frame buffer physical addresses
+ * @dma_offset:	DMA offset in bytes
+ * @fmt:	fimc color format pointer
+ */
+struct fimc_frame {
+	u32	f_width;
+	u32	f_height;
+	u32	o_width;
+	u32	o_height;
+	u32	offs_h;
+	u32	offs_v;
+	u32	width;
+	u32	height;
+	unsigned int		payload[VIDEO_MAX_PLANES];
+	unsigned int		bytesperline[VIDEO_MAX_PLANES];
+	struct fimc_addr	paddr;
+	struct fimc_dma_offset	dma_offset;
+	struct fimc_fmt		*fmt;
+	u8			alpha;
+};
+
+/**
+ * struct fimc_m2m_device - v4l2 memory-to-memory device data
+ * @vfd: the video device node for v4l2 m2m mode
+ * @m2m_dev: v4l2 memory-to-memory device data
+ * @ctx: hardware context data
+ * @refcnt: the reference counter
+ */
+struct fimc_m2m_device {
+	struct video_device	vfd;
+	struct v4l2_m2m_dev	*m2m_dev;
+	struct fimc_ctx		*ctx;
+	int			refcnt;
+};
+
+#define FIMC_SD_PAD_SINK_CAM	0
+#define FIMC_SD_PAD_SINK_FIFO	1
+#define FIMC_SD_PAD_SOURCE	2
+#define FIMC_SD_PADS_NUM	3
+
+/**
+ * struct fimc_vid_cap - camera capture device information
+ * @ctx: hardware context data
+ * @subdev: subdev exposing the FIMC processing block
+ * @ve: exynos video device entity structure
+ * @vd_pad: fimc video capture node pad
+ * @sd_pads: fimc video processing block pads
+ * @ci_fmt: image format at the FIMC camera input (and the scaler output)
+ * @wb_fmt: image format at the FIMC ISP Writeback input
+ * @source_config: external image source related configuration structure
+ * @pending_buf_q: the pending buffer queue head
+ * @active_buf_q: the queue head of buffers scheduled in hardware
+ * @vbq: the capture am video buffer queue
+ * @active_buf_cnt: number of video buffers scheduled in hardware
+ * @buf_index: index for managing the output DMA buffers
+ * @frame_count: the frame counter for statistics
+ * @reqbufs_count: the number of buffers requested in REQBUFS ioctl
+ * @input_index: input (camera sensor) index
+ * @input: capture input type, grp_id of the attached subdev
+ * @user_subdev_api: true if subdevs are not configured by the host driver
+ * @inh_sensor_ctrls: a flag indicating v4l2 controls are inherited from
+ * 		      an image sensor subdev
+ */
+struct fimc_vid_cap {
+	struct fimc_ctx			*ctx;
+	struct vb2_alloc_ctx		*alloc_ctx;
+	struct v4l2_subdev		subdev;
+	struct exynos_video_entity	ve;
+	struct media_pad		vd_pad;
+	struct media_pad		sd_pads[FIMC_SD_PADS_NUM];
+	struct v4l2_mbus_framefmt	ci_fmt;
+	struct v4l2_mbus_framefmt	wb_fmt;
+	struct fimc_source_info		source_config;
+	struct list_head		pending_buf_q;
+	struct list_head		active_buf_q;
+	struct vb2_queue		vbq;
+	int				active_buf_cnt;
+	int				buf_index;
+	unsigned int			frame_count;
+	unsigned int			reqbufs_count;
+	bool				streaming;
+	int				input_index;
+	u32				input;
+	bool				user_subdev_api;
+	bool				inh_sensor_ctrls;
+};
+
+/**
+ *  struct fimc_pix_limit - image pixel size limits in various IP configurations
+ *
+ *  @scaler_en_w: max input pixel width when the scaler is enabled
+ *  @scaler_dis_w: max input pixel width when the scaler is disabled
+ *  @in_rot_en_h: max input width with the input rotator is on
+ *  @in_rot_dis_w: max input width with the input rotator is off
+ *  @out_rot_en_w: max output width with the output rotator on
+ *  @out_rot_dis_w: max output width with the output rotator off
+ */
+struct fimc_pix_limit {
+	u16 scaler_en_w;
+	u16 scaler_dis_w;
+	u16 in_rot_en_h;
+	u16 in_rot_dis_w;
+	u16 out_rot_en_w;
+	u16 out_rot_dis_w;
+};
+
+/**
+ * struct fimc_variant - FIMC device variant information
+ * @has_inp_rot: set if has input rotator
+ * @has_out_rot: set if has output rotator
+ * @has_mainscaler_ext: 1 if extended mainscaler ratios in CIEXTEN register
+ *			 are present in this IP revision
+ * @has_cam_if: set if this instance has a camera input interface
+ * @has_isp_wb: set if this instance has ISP writeback input
+ * @pix_limit: pixel size constraints for the scaler
+ * @min_inp_pixsize: minimum input pixel size
+ * @min_out_pixsize: minimum output pixel size
+ * @hor_offs_align: horizontal pixel offset aligment
+ * @min_vsize_align: minimum vertical pixel size alignment
+ */
+struct fimc_variant {
+	unsigned int	has_inp_rot:1;
+	unsigned int	has_out_rot:1;
+	unsigned int	has_mainscaler_ext:1;
+	unsigned int	has_cam_if:1;
+	unsigned int	has_isp_wb:1;
+	const struct fimc_pix_limit *pix_limit;
+	u16		min_inp_pixsize;
+	u16		min_out_pixsize;
+	u16		hor_offs_align;
+	u16		min_vsize_align;
+};
+
+/**
+ * struct fimc_drvdata - per device type driver data
+ * @variant: variant information for this device
+ * @num_entities: number of fimc instances available in a SoC
+ * @lclk_frequency: local bus clock frequency
+ * @cistatus2: 1 if the FIMC IPs have CISTATUS2 register
+ * @dma_pix_hoff: the horizontal DMA offset unit: 1 - pixels, 0 - bytes
+ * @alpha_color: 1 if alpha color component is supported
+ * @out_buf_count: maximum number of output DMA buffers supported
+ */
+struct fimc_drvdata {
+	const struct fimc_variant *variant[FIMC_MAX_DEVS];
+	int num_entities;
+	unsigned long lclk_frequency;
+	/* Fields common to all FIMC IP instances */
+	u8 cistatus2;
+	u8 dma_pix_hoff;
+	u8 alpha_color;
+	u8 out_buf_count;
+};
+
+#define fimc_get_drvdata(_pdev) \
+	((struct fimc_drvdata *) platform_get_device_id(_pdev)->driver_data)
+
+struct fimc_ctx;
+
+/**
+ * struct fimc_dev - abstraction for FIMC entity
+ * @slock:	the spinlock protecting this data structure
+ * @lock:	the mutex protecting this data structure
+ * @pdev:	pointer to the FIMC platform device
+ * @pdata:	pointer to the device platform data
+ * @sysreg:	pointer to the SYSREG regmap
+ * @variant:	the IP variant information
+ * @id:		FIMC device index (0..FIMC_MAX_DEVS)
+ * @clock:	clocks required for FIMC operation
+ * @regs:	the mapped hardware registers
+ * @irq_queue:	interrupt handler waitqueue
+ * @v4l2_dev:	root v4l2_device
+ * @m2m:	memory-to-memory V4L2 device information
+ * @vid_cap:	camera capture device information
+ * @state:	flags used to synchronize m2m and capture mode operation
+ * @alloc_ctx:	videobuf2 memory allocator context
+ * @pipeline:	fimc video capture pipeline data structure
+ */
+struct fimc_dev {
+	spinlock_t			slock;
+	struct mutex			lock;
+	struct platform_device		*pdev;
+	struct s5p_platform_fimc	*pdata;
+	struct regmap			*sysreg;
+	const struct fimc_variant	*variant;
+	const struct fimc_drvdata	*drv_data;
+	int				id;
+	struct clk			*clock[MAX_FIMC_CLOCKS];
+	void __iomem			*regs;
+	wait_queue_head_t		irq_queue;
+	struct v4l2_device		*v4l2_dev;
+	struct fimc_m2m_device		m2m;
+	struct fimc_vid_cap		vid_cap;
+	unsigned long			state;
+	struct vb2_alloc_ctx		*alloc_ctx;
+};
+
+/**
+ * struct fimc_ctrls - v4l2 controls structure
+ * @handler: the control handler
+ * @colorfx: image effect control
+ * @colorfx_cbcr: Cb/Cr coefficients control
+ * @rotate: image rotation control
+ * @hflip: horizontal flip control
+ * @vflip: vertical flip control
+ * @alpha: RGB alpha control
+ * @ready: true if @handler is initialized
+ */
+struct fimc_ctrls {
+	struct v4l2_ctrl_handler handler;
+	struct {
+		struct v4l2_ctrl *colorfx;
+		struct v4l2_ctrl *colorfx_cbcr;
+	};
+	struct v4l2_ctrl *rotate;
+	struct v4l2_ctrl *hflip;
+	struct v4l2_ctrl *vflip;
+	struct v4l2_ctrl *alpha;
+	bool ready;
+};
+
+/**
+ * fimc_ctx - the device context data
+ * @s_frame:		source frame properties
+ * @d_frame:		destination frame properties
+ * @out_order_1p:	output 1-plane YCBCR order
+ * @out_order_2p:	output 2-plane YCBCR order
+ * @in_order_1p		input 1-plane YCBCR order
+ * @in_order_2p:	input 2-plane YCBCR order
+ * @in_path:		input mode (DMA or camera)
+ * @out_path:		output mode (DMA or FIFO)
+ * @scaler:		image scaler properties
+ * @effect:		image effect
+ * @rotation:		image clockwise rotation in degrees
+ * @hflip:		indicates image horizontal flip if set
+ * @vflip:		indicates image vertical flip if set
+ * @flags:		additional flags for image conversion
+ * @state:		flags to keep track of user configuration
+ * @fimc_dev:		the FIMC device this context applies to
+ * @fh:			v4l2 file handle
+ * @ctrls:		v4l2 controls structure
+ */
+struct fimc_ctx {
+	struct fimc_frame	s_frame;
+	struct fimc_frame	d_frame;
+	u32			out_order_1p;
+	u32			out_order_2p;
+	u32			in_order_1p;
+	u32			in_order_2p;
+	enum fimc_datapath	in_path;
+	enum fimc_datapath	out_path;
+	struct fimc_scaler	scaler;
+	struct fimc_effect	effect;
+	int			rotation;
+	unsigned int		hflip:1;
+	unsigned int		vflip:1;
+	u32			flags;
+	u32			state;
+	struct fimc_dev		*fimc_dev;
+	struct v4l2_fh		fh;
+	struct fimc_ctrls	ctrls;
+};
+
+#define fh_to_ctx(__fh) container_of(__fh, struct fimc_ctx, fh)
+
+static inline void set_frame_bounds(struct fimc_frame *f, u32 width, u32 height)
+{
+	f->o_width  = width;
+	f->o_height = height;
+	f->f_width  = width;
+	f->f_height = height;
+}
+
+static inline void set_frame_crop(struct fimc_frame *f,
+				  u32 left, u32 top, u32 width, u32 height)
+{
+	f->offs_h = left;
+	f->offs_v = top;
+	f->width  = width;
+	f->height = height;
+}
+
+static inline u32 fimc_get_format_depth(struct fimc_fmt *ff)
+{
+	u32 i, depth = 0;
+
+	if (ff != NULL)
+		for (i = 0; i < ff->colplanes; i++)
+			depth += ff->depth[i];
+	return depth;
+}
+
+static inline bool fimc_capture_active(struct fimc_dev *fimc)
+{
+	unsigned long flags;
+	bool ret;
+
+	spin_lock_irqsave(&fimc->slock, flags);
+	ret = !!(fimc->state & (1 << ST_CAPT_RUN) ||
+		 fimc->state & (1 << ST_CAPT_PEND));
+	spin_unlock_irqrestore(&fimc->slock, flags);
+	return ret;
+}
+
+static inline void fimc_ctx_state_set(u32 state, struct fimc_ctx *ctx)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&ctx->fimc_dev->slock, flags);
+	ctx->state |= state;
+	spin_unlock_irqrestore(&ctx->fimc_dev->slock, flags);
+}
+
+static inline bool fimc_ctx_state_is_set(u32 mask, struct fimc_ctx *ctx)
+{
+	unsigned long flags;
+	bool ret;
+
+	spin_lock_irqsave(&ctx->fimc_dev->slock, flags);
+	ret = (ctx->state & mask) == mask;
+	spin_unlock_irqrestore(&ctx->fimc_dev->slock, flags);
+	return ret;
+}
+
+static inline int tiled_fmt(struct fimc_fmt *fmt)
+{
+	return fmt->fourcc == V4L2_PIX_FMT_NV12MT;
+}
+
+static inline bool fimc_jpeg_fourcc(u32 pixelformat)
+{
+	return (pixelformat == V4L2_PIX_FMT_JPEG ||
+		pixelformat == V4L2_PIX_FMT_S5C_UYVY_JPG);
+}
+
+static inline bool fimc_user_defined_mbus_fmt(u32 code)
+{
+	return (code == MEDIA_BUS_FMT_JPEG_1X8 ||
+		code == MEDIA_BUS_FMT_S5C_UYVY_JPEG_1X8);
+}
+
+/* Return the alpha component bit mask */
+static inline int fimc_get_alpha_mask(struct fimc_fmt *fmt)
+{
+	switch (fmt->color) {
+	case FIMC_FMT_RGB444:	return 0x0f;
+	case FIMC_FMT_RGB555:	return 0x01;
+	case FIMC_FMT_RGB888:	return 0xff;
+	default:		return 0;
+	};
+}
+
+static inline struct fimc_frame *ctx_get_frame(struct fimc_ctx *ctx,
+					       enum v4l2_buf_type type)
+{
+	struct fimc_frame *frame;
+
+	if (V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE == type) {
+		if (fimc_ctx_state_is_set(FIMC_CTX_M2M, ctx))
+			frame = &ctx->s_frame;
+		else
+			return ERR_PTR(-EINVAL);
+	} else if (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE == type) {
+		frame = &ctx->d_frame;
+	} else {
+		v4l2_err(ctx->fimc_dev->v4l2_dev,
+			"Wrong buffer/video queue type (%d)\n", type);
+		return ERR_PTR(-EINVAL);
+	}
+
+	return frame;
+}
+
+/* -----------------------------------------------------*/
+/* fimc-core.c */
+int fimc_vidioc_enum_fmt_mplane(struct file *file, void *priv,
+				struct v4l2_fmtdesc *f);
+int fimc_ctrls_create(struct fimc_ctx *ctx);
+void fimc_ctrls_delete(struct fimc_ctx *ctx);
+void fimc_ctrls_activate(struct fimc_ctx *ctx, bool active);
+void fimc_alpha_ctrl_update(struct fimc_ctx *ctx);
+void __fimc_get_format(struct fimc_frame *frame, struct v4l2_format *f);
+void fimc_adjust_mplane_format(struct fimc_fmt *fmt, u32 width, u32 height,
+			       struct v4l2_pix_format_mplane *pix);
+struct fimc_fmt *fimc_find_format(const u32 *pixelformat, const u32 *mbus_code,
+				  unsigned int mask, int index);
+struct fimc_fmt *fimc_get_format(unsigned int index);
+
+int fimc_check_scaler_ratio(struct fimc_ctx *ctx, int sw, int sh,
+			    int dw, int dh, int rotation);
+int fimc_set_scaler_info(struct fimc_ctx *ctx);
+int fimc_prepare_config(struct fimc_ctx *ctx, u32 flags);
+int fimc_prepare_addr(struct fimc_ctx *ctx, struct vb2_buffer *vb,
+		      struct fimc_frame *frame, struct fimc_addr *paddr);
+void fimc_prepare_dma_offset(struct fimc_ctx *ctx, struct fimc_frame *f);
+void fimc_set_yuv_order(struct fimc_ctx *ctx);
+void fimc_capture_irq_handler(struct fimc_dev *fimc, int deq_buf);
+
+int fimc_register_m2m_device(struct fimc_dev *fimc,
+			     struct v4l2_device *v4l2_dev);
+void fimc_unregister_m2m_device(struct fimc_dev *fimc);
+int fimc_register_driver(void);
+void fimc_unregister_driver(void);
+
+#ifdef CONFIG_MFD_SYSCON
+static inline struct regmap * fimc_get_sysreg_regmap(struct device_node *node)
+{
+	return syscon_regmap_lookup_by_phandle(node, "samsung,sysreg");
+}
+#else
+#define fimc_get_sysreg_regmap(node) (NULL)
+#endif
+
+/* -----------------------------------------------------*/
+/* fimc-m2m.c */
+void fimc_m2m_job_finish(struct fimc_ctx *ctx, int vb_state);
+
+/* -----------------------------------------------------*/
+/* fimc-capture.c					*/
+int fimc_initialize_capture_subdev(struct fimc_dev *fimc);
+void fimc_unregister_capture_subdev(struct fimc_dev *fimc);
+int fimc_capture_ctrls_create(struct fimc_dev *fimc);
+void fimc_sensor_notify(struct v4l2_subdev *sd, unsigned int notification,
+			void *arg);
+int fimc_capture_suspend(struct fimc_dev *fimc);
+int fimc_capture_resume(struct fimc_dev *fimc);
+
+/*
+ * Buffer list manipulation functions. Must be called with fimc.slock held.
+ */
+
+/**
+ * fimc_active_queue_add - add buffer to the capture active buffers queue
+ * @buf: buffer to add to the active buffers list
+ */
+static inline void fimc_active_queue_add(struct fimc_vid_cap *vid_cap,
+					 struct fimc_vid_buffer *buf)
+{
+	list_add_tail(&buf->list, &vid_cap->active_buf_q);
+	vid_cap->active_buf_cnt++;
+}
+
+/**
+ * fimc_active_queue_pop - pop buffer from the capture active buffers queue
+ *
+ * The caller must assure the active_buf_q list is not empty.
+ */
+static inline struct fimc_vid_buffer *fimc_active_queue_pop(
+				    struct fimc_vid_cap *vid_cap)
+{
+	struct fimc_vid_buffer *buf;
+	buf = list_entry(vid_cap->active_buf_q.next,
+			 struct fimc_vid_buffer, list);
+	list_del(&buf->list);
+	vid_cap->active_buf_cnt--;
+	return buf;
+}
+
+/**
+ * fimc_pending_queue_add - add buffer to the capture pending buffers queue
+ * @buf: buffer to add to the pending buffers list
+ */
+static inline void fimc_pending_queue_add(struct fimc_vid_cap *vid_cap,
+					  struct fimc_vid_buffer *buf)
+{
+	list_add_tail(&buf->list, &vid_cap->pending_buf_q);
+}
+
+/**
+ * fimc_pending_queue_pop - pop buffer from the capture pending buffers queue
+ *
+ * The caller must assure the pending_buf_q list is not empty.
+ */
+static inline struct fimc_vid_buffer *fimc_pending_queue_pop(
+				     struct fimc_vid_cap *vid_cap)
+{
+	struct fimc_vid_buffer *buf;
+	buf = list_entry(vid_cap->pending_buf_q.next,
+			struct fimc_vid_buffer, list);
+	list_del(&buf->list);
+	return buf;
+}
+
+#endif /* FIMC_CORE_H_ */
diff --git a/drivers/media/platform/exynos4-is/fimc-is-command.h b/drivers/media/platform/exynos4-is/fimc-is-command.h
new file mode 100644
index 0000000..0d1f52e
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is-command.h
@@ -0,0 +1,137 @@
+/*
+ * Samsung Exynos4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * FIMC-IS command set definitions
+ *
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ *
+ * Authors: Younghwan Joo <yhwan.joo@samsung.com>
+ *          Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef FIMC_IS_CMD_H_
+#define FIMC_IS_CMD_H_
+
+#define FIMC_IS_COMMAND_VER	110 /* FIMC-IS command set version 1.10 */
+
+/* Enumeration of commands beetween the FIMC-IS and the host processor. */
+
+/* HOST to FIMC-IS */
+#define HIC_PREVIEW_STILL	0x0001
+#define HIC_PREVIEW_VIDEO	0x0002
+#define HIC_CAPTURE_STILL	0x0003
+#define HIC_CAPTURE_VIDEO	0x0004
+#define HIC_STREAM_ON		0x0005
+#define HIC_STREAM_OFF		0x0006
+#define HIC_SET_PARAMETER	0x0007
+#define HIC_GET_PARAMETER	0x0008
+#define HIC_SET_TUNE		0x0009
+#define HIC_GET_STATUS		0x000b
+/* Sensor part */
+#define HIC_OPEN_SENSOR		0x000c
+#define HIC_CLOSE_SENSOR	0x000d
+#define HIC_SIMMIAN_INIT	0x000e
+#define HIC_SIMMIAN_WRITE	0x000f
+#define HIC_SIMMIAN_READ	0x0010
+#define HIC_POWER_DOWN		0x0011
+#define HIC_GET_SET_FILE_ADDR	0x0012
+#define HIC_LOAD_SET_FILE	0x0013
+#define HIC_MSG_CONFIG		0x0014
+#define HIC_MSG_TEST		0x0015
+/* FIMC-IS to HOST */
+#define IHC_GET_SENSOR_NUM	0x1000
+#define IHC_SET_SHOT_MARK	0x1001
+/* parameter1: frame number */
+/* parameter2: confidence level (smile 0~100) */
+/* parameter3: confidence level (blink 0~100) */
+#define IHC_SET_FACE_MARK	0x1002
+/* parameter1: coordinate count */
+/* parameter2: coordinate buffer address */
+#define IHC_FRAME_DONE		0x1003
+/* parameter1: frame start number */
+/* parameter2: frame count */
+#define IHC_AA_DONE		0x1004
+#define IHC_NOT_READY		0x1005
+
+#define IH_REPLY_DONE		0x2000
+#define IH_REPLY_NOT_DONE	0x2001
+
+enum fimc_is_scenario {
+	IS_SC_PREVIEW_STILL,
+	IS_SC_PREVIEW_VIDEO,
+	IS_SC_CAPTURE_STILL,
+	IS_SC_CAPTURE_VIDEO,
+	IS_SC_MAX
+};
+
+enum fimc_is_sub_scenario {
+	IS_SC_SUB_DEFAULT,
+	IS_SC_SUB_PS_VTCALL,
+	IS_SC_SUB_CS_VTCALL,
+	IS_SC_SUB_PV_VTCALL,
+	IS_SC_SUB_CV_VTCALL,
+};
+
+struct is_common_regs {
+	u32 hicmd;
+	u32 hic_sensorid;
+	u32 hic_param[4];
+	u32 reserved1[4];
+
+	u32 ihcmd;
+	u32 ihc_sensorid;
+	u32 ihc_param[4];
+	u32 reserved2[4];
+
+	u32 isp_sensor_id;
+	u32 isp_param[2];
+	u32 reserved3[1];
+
+	u32 scc_sensor_id;
+	u32 scc_param[2];
+	u32 reserved4[1];
+
+	u32 dnr_sensor_id;
+	u32 dnr_param[2];
+	u32 reserved5[1];
+
+	u32 scp_sensor_id;
+	u32 scp_param[2];
+	u32 reserved6[29];
+} __packed;
+
+struct is_mcuctl_reg {
+	u32 mcuctl;
+	u32 bboar;
+
+	u32 intgr0;
+	u32 intcr0;
+	u32 intmr0;
+	u32 intsr0;
+	u32 intmsr0;
+
+	u32 intgr1;
+	u32 intcr1;
+	u32 intmr1;
+	u32 intsr1;
+	u32 intmsr1;
+
+	u32 intcr2;
+	u32 intmr2;
+	u32 intsr2;
+	u32 intmsr2;
+
+	u32 gpoctrl;
+	u32 cpoenctlr;
+	u32 gpictlr;
+
+	u32 reserved[0xd];
+
+	struct is_common_regs common;
+} __packed;
+
+#endif /* FIMC_IS_CMD_H_ */
diff --git a/drivers/media/platform/exynos4-is/fimc-is-errno.c b/drivers/media/platform/exynos4-is/fimc-is-errno.c
new file mode 100644
index 0000000..e050e63
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is-errno.c
@@ -0,0 +1,272 @@
+/*
+ * Samsung Exynos4 SoC series FIMC-IS slave interface driver
+ *
+ * Error log interface functions
+ *
+ * Copyright (C) 2011 - 2013 Samsung Electronics Co., Ltd.
+ *
+ * Authors: Younghwan Joo <yhwan.joo@samsung.com>
+ *          Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include "fimc-is-errno.h"
+
+const char *fimc_is_param_strerr(unsigned int error)
+{
+	switch (error) {
+	case ERROR_COMMON_CMD:
+		return "ERROR_COMMON_CMD: Invalid Command";
+	case ERROR_COMMON_PARAMETER:
+		return "ERROR_COMMON_PARAMETER: Invalid Parameter";
+	case ERROR_COMMON_SETFILE_LOAD:
+		return "ERROR_COMMON_SETFILE_LOAD: Illegal Setfile Loading";
+	case ERROR_COMMON_SETFILE_ADJUST:
+		return "ERROR_COMMON_SETFILE_ADJUST: Setfile isn't adjusted";
+	case ERROR_COMMON_SETFILE_INDEX:
+		return "ERROR_COMMON_SETFILE_INDEX: Invalid setfile index";
+	case ERROR_COMMON_INPUT_PATH:
+		return "ERROR_COMMON_INPUT_PATH: Input path can be changed in ready state";
+	case ERROR_COMMON_INPUT_INIT:
+		return "ERROR_COMMON_INPUT_INIT: IP can not start if input path is not set";
+	case ERROR_COMMON_OUTPUT_PATH:
+		return "ERROR_COMMON_OUTPUT_PATH: Output path can be changed in ready state (stop)";
+	case ERROR_COMMON_OUTPUT_INIT:
+		return "ERROR_COMMON_OUTPUT_INIT: IP can not start if output path is not set";
+	case ERROR_CONTROL_BYPASS:
+		return "ERROR_CONTROL_BYPASS";
+	case ERROR_OTF_INPUT_FORMAT:
+		return "ERROR_OTF_INPUT_FORMAT: Invalid format  (DRC: YUV444, FD: YUV444, 422, 420)";
+	case ERROR_OTF_INPUT_WIDTH:
+		return "ERROR_OTF_INPUT_WIDTH: Invalid width (DRC: 128~8192, FD: 32~8190)";
+	case ERROR_OTF_INPUT_HEIGHT:
+		return "ERROR_OTF_INPUT_HEIGHT: Invalid bit-width (DRC: 8~12bits, FD: 8bit)";
+	case ERROR_OTF_INPUT_BIT_WIDTH:
+		return "ERROR_OTF_INPUT_BIT_WIDTH: Invalid bit-width (DRC: 8~12bits, FD: 8bit)";
+	case ERROR_DMA_INPUT_WIDTH:
+		return "ERROR_DMA_INPUT_WIDTH: Invalid width (DRC: 128~8192, FD: 32~8190)";
+	case ERROR_DMA_INPUT_HEIGHT:
+		return "ERROR_DMA_INPUT_HEIGHT: Invalid height (DRC: 64~8192, FD: 16~8190)";
+	case ERROR_DMA_INPUT_FORMAT:
+		return "ERROR_DMA_INPUT_FORMAT: Invalid format (DRC: YUV444 or YUV422, FD: YUV444,422,420)";
+	case ERROR_DMA_INPUT_BIT_WIDTH:
+		return "ERROR_DMA_INPUT_BIT_WIDTH: Invalid bit-width (DRC: 8~12bits, FD: 8bit)";
+	case ERROR_DMA_INPUT_ORDER:
+		return "ERROR_DMA_INPUT_ORDER: Invalid order(DRC: YYCbCr,YCbYCr,FD:NO,YYCbCr,YCbYCr,CbCr,CrCb)";
+	case ERROR_DMA_INPUT_PLANE:
+		return "ERROR_DMA_INPUT_PLANE: Invalid palne (DRC: 3, FD: 1, 2, 3)";
+	case ERROR_OTF_OUTPUT_WIDTH:
+		return "ERROR_OTF_OUTPUT_WIDTH: Invalid width (DRC: 128~8192)";
+	case ERROR_OTF_OUTPUT_HEIGHT:
+		return "ERROR_OTF_OUTPUT_HEIGHT: Invalid height (DRC: 64~8192)";
+	case ERROR_OTF_OUTPUT_FORMAT:
+		return "ERROR_OTF_OUTPUT_FORMAT: Invalid format (DRC: YUV444)";
+	case ERROR_OTF_OUTPUT_BIT_WIDTH:
+		return "ERROR_OTF_OUTPUT_BIT_WIDTH: Invalid bit-width (DRC: 8~12bits, FD: 8bit)";
+	case ERROR_DMA_OUTPUT_WIDTH:
+		return "ERROR_DMA_OUTPUT_WIDTH";
+	case ERROR_DMA_OUTPUT_HEIGHT:
+		return "ERROR_DMA_OUTPUT_HEIGHT";
+	case ERROR_DMA_OUTPUT_FORMAT:
+		return "ERROR_DMA_OUTPUT_FORMAT";
+	case ERROR_DMA_OUTPUT_BIT_WIDTH:
+		return "ERROR_DMA_OUTPUT_BIT_WIDTH";
+	case ERROR_DMA_OUTPUT_PLANE:
+		return "ERROR_DMA_OUTPUT_PLANE";
+	case ERROR_DMA_OUTPUT_ORDER:
+		return "ERROR_DMA_OUTPUT_ORDER";
+
+	/* Sensor Error(100~199) */
+	case ERROR_SENSOR_I2C_FAIL:
+		return "ERROR_SENSOR_I2C_FAIL";
+	case ERROR_SENSOR_INVALID_FRAMERATE:
+		return "ERROR_SENSOR_INVALID_FRAMERATE";
+	case ERROR_SENSOR_INVALID_EXPOSURETIME:
+		return "ERROR_SENSOR_INVALID_EXPOSURETIME";
+	case ERROR_SENSOR_INVALID_SIZE:
+		return "ERROR_SENSOR_INVALID_SIZE";
+	case ERROR_SENSOR_INVALID_SETTING:
+		return "ERROR_SENSOR_INVALID_SETTING";
+	case ERROR_SENSOR_ACTURATOR_INIT_FAIL:
+		return "ERROR_SENSOR_ACTURATOR_INIT_FAIL";
+	case ERROR_SENSOR_INVALID_AF_POS:
+		return "ERROR_SENSOR_INVALID_AF_POS";
+	case ERROR_SENSOR_UNSUPPORT_FUNC:
+		return "ERROR_SENSOR_UNSUPPORT_FUNC";
+	case ERROR_SENSOR_UNSUPPORT_PERI:
+		return "ERROR_SENSOR_UNSUPPORT_PERI";
+	case ERROR_SENSOR_UNSUPPORT_AF:
+		return "ERROR_SENSOR_UNSUPPORT_AF";
+
+	/* ISP Error (200~299) */
+	case ERROR_ISP_AF_BUSY:
+		return "ERROR_ISP_AF_BUSY";
+	case ERROR_ISP_AF_INVALID_COMMAND:
+		return "ERROR_ISP_AF_INVALID_COMMAND";
+	case ERROR_ISP_AF_INVALID_MODE:
+		return "ERROR_ISP_AF_INVALID_MODE";
+
+	/* DRC Error (300~399) */
+	/* FD Error  (400~499) */
+	case ERROR_FD_CONFIG_MAX_NUMBER_STATE:
+		return "ERROR_FD_CONFIG_MAX_NUMBER_STATE";
+	case ERROR_FD_CONFIG_MAX_NUMBER_INVALID:
+		return "ERROR_FD_CONFIG_MAX_NUMBER_INVALID";
+	case ERROR_FD_CONFIG_YAW_ANGLE_STATE:
+		return "ERROR_FD_CONFIG_YAW_ANGLE_STATE";
+	case ERROR_FD_CONFIG_YAW_ANGLE_INVALID:
+		return "ERROR_FD_CONFIG_YAW_ANGLE_INVALID\n";
+	case ERROR_FD_CONFIG_ROLL_ANGLE_STATE:
+		return "ERROR_FD_CONFIG_ROLL_ANGLE_STATE";
+	case ERROR_FD_CONFIG_ROLL_ANGLE_INVALID:
+		return "ERROR_FD_CONFIG_ROLL_ANGLE_INVALID";
+	case ERROR_FD_CONFIG_SMILE_MODE_INVALID:
+		return "ERROR_FD_CONFIG_SMILE_MODE_INVALID";
+	case ERROR_FD_CONFIG_BLINK_MODE_INVALID:
+		return "ERROR_FD_CONFIG_BLINK_MODE_INVALID";
+	case ERROR_FD_CONFIG_EYES_DETECT_INVALID:
+		return "ERROR_FD_CONFIG_EYES_DETECT_INVALID";
+	case ERROR_FD_CONFIG_MOUTH_DETECT_INVALID:
+		return "ERROR_FD_CONFIG_MOUTH_DETECT_INVALID";
+	case ERROR_FD_CONFIG_ORIENTATION_STATE:
+		return "ERROR_FD_CONFIG_ORIENTATION_STATE";
+	case ERROR_FD_CONFIG_ORIENTATION_INVALID:
+		return "ERROR_FD_CONFIG_ORIENTATION_INVALID";
+	case ERROR_FD_CONFIG_ORIENTATION_VALUE_INVALID:
+		return "ERROR_FD_CONFIG_ORIENTATION_VALUE_INVALID";
+	case ERROR_FD_RESULT:
+		return "ERROR_FD_RESULT";
+	case ERROR_FD_MODE:
+		return "ERROR_FD_MODE";
+	default:
+		return "Unknown";
+	}
+}
+
+const char *fimc_is_strerr(unsigned int error)
+{
+	error &= ~IS_ERROR_TIME_OUT_FLAG;
+
+	switch (error) {
+	/* General */
+	case IS_ERROR_INVALID_COMMAND:
+		return "IS_ERROR_INVALID_COMMAND";
+	case IS_ERROR_REQUEST_FAIL:
+		return "IS_ERROR_REQUEST_FAIL";
+	case IS_ERROR_INVALID_SCENARIO:
+		return "IS_ERROR_INVALID_SCENARIO";
+	case IS_ERROR_INVALID_SENSORID:
+		return "IS_ERROR_INVALID_SENSORID";
+	case IS_ERROR_INVALID_MODE_CHANGE:
+		return "IS_ERROR_INVALID_MODE_CHANGE";
+	case IS_ERROR_INVALID_MAGIC_NUMBER:
+		return "IS_ERROR_INVALID_MAGIC_NUMBER";
+	case IS_ERROR_INVALID_SETFILE_HDR:
+		return "IS_ERROR_INVALID_SETFILE_HDR";
+	case IS_ERROR_BUSY:
+		return "IS_ERROR_BUSY";
+	case IS_ERROR_SET_PARAMETER:
+		return "IS_ERROR_SET_PARAMETER";
+	case IS_ERROR_INVALID_PATH:
+		return "IS_ERROR_INVALID_PATH";
+	case IS_ERROR_OPEN_SENSOR_FAIL:
+		return "IS_ERROR_OPEN_SENSOR_FAIL";
+	case IS_ERROR_ENTRY_MSG_THREAD_DOWN:
+		return "IS_ERROR_ENTRY_MSG_THREAD_DOWN";
+	case IS_ERROR_ISP_FRAME_END_NOT_DONE:
+		return "IS_ERROR_ISP_FRAME_END_NOT_DONE";
+	case IS_ERROR_DRC_FRAME_END_NOT_DONE:
+		return "IS_ERROR_DRC_FRAME_END_NOT_DONE";
+	case IS_ERROR_SCALERC_FRAME_END_NOT_DONE:
+		return "IS_ERROR_SCALERC_FRAME_END_NOT_DONE";
+	case IS_ERROR_ODC_FRAME_END_NOT_DONE:
+		return "IS_ERROR_ODC_FRAME_END_NOT_DONE";
+	case IS_ERROR_DIS_FRAME_END_NOT_DONE:
+		return "IS_ERROR_DIS_FRAME_END_NOT_DONE";
+	case IS_ERROR_TDNR_FRAME_END_NOT_DONE:
+		return "IS_ERROR_TDNR_FRAME_END_NOT_DONE";
+	case IS_ERROR_SCALERP_FRAME_END_NOT_DONE:
+		return "IS_ERROR_SCALERP_FRAME_END_NOT_DONE";
+	case IS_ERROR_WAIT_STREAM_OFF_NOT_DONE:
+		return "IS_ERROR_WAIT_STREAM_OFF_NOT_DONE";
+	case IS_ERROR_NO_MSG_IS_RECEIVED:
+		return "IS_ERROR_NO_MSG_IS_RECEIVED";
+	case IS_ERROR_SENSOR_MSG_FAIL:
+		return "IS_ERROR_SENSOR_MSG_FAIL";
+	case IS_ERROR_ISP_MSG_FAIL:
+		return "IS_ERROR_ISP_MSG_FAIL";
+	case IS_ERROR_DRC_MSG_FAIL:
+		return "IS_ERROR_DRC_MSG_FAIL";
+	case IS_ERROR_LHFD_MSG_FAIL:
+		return "IS_ERROR_LHFD_MSG_FAIL";
+	case IS_ERROR_UNKNOWN:
+		return "IS_ERROR_UNKNOWN";
+
+	/* Sensor */
+	case IS_ERROR_SENSOR_PWRDN_FAIL:
+		return "IS_ERROR_SENSOR_PWRDN_FAIL";
+
+	/* ISP */
+	case IS_ERROR_ISP_PWRDN_FAIL:
+		return "IS_ERROR_ISP_PWRDN_FAIL";
+	case IS_ERROR_ISP_MULTIPLE_INPUT:
+		return "IS_ERROR_ISP_MULTIPLE_INPUT";
+	case IS_ERROR_ISP_ABSENT_INPUT:
+		return "IS_ERROR_ISP_ABSENT_INPUT";
+	case IS_ERROR_ISP_ABSENT_OUTPUT:
+		return "IS_ERROR_ISP_ABSENT_OUTPUT";
+	case IS_ERROR_ISP_NONADJACENT_OUTPUT:
+		return "IS_ERROR_ISP_NONADJACENT_OUTPUT";
+	case IS_ERROR_ISP_FORMAT_MISMATCH:
+		return "IS_ERROR_ISP_FORMAT_MISMATCH";
+	case IS_ERROR_ISP_WIDTH_MISMATCH:
+		return "IS_ERROR_ISP_WIDTH_MISMATCH";
+	case IS_ERROR_ISP_HEIGHT_MISMATCH:
+		return "IS_ERROR_ISP_HEIGHT_MISMATCH";
+	case IS_ERROR_ISP_BITWIDTH_MISMATCH:
+		return "IS_ERROR_ISP_BITWIDTH_MISMATCH";
+	case IS_ERROR_ISP_FRAME_END_TIME_OUT:
+		return "IS_ERROR_ISP_FRAME_END_TIME_OUT";
+
+	/* DRC */
+	case IS_ERROR_DRC_PWRDN_FAIL:
+		return "IS_ERROR_DRC_PWRDN_FAIL";
+	case IS_ERROR_DRC_MULTIPLE_INPUT:
+		return "IS_ERROR_DRC_MULTIPLE_INPUT";
+	case IS_ERROR_DRC_ABSENT_INPUT:
+		return "IS_ERROR_DRC_ABSENT_INPUT";
+	case IS_ERROR_DRC_NONADJACENT_INPUT:
+		return "IS_ERROR_DRC_NONADJACENT_INPUT";
+	case IS_ERROR_DRC_ABSENT_OUTPUT:
+		return "IS_ERROR_DRC_ABSENT_OUTPUT";
+	case IS_ERROR_DRC_NONADJACENT_OUTPUT:
+		return "IS_ERROR_DRC_NONADJACENT_OUTPUT";
+	case IS_ERROR_DRC_FORMAT_MISMATCH:
+		return "IS_ERROR_DRC_FORMAT_MISMATCH";
+	case IS_ERROR_DRC_WIDTH_MISMATCH:
+		return "IS_ERROR_DRC_WIDTH_MISMATCH";
+	case IS_ERROR_DRC_HEIGHT_MISMATCH:
+		return "IS_ERROR_DRC_HEIGHT_MISMATCH";
+	case IS_ERROR_DRC_BITWIDTH_MISMATCH:
+		return "IS_ERROR_DRC_BITWIDTH_MISMATCH";
+	case IS_ERROR_DRC_FRAME_END_TIME_OUT:
+		return "IS_ERROR_DRC_FRAME_END_TIME_OUT";
+
+	/* FD */
+	case IS_ERROR_FD_PWRDN_FAIL:
+		return "IS_ERROR_FD_PWRDN_FAIL";
+	case IS_ERROR_FD_MULTIPLE_INPUT:
+		return "IS_ERROR_FD_MULTIPLE_INPUT";
+	case IS_ERROR_FD_ABSENT_INPUT:
+		return "IS_ERROR_FD_ABSENT_INPUT";
+	case IS_ERROR_FD_NONADJACENT_INPUT:
+		return "IS_ERROR_FD_NONADJACENT_INPUT";
+	case IS_ERROR_LHFD_FRAME_END_TIME_OUT:
+		return "IS_ERROR_LHFD_FRAME_END_TIME_OUT";
+	default:
+		return "Unknown";
+	}
+}
diff --git a/drivers/media/platform/exynos4-is/fimc-is-errno.h b/drivers/media/platform/exynos4-is/fimc-is-errno.h
new file mode 100644
index 0000000..ef981e7
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is-errno.h
@@ -0,0 +1,248 @@
+/*
+ * Samsung Exynos4 SoC series FIMC-IS slave interface driver
+ *
+ * FIMC-IS error code definition
+ *
+ * Copyright (C) 2011 - 2013 Samsung Electronics Co., Ltd.
+ *
+ * Authors: Younghwan Joo <yhwan.joo@samsung.com>
+ *          Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+*/
+
+#ifndef FIMC_IS_ERR_H_
+#define FIMC_IS_ERR_H_
+
+#define IS_ERROR_VER			011 /* IS ERROR VERSION 0.11 */
+
+enum {
+	IS_ERROR_NONE,
+
+	/* General 1 ~ 99 */
+	IS_ERROR_INVALID_COMMAND,
+	IS_ERROR_REQUEST_FAIL,
+	IS_ERROR_INVALID_SCENARIO,
+	IS_ERROR_INVALID_SENSORID,
+	IS_ERROR_INVALID_MODE_CHANGE,
+	IS_ERROR_INVALID_MAGIC_NUMBER,
+	IS_ERROR_INVALID_SETFILE_HDR,
+	IS_ERROR_BUSY,
+	IS_ERROR_SET_PARAMETER,
+	IS_ERROR_INVALID_PATH,
+	IS_ERROR_OPEN_SENSOR_FAIL,
+	IS_ERROR_ENTRY_MSG_THREAD_DOWN,
+	IS_ERROR_ISP_FRAME_END_NOT_DONE,
+	IS_ERROR_DRC_FRAME_END_NOT_DONE,
+	IS_ERROR_SCALERC_FRAME_END_NOT_DONE,
+	IS_ERROR_ODC_FRAME_END_NOT_DONE,
+	IS_ERROR_DIS_FRAME_END_NOT_DONE,
+	IS_ERROR_TDNR_FRAME_END_NOT_DONE,
+	IS_ERROR_SCALERP_FRAME_END_NOT_DONE,
+	IS_ERROR_WAIT_STREAM_OFF_NOT_DONE,
+	IS_ERROR_NO_MSG_IS_RECEIVED,
+	IS_ERROR_SENSOR_MSG_FAIL,
+	IS_ERROR_ISP_MSG_FAIL,
+	IS_ERROR_DRC_MSG_FAIL,
+	IS_ERROR_SCALERC_MSG_FAIL,
+	IS_ERROR_ODC_MSG_FAIL,
+	IS_ERROR_DIS_MSG_FAIL,
+	IS_ERROR_TDNR_MSG_FAIL,
+	IS_ERROR_SCALERP_MSG_FAIL,
+	IS_ERROR_LHFD_MSG_FAIL,
+	IS_ERROR_LHFD_INTERNAL_STOP,
+
+	/* Sensor 100 ~ 199 */
+	IS_ERROR_SENSOR_PWRDN_FAIL	= 100,
+	IS_ERROR_SENSOR_STREAM_ON_FAIL,
+	IS_ERROR_SENSOR_STREAM_OFF_FAIL,
+
+	/* ISP 200 ~ 299 */
+	IS_ERROR_ISP_PWRDN_FAIL		= 200,
+	IS_ERROR_ISP_MULTIPLE_INPUT,
+	IS_ERROR_ISP_ABSENT_INPUT,
+	IS_ERROR_ISP_ABSENT_OUTPUT,
+	IS_ERROR_ISP_NONADJACENT_OUTPUT,
+	IS_ERROR_ISP_FORMAT_MISMATCH,
+	IS_ERROR_ISP_WIDTH_MISMATCH,
+	IS_ERROR_ISP_HEIGHT_MISMATCH,
+	IS_ERROR_ISP_BITWIDTH_MISMATCH,
+	IS_ERROR_ISP_FRAME_END_TIME_OUT,
+
+	/* DRC 300 ~ 399 */
+	IS_ERROR_DRC_PWRDN_FAIL		= 300,
+	IS_ERROR_DRC_MULTIPLE_INPUT,
+	IS_ERROR_DRC_ABSENT_INPUT,
+	IS_ERROR_DRC_NONADJACENT_INPUT,
+	IS_ERROR_DRC_ABSENT_OUTPUT,
+	IS_ERROR_DRC_NONADJACENT_OUTPUT,
+	IS_ERROR_DRC_FORMAT_MISMATCH,
+	IS_ERROR_DRC_WIDTH_MISMATCH,
+	IS_ERROR_DRC_HEIGHT_MISMATCH,
+	IS_ERROR_DRC_BITWIDTH_MISMATCH,
+	IS_ERROR_DRC_FRAME_END_TIME_OUT,
+
+	/* SCALERC 400 ~ 499 */
+	IS_ERROR_SCALERC_PWRDN_FAIL	= 400,
+
+	/* ODC 500 ~ 599 */
+	IS_ERROR_ODC_PWRDN_FAIL		= 500,
+
+	/* DIS 600 ~ 699 */
+	IS_ERROR_DIS_PWRDN_FAIL		= 600,
+
+	/* TDNR 700 ~ 799 */
+	IS_ERROR_TDNR_PWRDN_FAIL	= 700,
+
+	/* SCALERC 800 ~ 899 */
+	IS_ERROR_SCALERP_PWRDN_FAIL	= 800,
+
+	/* FD 900 ~ 999 */
+	IS_ERROR_FD_PWRDN_FAIL		= 900,
+	IS_ERROR_FD_MULTIPLE_INPUT,
+	IS_ERROR_FD_ABSENT_INPUT,
+	IS_ERROR_FD_NONADJACENT_INPUT,
+	IS_ERROR_LHFD_FRAME_END_TIME_OUT,
+
+	IS_ERROR_UNKNOWN		= 1000,
+};
+
+#define IS_ERROR_TIME_OUT_FLAG	0x80000000
+
+/* Set parameter error enum */
+enum fimc_is_error {
+	/* Common error (0~99) */
+	ERROR_COMMON_NONE		= 0,
+	ERROR_COMMON_CMD		= 1,	/* Invalid command */
+	ERROR_COMMON_PARAMETER		= 2,	/* Invalid parameter */
+	/* setfile is not loaded before adjusting */
+	ERROR_COMMON_SETFILE_LOAD	= 3,
+	/* setfile is not Adjusted before runnng. */
+	ERROR_COMMON_SETFILE_ADJUST	= 4,
+	/* Index of setfile is not valid (0~MAX_SETFILE_NUM-1) */
+	ERROR_COMMON_SETFILE_INDEX	= 5,
+	/* Input path can be changed in ready state(stop) */
+	ERROR_COMMON_INPUT_PATH		= 6,
+	/* IP can not start if input path is not set */
+	ERROR_COMMON_INPUT_INIT		= 7,
+	/* Output path can be changed in ready state (stop) */
+	ERROR_COMMON_OUTPUT_PATH	= 8,
+	/* IP can not start if output path is not set */
+	ERROR_COMMON_OUTPUT_INIT	= 9,
+
+	ERROR_CONTROL_NONE		= ERROR_COMMON_NONE,
+	ERROR_CONTROL_BYPASS		= 11,	/* Enable or Disable */
+
+	ERROR_OTF_INPUT_NONE		= ERROR_COMMON_NONE,
+	ERROR_OTF_INPUT_CMD		= 21,
+	/* invalid format  (DRC: YUV444, FD: YUV444, 422, 420) */
+	ERROR_OTF_INPUT_FORMAT		= 22,
+	/* invalid width (DRC: 128~8192, FD: 32~8190) */
+	ERROR_OTF_INPUT_WIDTH		= 23,
+	/* invalid height (DRC: 64~8192, FD: 16~8190) */
+	ERROR_OTF_INPUT_HEIGHT		= 24,
+	/* invalid bit-width (DRC: 8~12bits, FD: 8bit) */
+	ERROR_OTF_INPUT_BIT_WIDTH	= 25,
+	/* invalid FrameTime for ISP */
+	ERROR_OTF_INPUT_USER_FRAMETIIME	= 26,
+
+	ERROR_DMA_INPUT_NONE		= ERROR_COMMON_NONE,
+	/* invalid width (DRC: 128~8192, FD: 32~8190) */
+	ERROR_DMA_INPUT_WIDTH		= 31,
+	/* invalid height (DRC: 64~8192, FD: 16~8190) */
+	ERROR_DMA_INPUT_HEIGHT		= 32,
+	/* invalid format (DRC: YUV444 or YUV422, FD: YUV444, 422, 420) */
+	ERROR_DMA_INPUT_FORMAT		= 33,
+	/* invalid bit-width (DRC: 8~12bit, FD: 8bit) */
+	ERROR_DMA_INPUT_BIT_WIDTH	= 34,
+	/* invalid order(DRC: YYCbCrorYCbYCr, FD:NO,YYCbCr,YCbYCr,CbCr,CrCb) */
+	ERROR_DMA_INPUT_ORDER		= 35,
+	/* invalid palne (DRC: 3, FD: 1, 2, 3) */
+	ERROR_DMA_INPUT_PLANE		= 36,
+
+	ERROR_OTF_OUTPUT_NONE		= ERROR_COMMON_NONE,
+	/* invalid width (DRC: 128~8192) */
+	ERROR_OTF_OUTPUT_WIDTH		= 41,
+	/* invalid height (DRC: 64~8192) */
+	ERROR_OTF_OUTPUT_HEIGHT		= 42,
+	/* invalid format (DRC: YUV444) */
+	ERROR_OTF_OUTPUT_FORMAT		= 43,
+	/* invalid bit-width (DRC: 8~12bits) */
+	ERROR_OTF_OUTPUT_BIT_WIDTH	= 44,
+
+	ERROR_DMA_OUTPUT_NONE		= ERROR_COMMON_NONE,
+	ERROR_DMA_OUTPUT_WIDTH		= 51,	/* invalid width */
+	ERROR_DMA_OUTPUT_HEIGHT		= 52,	/* invalid height */
+	ERROR_DMA_OUTPUT_FORMAT		= 53,	/* invalid format */
+	ERROR_DMA_OUTPUT_BIT_WIDTH	= 54,	/* invalid bit-width */
+	ERROR_DMA_OUTPUT_PLANE		= 55,	/* invalid plane */
+	ERROR_DMA_OUTPUT_ORDER		= 56,	/* invalid order */
+
+	ERROR_GLOBAL_SHOTMODE_NONE	= ERROR_COMMON_NONE,
+
+	/* SENSOR Error(100~199) */
+	ERROR_SENSOR_NONE		= ERROR_COMMON_NONE,
+	ERROR_SENSOR_I2C_FAIL		= 101,
+	ERROR_SENSOR_INVALID_FRAMERATE,
+	ERROR_SENSOR_INVALID_EXPOSURETIME,
+	ERROR_SENSOR_INVALID_SIZE,
+	ERROR_SENSOR_INVALID_SETTING,
+	ERROR_SENSOR_ACTURATOR_INIT_FAIL,
+	ERROR_SENSOR_INVALID_AF_POS,
+	ERROR_SENSOR_UNSUPPORT_FUNC,
+	ERROR_SENSOR_UNSUPPORT_PERI,
+	ERROR_SENSOR_UNSUPPORT_AF,
+
+	/* ISP Error (200~299) */
+	ERROR_ISP_AF_NONE		= ERROR_COMMON_NONE,
+	ERROR_ISP_AF_BUSY		= 201,
+	ERROR_ISP_AF_INVALID_COMMAND	= 202,
+	ERROR_ISP_AF_INVALID_MODE	= 203,
+	ERROR_ISP_FLASH_NONE		= ERROR_COMMON_NONE,
+	ERROR_ISP_AWB_NONE		= ERROR_COMMON_NONE,
+	ERROR_ISP_IMAGE_EFFECT_NONE	= ERROR_COMMON_NONE,
+	ERROR_ISP_ISO_NONE		= ERROR_COMMON_NONE,
+	ERROR_ISP_ADJUST_NONE		= ERROR_COMMON_NONE,
+	ERROR_ISP_METERING_NONE		= ERROR_COMMON_NONE,
+	ERROR_ISP_AFC_NONE		= ERROR_COMMON_NONE,
+
+	/* DRC Error (300~399) */
+
+	/* FD Error  (400~499) */
+	ERROR_FD_NONE					= ERROR_COMMON_NONE,
+	/* Invalid max number (1~16) */
+	ERROR_FD_CONFIG_MAX_NUMBER_STATE		= 401,
+	ERROR_FD_CONFIG_MAX_NUMBER_INVALID		= 402,
+	ERROR_FD_CONFIG_YAW_ANGLE_STATE			= 403,
+	ERROR_FD_CONFIG_YAW_ANGLE_INVALID		= 404,
+	ERROR_FD_CONFIG_ROLL_ANGLE_STATE		= 405,
+	ERROR_FD_CONFIG_ROLL_ANGLE_INVALID		= 406,
+	ERROR_FD_CONFIG_SMILE_MODE_INVALID		= 407,
+	ERROR_FD_CONFIG_BLINK_MODE_INVALID		= 408,
+	ERROR_FD_CONFIG_EYES_DETECT_INVALID		= 409,
+	ERROR_FD_CONFIG_MOUTH_DETECT_INVALID		= 410,
+	ERROR_FD_CONFIG_ORIENTATION_STATE		= 411,
+	ERROR_FD_CONFIG_ORIENTATION_INVALID		= 412,
+	ERROR_FD_CONFIG_ORIENTATION_VALUE_INVALID	= 413,
+	/* PARAM_FdResultStr can be only applied in ready-state or stream off */
+	ERROR_FD_RESULT					= 414,
+	/* PARAM_FdModeStr can be only applied in ready-state or stream off */
+	ERROR_FD_MODE					= 415,
+	/* Scaler Error  (500 ~ 599) */
+	ERROR_SCALER_NO_NONE				= ERROR_COMMON_NONE,
+	ERROR_SCALER_DMA_OUTSEL				= 501,
+	ERROR_SCALER_H_RATIO				= 502,
+	ERROR_SCALER_V_RATIO				= 503,
+
+	ERROR_SCALER_IMAGE_EFFECT			= 510,
+
+	ERROR_SCALER_ROTATE				= 520,
+	ERROR_SCALER_FLIP				= 521,
+};
+
+const char *fimc_is_strerr(unsigned int error);
+const char *fimc_is_param_strerr(unsigned int error);
+
+#endif /* FIMC_IS_ERR_H_ */
diff --git a/drivers/media/platform/exynos4-is/fimc-is-i2c.c b/drivers/media/platform/exynos4-is/fimc-is-i2c.c
new file mode 100644
index 0000000..7521aa5
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is-i2c.c
@@ -0,0 +1,148 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ *
+ * Author: Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/clk.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include "fimc-is-i2c.h"
+
+struct fimc_is_i2c {
+	struct i2c_adapter adapter;
+	struct clk *clock;
+};
+
+/*
+ * An empty algorithm is used as the actual I2C bus controller driver
+ * is implemented in the FIMC-IS subsystem firmware and the host CPU
+ * doesn't access the I2C bus controller.
+ */
+static const struct i2c_algorithm fimc_is_i2c_algorithm;
+
+static int fimc_is_i2c_probe(struct platform_device *pdev)
+{
+	struct device_node *node = pdev->dev.of_node;
+	struct fimc_is_i2c *isp_i2c;
+	struct i2c_adapter *i2c_adap;
+	int ret;
+
+	isp_i2c = devm_kzalloc(&pdev->dev, sizeof(*isp_i2c), GFP_KERNEL);
+	if (!isp_i2c)
+		return -ENOMEM;
+
+	isp_i2c->clock = devm_clk_get(&pdev->dev, "i2c_isp");
+	if (IS_ERR(isp_i2c->clock)) {
+		dev_err(&pdev->dev, "failed to get the clock\n");
+		return PTR_ERR(isp_i2c->clock);
+	}
+
+	i2c_adap = &isp_i2c->adapter;
+	i2c_adap->dev.of_node = node;
+	i2c_adap->dev.parent = &pdev->dev;
+	strlcpy(i2c_adap->name, "exynos4x12-isp-i2c", sizeof(i2c_adap->name));
+	i2c_adap->owner = THIS_MODULE;
+	i2c_adap->algo = &fimc_is_i2c_algorithm;
+	i2c_adap->class = I2C_CLASS_SPD;
+
+	ret = i2c_add_adapter(i2c_adap);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to add I2C bus %s\n",
+						node->full_name);
+		return ret;
+	}
+
+	platform_set_drvdata(pdev, isp_i2c);
+
+	pm_runtime_enable(&pdev->dev);
+	pm_runtime_enable(&i2c_adap->dev);
+
+	return 0;
+}
+
+static int fimc_is_i2c_remove(struct platform_device *pdev)
+{
+	struct fimc_is_i2c *isp_i2c = platform_get_drvdata(pdev);
+
+	pm_runtime_disable(&isp_i2c->adapter.dev);
+	pm_runtime_disable(&pdev->dev);
+	i2c_del_adapter(&isp_i2c->adapter);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int fimc_is_i2c_runtime_suspend(struct device *dev)
+{
+	struct fimc_is_i2c *isp_i2c = dev_get_drvdata(dev);
+
+	clk_disable_unprepare(isp_i2c->clock);
+	return 0;
+}
+
+static int fimc_is_i2c_runtime_resume(struct device *dev)
+{
+	struct fimc_is_i2c *isp_i2c = dev_get_drvdata(dev);
+
+	return clk_prepare_enable(isp_i2c->clock);
+}
+#endif
+
+#ifdef CONFIG_PM_SLEEP
+static int fimc_is_i2c_suspend(struct device *dev)
+{
+	if (pm_runtime_suspended(dev))
+		return 0;
+
+	return fimc_is_i2c_runtime_suspend(dev);
+}
+
+static int fimc_is_i2c_resume(struct device *dev)
+{
+	if (pm_runtime_suspended(dev))
+		return 0;
+
+	return fimc_is_i2c_runtime_resume(dev);
+}
+#endif
+
+static struct dev_pm_ops fimc_is_i2c_pm_ops = {
+	SET_RUNTIME_PM_OPS(fimc_is_i2c_runtime_suspend,
+					fimc_is_i2c_runtime_resume, NULL)
+	SET_SYSTEM_SLEEP_PM_OPS(fimc_is_i2c_suspend, fimc_is_i2c_resume)
+};
+
+static const struct of_device_id fimc_is_i2c_of_match[] = {
+	{ .compatible = FIMC_IS_I2C_COMPATIBLE },
+	{ },
+};
+
+static struct platform_driver fimc_is_i2c_driver = {
+	.probe		= fimc_is_i2c_probe,
+	.remove		= fimc_is_i2c_remove,
+	.driver = {
+		.of_match_table = fimc_is_i2c_of_match,
+		.name		= "fimc-isp-i2c",
+		.pm		= &fimc_is_i2c_pm_ops,
+	}
+};
+
+int fimc_is_register_i2c_driver(void)
+{
+	return platform_driver_register(&fimc_is_i2c_driver);
+}
+
+void fimc_is_unregister_i2c_driver(void)
+{
+	platform_driver_unregister(&fimc_is_i2c_driver);
+}
diff --git a/drivers/media/platform/exynos4-is/fimc-is-i2c.h b/drivers/media/platform/exynos4-is/fimc-is-i2c.h
new file mode 100644
index 0000000..0d38d6b
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is-i2c.h
@@ -0,0 +1,15 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ * Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#define FIMC_IS_I2C_COMPATIBLE	"samsung,exynos4212-i2c-isp"
+
+int fimc_is_register_i2c_driver(void);
+void fimc_is_unregister_i2c_driver(void);
diff --git a/drivers/media/platform/exynos4-is/fimc-is-param.c b/drivers/media/platform/exynos4-is/fimc-is-param.c
new file mode 100644
index 0000000..72b9b43
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is-param.c
@@ -0,0 +1,896 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ *
+ * Authors: Younghwan Joo <yhwan.joo@samsung.com>
+ *          Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#define pr_fmt(fmt) "%s:%d " fmt, __func__, __LINE__
+
+#include <linux/bitops.h>
+#include <linux/bug.h>
+#include <linux/device.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <linux/videodev2.h>
+
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+
+#include "fimc-is.h"
+#include "fimc-is-command.h"
+#include "fimc-is-errno.h"
+#include "fimc-is-param.h"
+#include "fimc-is-regs.h"
+#include "fimc-is-sensor.h"
+
+static void __hw_param_copy(void *dst, void *src)
+{
+	memcpy(dst, src, FIMC_IS_PARAM_MAX_SIZE);
+}
+
+static void __fimc_is_hw_update_param_global_shotmode(struct fimc_is *is)
+{
+	struct param_global_shotmode *dst, *src;
+
+	dst = &is->is_p_region->parameter.global.shotmode;
+	src = &is->config[is->config_index].global.shotmode;
+	__hw_param_copy(dst, src);
+}
+
+static void __fimc_is_hw_update_param_sensor_framerate(struct fimc_is *is)
+{
+	struct param_sensor_framerate *dst, *src;
+
+	dst = &is->is_p_region->parameter.sensor.frame_rate;
+	src = &is->config[is->config_index].sensor.frame_rate;
+	__hw_param_copy(dst, src);
+}
+
+int __fimc_is_hw_update_param(struct fimc_is *is, u32 offset)
+{
+	struct is_param_region *par = &is->is_p_region->parameter;
+	struct chain_config *cfg = &is->config[is->config_index];
+
+	switch (offset) {
+	case PARAM_ISP_CONTROL:
+		__hw_param_copy(&par->isp.control, &cfg->isp.control);
+		break;
+
+	case PARAM_ISP_OTF_INPUT:
+		__hw_param_copy(&par->isp.otf_input, &cfg->isp.otf_input);
+		break;
+
+	case PARAM_ISP_DMA1_INPUT:
+		__hw_param_copy(&par->isp.dma1_input, &cfg->isp.dma1_input);
+		break;
+
+	case PARAM_ISP_DMA2_INPUT:
+		__hw_param_copy(&par->isp.dma2_input, &cfg->isp.dma2_input);
+		break;
+
+	case PARAM_ISP_AA:
+		__hw_param_copy(&par->isp.aa, &cfg->isp.aa);
+		break;
+
+	case PARAM_ISP_FLASH:
+		__hw_param_copy(&par->isp.flash, &cfg->isp.flash);
+		break;
+
+	case PARAM_ISP_AWB:
+		__hw_param_copy(&par->isp.awb, &cfg->isp.awb);
+		break;
+
+	case PARAM_ISP_IMAGE_EFFECT:
+		__hw_param_copy(&par->isp.effect, &cfg->isp.effect);
+		break;
+
+	case PARAM_ISP_ISO:
+		__hw_param_copy(&par->isp.iso, &cfg->isp.iso);
+		break;
+
+	case PARAM_ISP_ADJUST:
+		__hw_param_copy(&par->isp.adjust, &cfg->isp.adjust);
+		break;
+
+	case PARAM_ISP_METERING:
+		__hw_param_copy(&par->isp.metering, &cfg->isp.metering);
+		break;
+
+	case PARAM_ISP_AFC:
+		__hw_param_copy(&par->isp.afc, &cfg->isp.afc);
+		break;
+
+	case PARAM_ISP_OTF_OUTPUT:
+		__hw_param_copy(&par->isp.otf_output, &cfg->isp.otf_output);
+		break;
+
+	case PARAM_ISP_DMA1_OUTPUT:
+		__hw_param_copy(&par->isp.dma1_output, &cfg->isp.dma1_output);
+		break;
+
+	case PARAM_ISP_DMA2_OUTPUT:
+		__hw_param_copy(&par->isp.dma2_output, &cfg->isp.dma2_output);
+		break;
+
+	case PARAM_DRC_CONTROL:
+		__hw_param_copy(&par->drc.control, &cfg->drc.control);
+		break;
+
+	case PARAM_DRC_OTF_INPUT:
+		__hw_param_copy(&par->drc.otf_input, &cfg->drc.otf_input);
+		break;
+
+	case PARAM_DRC_DMA_INPUT:
+		__hw_param_copy(&par->drc.dma_input, &cfg->drc.dma_input);
+		break;
+
+	case PARAM_DRC_OTF_OUTPUT:
+		__hw_param_copy(&par->drc.otf_output, &cfg->drc.otf_output);
+		break;
+
+	case PARAM_FD_CONTROL:
+		__hw_param_copy(&par->fd.control, &cfg->fd.control);
+		break;
+
+	case PARAM_FD_OTF_INPUT:
+		__hw_param_copy(&par->fd.otf_input, &cfg->fd.otf_input);
+		break;
+
+	case PARAM_FD_DMA_INPUT:
+		__hw_param_copy(&par->fd.dma_input, &cfg->fd.dma_input);
+		break;
+
+	case PARAM_FD_CONFIG:
+		__hw_param_copy(&par->fd.config, &cfg->fd.config);
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+unsigned int __get_pending_param_count(struct fimc_is *is)
+{
+	struct chain_config *config = &is->config[is->config_index];
+	unsigned long flags;
+	unsigned int count;
+
+	spin_lock_irqsave(&is->slock, flags);
+	count = hweight32(config->p_region_index[0]);
+	count += hweight32(config->p_region_index[1]);
+	spin_unlock_irqrestore(&is->slock, flags);
+
+	return count;
+}
+
+int __is_hw_update_params(struct fimc_is *is)
+{
+	unsigned long *p_index;
+	int i, id, ret = 0;
+
+	id = is->config_index;
+	p_index = &is->config[id].p_region_index[0];
+
+	if (test_bit(PARAM_GLOBAL_SHOTMODE, p_index))
+		__fimc_is_hw_update_param_global_shotmode(is);
+
+	if (test_bit(PARAM_SENSOR_FRAME_RATE, p_index))
+		__fimc_is_hw_update_param_sensor_framerate(is);
+
+	for (i = PARAM_ISP_CONTROL; i < PARAM_DRC_CONTROL; i++) {
+		if (test_bit(i, p_index))
+			ret = __fimc_is_hw_update_param(is, i);
+	}
+
+	for (i = PARAM_DRC_CONTROL; i < PARAM_SCALERC_CONTROL; i++) {
+		if (test_bit(i, p_index))
+			ret = __fimc_is_hw_update_param(is, i);
+	}
+
+	for (i = PARAM_FD_CONTROL; i <= PARAM_FD_CONFIG; i++) {
+		if (test_bit(i, p_index))
+			ret = __fimc_is_hw_update_param(is, i);
+	}
+
+	return ret;
+}
+
+void __is_get_frame_size(struct fimc_is *is, struct v4l2_mbus_framefmt *mf)
+{
+	struct isp_param *isp;
+
+	isp = &is->config[is->config_index].isp;
+	mf->width = isp->otf_input.width;
+	mf->height = isp->otf_input.height;
+}
+
+void __is_set_frame_size(struct fimc_is *is, struct v4l2_mbus_framefmt *mf)
+{
+	unsigned int index = is->config_index;
+	struct isp_param *isp;
+	struct drc_param *drc;
+	struct fd_param *fd;
+
+	isp = &is->config[index].isp;
+	drc = &is->config[index].drc;
+	fd = &is->config[index].fd;
+
+	/* Update isp size info (OTF only) */
+	isp->otf_input.width = mf->width;
+	isp->otf_input.height = mf->height;
+	isp->otf_output.width = mf->width;
+	isp->otf_output.height = mf->height;
+	/* Update drc size info (OTF only) */
+	drc->otf_input.width = mf->width;
+	drc->otf_input.height = mf->height;
+	drc->otf_output.width = mf->width;
+	drc->otf_output.height = mf->height;
+	/* Update fd size info (OTF only) */
+	fd->otf_input.width = mf->width;
+	fd->otf_input.height = mf->height;
+
+	if (test_bit(PARAM_ISP_OTF_INPUT,
+		      &is->config[index].p_region_index[0]))
+		return;
+
+	/* Update field */
+	fimc_is_set_param_bit(is, PARAM_ISP_OTF_INPUT);
+	fimc_is_set_param_bit(is, PARAM_ISP_OTF_OUTPUT);
+	fimc_is_set_param_bit(is, PARAM_DRC_OTF_INPUT);
+	fimc_is_set_param_bit(is, PARAM_DRC_OTF_OUTPUT);
+	fimc_is_set_param_bit(is, PARAM_FD_OTF_INPUT);
+}
+
+int fimc_is_hw_get_sensor_max_framerate(struct fimc_is *is)
+{
+	switch (is->sensor->drvdata->id) {
+	case FIMC_IS_SENSOR_ID_S5K6A3:
+		return 30;
+	default:
+		return 15;
+	}
+}
+
+void __is_set_sensor(struct fimc_is *is, int fps)
+{
+	unsigned int index = is->config_index;
+	struct sensor_param *sensor;
+	struct isp_param *isp;
+
+	sensor = &is->config[index].sensor;
+	isp = &is->config[index].isp;
+
+	if (fps == 0) {
+		sensor->frame_rate.frame_rate =
+				fimc_is_hw_get_sensor_max_framerate(is);
+		isp->otf_input.frametime_min = 0;
+		isp->otf_input.frametime_max = 66666;
+	} else {
+		sensor->frame_rate.frame_rate = fps;
+		isp->otf_input.frametime_min = 0;
+		isp->otf_input.frametime_max = (u32)1000000 / fps;
+	}
+
+	fimc_is_set_param_bit(is, PARAM_SENSOR_FRAME_RATE);
+	fimc_is_set_param_bit(is, PARAM_ISP_OTF_INPUT);
+}
+
+static void __maybe_unused __is_set_init_isp_aa(struct fimc_is *is)
+{
+	struct isp_param *isp;
+
+	isp = &is->config[is->config_index].isp;
+
+	isp->aa.cmd = ISP_AA_COMMAND_START;
+	isp->aa.target = ISP_AA_TARGET_AF | ISP_AA_TARGET_AE |
+			 ISP_AA_TARGET_AWB;
+	isp->aa.mode = 0;
+	isp->aa.scene = 0;
+	isp->aa.sleep = 0;
+	isp->aa.face = 0;
+	isp->aa.touch_x = 0;
+	isp->aa.touch_y = 0;
+	isp->aa.manual_af_setting = 0;
+	isp->aa.err = ISP_AF_ERROR_NONE;
+
+	fimc_is_set_param_bit(is, PARAM_ISP_AA);
+}
+
+void __is_set_isp_flash(struct fimc_is *is, u32 cmd, u32 redeye)
+{
+	unsigned int index = is->config_index;
+	struct isp_param *isp = &is->config[index].isp;
+
+	isp->flash.cmd = cmd;
+	isp->flash.redeye = redeye;
+	isp->flash.err = ISP_FLASH_ERROR_NONE;
+
+	fimc_is_set_param_bit(is, PARAM_ISP_FLASH);
+}
+
+void __is_set_isp_awb(struct fimc_is *is, u32 cmd, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct isp_param *isp;
+
+	isp = &is->config[index].isp;
+
+	isp->awb.cmd = cmd;
+	isp->awb.illumination = val;
+	isp->awb.err = ISP_AWB_ERROR_NONE;
+
+	fimc_is_set_param_bit(is, PARAM_ISP_AWB);
+}
+
+void __is_set_isp_effect(struct fimc_is *is, u32 cmd)
+{
+	unsigned int index = is->config_index;
+	struct isp_param *isp;
+
+	isp = &is->config[index].isp;
+
+	isp->effect.cmd = cmd;
+	isp->effect.err = ISP_IMAGE_EFFECT_ERROR_NONE;
+
+	fimc_is_set_param_bit(is, PARAM_ISP_IMAGE_EFFECT);
+}
+
+void __is_set_isp_iso(struct fimc_is *is, u32 cmd, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct isp_param *isp;
+
+	isp = &is->config[index].isp;
+
+	isp->iso.cmd = cmd;
+	isp->iso.value = val;
+	isp->iso.err = ISP_ISO_ERROR_NONE;
+
+	fimc_is_set_param_bit(is, PARAM_ISP_ISO);
+}
+
+void __is_set_isp_adjust(struct fimc_is *is, u32 cmd, u32 val)
+{
+	unsigned int index = is->config_index;
+	unsigned long *p_index;
+	struct isp_param *isp;
+
+	p_index = &is->config[index].p_region_index[0];
+	isp = &is->config[index].isp;
+
+	switch (cmd) {
+	case ISP_ADJUST_COMMAND_MANUAL_CONTRAST:
+		isp->adjust.contrast = val;
+		break;
+	case ISP_ADJUST_COMMAND_MANUAL_SATURATION:
+		isp->adjust.saturation = val;
+		break;
+	case ISP_ADJUST_COMMAND_MANUAL_SHARPNESS:
+		isp->adjust.sharpness = val;
+		break;
+	case ISP_ADJUST_COMMAND_MANUAL_EXPOSURE:
+		isp->adjust.exposure = val;
+		break;
+	case ISP_ADJUST_COMMAND_MANUAL_BRIGHTNESS:
+		isp->adjust.brightness = val;
+		break;
+	case ISP_ADJUST_COMMAND_MANUAL_HUE:
+		isp->adjust.hue = val;
+		break;
+	case ISP_ADJUST_COMMAND_AUTO:
+		isp->adjust.contrast = 0;
+		isp->adjust.saturation = 0;
+		isp->adjust.sharpness = 0;
+		isp->adjust.exposure = 0;
+		isp->adjust.brightness = 0;
+		isp->adjust.hue = 0;
+		break;
+	}
+
+	if (!test_bit(PARAM_ISP_ADJUST, p_index)) {
+		isp->adjust.cmd = cmd;
+		isp->adjust.err = ISP_ADJUST_ERROR_NONE;
+		fimc_is_set_param_bit(is, PARAM_ISP_ADJUST);
+	} else {
+		isp->adjust.cmd |= cmd;
+	}
+}
+
+void __is_set_isp_metering(struct fimc_is *is, u32 id, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct isp_param *isp;
+	unsigned long *p_index;
+
+	p_index = &is->config[index].p_region_index[0];
+	isp = &is->config[index].isp;
+
+	switch (id) {
+	case IS_METERING_CONFIG_CMD:
+		isp->metering.cmd = val;
+		break;
+	case IS_METERING_CONFIG_WIN_POS_X:
+		isp->metering.win_pos_x = val;
+		break;
+	case IS_METERING_CONFIG_WIN_POS_Y:
+		isp->metering.win_pos_y = val;
+		break;
+	case IS_METERING_CONFIG_WIN_WIDTH:
+		isp->metering.win_width = val;
+		break;
+	case IS_METERING_CONFIG_WIN_HEIGHT:
+		isp->metering.win_height = val;
+		break;
+	default:
+		return;
+	}
+
+	if (!test_bit(PARAM_ISP_METERING, p_index)) {
+		isp->metering.err = ISP_METERING_ERROR_NONE;
+		fimc_is_set_param_bit(is, PARAM_ISP_METERING);
+	}
+}
+
+void __is_set_isp_afc(struct fimc_is *is, u32 cmd, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct isp_param *isp;
+
+	isp = &is->config[index].isp;
+
+	isp->afc.cmd = cmd;
+	isp->afc.manual = val;
+	isp->afc.err = ISP_AFC_ERROR_NONE;
+
+	fimc_is_set_param_bit(is, PARAM_ISP_AFC);
+}
+
+void __is_set_drc_control(struct fimc_is *is, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct drc_param *drc;
+
+	drc = &is->config[index].drc;
+
+	drc->control.bypass = val;
+
+	fimc_is_set_param_bit(is, PARAM_DRC_CONTROL);
+}
+
+void __is_set_fd_control(struct fimc_is *is, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct fd_param *fd;
+	unsigned long *p_index;
+
+	p_index = &is->config[index].p_region_index[1];
+	fd = &is->config[index].fd;
+
+	fd->control.cmd = val;
+
+	if (!test_bit((PARAM_FD_CONFIG - 32), p_index))
+		fimc_is_set_param_bit(is, PARAM_FD_CONTROL);
+}
+
+void __is_set_fd_config_maxface(struct fimc_is *is, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct fd_param *fd;
+	unsigned long *p_index;
+
+	p_index = &is->config[index].p_region_index[1];
+	fd = &is->config[index].fd;
+
+	fd->config.max_number = val;
+
+	if (!test_bit((PARAM_FD_CONFIG - 32), p_index)) {
+		fd->config.cmd = FD_CONFIG_COMMAND_MAXIMUM_NUMBER;
+		fd->config.err = ERROR_FD_NONE;
+		fimc_is_set_param_bit(is, PARAM_FD_CONFIG);
+	} else {
+		fd->config.cmd |= FD_CONFIG_COMMAND_MAXIMUM_NUMBER;
+	}
+}
+
+void __is_set_fd_config_rollangle(struct fimc_is *is, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct fd_param *fd;
+	unsigned long *p_index;
+
+	p_index = &is->config[index].p_region_index[1];
+	fd = &is->config[index].fd;
+
+	fd->config.roll_angle = val;
+
+	if (!test_bit((PARAM_FD_CONFIG - 32), p_index)) {
+		fd->config.cmd = FD_CONFIG_COMMAND_ROLL_ANGLE;
+		fd->config.err = ERROR_FD_NONE;
+		fimc_is_set_param_bit(is, PARAM_FD_CONFIG);
+	} else {
+		fd->config.cmd |= FD_CONFIG_COMMAND_ROLL_ANGLE;
+	}
+}
+
+void __is_set_fd_config_yawangle(struct fimc_is *is, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct fd_param *fd;
+	unsigned long *p_index;
+
+	p_index = &is->config[index].p_region_index[1];
+	fd = &is->config[index].fd;
+
+	fd->config.yaw_angle = val;
+
+	if (!test_bit((PARAM_FD_CONFIG - 32), p_index)) {
+		fd->config.cmd = FD_CONFIG_COMMAND_YAW_ANGLE;
+		fd->config.err = ERROR_FD_NONE;
+		fimc_is_set_param_bit(is, PARAM_FD_CONFIG);
+	} else {
+		fd->config.cmd |= FD_CONFIG_COMMAND_YAW_ANGLE;
+	}
+}
+
+void __is_set_fd_config_smilemode(struct fimc_is *is, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct fd_param *fd;
+	unsigned long *p_index;
+
+	p_index = &is->config[index].p_region_index[1];
+	fd = &is->config[index].fd;
+
+	fd->config.smile_mode = val;
+
+	if (!test_bit((PARAM_FD_CONFIG - 32), p_index)) {
+		fd->config.cmd = FD_CONFIG_COMMAND_SMILE_MODE;
+		fd->config.err = ERROR_FD_NONE;
+		fimc_is_set_param_bit(is, PARAM_FD_CONFIG);
+	} else {
+		fd->config.cmd |= FD_CONFIG_COMMAND_SMILE_MODE;
+	}
+}
+
+void __is_set_fd_config_blinkmode(struct fimc_is *is, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct fd_param *fd;
+	unsigned long *p_index;
+
+	p_index = &is->config[index].p_region_index[1];
+	fd = &is->config[index].fd;
+
+	fd->config.blink_mode = val;
+
+	if (!test_bit((PARAM_FD_CONFIG - 32), p_index)) {
+		fd->config.cmd = FD_CONFIG_COMMAND_BLINK_MODE;
+		fd->config.err = ERROR_FD_NONE;
+		fimc_is_set_param_bit(is, PARAM_FD_CONFIG);
+	} else {
+		fd->config.cmd |= FD_CONFIG_COMMAND_BLINK_MODE;
+	}
+}
+
+void __is_set_fd_config_eyedetect(struct fimc_is *is, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct fd_param *fd;
+	unsigned long *p_index;
+
+	p_index = &is->config[index].p_region_index[1];
+	fd = &is->config[index].fd;
+
+	fd->config.eye_detect = val;
+
+	if (!test_bit((PARAM_FD_CONFIG - 32), p_index)) {
+		fd->config.cmd = FD_CONFIG_COMMAND_EYES_DETECT;
+		fd->config.err = ERROR_FD_NONE;
+		fimc_is_set_param_bit(is, PARAM_FD_CONFIG);
+	} else {
+		fd->config.cmd |= FD_CONFIG_COMMAND_EYES_DETECT;
+	}
+}
+
+void __is_set_fd_config_mouthdetect(struct fimc_is *is, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct fd_param *fd;
+	unsigned long *p_index;
+
+	p_index = &is->config[index].p_region_index[1];
+	fd = &is->config[index].fd;
+
+	fd->config.mouth_detect = val;
+
+	if (!test_bit((PARAM_FD_CONFIG - 32), p_index)) {
+		fd->config.cmd = FD_CONFIG_COMMAND_MOUTH_DETECT;
+		fd->config.err = ERROR_FD_NONE;
+		fimc_is_set_param_bit(is, PARAM_FD_CONFIG);
+	} else {
+		fd->config.cmd |= FD_CONFIG_COMMAND_MOUTH_DETECT;
+	}
+}
+
+void __is_set_fd_config_orientation(struct fimc_is *is, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct fd_param *fd;
+	unsigned long *p_index;
+
+	p_index = &is->config[index].p_region_index[1];
+	fd = &is->config[index].fd;
+
+	fd->config.orientation = val;
+
+	if (!test_bit((PARAM_FD_CONFIG - 32), p_index)) {
+		fd->config.cmd = FD_CONFIG_COMMAND_ORIENTATION;
+		fd->config.err = ERROR_FD_NONE;
+		fimc_is_set_param_bit(is, PARAM_FD_CONFIG);
+	} else {
+		fd->config.cmd |= FD_CONFIG_COMMAND_ORIENTATION;
+	}
+}
+
+void __is_set_fd_config_orientation_val(struct fimc_is *is, u32 val)
+{
+	unsigned int index = is->config_index;
+	struct fd_param *fd;
+	unsigned long *p_index;
+
+	p_index = &is->config[index].p_region_index[1];
+	fd = &is->config[index].fd;
+
+	fd->config.orientation_value = val;
+
+	if (!test_bit((PARAM_FD_CONFIG - 32), p_index)) {
+		fd->config.cmd = FD_CONFIG_COMMAND_ORIENTATION_VALUE;
+		fd->config.err = ERROR_FD_NONE;
+		fimc_is_set_param_bit(is, PARAM_FD_CONFIG);
+	} else {
+		fd->config.cmd |= FD_CONFIG_COMMAND_ORIENTATION_VALUE;
+	}
+}
+
+void fimc_is_set_initial_params(struct fimc_is *is)
+{
+	struct global_param *global;
+	struct isp_param *isp;
+	struct drc_param *drc;
+	struct fd_param *fd;
+	unsigned long *p_index;
+	unsigned int index;
+
+	index = is->config_index;
+	global = &is->config[index].global;
+	isp = &is->config[index].isp;
+	drc = &is->config[index].drc;
+	fd = &is->config[index].fd;
+	p_index = &is->config[index].p_region_index[0];
+
+	/* Global */
+	global->shotmode.cmd = 1;
+	fimc_is_set_param_bit(is, PARAM_GLOBAL_SHOTMODE);
+
+	/* ISP */
+	isp->control.cmd = CONTROL_COMMAND_START;
+	isp->control.bypass = CONTROL_BYPASS_DISABLE;
+	isp->control.err = CONTROL_ERROR_NONE;
+	fimc_is_set_param_bit(is, PARAM_ISP_CONTROL);
+
+	isp->otf_input.cmd = OTF_INPUT_COMMAND_ENABLE;
+	if (!test_bit(PARAM_ISP_OTF_INPUT, p_index)) {
+		isp->otf_input.width = DEFAULT_PREVIEW_STILL_WIDTH;
+		isp->otf_input.height = DEFAULT_PREVIEW_STILL_HEIGHT;
+		fimc_is_set_param_bit(is, PARAM_ISP_OTF_INPUT);
+	}
+	if (is->sensor->test_pattern)
+		isp->otf_input.format = OTF_INPUT_FORMAT_STRGEN_COLORBAR_BAYER;
+	else
+		isp->otf_input.format = OTF_INPUT_FORMAT_BAYER;
+	isp->otf_input.bitwidth = 10;
+	isp->otf_input.order = OTF_INPUT_ORDER_BAYER_GR_BG;
+	isp->otf_input.crop_offset_x = 0;
+	isp->otf_input.crop_offset_y = 0;
+	isp->otf_input.err = OTF_INPUT_ERROR_NONE;
+
+	isp->dma1_input.cmd = DMA_INPUT_COMMAND_DISABLE;
+	isp->dma1_input.width = 0;
+	isp->dma1_input.height = 0;
+	isp->dma1_input.format = 0;
+	isp->dma1_input.bitwidth = 0;
+	isp->dma1_input.plane = 0;
+	isp->dma1_input.order = 0;
+	isp->dma1_input.buffer_number = 0;
+	isp->dma1_input.width = 0;
+	isp->dma1_input.err = DMA_INPUT_ERROR_NONE;
+	fimc_is_set_param_bit(is, PARAM_ISP_DMA1_INPUT);
+
+	isp->dma2_input.cmd = DMA_INPUT_COMMAND_DISABLE;
+	isp->dma2_input.width = 0;
+	isp->dma2_input.height = 0;
+	isp->dma2_input.format = 0;
+	isp->dma2_input.bitwidth = 0;
+	isp->dma2_input.plane = 0;
+	isp->dma2_input.order = 0;
+	isp->dma2_input.buffer_number = 0;
+	isp->dma2_input.width = 0;
+	isp->dma2_input.err = DMA_INPUT_ERROR_NONE;
+	fimc_is_set_param_bit(is, PARAM_ISP_DMA2_INPUT);
+
+	isp->aa.cmd = ISP_AA_COMMAND_START;
+	isp->aa.target = ISP_AA_TARGET_AE | ISP_AA_TARGET_AWB;
+	fimc_is_set_param_bit(is, PARAM_ISP_AA);
+
+	if (!test_bit(PARAM_ISP_FLASH, p_index))
+		__is_set_isp_flash(is, ISP_FLASH_COMMAND_DISABLE,
+						ISP_FLASH_REDEYE_DISABLE);
+
+	if (!test_bit(PARAM_ISP_AWB, p_index))
+		__is_set_isp_awb(is, ISP_AWB_COMMAND_AUTO, 0);
+
+	if (!test_bit(PARAM_ISP_IMAGE_EFFECT, p_index))
+		__is_set_isp_effect(is, ISP_IMAGE_EFFECT_DISABLE);
+
+	if (!test_bit(PARAM_ISP_ISO, p_index))
+		__is_set_isp_iso(is, ISP_ISO_COMMAND_AUTO, 0);
+
+	if (!test_bit(PARAM_ISP_ADJUST, p_index)) {
+		__is_set_isp_adjust(is, ISP_ADJUST_COMMAND_MANUAL_CONTRAST, 0);
+		__is_set_isp_adjust(is,
+				ISP_ADJUST_COMMAND_MANUAL_SATURATION, 0);
+		__is_set_isp_adjust(is, ISP_ADJUST_COMMAND_MANUAL_SHARPNESS, 0);
+		__is_set_isp_adjust(is, ISP_ADJUST_COMMAND_MANUAL_EXPOSURE, 0);
+		__is_set_isp_adjust(is,
+				ISP_ADJUST_COMMAND_MANUAL_BRIGHTNESS, 0);
+		__is_set_isp_adjust(is, ISP_ADJUST_COMMAND_MANUAL_HUE, 0);
+	}
+
+	if (!test_bit(PARAM_ISP_METERING, p_index)) {
+		__is_set_isp_metering(is, 0, ISP_METERING_COMMAND_CENTER);
+		__is_set_isp_metering(is, 1, 0);
+		__is_set_isp_metering(is, 2, 0);
+		__is_set_isp_metering(is, 3, 0);
+		__is_set_isp_metering(is, 4, 0);
+	}
+
+	if (!test_bit(PARAM_ISP_AFC, p_index))
+		__is_set_isp_afc(is, ISP_AFC_COMMAND_AUTO, 0);
+
+	isp->otf_output.cmd = OTF_OUTPUT_COMMAND_ENABLE;
+	if (!test_bit(PARAM_ISP_OTF_OUTPUT, p_index)) {
+		isp->otf_output.width = DEFAULT_PREVIEW_STILL_WIDTH;
+		isp->otf_output.height = DEFAULT_PREVIEW_STILL_HEIGHT;
+		fimc_is_set_param_bit(is, PARAM_ISP_OTF_OUTPUT);
+	}
+	isp->otf_output.format = OTF_OUTPUT_FORMAT_YUV444;
+	isp->otf_output.bitwidth = 12;
+	isp->otf_output.order = 0;
+	isp->otf_output.err = OTF_OUTPUT_ERROR_NONE;
+
+	if (!test_bit(PARAM_ISP_DMA1_OUTPUT, p_index)) {
+		isp->dma1_output.cmd = DMA_OUTPUT_COMMAND_DISABLE;
+		isp->dma1_output.width = 0;
+		isp->dma1_output.height = 0;
+		isp->dma1_output.format = 0;
+		isp->dma1_output.bitwidth = 0;
+		isp->dma1_output.plane = 0;
+		isp->dma1_output.order = 0;
+		isp->dma1_output.buffer_number = 0;
+		isp->dma1_output.buffer_address = 0;
+		isp->dma1_output.notify_dma_done = 0;
+		isp->dma1_output.dma_out_mask = 0;
+		isp->dma1_output.err = DMA_OUTPUT_ERROR_NONE;
+		fimc_is_set_param_bit(is, PARAM_ISP_DMA1_OUTPUT);
+	}
+
+	if (!test_bit(PARAM_ISP_DMA2_OUTPUT, p_index)) {
+		isp->dma2_output.cmd = DMA_OUTPUT_COMMAND_DISABLE;
+		isp->dma2_output.width = 0;
+		isp->dma2_output.height = 0;
+		isp->dma2_output.format = 0;
+		isp->dma2_output.bitwidth = 0;
+		isp->dma2_output.plane = 0;
+		isp->dma2_output.order = 0;
+		isp->dma2_output.buffer_number = 0;
+		isp->dma2_output.buffer_address = 0;
+		isp->dma2_output.notify_dma_done = 0;
+		isp->dma2_output.dma_out_mask = 0;
+		isp->dma2_output.err = DMA_OUTPUT_ERROR_NONE;
+		fimc_is_set_param_bit(is, PARAM_ISP_DMA2_OUTPUT);
+	}
+
+	/* Sensor */
+	if (!test_bit(PARAM_SENSOR_FRAME_RATE, p_index)) {
+		if (is->config_index == 0)
+			__is_set_sensor(is, 0);
+	}
+
+	/* DRC */
+	drc->control.cmd = CONTROL_COMMAND_START;
+	__is_set_drc_control(is, CONTROL_BYPASS_ENABLE);
+
+	drc->otf_input.cmd = OTF_INPUT_COMMAND_ENABLE;
+	if (!test_bit(PARAM_DRC_OTF_INPUT, p_index)) {
+		drc->otf_input.width = DEFAULT_PREVIEW_STILL_WIDTH;
+		drc->otf_input.height = DEFAULT_PREVIEW_STILL_HEIGHT;
+		fimc_is_set_param_bit(is, PARAM_DRC_OTF_INPUT);
+	}
+	drc->otf_input.format = OTF_INPUT_FORMAT_YUV444;
+	drc->otf_input.bitwidth = 12;
+	drc->otf_input.order = 0;
+	drc->otf_input.err = OTF_INPUT_ERROR_NONE;
+
+	drc->dma_input.cmd = DMA_INPUT_COMMAND_DISABLE;
+	drc->dma_input.width = 0;
+	drc->dma_input.height = 0;
+	drc->dma_input.format = 0;
+	drc->dma_input.bitwidth = 0;
+	drc->dma_input.plane = 0;
+	drc->dma_input.order = 0;
+	drc->dma_input.buffer_number = 0;
+	drc->dma_input.width = 0;
+	drc->dma_input.err = DMA_INPUT_ERROR_NONE;
+	fimc_is_set_param_bit(is, PARAM_DRC_DMA_INPUT);
+
+	drc->otf_output.cmd = OTF_OUTPUT_COMMAND_ENABLE;
+	if (!test_bit(PARAM_DRC_OTF_OUTPUT, p_index)) {
+		drc->otf_output.width = DEFAULT_PREVIEW_STILL_WIDTH;
+		drc->otf_output.height = DEFAULT_PREVIEW_STILL_HEIGHT;
+		fimc_is_set_param_bit(is, PARAM_DRC_OTF_OUTPUT);
+	}
+	drc->otf_output.format = OTF_OUTPUT_FORMAT_YUV444;
+	drc->otf_output.bitwidth = 8;
+	drc->otf_output.order = 0;
+	drc->otf_output.err = OTF_OUTPUT_ERROR_NONE;
+
+	/* FD */
+	__is_set_fd_control(is, CONTROL_COMMAND_STOP);
+	fd->control.bypass = CONTROL_BYPASS_DISABLE;
+
+	fd->otf_input.cmd = OTF_INPUT_COMMAND_ENABLE;
+	if (!test_bit(PARAM_FD_OTF_INPUT, p_index)) {
+		fd->otf_input.width = DEFAULT_PREVIEW_STILL_WIDTH;
+		fd->otf_input.height = DEFAULT_PREVIEW_STILL_HEIGHT;
+		fimc_is_set_param_bit(is, PARAM_FD_OTF_INPUT);
+	}
+
+	fd->otf_input.format = OTF_INPUT_FORMAT_YUV444;
+	fd->otf_input.bitwidth = 8;
+	fd->otf_input.order = 0;
+	fd->otf_input.err = OTF_INPUT_ERROR_NONE;
+
+	fd->dma_input.cmd = DMA_INPUT_COMMAND_DISABLE;
+	fd->dma_input.width = 0;
+	fd->dma_input.height = 0;
+	fd->dma_input.format = 0;
+	fd->dma_input.bitwidth = 0;
+	fd->dma_input.plane = 0;
+	fd->dma_input.order = 0;
+	fd->dma_input.buffer_number = 0;
+	fd->dma_input.width = 0;
+	fd->dma_input.err = DMA_INPUT_ERROR_NONE;
+	fimc_is_set_param_bit(is, PARAM_FD_DMA_INPUT);
+
+	__is_set_fd_config_maxface(is, 5);
+	__is_set_fd_config_rollangle(is, FD_CONFIG_ROLL_ANGLE_FULL);
+	__is_set_fd_config_yawangle(is, FD_CONFIG_YAW_ANGLE_45_90);
+	__is_set_fd_config_smilemode(is, FD_CONFIG_SMILE_MODE_DISABLE);
+	__is_set_fd_config_blinkmode(is, FD_CONFIG_BLINK_MODE_DISABLE);
+	__is_set_fd_config_eyedetect(is, FD_CONFIG_EYES_DETECT_ENABLE);
+	__is_set_fd_config_mouthdetect(is, FD_CONFIG_MOUTH_DETECT_DISABLE);
+	__is_set_fd_config_orientation(is, FD_CONFIG_ORIENTATION_DISABLE);
+	__is_set_fd_config_orientation_val(is, 0);
+}
diff --git a/drivers/media/platform/exynos4-is/fimc-is-param.h b/drivers/media/platform/exynos4-is/fimc-is-param.h
new file mode 100644
index 0000000..8e31f76
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is-param.h
@@ -0,0 +1,1025 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2011 - 2013 Samsung Electronics Co., Ltd.
+ *
+ * Authors: Younghwan Joo <yhwan.joo@samsung.com>
+ *	    Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#ifndef FIMC_IS_PARAM_H_
+#define FIMC_IS_PARAM_H_
+
+#include <linux/compiler.h>
+
+#define FIMC_IS_CONFIG_TIMEOUT		3000 /* ms */
+#define IS_DEFAULT_WIDTH		1280
+#define IS_DEFAULT_HEIGHT		720
+
+#define DEFAULT_PREVIEW_STILL_WIDTH	IS_DEFAULT_WIDTH
+#define DEFAULT_PREVIEW_STILL_HEIGHT	IS_DEFAULT_HEIGHT
+#define DEFAULT_CAPTURE_STILL_WIDTH	IS_DEFAULT_WIDTH
+#define DEFAULT_CAPTURE_STILL_HEIGHT	IS_DEFAULT_HEIGHT
+#define DEFAULT_PREVIEW_VIDEO_WIDTH	IS_DEFAULT_WIDTH
+#define DEFAULT_PREVIEW_VIDEO_HEIGHT	IS_DEFAULT_HEIGHT
+#define DEFAULT_CAPTURE_VIDEO_WIDTH	IS_DEFAULT_WIDTH
+#define DEFAULT_CAPTURE_VIDEO_HEIGHT	IS_DEFAULT_HEIGHT
+
+#define DEFAULT_PREVIEW_STILL_FRAMERATE	30
+#define DEFAULT_CAPTURE_STILL_FRAMERATE	15
+#define DEFAULT_PREVIEW_VIDEO_FRAMERATE	30
+#define DEFAULT_CAPTURE_VIDEO_FRAMERATE	30
+
+#define FIMC_IS_REGION_VER		124 /* IS REGION VERSION 1.24 */
+#define FIMC_IS_PARAM_SIZE		(FIMC_IS_REGION_SIZE + 1)
+#define FIMC_IS_MAGIC_NUMBER		0x01020304
+#define FIMC_IS_PARAM_MAX_SIZE		64 /* in bytes */
+#define FIMC_IS_PARAM_MAX_ENTRIES	(FIMC_IS_PARAM_MAX_SIZE / 4)
+
+/* The parameter bitmask bit definitions. */
+enum is_param_bit {
+	PARAM_GLOBAL_SHOTMODE,
+	PARAM_SENSOR_CONTROL,
+	PARAM_SENSOR_OTF_OUTPUT,
+	PARAM_SENSOR_FRAME_RATE,
+	PARAM_BUFFER_CONTROL,
+	PARAM_BUFFER_OTF_INPUT,
+	PARAM_BUFFER_OTF_OUTPUT,
+	PARAM_ISP_CONTROL,
+	PARAM_ISP_OTF_INPUT,
+	PARAM_ISP_DMA1_INPUT,
+	/* 10 */
+	PARAM_ISP_DMA2_INPUT,
+	PARAM_ISP_AA,
+	PARAM_ISP_FLASH,
+	PARAM_ISP_AWB,
+	PARAM_ISP_IMAGE_EFFECT,
+	PARAM_ISP_ISO,
+	PARAM_ISP_ADJUST,
+	PARAM_ISP_METERING,
+	PARAM_ISP_AFC,
+	PARAM_ISP_OTF_OUTPUT,
+	/* 20 */
+	PARAM_ISP_DMA1_OUTPUT,
+	PARAM_ISP_DMA2_OUTPUT,
+	PARAM_DRC_CONTROL,
+	PARAM_DRC_OTF_INPUT,
+	PARAM_DRC_DMA_INPUT,
+	PARAM_DRC_OTF_OUTPUT,
+	PARAM_SCALERC_CONTROL,
+	PARAM_SCALERC_OTF_INPUT,
+	PARAM_SCALERC_IMAGE_EFFECT,
+	PARAM_SCALERC_INPUT_CROP,
+	/* 30 */
+	PARAM_SCALERC_OUTPUT_CROP,
+	PARAM_SCALERC_OTF_OUTPUT,
+	PARAM_SCALERC_DMA_OUTPUT,
+	PARAM_ODC_CONTROL,
+	PARAM_ODC_OTF_INPUT,
+	PARAM_ODC_OTF_OUTPUT,
+	PARAM_DIS_CONTROL,
+	PARAM_DIS_OTF_INPUT,
+	PARAM_DIS_OTF_OUTPUT,
+	PARAM_TDNR_CONTROL,
+	/* 40 */
+	PARAM_TDNR_OTF_INPUT,
+	PARAM_TDNR_1ST_FRAME,
+	PARAM_TDNR_OTF_OUTPUT,
+	PARAM_TDNR_DMA_OUTPUT,
+	PARAM_SCALERP_CONTROL,
+	PARAM_SCALERP_OTF_INPUT,
+	PARAM_SCALERP_IMAGE_EFFECT,
+	PARAM_SCALERP_INPUT_CROP,
+	PARAM_SCALERP_OUTPUT_CROP,
+	PARAM_SCALERP_ROTATION,
+	/* 50 */
+	PARAM_SCALERP_FLIP,
+	PARAM_SCALERP_OTF_OUTPUT,
+	PARAM_SCALERP_DMA_OUTPUT,
+	PARAM_FD_CONTROL,
+	PARAM_FD_OTF_INPUT,
+	PARAM_FD_DMA_INPUT,
+	PARAM_FD_CONFIG,
+};
+
+/* Interrupt map */
+#define	FIMC_IS_INT_GENERAL			0
+#define	FIMC_IS_INT_FRAME_DONE_ISP		1
+
+/* Input */
+
+#define CONTROL_COMMAND_STOP			0
+#define CONTROL_COMMAND_START			1
+
+#define CONTROL_BYPASS_DISABLE			0
+#define CONTROL_BYPASS_ENABLE			1
+
+#define CONTROL_ERROR_NONE			0
+
+/* OTF (On-The-Fly) input interface commands */
+#define OTF_INPUT_COMMAND_DISABLE		0
+#define OTF_INPUT_COMMAND_ENABLE		1
+
+/* OTF input interface color formats */
+enum oft_input_fmt {
+	OTF_INPUT_FORMAT_BAYER			= 0, /* 1 channel */
+	OTF_INPUT_FORMAT_YUV444			= 1, /* 3 channels */
+	OTF_INPUT_FORMAT_YUV422			= 2, /* 3 channels */
+	OTF_INPUT_FORMAT_YUV420			= 3, /* 3 channels */
+	OTF_INPUT_FORMAT_STRGEN_COLORBAR_BAYER	= 10,
+	OTF_INPUT_FORMAT_BAYER_DMA		= 11,
+};
+
+#define OTF_INPUT_ORDER_BAYER_GR_BG		0
+
+/* OTF input error codes */
+#define OTF_INPUT_ERROR_NONE			0 /* Input setting is done */
+
+/* DMA input commands */
+#define DMA_INPUT_COMMAND_DISABLE		0
+#define DMA_INPUT_COMMAND_ENABLE		1
+
+/* DMA input color formats */
+enum dma_input_fmt {
+	DMA_INPUT_FORMAT_BAYER			= 0,
+	DMA_INPUT_FORMAT_YUV444			= 1,
+	DMA_INPUT_FORMAT_YUV422			= 2,
+	DMA_INPUT_FORMAT_YUV420			= 3,
+};
+
+enum dma_input_order {
+	/* (for DMA_INPUT_PLANE_3) */
+	DMA_INPUT_ORDER_NO	= 0,
+	/* (only valid at DMA_INPUT_PLANE_2) */
+	DMA_INPUT_ORDER_CBCR	= 1,
+	/* (only valid at DMA_INPUT_PLANE_2) */
+	DMA_INPUT_ORDER_CRCB	= 2,
+	/* (only valid at DMA_INPUT_PLANE_1 & DMA_INPUT_FORMAT_YUV444) */
+	DMA_INPUT_ORDER_YCBCR	= 3,
+	/* (only valid at DMA_INPUT_FORMAT_YUV422 & DMA_INPUT_PLANE_1) */
+	DMA_INPUT_ORDER_YYCBCR	= 4,
+	/* (only valid at DMA_INPUT_FORMAT_YUV422 & DMA_INPUT_PLANE_1) */
+	DMA_INPUT_ORDER_YCBYCR	= 5,
+	/* (only valid at DMA_INPUT_FORMAT_YUV422 & DMA_INPUT_PLANE_1) */
+	DMA_INPUT_ORDER_YCRYCB	= 6,
+	/* (only valid at DMA_INPUT_FORMAT_YUV422 & DMA_INPUT_PLANE_1) */
+	DMA_INPUT_ORDER_CBYCRY	= 7,
+	/* (only valid at DMA_INPUT_FORMAT_YUV422 & DMA_INPUT_PLANE_1) */
+	DMA_INPUT_ORDER_CRYCBY	= 8,
+	/* (only valid at DMA_INPUT_FORMAT_BAYER) */
+	DMA_INPUT_ORDER_GR_BG	= 9
+};
+
+#define DMA_INPUT_ERROR_NONE			0 /* DMA input setting
+						     is done */
+/*
+ * Data output parameter definitions
+ */
+#define OTF_OUTPUT_CROP_DISABLE			0
+#define OTF_OUTPUT_CROP_ENABLE			1
+
+#define OTF_OUTPUT_COMMAND_DISABLE		0
+#define OTF_OUTPUT_COMMAND_ENABLE		1
+
+enum otf_output_fmt {
+	OTF_OUTPUT_FORMAT_YUV444		= 1,
+	OTF_OUTPUT_FORMAT_YUV422		= 2,
+	OTF_OUTPUT_FORMAT_YUV420		= 3,
+	OTF_OUTPUT_FORMAT_RGB			= 4,
+};
+
+#define OTF_OUTPUT_ORDER_BAYER_GR_BG		0
+
+#define OTF_OUTPUT_ERROR_NONE			0 /* Output Setting is done */
+
+#define DMA_OUTPUT_COMMAND_DISABLE		0
+#define DMA_OUTPUT_COMMAND_ENABLE		1
+
+enum dma_output_fmt {
+	DMA_OUTPUT_FORMAT_BAYER			= 0,
+	DMA_OUTPUT_FORMAT_YUV444		= 1,
+	DMA_OUTPUT_FORMAT_YUV422		= 2,
+	DMA_OUTPUT_FORMAT_YUV420		= 3,
+	DMA_OUTPUT_FORMAT_RGB			= 4,
+};
+
+enum dma_output_order {
+	DMA_OUTPUT_ORDER_NO		= 0,
+	/* for DMA_OUTPUT_PLANE_3 */
+	DMA_OUTPUT_ORDER_CBCR		= 1,
+	/* only valid at DMA_INPUT_PLANE_2) */
+	DMA_OUTPUT_ORDER_CRCB		= 2,
+	/* only valid at DMA_OUTPUT_PLANE_2) */
+	DMA_OUTPUT_ORDER_YYCBCR		= 3,
+	/* only valid at DMA_OUTPUT_FORMAT_YUV422 & DMA_OUTPUT_PLANE_1 */
+	DMA_OUTPUT_ORDER_YCBYCR		= 4,
+	/* only valid at DMA_OUTPUT_FORMAT_YUV422 & DMA_OUTPUT_PLANE_1 */
+	DMA_OUTPUT_ORDER_YCRYCB		= 5,
+	/* only valid at DMA_OUTPUT_FORMAT_YUV422 & DMA_OUTPUT_PLANE_1 */
+	DMA_OUTPUT_ORDER_CBYCRY		= 6,
+	/* only valid at DMA_OUTPUT_FORMAT_YUV422 & DMA_OUTPUT_PLANE_1 */
+	DMA_OUTPUT_ORDER_CRYCBY		= 7,
+	/* only valid at DMA_OUTPUT_FORMAT_YUV422 & DMA_OUTPUT_PLANE_1 */
+	DMA_OUTPUT_ORDER_YCBCR		= 8,
+	/* only valid at DMA_OUTPUT_FORMAT_YUV444 & DMA_OUPUT_PLANE_1 */
+	DMA_OUTPUT_ORDER_CRYCB		= 9,
+	/* only valid at DMA_OUTPUT_FORMAT_YUV444 & DMA_OUPUT_PLANE_1 */
+	DMA_OUTPUT_ORDER_CRCBY		= 10,
+	/* only valid at DMA_OUTPUT_FORMAT_YUV444 & DMA_OUPUT_PLANE_1 */
+	DMA_OUTPUT_ORDER_CBYCR		= 11,
+	/* only valid at DMA_OUTPUT_FORMAT_YUV444 & DMA_OUPUT_PLANE_1 */
+	DMA_OUTPUT_ORDER_YCRCB		= 12,
+	/* only valid at DMA_OUTPUT_FORMAT_YUV444 & DMA_OUPUT_PLANE_1 */
+	DMA_OUTPUT_ORDER_CBCRY		= 13,
+	/* only valid at DMA_OUTPUT_FORMAT_YUV444 & DMA_OUPUT_PLANE_1 */
+	DMA_OUTPUT_ORDER_BGR		= 14,
+	/* only valid at DMA_OUTPUT_FORMAT_RGB */
+	DMA_OUTPUT_ORDER_GB_BG		= 15
+	/* only valid at DMA_OUTPUT_FORMAT_BAYER */
+};
+
+/* enum dma_output_notify_dma_done */
+#define DMA_OUTPUT_NOTIFY_DMA_DONE_DISABLE	0
+#define DMA_OUTPUT_NOTIFY_DMA_DONE_ENABLE	1
+
+/* DMA output error codes */
+#define DMA_OUTPUT_ERROR_NONE			0 /* DMA output setting
+						     is done */
+
+/* ----------------------  Global  ----------------------------------- */
+#define GLOBAL_SHOTMODE_ERROR_NONE		0 /* shot-mode setting
+						     is done */
+/* 3A lock commands */
+#define ISP_AA_COMMAND_START			0
+#define ISP_AA_COMMAND_STOP			1
+
+/* 3A lock target */
+#define ISP_AA_TARGET_AF			1
+#define ISP_AA_TARGET_AE			2
+#define ISP_AA_TARGET_AWB			4
+
+enum isp_af_mode {
+	ISP_AF_MODE_MANUAL			= 0,
+	ISP_AF_MODE_SINGLE			= 1,
+	ISP_AF_MODE_CONTINUOUS			= 2,
+	ISP_AF_MODE_TOUCH			= 3,
+	ISP_AF_MODE_SLEEP			= 4,
+	ISP_AF_MODE_INIT			= 5,
+	ISP_AF_MODE_SET_CENTER_WINDOW		= 6,
+	ISP_AF_MODE_SET_TOUCH_WINDOW		= 7
+};
+
+/* Face AF commands */
+#define ISP_AF_FACE_DISABLE			0
+#define ISP_AF_FACE_ENABLE			1
+
+/* AF range */
+#define ISP_AF_RANGE_NORMAL			0
+#define ISP_AF_RANGE_MACRO			1
+
+/* AF sleep */
+#define ISP_AF_SLEEP_OFF			0
+#define ISP_AF_SLEEP_ON				1
+
+/* Continuous AF commands */
+#define ISP_AF_CONTINUOUS_DISABLE		0
+#define ISP_AF_CONTINUOUS_ENABLE		1
+
+/* ISP AF error codes */
+#define ISP_AF_ERROR_NONE			0 /* AF mode change is done */
+#define ISP_AF_ERROR_NONE_LOCK_DONE		1 /* AF lock is done */
+
+/* Flash commands */
+#define ISP_FLASH_COMMAND_DISABLE		0
+#define ISP_FLASH_COMMAND_MANUAL_ON		1 /* (forced flash) */
+#define ISP_FLASH_COMMAND_AUTO			2
+#define ISP_FLASH_COMMAND_TORCH			3 /* 3 sec */
+
+/* Flash red-eye commads */
+#define ISP_FLASH_REDEYE_DISABLE		0
+#define ISP_FLASH_REDEYE_ENABLE			1
+
+/* Flash error codes */
+#define ISP_FLASH_ERROR_NONE			0 /* Flash setting is done */
+
+/* --------------------------  AWB  ------------------------------------ */
+enum isp_awb_command {
+	ISP_AWB_COMMAND_AUTO			= 0,
+	ISP_AWB_COMMAND_ILLUMINATION		= 1,
+	ISP_AWB_COMMAND_MANUAL			= 2
+};
+
+enum isp_awb_illumination {
+	ISP_AWB_ILLUMINATION_DAYLIGHT		= 0,
+	ISP_AWB_ILLUMINATION_CLOUDY		= 1,
+	ISP_AWB_ILLUMINATION_TUNGSTEN		= 2,
+	ISP_AWB_ILLUMINATION_FLUORESCENT	= 3
+};
+
+/* ISP AWN error codes */
+#define ISP_AWB_ERROR_NONE			0 /* AWB setting is done */
+
+/* --------------------------  Effect  ----------------------------------- */
+enum isp_imageeffect_command {
+	ISP_IMAGE_EFFECT_DISABLE		= 0,
+	ISP_IMAGE_EFFECT_MONOCHROME		= 1,
+	ISP_IMAGE_EFFECT_NEGATIVE_MONO		= 2,
+	ISP_IMAGE_EFFECT_NEGATIVE_COLOR		= 3,
+	ISP_IMAGE_EFFECT_SEPIA			= 4
+};
+
+/* Image effect error codes */
+#define ISP_IMAGE_EFFECT_ERROR_NONE		0 /* Image effect setting
+						     is done */
+/* ISO commands */
+#define ISP_ISO_COMMAND_AUTO			0
+#define ISP_ISO_COMMAND_MANUAL			1
+
+/* ISO error codes */
+#define ISP_ISO_ERROR_NONE			0 /* ISO setting is done */
+
+/* ISP adjust commands */
+#define ISP_ADJUST_COMMAND_AUTO			(0 << 0)
+#define ISP_ADJUST_COMMAND_MANUAL_CONTRAST	(1 << 0)
+#define ISP_ADJUST_COMMAND_MANUAL_SATURATION	(1 << 1)
+#define ISP_ADJUST_COMMAND_MANUAL_SHARPNESS	(1 << 2)
+#define ISP_ADJUST_COMMAND_MANUAL_EXPOSURE	(1 << 3)
+#define ISP_ADJUST_COMMAND_MANUAL_BRIGHTNESS	(1 << 4)
+#define ISP_ADJUST_COMMAND_MANUAL_HUE		(1 << 5)
+#define ISP_ADJUST_COMMAND_MANUAL_ALL		0x7f
+
+/* ISP adjustment error codes */
+#define ISP_ADJUST_ERROR_NONE			0 /* Adjust setting is done */
+
+/*
+ *  Exposure metering
+ */
+enum isp_metering_command {
+	ISP_METERING_COMMAND_AVERAGE	= 0,
+	ISP_METERING_COMMAND_SPOT	= 1,
+	ISP_METERING_COMMAND_MATRIX	= 2,
+	ISP_METERING_COMMAND_CENTER	= 3
+};
+
+/* ISP metering error codes */
+#define ISP_METERING_ERROR_NONE		0 /* Metering setting is done */
+
+/*
+ * AFC
+ */
+enum isp_afc_command {
+	ISP_AFC_COMMAND_DISABLE		= 0,
+	ISP_AFC_COMMAND_AUTO		= 1,
+	ISP_AFC_COMMAND_MANUAL		= 2,
+};
+
+#define ISP_AFC_MANUAL_50HZ		50
+#define ISP_AFC_MANUAL_60HZ		60
+
+/* ------------------------  SCENE MODE--------------------------------- */
+enum isp_scene_mode {
+	ISP_SCENE_NONE			= 0,
+	ISP_SCENE_PORTRAIT		= 1,
+	ISP_SCENE_LANDSCAPE		= 2,
+	ISP_SCENE_SPORTS		= 3,
+	ISP_SCENE_PARTYINDOOR		= 4,
+	ISP_SCENE_BEACHSNOW		= 5,
+	ISP_SCENE_SUNSET		= 6,
+	ISP_SCENE_DAWN			= 7,
+	ISP_SCENE_FALL			= 8,
+	ISP_SCENE_NIGHT			= 9,
+	ISP_SCENE_AGAINSTLIGHTWLIGHT	= 10,
+	ISP_SCENE_AGAINSTLIGHTWOLIGHT	= 11,
+	ISP_SCENE_FIRE			= 12,
+	ISP_SCENE_TEXT			= 13,
+	ISP_SCENE_CANDLE		= 14
+};
+
+/* AFC error codes */
+#define ISP_AFC_ERROR_NONE		0 /* AFC setting is done */
+
+/* ----------------------------  FD  ------------------------------------- */
+enum fd_config_command {
+	FD_CONFIG_COMMAND_MAXIMUM_NUMBER	= 0x1,
+	FD_CONFIG_COMMAND_ROLL_ANGLE		= 0x2,
+	FD_CONFIG_COMMAND_YAW_ANGLE		= 0x4,
+	FD_CONFIG_COMMAND_SMILE_MODE		= 0x8,
+	FD_CONFIG_COMMAND_BLINK_MODE		= 0x10,
+	FD_CONFIG_COMMAND_EYES_DETECT		= 0x20,
+	FD_CONFIG_COMMAND_MOUTH_DETECT		= 0x40,
+	FD_CONFIG_COMMAND_ORIENTATION		= 0x80,
+	FD_CONFIG_COMMAND_ORIENTATION_VALUE	= 0x100
+};
+
+enum fd_config_roll_angle {
+	FD_CONFIG_ROLL_ANGLE_BASIC		= 0,
+	FD_CONFIG_ROLL_ANGLE_PRECISE_BASIC	= 1,
+	FD_CONFIG_ROLL_ANGLE_SIDES		= 2,
+	FD_CONFIG_ROLL_ANGLE_PRECISE_SIDES	= 3,
+	FD_CONFIG_ROLL_ANGLE_FULL		= 4,
+	FD_CONFIG_ROLL_ANGLE_PRECISE_FULL	= 5,
+};
+
+enum fd_config_yaw_angle {
+	FD_CONFIG_YAW_ANGLE_0			= 0,
+	FD_CONFIG_YAW_ANGLE_45			= 1,
+	FD_CONFIG_YAW_ANGLE_90			= 2,
+	FD_CONFIG_YAW_ANGLE_45_90		= 3,
+};
+
+/* Smile mode configuration */
+#define FD_CONFIG_SMILE_MODE_DISABLE		0
+#define FD_CONFIG_SMILE_MODE_ENABLE		1
+
+/* Blink mode configuration */
+#define FD_CONFIG_BLINK_MODE_DISABLE		0
+#define FD_CONFIG_BLINK_MODE_ENABLE		1
+
+/* Eyes detection configuration */
+#define FD_CONFIG_EYES_DETECT_DISABLE		0
+#define FD_CONFIG_EYES_DETECT_ENABLE		1
+
+/* Mouth detection configuration */
+#define FD_CONFIG_MOUTH_DETECT_DISABLE		0
+#define FD_CONFIG_MOUTH_DETECT_ENABLE		1
+
+#define FD_CONFIG_ORIENTATION_DISABLE		0
+#define FD_CONFIG_ORIENTATION_ENABLE		1
+
+struct param_control {
+	u32 cmd;
+	u32 bypass;
+	u32 buffer_address;
+	u32 buffer_size;
+	u32 skip_frames; /* only valid at ISP */
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 6];
+	u32 err;
+};
+
+struct param_otf_input {
+	u32 cmd;
+	u32 width;
+	u32 height;
+	u32 format;
+	u32 bitwidth;
+	u32 order;
+	u32 crop_offset_x;
+	u32 crop_offset_y;
+	u32 crop_width;
+	u32 crop_height;
+	u32 frametime_min;
+	u32 frametime_max;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 13];
+	u32 err;
+};
+
+struct param_dma_input {
+	u32 cmd;
+	u32 width;
+	u32 height;
+	u32 format;
+	u32 bitwidth;
+	u32 plane;
+	u32 order;
+	u32 buffer_number;
+	u32 buffer_address;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 10];
+	u32 err;
+};
+
+struct param_otf_output {
+	u32 cmd;
+	u32 width;
+	u32 height;
+	u32 format;
+	u32 bitwidth;
+	u32 order;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 7];
+	u32 err;
+};
+
+struct param_dma_output {
+	u32 cmd;
+	u32 width;
+	u32 height;
+	u32 format;
+	u32 bitwidth;
+	u32 plane;
+	u32 order;
+	u32 buffer_number;
+	u32 buffer_address;
+	u32 notify_dma_done;
+	u32 dma_out_mask;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 12];
+	u32 err;
+};
+
+struct param_global_shotmode {
+	u32 cmd;
+	u32 skip_frames;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 3];
+	u32 err;
+};
+
+struct param_sensor_framerate {
+	u32 frame_rate;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 2];
+	u32 err;
+};
+
+struct param_isp_aa {
+	u32 cmd;
+	u32 target;
+	u32 mode;
+	u32 scene;
+	u32 sleep;
+	u32 face;
+	u32 touch_x;
+	u32 touch_y;
+	u32 manual_af_setting;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 10];
+	u32 err;
+};
+
+struct param_isp_flash {
+	u32 cmd;
+	u32 redeye;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 3];
+	u32 err;
+};
+
+struct param_isp_awb {
+	u32 cmd;
+	u32 illumination;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 3];
+	u32 err;
+};
+
+struct param_isp_imageeffect {
+	u32 cmd;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 2];
+	u32 err;
+};
+
+struct param_isp_iso {
+	u32 cmd;
+	u32 value;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 3];
+	u32 err;
+};
+
+struct param_isp_adjust {
+	u32 cmd;
+	s32 contrast;
+	s32 saturation;
+	s32 sharpness;
+	s32 exposure;
+	s32 brightness;
+	s32 hue;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 8];
+	u32 err;
+};
+
+struct param_isp_metering {
+	u32 cmd;
+	u32 win_pos_x;
+	u32 win_pos_y;
+	u32 win_width;
+	u32 win_height;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 6];
+	u32 err;
+};
+
+struct param_isp_afc {
+	u32 cmd;
+	u32 manual;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 3];
+	u32 err;
+};
+
+struct param_scaler_imageeffect {
+	u32 cmd;
+	u32 arbitrary_cb;
+	u32 arbitrary_cr;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 4];
+	u32 err;
+};
+
+struct param_scaler_input_crop {
+	u32 cmd;
+	u32 crop_offset_x;
+	u32 crop_offset_y;
+	u32 crop_width;
+	u32 crop_height;
+	u32 in_width;
+	u32 in_height;
+	u32 out_width;
+	u32 out_height;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 10];
+	u32 err;
+};
+
+struct param_scaler_output_crop {
+	u32 cmd;
+	u32 crop_offset_x;
+	u32 crop_offset_y;
+	u32 crop_width;
+	u32 crop_height;
+	u32 out_format;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 7];
+	u32 err;
+};
+
+struct param_scaler_rotation {
+	u32 cmd;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 2];
+	u32 err;
+};
+
+struct param_scaler_flip {
+	u32 cmd;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 2];
+	u32 err;
+};
+
+struct param_3dnr_1stframe {
+	u32 cmd;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 2];
+	u32 err;
+};
+
+struct param_fd_config {
+	u32 cmd;
+	u32 max_number;
+	u32 roll_angle;
+	u32 yaw_angle;
+	u32 smile_mode;
+	u32 blink_mode;
+	u32 eye_detect;
+	u32 mouth_detect;
+	u32 orientation;
+	u32 orientation_value;
+	u32 reserved[FIMC_IS_PARAM_MAX_ENTRIES - 11];
+	u32 err;
+};
+
+struct global_param {
+	struct param_global_shotmode	shotmode;
+};
+
+struct sensor_param {
+	struct param_control		control;
+	struct param_otf_output		otf_output;
+	struct param_sensor_framerate	frame_rate;
+} __packed;
+
+struct buffer_param {
+	struct param_control		control;
+	struct param_otf_input		otf_input;
+	struct param_otf_output		otf_output;
+} __packed;
+
+struct isp_param {
+	struct param_control		control;
+	struct param_otf_input		otf_input;
+	struct param_dma_input		dma1_input;
+	struct param_dma_input		dma2_input;
+	struct param_isp_aa		aa;
+	struct param_isp_flash		flash;
+	struct param_isp_awb		awb;
+	struct param_isp_imageeffect	effect;
+	struct param_isp_iso		iso;
+	struct param_isp_adjust		adjust;
+	struct param_isp_metering	metering;
+	struct param_isp_afc		afc;
+	struct param_otf_output		otf_output;
+	struct param_dma_output		dma1_output;
+	struct param_dma_output		dma2_output;
+} __packed;
+
+struct drc_param {
+	struct param_control		control;
+	struct param_otf_input		otf_input;
+	struct param_dma_input		dma_input;
+	struct param_otf_output		otf_output;
+} __packed;
+
+struct scalerc_param {
+	struct param_control		control;
+	struct param_otf_input		otf_input;
+	struct param_scaler_imageeffect	effect;
+	struct param_scaler_input_crop	input_crop;
+	struct param_scaler_output_crop	output_crop;
+	struct param_otf_output		otf_output;
+	struct param_dma_output		dma_output;
+} __packed;
+
+struct odc_param {
+	struct param_control		control;
+	struct param_otf_input		otf_input;
+	struct param_otf_output		otf_output;
+} __packed;
+
+struct dis_param {
+	struct param_control		control;
+	struct param_otf_output		otf_input;
+	struct param_otf_output		otf_output;
+} __packed;
+
+struct tdnr_param {
+	struct param_control		control;
+	struct param_otf_input		otf_input;
+	struct param_3dnr_1stframe	frame;
+	struct param_otf_output		otf_output;
+	struct param_dma_output		dma_output;
+} __packed;
+
+struct scalerp_param {
+	struct param_control		control;
+	struct param_otf_input		otf_input;
+	struct param_scaler_imageeffect	effect;
+	struct param_scaler_input_crop	input_crop;
+	struct param_scaler_output_crop	output_crop;
+	struct param_scaler_rotation	rotation;
+	struct param_scaler_flip	flip;
+	struct param_otf_output		otf_output;
+	struct param_dma_output		dma_output;
+} __packed;
+
+struct fd_param {
+	struct param_control		control;
+	struct param_otf_input		otf_input;
+	struct param_dma_input		dma_input;
+	struct param_fd_config		config;
+} __packed;
+
+struct is_param_region {
+	struct global_param		global;
+	struct sensor_param		sensor;
+	struct buffer_param		buf;
+	struct isp_param		isp;
+	struct drc_param		drc;
+	struct scalerc_param		scalerc;
+	struct odc_param		odc;
+	struct dis_param		dis;
+	struct tdnr_param		tdnr;
+	struct scalerp_param		scalerp;
+	struct fd_param			fd;
+} __packed;
+
+#define NUMBER_OF_GAMMA_CURVE_POINTS	32
+
+struct is_tune_sensor {
+	u32 exposure;
+	u32 analog_gain;
+	u32 frame_rate;
+	u32 actuator_position;
+};
+
+struct is_tune_gammacurve {
+	u32 num_pts_x[NUMBER_OF_GAMMA_CURVE_POINTS];
+	u32 num_pts_y_r[NUMBER_OF_GAMMA_CURVE_POINTS];
+	u32 num_pts_y_g[NUMBER_OF_GAMMA_CURVE_POINTS];
+	u32 num_pts_y_b[NUMBER_OF_GAMMA_CURVE_POINTS];
+};
+
+struct is_tune_isp {
+	/* Brightness level: range 0...100, default 7. */
+	u32 brightness_level;
+	/* Contrast level: range -127...127, default 0. */
+	s32 contrast_level;
+	/* Saturation level: range -127...127, default 0. */
+	s32 saturation_level;
+	s32 gamma_level;
+	struct is_tune_gammacurve gamma_curve[4];
+	/* Hue: range -127...127, default 0. */
+	s32 hue;
+	/* Sharpness blur: range -127...127, default 0. */
+	s32 sharpness_blur;
+	/* Despeckle : range -127~127, default : 0 */
+	s32 despeckle;
+	/* Edge color supression: range -127...127, default 0. */
+	s32 edge_color_supression;
+	/* Noise reduction: range -127...127, default 0. */
+	s32 noise_reduction;
+	/* (32 * 4 + 9) * 4 = 548 bytes */
+} __packed;
+
+struct is_tune_region {
+	struct is_tune_sensor sensor;
+	struct is_tune_isp isp;
+} __packed;
+
+struct rational {
+	u32 num;
+	u32 den;
+};
+
+struct srational {
+	s32 num;
+	s32 den;
+};
+
+#define FLASH_FIRED_SHIFT			0
+#define FLASH_NOT_FIRED				0
+#define FLASH_FIRED				1
+
+#define FLASH_STROBE_SHIFT			1
+#define FLASH_STROBE_NO_DETECTION		0
+#define FLASH_STROBE_RESERVED			1
+#define FLASH_STROBE_RETURN_LIGHT_NOT_DETECTED	2
+#define FLASH_STROBE_RETURN_LIGHT_DETECTED	3
+
+#define FLASH_MODE_SHIFT			3
+#define FLASH_MODE_UNKNOWN			0
+#define FLASH_MODE_COMPULSORY_FLASH_FIRING	1
+#define FLASH_MODE_COMPULSORY_FLASH_SUPPRESSION	2
+#define FLASH_MODE_AUTO_MODE			3
+
+#define FLASH_FUNCTION_SHIFT			5
+#define FLASH_FUNCTION_PRESENT			0
+#define FLASH_FUNCTION_NONE			1
+
+#define FLASH_RED_EYE_SHIFT			6
+#define FLASH_RED_EYE_DISABLED			0
+#define FLASH_RED_EYE_SUPPORTED			1
+
+enum apex_aperture_value {
+	F1_0	= 0,
+	F1_4	= 1,
+	F2_0	= 2,
+	F2_8	= 3,
+	F4_0	= 4,
+	F5_6	= 5,
+	F8_9	= 6,
+	F11_0	= 7,
+	F16_0	= 8,
+	F22_0	= 9,
+	F32_0	= 10,
+};
+
+struct exif_attribute {
+	struct rational exposure_time;
+	struct srational shutter_speed;
+	u32 iso_speed_rating;
+	u32 flash;
+	struct srational brightness;
+} __packed;
+
+struct is_frame_header {
+	u32 valid;
+	u32 bad_mark;
+	u32 captured;
+	u32 frame_number;
+	struct exif_attribute exif;
+} __packed;
+
+struct is_fd_rect {
+	u32 offset_x;
+	u32 offset_y;
+	u32 width;
+	u32 height;
+};
+
+struct is_face_marker {
+	u32 frame_number;
+	struct is_fd_rect face;
+	struct is_fd_rect left_eye;
+	struct is_fd_rect right_eye;
+	struct is_fd_rect mouth;
+	u32 roll_angle;
+	u32 yaw_angle;
+	u32 confidence;
+	s32 smile_level;
+	s32 blink_level;
+} __packed;
+
+#define MAX_FRAME_COUNT				8
+#define MAX_FRAME_COUNT_PREVIEW			4
+#define MAX_FRAME_COUNT_CAPTURE			1
+#define MAX_FACE_COUNT				16
+#define MAX_SHARED_COUNT			500
+
+struct is_region {
+	struct is_param_region parameter;
+	struct is_tune_region tune;
+	struct is_frame_header header[MAX_FRAME_COUNT];
+	struct is_face_marker face[MAX_FACE_COUNT];
+	u32 shared[MAX_SHARED_COUNT];
+} __packed;
+
+/* Offset to the ISP DMA2 output buffer address array. */
+#define DMA2_OUTPUT_ADDR_ARRAY_OFFS \
+	(offsetof(struct is_region, shared) + 32 * sizeof(u32))
+
+struct is_debug_frame_descriptor {
+	u32 sensor_frame_time;
+	u32 sensor_exposure_time;
+	s32 sensor_analog_gain;
+	/* monitor for AA */
+	u32 req_lei;
+
+	u32 next_next_lei_exp;
+	u32 next_next_lei_a_gain;
+	u32 next_next_lei_d_gain;
+	u32 next_next_lei_statlei;
+	u32 next_next_lei_lei;
+
+	u32 dummy0;
+};
+
+#define MAX_FRAMEDESCRIPTOR_CONTEXT_NUM	(30*20)	/* 600 frames */
+#define MAX_VERSION_DISPLAY_BUF	32
+
+struct is_share_region {
+	u32 frame_time;
+	u32 exposure_time;
+	s32 analog_gain;
+
+	u32 r_gain;
+	u32 g_gain;
+	u32 b_gain;
+
+	u32 af_position;
+	u32 af_status;
+	/* 0 : SIRC_ISP_CAMERA_AUTOFOCUSMESSAGE_NOMESSAGE */
+	/* 1 : SIRC_ISP_CAMERA_AUTOFOCUSMESSAGE_REACHED */
+	/* 2 : SIRC_ISP_CAMERA_AUTOFOCUSMESSAGE_UNABLETOREACH */
+	/* 3 : SIRC_ISP_CAMERA_AUTOFOCUSMESSAGE_LOST */
+	/* default : unknown */
+	u32 af_scene_type;
+
+	u32 frame_descp_onoff_control;
+	u32 frame_descp_update_done;
+	u32 frame_descp_idx;
+	u32 frame_descp_max_idx;
+	struct is_debug_frame_descriptor
+		dbg_frame_descp_ctx[MAX_FRAMEDESCRIPTOR_CONTEXT_NUM];
+
+	u32 chip_id;
+	u32 chip_rev_no;
+	u8 isp_fw_ver_no[MAX_VERSION_DISPLAY_BUF];
+	u8 isp_fw_ver_date[MAX_VERSION_DISPLAY_BUF];
+	u8 sirc_sdk_ver_no[MAX_VERSION_DISPLAY_BUF];
+	u8 sirc_sdk_rev_no[MAX_VERSION_DISPLAY_BUF];
+	u8 sirc_sdk_rev_date[MAX_VERSION_DISPLAY_BUF];
+} __packed;
+
+struct is_debug_control {
+	u32 write_point;	/* 0~ 500KB boundary */
+	u32 assert_flag;	/* 0: Not invoked, 1: Invoked */
+	u32 pabort_flag;	/* 0: Not invoked, 1: Invoked */
+	u32 dabort_flag;	/* 0: Not invoked, 1: Invoked */
+};
+
+struct sensor_open_extended {
+	u32 actuator_type;
+	u32 mclk;
+	u32 mipi_lane_num;
+	u32 mipi_speed;
+	/* Skip setfile loading when fast_open_sensor is not 0 */
+	u32 fast_open_sensor;
+	/* Activating sensor self calibration mode (6A3) */
+	u32 self_calibration_mode;
+	/* This field is to adjust I2c clock based on ACLK200 */
+	/* This value is varied in case of rev 0.2 */
+	u32 i2c_sclk;
+};
+
+struct fimc_is;
+
+int fimc_is_hw_get_sensor_max_framerate(struct fimc_is *is);
+int __fimc_is_hw_update_param(struct fimc_is *is, u32 offset);
+void fimc_is_set_initial_params(struct fimc_is *is);
+unsigned int __get_pending_param_count(struct fimc_is *is);
+
+int  __is_hw_update_params(struct fimc_is *is);
+void __is_get_frame_size(struct fimc_is *is, struct v4l2_mbus_framefmt *mf);
+void __is_set_frame_size(struct fimc_is *is, struct v4l2_mbus_framefmt *mf);
+void __is_set_sensor(struct fimc_is *is, int fps);
+void __is_set_isp_aa_ae(struct fimc_is *is);
+void __is_set_isp_flash(struct fimc_is *is, u32 cmd, u32 redeye);
+void __is_set_isp_awb(struct fimc_is *is, u32 cmd, u32 val);
+void __is_set_isp_effect(struct fimc_is *is, u32 cmd);
+void __is_set_isp_iso(struct fimc_is *is, u32 cmd, u32 val);
+void __is_set_isp_adjust(struct fimc_is *is, u32 cmd, u32 val);
+void __is_set_isp_metering(struct fimc_is *is, u32 id, u32 val);
+void __is_set_isp_afc(struct fimc_is *is, u32 cmd, u32 val);
+void __is_set_drc_control(struct fimc_is *is, u32 val);
+void __is_set_fd_control(struct fimc_is *is, u32 val);
+void __is_set_fd_config_maxface(struct fimc_is *is, u32 val);
+void __is_set_fd_config_rollangle(struct fimc_is *is, u32 val);
+void __is_set_fd_config_yawangle(struct fimc_is *is, u32 val);
+void __is_set_fd_config_smilemode(struct fimc_is *is, u32 val);
+void __is_set_fd_config_blinkmode(struct fimc_is *is, u32 val);
+void __is_set_fd_config_eyedetect(struct fimc_is *is, u32 val);
+void __is_set_fd_config_mouthdetect(struct fimc_is *is, u32 val);
+void __is_set_fd_config_orientation(struct fimc_is *is, u32 val);
+void __is_set_fd_config_orientation_val(struct fimc_is *is, u32 val);
+void __is_set_isp_aa_af_mode(struct fimc_is *is, int cmd);
+void __is_set_isp_aa_af_start_stop(struct fimc_is *is, int cmd);
+
+#endif
diff --git a/drivers/media/platform/exynos4-is/fimc-is-regs.c b/drivers/media/platform/exynos4-is/fimc-is-regs.c
new file mode 100644
index 0000000..cfe4406
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is-regs.c
@@ -0,0 +1,233 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2012 - 2013 Samsung Electronics Co., Ltd.
+ *
+ * Authors: Younghwan Joo <yhwan.joo@samsung.com>
+ *          Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#include <linux/delay.h>
+
+#include "fimc-is.h"
+#include "fimc-is-command.h"
+#include "fimc-is-regs.h"
+#include "fimc-is-sensor.h"
+
+void fimc_is_fw_clear_irq1(struct fimc_is *is, unsigned int nr)
+{
+	mcuctl_write(1UL << nr, is, MCUCTL_REG_INTCR1);
+}
+
+void fimc_is_fw_clear_irq2(struct fimc_is *is)
+{
+	u32 cfg = mcuctl_read(is, MCUCTL_REG_INTSR2);
+	mcuctl_write(cfg, is, MCUCTL_REG_INTCR2);
+}
+
+void fimc_is_hw_set_intgr0_gd0(struct fimc_is *is)
+{
+	mcuctl_write(INTGR0_INTGD(0), is, MCUCTL_REG_INTGR0);
+}
+
+int fimc_is_hw_wait_intmsr0_intmsd0(struct fimc_is *is)
+{
+	unsigned int timeout = 2000;
+	u32 cfg, status;
+
+	do {
+		cfg = mcuctl_read(is, MCUCTL_REG_INTMSR0);
+		status = INTMSR0_GET_INTMSD(0, cfg);
+
+		if (--timeout == 0) {
+			dev_warn(&is->pdev->dev, "%s timeout\n",
+				 __func__);
+			return -ETIMEDOUT;
+		}
+		udelay(1);
+	} while (status != 0);
+
+	return 0;
+}
+
+int fimc_is_hw_set_param(struct fimc_is *is)
+{
+	struct chain_config *config = &is->config[is->config_index];
+	unsigned int param_count = __get_pending_param_count(is);
+
+	fimc_is_hw_wait_intmsr0_intmsd0(is);
+
+	mcuctl_write(HIC_SET_PARAMETER, is, MCUCTL_REG_ISSR(0));
+	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(1));
+	mcuctl_write(is->config_index, is, MCUCTL_REG_ISSR(2));
+
+	mcuctl_write(param_count, is, MCUCTL_REG_ISSR(3));
+	mcuctl_write(config->p_region_index[0], is, MCUCTL_REG_ISSR(4));
+	mcuctl_write(config->p_region_index[1], is, MCUCTL_REG_ISSR(5));
+
+	fimc_is_hw_set_intgr0_gd0(is);
+	return 0;
+}
+
+static int __maybe_unused fimc_is_hw_set_tune(struct fimc_is *is)
+{
+	fimc_is_hw_wait_intmsr0_intmsd0(is);
+
+	mcuctl_write(HIC_SET_TUNE, is, MCUCTL_REG_ISSR(0));
+	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(1));
+	mcuctl_write(is->h2i_cmd.entry_id, is, MCUCTL_REG_ISSR(2));
+
+	fimc_is_hw_set_intgr0_gd0(is);
+	return 0;
+}
+
+#define FIMC_IS_MAX_PARAMS	4
+
+int fimc_is_hw_get_params(struct fimc_is *is, unsigned int num_args)
+{
+	int i;
+
+	if (num_args > FIMC_IS_MAX_PARAMS)
+		return -EINVAL;
+
+	is->i2h_cmd.num_args = num_args;
+
+	for (i = 0; i < FIMC_IS_MAX_PARAMS; i++) {
+		if (i < num_args)
+			is->i2h_cmd.args[i] = mcuctl_read(is,
+					MCUCTL_REG_ISSR(12 + i));
+		else
+			is->i2h_cmd.args[i] = 0;
+	}
+	return 0;
+}
+
+void fimc_is_hw_set_isp_buf_mask(struct fimc_is *is, unsigned int mask)
+{
+	if (hweight32(mask) == 1) {
+		dev_err(&is->pdev->dev, "%s(): not enough buffers (mask %#x)\n",
+							__func__, mask);
+		return;
+	}
+
+	if (mcuctl_read(is, MCUCTL_REG_ISSR(23)) != 0)
+		dev_dbg(&is->pdev->dev, "non-zero DMA buffer mask\n");
+
+	mcuctl_write(mask, is, MCUCTL_REG_ISSR(23));
+}
+
+void fimc_is_hw_set_sensor_num(struct fimc_is *is)
+{
+	pr_debug("setting sensor index to: %d\n", is->sensor_index);
+
+	mcuctl_write(IH_REPLY_DONE, is, MCUCTL_REG_ISSR(0));
+	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(1));
+	mcuctl_write(IHC_GET_SENSOR_NUM, is, MCUCTL_REG_ISSR(2));
+	mcuctl_write(FIMC_IS_SENSORS_NUM, is, MCUCTL_REG_ISSR(3));
+}
+
+void fimc_is_hw_close_sensor(struct fimc_is *is, unsigned int index)
+{
+	if (is->sensor_index != index)
+		return;
+
+	fimc_is_hw_wait_intmsr0_intmsd0(is);
+	mcuctl_write(HIC_CLOSE_SENSOR, is, MCUCTL_REG_ISSR(0));
+	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(1));
+	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(2));
+	fimc_is_hw_set_intgr0_gd0(is);
+}
+
+void fimc_is_hw_get_setfile_addr(struct fimc_is *is)
+{
+	fimc_is_hw_wait_intmsr0_intmsd0(is);
+	mcuctl_write(HIC_GET_SET_FILE_ADDR, is, MCUCTL_REG_ISSR(0));
+	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(1));
+	fimc_is_hw_set_intgr0_gd0(is);
+}
+
+void fimc_is_hw_load_setfile(struct fimc_is *is)
+{
+	fimc_is_hw_wait_intmsr0_intmsd0(is);
+	mcuctl_write(HIC_LOAD_SET_FILE, is, MCUCTL_REG_ISSR(0));
+	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(1));
+	fimc_is_hw_set_intgr0_gd0(is);
+}
+
+int fimc_is_hw_change_mode(struct fimc_is *is)
+{
+	const u8 cmd[] = {
+		HIC_PREVIEW_STILL, HIC_PREVIEW_VIDEO,
+		HIC_CAPTURE_STILL, HIC_CAPTURE_VIDEO,
+	};
+
+	if (WARN_ON(is->config_index >= ARRAY_SIZE(cmd)))
+		return -EINVAL;
+
+	mcuctl_write(cmd[is->config_index], is, MCUCTL_REG_ISSR(0));
+	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(1));
+	mcuctl_write(is->setfile.sub_index, is, MCUCTL_REG_ISSR(2));
+	fimc_is_hw_set_intgr0_gd0(is);
+	return 0;
+}
+
+void fimc_is_hw_stream_on(struct fimc_is *is)
+{
+	fimc_is_hw_wait_intmsr0_intmsd0(is);
+	mcuctl_write(HIC_STREAM_ON, is, MCUCTL_REG_ISSR(0));
+	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(1));
+	mcuctl_write(0, is, MCUCTL_REG_ISSR(2));
+	fimc_is_hw_set_intgr0_gd0(is);
+}
+
+void fimc_is_hw_stream_off(struct fimc_is *is)
+{
+	fimc_is_hw_wait_intmsr0_intmsd0(is);
+	mcuctl_write(HIC_STREAM_OFF, is, MCUCTL_REG_ISSR(0));
+	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(1));
+	fimc_is_hw_set_intgr0_gd0(is);
+}
+
+void fimc_is_hw_subip_power_off(struct fimc_is *is)
+{
+	fimc_is_hw_wait_intmsr0_intmsd0(is);
+	mcuctl_write(HIC_POWER_DOWN, is, MCUCTL_REG_ISSR(0));
+	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(1));
+	fimc_is_hw_set_intgr0_gd0(is);
+}
+
+int fimc_is_itf_s_param(struct fimc_is *is, bool update)
+{
+	int ret;
+
+	if (update)
+		__is_hw_update_params(is);
+
+	fimc_is_mem_barrier();
+
+	clear_bit(IS_ST_BLOCK_CMD_CLEARED, &is->state);
+	fimc_is_hw_set_param(is);
+	ret = fimc_is_wait_event(is, IS_ST_BLOCK_CMD_CLEARED, 1,
+				FIMC_IS_CONFIG_TIMEOUT);
+	if (ret < 0)
+		dev_err(&is->pdev->dev, "%s() timeout\n", __func__);
+
+	return ret;
+}
+
+int fimc_is_itf_mode_change(struct fimc_is *is)
+{
+	int ret;
+
+	clear_bit(IS_ST_CHANGE_MODE, &is->state);
+	fimc_is_hw_change_mode(is);
+	ret = fimc_is_wait_event(is, IS_ST_CHANGE_MODE, 1,
+				FIMC_IS_CONFIG_TIMEOUT);
+	if (ret < 0)
+		dev_err(&is->pdev->dev, "%s(): mode change (%d) timeout\n",
+			__func__, is->config_index);
+	return ret;
+}
diff --git a/drivers/media/platform/exynos4-is/fimc-is-regs.h b/drivers/media/platform/exynos4-is/fimc-is-regs.h
new file mode 100644
index 0000000..141e5dd
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is-regs.h
@@ -0,0 +1,164 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ *
+ * Authors: Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *          Younghwan Joo <yhwan.joo@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#ifndef FIMC_IS_REG_H_
+#define FIMC_IS_REG_H_
+
+/* WDT_ISP register */
+#define REG_WDT_ISP			0x00170000
+
+/* MCUCTL registers base offset */
+#define MCUCTL_BASE			0x00180000
+
+/* MCU Controller Register */
+#define MCUCTL_REG_MCUCTRL		(MCUCTL_BASE + 0x00)
+#define MCUCTRL_MSWRST			(1 << 0)
+
+/* Boot Base Offset Address Register */
+#define MCUCTL_REG_BBOAR		(MCUCTL_BASE + 0x04)
+
+/* Interrupt Generation Register 0 from Host CPU to VIC */
+#define MCUCTL_REG_INTGR0		(MCUCTL_BASE + 0x08)
+/* __n = 0...9 */
+#define INTGR0_INTGC(__n)		(1 << ((__n) + 16))
+/* __n = 0...5 */
+#define INTGR0_INTGD(__n)		(1 << (__n))
+
+/* Interrupt Clear Register 0 from Host CPU to VIC */
+#define MCUCTL_REG_INTCR0		(MCUCTL_BASE + 0x0c)
+/* __n = 0...9 */
+#define INTCR0_INTGC(__n)		(1 << ((__n) + 16))
+/* __n = 0...5 */
+#define INTCR0_INTCD(__n)		(1 << ((__n) + 16))
+
+/* Interrupt Mask Register 0 from Host CPU to VIC */
+#define MCUCTL_REG_INTMR0		(MCUCTL_BASE + 0x10)
+/* __n = 0...9 */
+#define INTMR0_INTMC(__n)		(1 << ((__n) + 16))
+/* __n = 0...5 */
+#define INTMR0_INTMD(__n)		(1 << (__n))
+
+/* Interrupt Status Register 0 from Host CPU to VIC */
+#define MCUCTL_REG_INTSR0		(MCUCTL_BASE + 0x14)
+/* __n (bit number) = 0...4 */
+#define INTSR0_GET_INTSD(x, __n)	(((x) >> (__n)) & 0x1)
+/* __n (bit number) = 0...9 */
+#define INTSR0_GET_INTSC(x, __n)	(((x) >> ((__n) + 16)) & 0x1)
+
+/* Interrupt Mask Status Register 0 from Host CPU to VIC */
+#define MCUCTL_REG_INTMSR0		(MCUCTL_BASE + 0x18)
+/* __n (bit number) = 0...4 */
+#define INTMSR0_GET_INTMSD(x, __n)	(((x) >> (__n)) & 0x1)
+/* __n (bit number) = 0...9 */
+#define INTMSR0_GET_INTMSC(x, __n)	(((x) >> ((__n) + 16)) & 0x1)
+
+/* Interrupt Generation Register 1 from ISP CPU to Host IC */
+#define MCUCTL_REG_INTGR1		(MCUCTL_BASE + 0x1c)
+/* __n = 0...9 */
+#define INTGR1_INTGC(__n)		(1 << (__n))
+
+/* Interrupt Clear Register 1 from ISP CPU to Host IC */
+#define MCUCTL_REG_INTCR1		(MCUCTL_BASE + 0x20)
+/* __n = 0...9 */
+#define INTCR1_INTCC(__n)		(1 << (__n))
+
+/* Interrupt Mask Register 1 from ISP CPU to Host IC */
+#define MCUCTL_REG_INTMR1		(MCUCTL_BASE + 0x24)
+/* __n = 0...9 */
+#define INTMR1_INTMC(__n)		(1 << (__n))
+
+/* Interrupt Status Register 1 from ISP CPU to Host IC */
+#define MCUCTL_REG_INTSR1		(MCUCTL_BASE + 0x28)
+/* Interrupt Mask Status Register 1 from ISP CPU to Host IC */
+#define MCUCTL_REG_INTMSR1		(MCUCTL_BASE + 0x2c)
+
+/* Interrupt Clear Register 2 from ISP BLK's interrupts to Host IC */
+#define MCUCTL_REG_INTCR2		(MCUCTL_BASE + 0x30)
+/* __n = 0...5 */
+#define INTCR2_INTCC(__n)		(1 << ((__n) + 16))
+
+/* Interrupt Mask Register 2 from ISP BLK's interrupts to Host IC */
+#define MCUCTL_REG_INTMR2		(MCUCTL_BASE + 0x34)
+/* __n = 0...25 */
+#define INTMR2_INTMCIS(__n)		(1 << (__n))
+
+/* Interrupt Status Register 2 from ISP BLK's interrupts to Host IC */
+#define MCUCTL_REG_INTSR2		(MCUCTL_BASE + 0x38)
+/* Interrupt Mask Status Register 2 from ISP BLK's interrupts to Host IC */
+#define MCUCTL_REG_INTMSR2		(MCUCTL_BASE + 0x3c)
+
+/* General Purpose Output Control Register (0~17) */
+#define MCUCTL_REG_GPOCTLR		(MCUCTL_BASE + 0x40)
+/* __n = 0...17 */
+#define GPOCTLR_GPOG(__n)		(1 << (__n))
+
+/* General Purpose Pad Output Enable Register (0~17) */
+#define MCUCTL_REG_GPOENCTLR		(MCUCTL_BASE + 0x44)
+/* __n = 0...17 */
+#define GPOENCTLR_GPOEN(__n)		(1 << (__n))
+
+/* General Purpose Input Control Register (0~17) */
+#define MCUCTL_REG_GPICTLR		(MCUCTL_BASE + 0x48)
+
+/* Shared registers between ISP CPU and the host CPU - ISSRxx */
+
+/* ISSR(1): Command Host -> IS */
+/* ISSR(1): Sensor ID for Command, ISSR2...5 = Parameter 1...4 */
+
+/* ISSR(10): Reply IS -> Host */
+/* ISSR(11): Sensor ID for Reply, ISSR12...15 = Parameter 1...4 */
+
+/* ISSR(20): ISP_FRAME_DONE : SENSOR ID */
+/* ISSR(21): ISP_FRAME_DONE : PARAMETER 1 */
+
+/* ISSR(24): SCALERC_FRAME_DONE : SENSOR ID */
+/* ISSR(25): SCALERC_FRAME_DONE : PARAMETER 1 */
+
+/* ISSR(28): 3DNR_FRAME_DONE : SENSOR ID */
+/* ISSR(29): 3DNR_FRAME_DONE : PARAMETER 1 */
+
+/* ISSR(32): SCALERP_FRAME_DONE : SENSOR ID */
+/* ISSR(33): SCALERP_FRAME_DONE : PARAMETER 1 */
+
+/* __n = 0...63 */
+#define MCUCTL_REG_ISSR(__n)		(MCUCTL_BASE + 0x80 + ((__n) * 4))
+
+/* PMU ISP register offsets */
+#define REG_CMU_RESET_ISP_SYS_PWR_REG	0x1174
+#define REG_CMU_SYSCLK_ISP_SYS_PWR_REG	0x13b8
+#define REG_PMU_ISP_ARM_SYS		0x1050
+#define REG_PMU_ISP_ARM_CONFIGURATION	0x2280
+#define REG_PMU_ISP_ARM_STATUS		0x2284
+#define REG_PMU_ISP_ARM_OPTION		0x2288
+
+void fimc_is_fw_clear_irq1(struct fimc_is *is, unsigned int bit);
+void fimc_is_fw_clear_irq2(struct fimc_is *is);
+int fimc_is_hw_get_params(struct fimc_is *is, unsigned int num);
+
+void fimc_is_hw_set_intgr0_gd0(struct fimc_is *is);
+int fimc_is_hw_wait_intmsr0_intmsd0(struct fimc_is *is);
+void fimc_is_hw_set_sensor_num(struct fimc_is *is);
+void fimc_is_hw_set_isp_buf_mask(struct fimc_is *is, unsigned int mask);
+void fimc_is_hw_stream_on(struct fimc_is *is);
+void fimc_is_hw_stream_off(struct fimc_is *is);
+int fimc_is_hw_set_param(struct fimc_is *is);
+int fimc_is_hw_change_mode(struct fimc_is *is);
+
+void fimc_is_hw_close_sensor(struct fimc_is *is, unsigned int index);
+void fimc_is_hw_get_setfile_addr(struct fimc_is *is);
+void fimc_is_hw_load_setfile(struct fimc_is *is);
+void fimc_is_hw_subip_power_off(struct fimc_is *is);
+
+int fimc_is_itf_s_param(struct fimc_is *is, bool update);
+int fimc_is_itf_mode_change(struct fimc_is *is);
+
+#endif /* FIMC_IS_REG_H_ */
diff --git a/drivers/media/platform/exynos4-is/fimc-is-sensor.c b/drivers/media/platform/exynos4-is/fimc-is-sensor.c
new file mode 100644
index 0000000..10e82e2
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is-sensor.c
@@ -0,0 +1,34 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ * Author: Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include "fimc-is-sensor.h"
+
+static const struct sensor_drv_data s5k6a3_drvdata = {
+	.id		= FIMC_IS_SENSOR_ID_S5K6A3,
+	.open_timeout	= S5K6A3_OPEN_TIMEOUT,
+};
+
+static const struct of_device_id fimc_is_sensor_of_ids[] = {
+	{
+		.compatible	= "samsung,s5k6a3",
+		.data		= &s5k6a3_drvdata,
+	},
+	{  }
+};
+
+const struct sensor_drv_data *fimc_is_sensor_get_drvdata(
+			struct device_node *node)
+{
+	const struct of_device_id *of_id;
+
+	of_id = of_match_node(fimc_is_sensor_of_ids, node);
+	return of_id ? of_id->data : NULL;
+}
diff --git a/drivers/media/platform/exynos4-is/fimc-is-sensor.h b/drivers/media/platform/exynos4-is/fimc-is-sensor.h
new file mode 100644
index 0000000..173ccff
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is-sensor.h
@@ -0,0 +1,56 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ *
+ * Authors:  Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *	     Younghwan Joo <yhwan.joo@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#ifndef FIMC_IS_SENSOR_H_
+#define FIMC_IS_SENSOR_H_
+
+#include <linux/of.h>
+#include <linux/types.h>
+
+#define S5K6A3_OPEN_TIMEOUT		2000 /* ms */
+#define S5K6A3_SENSOR_WIDTH		1392
+#define S5K6A3_SENSOR_HEIGHT		1392
+
+enum fimc_is_sensor_id {
+	FIMC_IS_SENSOR_ID_S5K3H2 = 1,
+	FIMC_IS_SENSOR_ID_S5K6A3,
+	FIMC_IS_SENSOR_ID_S5K4E5,
+	FIMC_IS_SENSOR_ID_S5K3H7,
+	FIMC_IS_SENSOR_ID_CUSTOM,
+	FIMC_IS_SENSOR_ID_END
+};
+
+#define IS_SENSOR_CTRL_BUS_I2C0		0
+#define IS_SENSOR_CTRL_BUS_I2C1		1
+
+struct sensor_drv_data {
+	enum fimc_is_sensor_id id;
+	/* sensor open timeout in ms */
+	unsigned short open_timeout;
+};
+
+/**
+ * struct fimc_is_sensor - fimc-is sensor data structure
+ * @drvdata: a pointer to the sensor's parameters data structure
+ * @i2c_bus: ISP I2C bus index (0...1)
+ * @test_pattern: true to enable video test pattern
+ */
+struct fimc_is_sensor {
+	const struct sensor_drv_data *drvdata;
+	unsigned int i2c_bus;
+	u8 test_pattern;
+};
+
+const struct sensor_drv_data *fimc_is_sensor_get_drvdata(
+				struct device_node *node);
+
+#endif /* FIMC_IS_SENSOR_H_ */
diff --git a/drivers/media/platform/exynos4-is/fimc-is.c b/drivers/media/platform/exynos4-is/fimc-is.c
new file mode 100644
index 0000000..a851f20
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is.c
@@ -0,0 +1,999 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ *
+ * Authors: Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *          Younghwan Joo <yhwan.joo@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#define pr_fmt(fmt) "%s:%d " fmt, __func__, __LINE__
+
+#include <linux/device.h>
+#include <linux/debugfs.h>
+#include <linux/delay.h>
+#include <linux/dma-contiguous.h>
+#include <linux/errno.h>
+#include <linux/firmware.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/of_irq.h>
+#include <linux/of_address.h>
+#include <linux/of_graph.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <linux/videodev2.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "media-dev.h"
+#include "fimc-is.h"
+#include "fimc-is-command.h"
+#include "fimc-is-errno.h"
+#include "fimc-is-i2c.h"
+#include "fimc-is-param.h"
+#include "fimc-is-regs.h"
+
+
+static char *fimc_is_clocks[ISS_CLKS_MAX] = {
+	[ISS_CLK_PPMUISPX]		= "ppmuispx",
+	[ISS_CLK_PPMUISPMX]		= "ppmuispmx",
+	[ISS_CLK_LITE0]			= "lite0",
+	[ISS_CLK_LITE1]			= "lite1",
+	[ISS_CLK_MPLL]			= "mpll",
+	[ISS_CLK_ISP]			= "isp",
+	[ISS_CLK_DRC]			= "drc",
+	[ISS_CLK_FD]			= "fd",
+	[ISS_CLK_MCUISP]		= "mcuisp",
+	[ISS_CLK_UART]			= "uart",
+	[ISS_CLK_ISP_DIV0]		= "ispdiv0",
+	[ISS_CLK_ISP_DIV1]		= "ispdiv1",
+	[ISS_CLK_MCUISP_DIV0]		= "mcuispdiv0",
+	[ISS_CLK_MCUISP_DIV1]		= "mcuispdiv1",
+	[ISS_CLK_ACLK200]		= "aclk200",
+	[ISS_CLK_ACLK200_DIV]		= "div_aclk200",
+	[ISS_CLK_ACLK400MCUISP]		= "aclk400mcuisp",
+	[ISS_CLK_ACLK400MCUISP_DIV]	= "div_aclk400mcuisp",
+};
+
+static void fimc_is_put_clocks(struct fimc_is *is)
+{
+	int i;
+
+	for (i = 0; i < ISS_CLKS_MAX; i++) {
+		if (IS_ERR(is->clocks[i]))
+			continue;
+		clk_put(is->clocks[i]);
+		is->clocks[i] = ERR_PTR(-EINVAL);
+	}
+}
+
+static int fimc_is_get_clocks(struct fimc_is *is)
+{
+	int i, ret;
+
+	for (i = 0; i < ISS_CLKS_MAX; i++)
+		is->clocks[i] = ERR_PTR(-EINVAL);
+
+	for (i = 0; i < ISS_CLKS_MAX; i++) {
+		is->clocks[i] = clk_get(&is->pdev->dev, fimc_is_clocks[i]);
+		if (IS_ERR(is->clocks[i])) {
+			ret = PTR_ERR(is->clocks[i]);
+			goto err;
+		}
+	}
+
+	return 0;
+err:
+	fimc_is_put_clocks(is);
+	dev_err(&is->pdev->dev, "failed to get clock: %s\n",
+		fimc_is_clocks[i]);
+	return ret;
+}
+
+static int fimc_is_setup_clocks(struct fimc_is *is)
+{
+	int ret;
+
+	ret = clk_set_parent(is->clocks[ISS_CLK_ACLK200],
+					is->clocks[ISS_CLK_ACLK200_DIV]);
+	if (ret < 0)
+		return ret;
+
+	ret = clk_set_parent(is->clocks[ISS_CLK_ACLK400MCUISP],
+					is->clocks[ISS_CLK_ACLK400MCUISP_DIV]);
+	if (ret < 0)
+		return ret;
+
+	ret = clk_set_rate(is->clocks[ISS_CLK_ISP_DIV0], ACLK_AXI_FREQUENCY);
+	if (ret < 0)
+		return ret;
+
+	ret = clk_set_rate(is->clocks[ISS_CLK_ISP_DIV1], ACLK_AXI_FREQUENCY);
+	if (ret < 0)
+		return ret;
+
+	ret = clk_set_rate(is->clocks[ISS_CLK_MCUISP_DIV0],
+					ATCLK_MCUISP_FREQUENCY);
+	if (ret < 0)
+		return ret;
+
+	return clk_set_rate(is->clocks[ISS_CLK_MCUISP_DIV1],
+					ATCLK_MCUISP_FREQUENCY);
+}
+
+static int fimc_is_enable_clocks(struct fimc_is *is)
+{
+	int i, ret;
+
+	for (i = 0; i < ISS_GATE_CLKS_MAX; i++) {
+		if (IS_ERR(is->clocks[i]))
+			continue;
+		ret = clk_prepare_enable(is->clocks[i]);
+		if (ret < 0) {
+			dev_err(&is->pdev->dev, "clock %s enable failed\n",
+				fimc_is_clocks[i]);
+			for (--i; i >= 0; i--)
+				clk_disable(is->clocks[i]);
+			return ret;
+		}
+		pr_debug("enabled clock: %s\n", fimc_is_clocks[i]);
+	}
+	return 0;
+}
+
+static void fimc_is_disable_clocks(struct fimc_is *is)
+{
+	int i;
+
+	for (i = 0; i < ISS_GATE_CLKS_MAX; i++) {
+		if (!IS_ERR(is->clocks[i])) {
+			clk_disable_unprepare(is->clocks[i]);
+			pr_debug("disabled clock: %s\n", fimc_is_clocks[i]);
+		}
+	}
+}
+
+static int fimc_is_parse_sensor_config(struct fimc_is *is, unsigned int index,
+						struct device_node *node)
+{
+	struct fimc_is_sensor *sensor = &is->sensor[index];
+	u32 tmp = 0;
+	int ret;
+
+	sensor->drvdata = fimc_is_sensor_get_drvdata(node);
+	if (!sensor->drvdata) {
+		dev_err(&is->pdev->dev, "no driver data found for: %s\n",
+							 node->full_name);
+		return -EINVAL;
+	}
+
+	node = of_graph_get_next_endpoint(node, NULL);
+	if (!node)
+		return -ENXIO;
+
+	node = of_graph_get_remote_port(node);
+	if (!node)
+		return -ENXIO;
+
+	/* Use MIPI-CSIS channel id to determine the ISP I2C bus index. */
+	ret = of_property_read_u32(node, "reg", &tmp);
+	if (ret < 0) {
+		dev_err(&is->pdev->dev, "reg property not found at: %s\n",
+							 node->full_name);
+		return ret;
+	}
+
+	sensor->i2c_bus = tmp - FIMC_INPUT_MIPI_CSI2_0;
+	return 0;
+}
+
+static int fimc_is_register_subdevs(struct fimc_is *is)
+{
+	struct device_node *i2c_bus, *child;
+	int ret, index = 0;
+
+	ret = fimc_isp_subdev_create(&is->isp);
+	if (ret < 0)
+		return ret;
+
+	/* Initialize memory allocator context for the ISP DMA. */
+	is->isp.alloc_ctx = is->alloc_ctx;
+
+	for_each_compatible_node(i2c_bus, NULL, FIMC_IS_I2C_COMPATIBLE) {
+		for_each_available_child_of_node(i2c_bus, child) {
+			ret = fimc_is_parse_sensor_config(is, index, child);
+
+			if (ret < 0 || index >= FIMC_IS_SENSORS_NUM) {
+				of_node_put(child);
+				return ret;
+			}
+			index++;
+		}
+	}
+	return 0;
+}
+
+static int fimc_is_unregister_subdevs(struct fimc_is *is)
+{
+	fimc_isp_subdev_destroy(&is->isp);
+	return 0;
+}
+
+static int fimc_is_load_setfile(struct fimc_is *is, char *file_name)
+{
+	const struct firmware *fw;
+	void *buf;
+	int ret;
+
+	ret = request_firmware(&fw, file_name, &is->pdev->dev);
+	if (ret < 0) {
+		dev_err(&is->pdev->dev, "firmware request failed (%d)\n", ret);
+		return ret;
+	}
+	buf = is->memory.vaddr + is->setfile.base;
+	memcpy(buf, fw->data, fw->size);
+	fimc_is_mem_barrier();
+	is->setfile.size = fw->size;
+
+	pr_debug("mem vaddr: %p, setfile buf: %p\n", is->memory.vaddr, buf);
+
+	memcpy(is->fw.setfile_info,
+		fw->data + fw->size - FIMC_IS_SETFILE_INFO_LEN,
+		FIMC_IS_SETFILE_INFO_LEN - 1);
+
+	is->fw.setfile_info[FIMC_IS_SETFILE_INFO_LEN - 1] = '\0';
+	is->setfile.state = 1;
+
+	pr_debug("FIMC-IS setfile loaded: base: %#x, size: %zu B\n",
+		 is->setfile.base, fw->size);
+
+	release_firmware(fw);
+	return ret;
+}
+
+int fimc_is_cpu_set_power(struct fimc_is *is, int on)
+{
+	unsigned int timeout = FIMC_IS_POWER_ON_TIMEOUT;
+
+	if (on) {
+		/* Disable watchdog */
+		mcuctl_write(0, is, REG_WDT_ISP);
+
+		/* Cortex-A5 start address setting */
+		mcuctl_write(is->memory.paddr, is, MCUCTL_REG_BBOAR);
+
+		/* Enable and start Cortex-A5 */
+		pmuisp_write(0x18000, is, REG_PMU_ISP_ARM_OPTION);
+		pmuisp_write(0x1, is, REG_PMU_ISP_ARM_CONFIGURATION);
+	} else {
+		/* A5 power off */
+		pmuisp_write(0x10000, is, REG_PMU_ISP_ARM_OPTION);
+		pmuisp_write(0x0, is, REG_PMU_ISP_ARM_CONFIGURATION);
+
+		while (pmuisp_read(is, REG_PMU_ISP_ARM_STATUS) & 1) {
+			if (timeout == 0)
+				return -ETIME;
+			timeout--;
+			udelay(1);
+		}
+	}
+
+	return 0;
+}
+
+/* Wait until @bit of @is->state is set to @state in the interrupt handler. */
+int fimc_is_wait_event(struct fimc_is *is, unsigned long bit,
+		       unsigned int state, unsigned int timeout)
+{
+
+	int ret = wait_event_timeout(is->irq_queue,
+				     !state ^ test_bit(bit, &is->state),
+				     timeout);
+	if (ret == 0) {
+		dev_WARN(&is->pdev->dev, "%s() timed out\n", __func__);
+		return -ETIME;
+	}
+	return 0;
+}
+
+int fimc_is_start_firmware(struct fimc_is *is)
+{
+	struct device *dev = &is->pdev->dev;
+	int ret;
+
+	if (is->fw.f_w == NULL) {
+		dev_err(dev, "firmware is not loaded\n");
+		return -EINVAL;
+	}
+
+	memcpy(is->memory.vaddr, is->fw.f_w->data, is->fw.f_w->size);
+	wmb();
+
+	ret = fimc_is_cpu_set_power(is, 1);
+	if (ret < 0)
+		return ret;
+
+	ret = fimc_is_wait_event(is, IS_ST_A5_PWR_ON, 1,
+				 msecs_to_jiffies(FIMC_IS_FW_LOAD_TIMEOUT));
+	if (ret < 0)
+		dev_err(dev, "FIMC-IS CPU power on failed\n");
+
+	return ret;
+}
+
+/* Allocate working memory for the FIMC-IS CPU. */
+static int fimc_is_alloc_cpu_memory(struct fimc_is *is)
+{
+	struct device *dev = &is->pdev->dev;
+
+	is->memory.vaddr = dma_alloc_coherent(dev, FIMC_IS_CPU_MEM_SIZE,
+					      &is->memory.paddr, GFP_KERNEL);
+	if (is->memory.vaddr == NULL)
+		return -ENOMEM;
+
+	is->memory.size = FIMC_IS_CPU_MEM_SIZE;
+	memset(is->memory.vaddr, 0, is->memory.size);
+
+	dev_info(dev, "FIMC-IS CPU memory base: %#x\n", (u32)is->memory.paddr);
+
+	if (((u32)is->memory.paddr) & FIMC_IS_FW_ADDR_MASK) {
+		dev_err(dev, "invalid firmware memory alignment: %#x\n",
+			(u32)is->memory.paddr);
+		dma_free_coherent(dev, is->memory.size, is->memory.vaddr,
+				  is->memory.paddr);
+		return -EIO;
+	}
+
+	is->is_p_region = (struct is_region *)(is->memory.vaddr +
+				FIMC_IS_CPU_MEM_SIZE - FIMC_IS_REGION_SIZE);
+
+	is->is_dma_p_region = is->memory.paddr +
+				FIMC_IS_CPU_MEM_SIZE - FIMC_IS_REGION_SIZE;
+
+	is->is_shared_region = (struct is_share_region *)(is->memory.vaddr +
+				FIMC_IS_SHARED_REGION_OFFSET);
+	return 0;
+}
+
+static void fimc_is_free_cpu_memory(struct fimc_is *is)
+{
+	struct device *dev = &is->pdev->dev;
+
+	if (is->memory.vaddr == NULL)
+		return;
+
+	dma_free_coherent(dev, is->memory.size, is->memory.vaddr,
+			  is->memory.paddr);
+}
+
+static void fimc_is_load_firmware(const struct firmware *fw, void *context)
+{
+	struct fimc_is *is = context;
+	struct device *dev = &is->pdev->dev;
+	void *buf;
+	int ret;
+
+	if (fw == NULL) {
+		dev_err(dev, "firmware request failed\n");
+		return;
+	}
+	mutex_lock(&is->lock);
+
+	if (fw->size < FIMC_IS_FW_SIZE_MIN || fw->size > FIMC_IS_FW_SIZE_MAX) {
+		dev_err(dev, "wrong firmware size: %zu\n", fw->size);
+		goto done;
+	}
+
+	is->fw.size = fw->size;
+
+	ret = fimc_is_alloc_cpu_memory(is);
+	if (ret < 0) {
+		dev_err(dev, "failed to allocate FIMC-IS CPU memory\n");
+		goto done;
+	}
+
+	memcpy(is->memory.vaddr, fw->data, fw->size);
+	wmb();
+
+	/* Read firmware description. */
+	buf = (void *)(is->memory.vaddr + fw->size - FIMC_IS_FW_DESC_LEN);
+	memcpy(&is->fw.info, buf, FIMC_IS_FW_INFO_LEN);
+	is->fw.info[FIMC_IS_FW_INFO_LEN] = 0;
+
+	buf = (void *)(is->memory.vaddr + fw->size - FIMC_IS_FW_VER_LEN);
+	memcpy(&is->fw.version, buf, FIMC_IS_FW_VER_LEN);
+	is->fw.version[FIMC_IS_FW_VER_LEN - 1] = 0;
+
+	is->fw.state = 1;
+
+	dev_info(dev, "loaded firmware: %s, rev. %s\n",
+		 is->fw.info, is->fw.version);
+	dev_dbg(dev, "FW size: %zu, paddr: %pad\n", fw->size, &is->memory.paddr);
+
+	is->is_shared_region->chip_id = 0xe4412;
+	is->is_shared_region->chip_rev_no = 1;
+
+	fimc_is_mem_barrier();
+
+	/*
+	 * FIXME: The firmware is not being released for now, as it is
+	 * needed around for copying to the IS working memory every
+	 * time before the Cortex-A5 is restarted.
+	 */
+	release_firmware(is->fw.f_w);
+	is->fw.f_w = fw;
+done:
+	mutex_unlock(&is->lock);
+}
+
+static int fimc_is_request_firmware(struct fimc_is *is, const char *fw_name)
+{
+	return request_firmware_nowait(THIS_MODULE,
+				FW_ACTION_HOTPLUG, fw_name, &is->pdev->dev,
+				GFP_KERNEL, is, fimc_is_load_firmware);
+}
+
+/* General IS interrupt handler */
+static void fimc_is_general_irq_handler(struct fimc_is *is)
+{
+	is->i2h_cmd.cmd = mcuctl_read(is, MCUCTL_REG_ISSR(10));
+
+	switch (is->i2h_cmd.cmd) {
+	case IHC_GET_SENSOR_NUM:
+		fimc_is_hw_get_params(is, 1);
+		fimc_is_hw_wait_intmsr0_intmsd0(is);
+		fimc_is_hw_set_sensor_num(is);
+		pr_debug("ISP FW version: %#x\n", is->i2h_cmd.args[0]);
+		break;
+	case IHC_SET_FACE_MARK:
+	case IHC_FRAME_DONE:
+		fimc_is_hw_get_params(is, 2);
+		break;
+	case IHC_SET_SHOT_MARK:
+	case IHC_AA_DONE:
+	case IH_REPLY_DONE:
+		fimc_is_hw_get_params(is, 3);
+		break;
+	case IH_REPLY_NOT_DONE:
+		fimc_is_hw_get_params(is, 4);
+		break;
+	case IHC_NOT_READY:
+		break;
+	default:
+		pr_info("unknown command: %#x\n", is->i2h_cmd.cmd);
+	}
+
+	fimc_is_fw_clear_irq1(is, FIMC_IS_INT_GENERAL);
+
+	switch (is->i2h_cmd.cmd) {
+	case IHC_GET_SENSOR_NUM:
+		fimc_is_hw_set_intgr0_gd0(is);
+		set_bit(IS_ST_A5_PWR_ON, &is->state);
+		break;
+
+	case IHC_SET_SHOT_MARK:
+		break;
+
+	case IHC_SET_FACE_MARK:
+		is->fd_header.count = is->i2h_cmd.args[0];
+		is->fd_header.index = is->i2h_cmd.args[1];
+		is->fd_header.offset = 0;
+		break;
+
+	case IHC_FRAME_DONE:
+		break;
+
+	case IHC_AA_DONE:
+		pr_debug("AA_DONE - %d, %d, %d\n", is->i2h_cmd.args[0],
+			 is->i2h_cmd.args[1], is->i2h_cmd.args[2]);
+		break;
+
+	case IH_REPLY_DONE:
+		pr_debug("ISR_DONE: args[0]: %#x\n", is->i2h_cmd.args[0]);
+
+		switch (is->i2h_cmd.args[0]) {
+		case HIC_PREVIEW_STILL...HIC_CAPTURE_VIDEO:
+			/* Get CAC margin */
+			set_bit(IS_ST_CHANGE_MODE, &is->state);
+			is->isp.cac_margin_x = is->i2h_cmd.args[1];
+			is->isp.cac_margin_y = is->i2h_cmd.args[2];
+			pr_debug("CAC margin (x,y): (%d,%d)\n",
+				 is->isp.cac_margin_x, is->isp.cac_margin_y);
+			break;
+
+		case HIC_STREAM_ON:
+			clear_bit(IS_ST_STREAM_OFF, &is->state);
+			set_bit(IS_ST_STREAM_ON, &is->state);
+			break;
+
+		case HIC_STREAM_OFF:
+			clear_bit(IS_ST_STREAM_ON, &is->state);
+			set_bit(IS_ST_STREAM_OFF, &is->state);
+			break;
+
+		case HIC_SET_PARAMETER:
+			is->config[is->config_index].p_region_index[0] = 0;
+			is->config[is->config_index].p_region_index[1] = 0;
+			set_bit(IS_ST_BLOCK_CMD_CLEARED, &is->state);
+			pr_debug("HIC_SET_PARAMETER\n");
+			break;
+
+		case HIC_GET_PARAMETER:
+			break;
+
+		case HIC_SET_TUNE:
+			break;
+
+		case HIC_GET_STATUS:
+			break;
+
+		case HIC_OPEN_SENSOR:
+			set_bit(IS_ST_OPEN_SENSOR, &is->state);
+			pr_debug("data lanes: %d, settle line: %d\n",
+				 is->i2h_cmd.args[2], is->i2h_cmd.args[1]);
+			break;
+
+		case HIC_CLOSE_SENSOR:
+			clear_bit(IS_ST_OPEN_SENSOR, &is->state);
+			is->sensor_index = 0;
+			break;
+
+		case HIC_MSG_TEST:
+			pr_debug("config MSG level completed\n");
+			break;
+
+		case HIC_POWER_DOWN:
+			clear_bit(IS_ST_PWR_SUBIP_ON, &is->state);
+			break;
+
+		case HIC_GET_SET_FILE_ADDR:
+			is->setfile.base = is->i2h_cmd.args[1];
+			set_bit(IS_ST_SETFILE_LOADED, &is->state);
+			break;
+
+		case HIC_LOAD_SET_FILE:
+			set_bit(IS_ST_SETFILE_LOADED, &is->state);
+			break;
+		}
+		break;
+
+	case IH_REPLY_NOT_DONE:
+		pr_err("ISR_NDONE: %d: %#x, %s\n", is->i2h_cmd.args[0],
+		       is->i2h_cmd.args[1],
+		       fimc_is_strerr(is->i2h_cmd.args[1]));
+
+		if (is->i2h_cmd.args[1] & IS_ERROR_TIME_OUT_FLAG)
+			pr_err("IS_ERROR_TIME_OUT\n");
+
+		switch (is->i2h_cmd.args[1]) {
+		case IS_ERROR_SET_PARAMETER:
+			fimc_is_mem_barrier();
+		}
+
+		switch (is->i2h_cmd.args[0]) {
+		case HIC_SET_PARAMETER:
+			is->config[is->config_index].p_region_index[0] = 0;
+			is->config[is->config_index].p_region_index[1] = 0;
+			set_bit(IS_ST_BLOCK_CMD_CLEARED, &is->state);
+			break;
+		}
+		break;
+
+	case IHC_NOT_READY:
+		pr_err("IS control sequence error: Not Ready\n");
+		break;
+	}
+
+	wake_up(&is->irq_queue);
+}
+
+static irqreturn_t fimc_is_irq_handler(int irq, void *priv)
+{
+	struct fimc_is *is = priv;
+	unsigned long flags;
+	u32 status;
+
+	spin_lock_irqsave(&is->slock, flags);
+	status = mcuctl_read(is, MCUCTL_REG_INTSR1);
+
+	if (status & (1UL << FIMC_IS_INT_GENERAL))
+		fimc_is_general_irq_handler(is);
+
+	if (status & (1UL << FIMC_IS_INT_FRAME_DONE_ISP))
+		fimc_isp_irq_handler(is);
+
+	spin_unlock_irqrestore(&is->slock, flags);
+	return IRQ_HANDLED;
+}
+
+static int fimc_is_hw_open_sensor(struct fimc_is *is,
+				  struct fimc_is_sensor *sensor)
+{
+	struct sensor_open_extended *soe = (void *)&is->is_p_region->shared;
+
+	fimc_is_hw_wait_intmsr0_intmsd0(is);
+
+	soe->self_calibration_mode = 1;
+	soe->actuator_type = 0;
+	soe->mipi_lane_num = 0;
+	soe->mclk = 0;
+	soe->mipi_speed	= 0;
+	soe->fast_open_sensor = 0;
+	soe->i2c_sclk = 88000000;
+
+	fimc_is_mem_barrier();
+
+	mcuctl_write(HIC_OPEN_SENSOR, is, MCUCTL_REG_ISSR(0));
+	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(1));
+	mcuctl_write(sensor->drvdata->id, is, MCUCTL_REG_ISSR(2));
+	mcuctl_write(sensor->i2c_bus, is, MCUCTL_REG_ISSR(3));
+	mcuctl_write(is->is_dma_p_region, is, MCUCTL_REG_ISSR(4));
+
+	fimc_is_hw_set_intgr0_gd0(is);
+
+	return fimc_is_wait_event(is, IS_ST_OPEN_SENSOR, 1,
+				  sensor->drvdata->open_timeout);
+}
+
+
+int fimc_is_hw_initialize(struct fimc_is *is)
+{
+	const int config_ids[] = {
+		IS_SC_PREVIEW_STILL, IS_SC_PREVIEW_VIDEO,
+		IS_SC_CAPTURE_STILL, IS_SC_CAPTURE_VIDEO
+	};
+	struct device *dev = &is->pdev->dev;
+	u32 prev_id;
+	int i, ret;
+
+	/* Sensor initialization. Only one sensor is currently supported. */
+	ret = fimc_is_hw_open_sensor(is, &is->sensor[0]);
+	if (ret < 0)
+		return ret;
+
+	/* Get the setfile address. */
+	fimc_is_hw_get_setfile_addr(is);
+
+	ret = fimc_is_wait_event(is, IS_ST_SETFILE_LOADED, 1,
+				 FIMC_IS_CONFIG_TIMEOUT);
+	if (ret < 0) {
+		dev_err(dev, "get setfile address timed out\n");
+		return ret;
+	}
+	pr_debug("setfile.base: %#x\n", is->setfile.base);
+
+	/* Load the setfile. */
+	fimc_is_load_setfile(is, FIMC_IS_SETFILE_6A3);
+	clear_bit(IS_ST_SETFILE_LOADED, &is->state);
+	fimc_is_hw_load_setfile(is);
+	ret = fimc_is_wait_event(is, IS_ST_SETFILE_LOADED, 1,
+				 FIMC_IS_CONFIG_TIMEOUT);
+	if (ret < 0) {
+		dev_err(dev, "loading setfile timed out\n");
+		return ret;
+	}
+
+	pr_debug("setfile: base: %#x, size: %d\n",
+		 is->setfile.base, is->setfile.size);
+	pr_info("FIMC-IS Setfile info: %s\n", is->fw.setfile_info);
+
+	/* Check magic number. */
+	if (is->is_p_region->shared[MAX_SHARED_COUNT - 1] !=
+	    FIMC_IS_MAGIC_NUMBER) {
+		dev_err(dev, "magic number error!\n");
+		return -EIO;
+	}
+
+	pr_debug("shared region: %pad, parameter region: %pad\n",
+		 &is->memory.paddr + FIMC_IS_SHARED_REGION_OFFSET,
+		 &is->is_dma_p_region);
+
+	is->setfile.sub_index = 0;
+
+	/* Stream off. */
+	fimc_is_hw_stream_off(is);
+	ret = fimc_is_wait_event(is, IS_ST_STREAM_OFF, 1,
+				 FIMC_IS_CONFIG_TIMEOUT);
+	if (ret < 0) {
+		dev_err(dev, "stream off timeout\n");
+		return ret;
+	}
+
+	/* Preserve previous mode. */
+	prev_id = is->config_index;
+
+	/* Set initial parameter values. */
+	for (i = 0; i < ARRAY_SIZE(config_ids); i++) {
+		is->config_index = config_ids[i];
+		fimc_is_set_initial_params(is);
+		ret = fimc_is_itf_s_param(is, true);
+		if (ret < 0) {
+			is->config_index = prev_id;
+			return ret;
+		}
+	}
+	is->config_index = prev_id;
+
+	set_bit(IS_ST_INIT_DONE, &is->state);
+	dev_info(dev, "initialization sequence completed (%d)\n",
+						is->config_index);
+	return 0;
+}
+
+static int fimc_is_log_show(struct seq_file *s, void *data)
+{
+	struct fimc_is *is = s->private;
+	const u8 *buf = is->memory.vaddr + FIMC_IS_DEBUG_REGION_OFFSET;
+
+	if (is->memory.vaddr == NULL) {
+		dev_err(&is->pdev->dev, "firmware memory is not initialized\n");
+		return -EIO;
+	}
+
+	seq_printf(s, "%s\n", buf);
+	return 0;
+}
+
+static int fimc_is_debugfs_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, fimc_is_log_show, inode->i_private);
+}
+
+static const struct file_operations fimc_is_debugfs_fops = {
+	.open		= fimc_is_debugfs_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static void fimc_is_debugfs_remove(struct fimc_is *is)
+{
+	debugfs_remove_recursive(is->debugfs_entry);
+	is->debugfs_entry = NULL;
+}
+
+static int fimc_is_debugfs_create(struct fimc_is *is)
+{
+	struct dentry *dentry;
+
+	is->debugfs_entry = debugfs_create_dir("fimc_is", NULL);
+
+	dentry = debugfs_create_file("fw_log", S_IRUGO, is->debugfs_entry,
+				     is, &fimc_is_debugfs_fops);
+	if (!dentry)
+		fimc_is_debugfs_remove(is);
+
+	return is->debugfs_entry == NULL ? -EIO : 0;
+}
+
+static int fimc_is_runtime_resume(struct device *dev);
+static int fimc_is_runtime_suspend(struct device *dev);
+
+static int fimc_is_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct fimc_is *is;
+	struct resource res;
+	struct device_node *node;
+	int ret;
+
+	is = devm_kzalloc(&pdev->dev, sizeof(*is), GFP_KERNEL);
+	if (!is)
+		return -ENOMEM;
+
+	is->pdev = pdev;
+	is->isp.pdev = pdev;
+
+	init_waitqueue_head(&is->irq_queue);
+	spin_lock_init(&is->slock);
+	mutex_init(&is->lock);
+
+	ret = of_address_to_resource(dev->of_node, 0, &res);
+	if (ret < 0)
+		return ret;
+
+	is->regs = devm_ioremap_resource(dev, &res);
+	if (IS_ERR(is->regs))
+		return PTR_ERR(is->regs);
+
+	node = of_get_child_by_name(dev->of_node, "pmu");
+	if (!node)
+		return -ENODEV;
+
+	is->pmu_regs = of_iomap(node, 0);
+	if (!is->pmu_regs)
+		return -ENOMEM;
+
+	is->irq = irq_of_parse_and_map(dev->of_node, 0);
+	if (!is->irq) {
+		dev_err(dev, "no irq found\n");
+		ret = -EINVAL;
+		goto err_iounmap;
+	}
+
+	ret = fimc_is_get_clocks(is);
+	if (ret < 0)
+		goto err_iounmap;
+
+	platform_set_drvdata(pdev, is);
+
+	ret = request_irq(is->irq, fimc_is_irq_handler, 0, dev_name(dev), is);
+	if (ret < 0) {
+		dev_err(dev, "irq request failed\n");
+		goto err_clk;
+	}
+	pm_runtime_enable(dev);
+
+	if (!pm_runtime_enabled(dev)) {
+		ret = fimc_is_runtime_resume(dev);
+		if (ret < 0)
+			goto err_irq;
+	}
+
+	ret = pm_runtime_get_sync(dev);
+	if (ret < 0)
+		goto err_pm;
+
+	is->alloc_ctx = vb2_dma_contig_init_ctx(dev);
+	if (IS_ERR(is->alloc_ctx)) {
+		ret = PTR_ERR(is->alloc_ctx);
+		goto err_pm;
+	}
+	/*
+	 * Register FIMC-IS V4L2 subdevs to this driver. The video nodes
+	 * will be created within the subdev's registered() callback.
+	 */
+	ret = fimc_is_register_subdevs(is);
+	if (ret < 0)
+		goto err_vb;
+
+	ret = fimc_is_debugfs_create(is);
+	if (ret < 0)
+		goto err_sd;
+
+	ret = fimc_is_request_firmware(is, FIMC_IS_FW_FILENAME);
+	if (ret < 0)
+		goto err_dfs;
+
+	pm_runtime_put_sync(dev);
+
+	dev_dbg(dev, "FIMC-IS registered successfully\n");
+	return 0;
+
+err_dfs:
+	fimc_is_debugfs_remove(is);
+err_sd:
+	fimc_is_unregister_subdevs(is);
+err_vb:
+	vb2_dma_contig_cleanup_ctx(is->alloc_ctx);
+err_pm:
+	if (!pm_runtime_enabled(dev))
+		fimc_is_runtime_suspend(dev);
+err_irq:
+	free_irq(is->irq, is);
+err_clk:
+	fimc_is_put_clocks(is);
+err_iounmap:
+	iounmap(is->pmu_regs);
+	return ret;
+}
+
+static int fimc_is_runtime_resume(struct device *dev)
+{
+	struct fimc_is *is = dev_get_drvdata(dev);
+	int ret;
+
+	ret = fimc_is_setup_clocks(is);
+	if (ret)
+		return ret;
+
+	return fimc_is_enable_clocks(is);
+}
+
+static int fimc_is_runtime_suspend(struct device *dev)
+{
+	struct fimc_is *is = dev_get_drvdata(dev);
+
+	fimc_is_disable_clocks(is);
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int fimc_is_resume(struct device *dev)
+{
+	/* TODO: */
+	return 0;
+}
+
+static int fimc_is_suspend(struct device *dev)
+{
+	struct fimc_is *is = dev_get_drvdata(dev);
+
+	/* TODO: */
+	if (test_bit(IS_ST_A5_PWR_ON, &is->state))
+		return -EBUSY;
+
+	return 0;
+}
+#endif /* CONFIG_PM_SLEEP */
+
+static int fimc_is_remove(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct fimc_is *is = dev_get_drvdata(dev);
+
+	pm_runtime_disable(dev);
+	pm_runtime_set_suspended(dev);
+	if (!pm_runtime_status_suspended(dev))
+		fimc_is_runtime_suspend(dev);
+	free_irq(is->irq, is);
+	fimc_is_unregister_subdevs(is);
+	vb2_dma_contig_cleanup_ctx(is->alloc_ctx);
+	fimc_is_put_clocks(is);
+	iounmap(is->pmu_regs);
+	fimc_is_debugfs_remove(is);
+	release_firmware(is->fw.f_w);
+	fimc_is_free_cpu_memory(is);
+
+	return 0;
+}
+
+static const struct of_device_id fimc_is_of_match[] = {
+	{ .compatible = "samsung,exynos4212-fimc-is" },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, fimc_is_of_match);
+
+static const struct dev_pm_ops fimc_is_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(fimc_is_suspend, fimc_is_resume)
+	SET_RUNTIME_PM_OPS(fimc_is_runtime_suspend, fimc_is_runtime_resume,
+			   NULL)
+};
+
+static struct platform_driver fimc_is_driver = {
+	.probe		= fimc_is_probe,
+	.remove		= fimc_is_remove,
+	.driver = {
+		.of_match_table	= fimc_is_of_match,
+		.name		= FIMC_IS_DRV_NAME,
+		.pm		= &fimc_is_pm_ops,
+	}
+};
+
+static int fimc_is_module_init(void)
+{
+	int ret;
+
+	ret = fimc_is_register_i2c_driver();
+	if (ret < 0)
+		return ret;
+
+	ret = platform_driver_register(&fimc_is_driver);
+
+	if (ret < 0)
+		fimc_is_unregister_i2c_driver();
+
+	return ret;
+}
+
+static void fimc_is_module_exit(void)
+{
+	fimc_is_unregister_i2c_driver();
+	platform_driver_unregister(&fimc_is_driver);
+}
+
+module_init(fimc_is_module_init);
+module_exit(fimc_is_module_exit);
+
+MODULE_ALIAS("platform:" FIMC_IS_DRV_NAME);
+MODULE_AUTHOR("Younghwan Joo <yhwan.joo@samsung.com>");
+MODULE_AUTHOR("Sylwester Nawrocki <s.nawrocki@samsung.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/platform/exynos4-is/fimc-is.h b/drivers/media/platform/exynos4-is/fimc-is.h
new file mode 100644
index 0000000..386eb49
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-is.h
@@ -0,0 +1,344 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ *
+ * Authors: Younghwan Joo <yhwan.joo@samsung.com>
+ *          Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#ifndef FIMC_IS_H_
+#define FIMC_IS_H_
+
+#include <asm/barrier.h>
+#include <linux/clk.h>
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/platform_device.h>
+#include <linux/sizes.h>
+#include <linux/spinlock.h>
+#include <linux/types.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/v4l2-ctrls.h>
+
+#include "fimc-isp.h"
+#include "fimc-is-command.h"
+#include "fimc-is-sensor.h"
+#include "fimc-is-param.h"
+#include "fimc-is-regs.h"
+
+#define FIMC_IS_DRV_NAME		"exynos4-fimc-is"
+
+#define FIMC_IS_FW_FILENAME		"exynos4_fimc_is_fw.bin"
+#define FIMC_IS_SETFILE_6A3		"exynos4_s5k6a3_setfile.bin"
+
+#define FIMC_IS_FW_LOAD_TIMEOUT		1000 /* ms */
+#define FIMC_IS_POWER_ON_TIMEOUT	1000 /* us */
+
+#define FIMC_IS_SENSORS_NUM		2
+
+/* Memory definitions */
+#define FIMC_IS_CPU_MEM_SIZE		(0xa00000)
+#define FIMC_IS_CPU_BASE_MASK		((1 << 26) - 1)
+#define FIMC_IS_REGION_SIZE		0x5000
+
+#define FIMC_IS_DEBUG_REGION_OFFSET	0x0084b000
+#define FIMC_IS_SHARED_REGION_OFFSET	0x008c0000
+#define FIMC_IS_FW_INFO_LEN		31
+#define FIMC_IS_FW_VER_LEN		7
+#define FIMC_IS_FW_DESC_LEN		(FIMC_IS_FW_INFO_LEN + \
+					 FIMC_IS_FW_VER_LEN)
+#define FIMC_IS_SETFILE_INFO_LEN	39
+
+#define FIMC_IS_EXTRA_MEM_SIZE		(FIMC_IS_EXTRA_FW_SIZE + \
+					 FIMC_IS_EXTRA_SETFILE_SIZE + 0x1000)
+#define FIMC_IS_EXTRA_FW_SIZE		0x180000
+#define FIMC_IS_EXTRA_SETFILE_SIZE	0x4b000
+
+/* TODO: revisit */
+#define FIMC_IS_FW_ADDR_MASK		((1 << 26) - 1)
+#define FIMC_IS_FW_SIZE_MAX		(SZ_4M)
+#define FIMC_IS_FW_SIZE_MIN		(SZ_32K)
+
+#define ATCLK_MCUISP_FREQUENCY		100000000UL
+#define ACLK_AXI_FREQUENCY		100000000UL
+
+enum {
+	ISS_CLK_PPMUISPX,
+	ISS_CLK_PPMUISPMX,
+	ISS_CLK_LITE0,
+	ISS_CLK_LITE1,
+	ISS_CLK_MPLL,
+	ISS_CLK_ISP,
+	ISS_CLK_DRC,
+	ISS_CLK_FD,
+	ISS_CLK_MCUISP,
+	ISS_CLK_UART,
+	ISS_GATE_CLKS_MAX,
+	ISS_CLK_ISP_DIV0 = ISS_GATE_CLKS_MAX,
+	ISS_CLK_ISP_DIV1,
+	ISS_CLK_MCUISP_DIV0,
+	ISS_CLK_MCUISP_DIV1,
+	ISS_CLK_ACLK200,
+	ISS_CLK_ACLK200_DIV,
+	ISS_CLK_ACLK400MCUISP,
+	ISS_CLK_ACLK400MCUISP_DIV,
+	ISS_CLKS_MAX
+};
+
+/* The driver's internal state flags */
+enum {
+	IS_ST_IDLE,
+	IS_ST_PWR_ON,
+	IS_ST_A5_PWR_ON,
+	IS_ST_FW_LOADED,
+	IS_ST_OPEN_SENSOR,
+	IS_ST_SETFILE_LOADED,
+	IS_ST_INIT_DONE,
+	IS_ST_STREAM_ON,
+	IS_ST_STREAM_OFF,
+	IS_ST_CHANGE_MODE,
+	IS_ST_BLOCK_CMD_CLEARED,
+	IS_ST_SET_ZOOM,
+	IS_ST_PWR_SUBIP_ON,
+	IS_ST_END,
+};
+
+enum af_state {
+	FIMC_IS_AF_IDLE		= 0,
+	FIMC_IS_AF_SETCONFIG	= 1,
+	FIMC_IS_AF_RUNNING	= 2,
+	FIMC_IS_AF_LOCK		= 3,
+	FIMC_IS_AF_ABORT	= 4,
+	FIMC_IS_AF_FAILED	= 5,
+};
+
+enum af_lock_state {
+	FIMC_IS_AF_UNLOCKED	= 0,
+	FIMC_IS_AF_LOCKED	= 2
+};
+
+enum ae_lock_state {
+	FIMC_IS_AE_UNLOCKED	= 0,
+	FIMC_IS_AE_LOCKED	= 1
+};
+
+enum awb_lock_state {
+	FIMC_IS_AWB_UNLOCKED	= 0,
+	FIMC_IS_AWB_LOCKED	= 1
+};
+
+enum {
+	IS_METERING_CONFIG_CMD,
+	IS_METERING_CONFIG_WIN_POS_X,
+	IS_METERING_CONFIG_WIN_POS_Y,
+	IS_METERING_CONFIG_WIN_WIDTH,
+	IS_METERING_CONFIG_WIN_HEIGHT,
+	IS_METERING_CONFIG_MAX
+};
+
+struct is_setfile {
+	const struct firmware *info;
+	int state;
+	u32 sub_index;
+	u32 base;
+	size_t size;
+};
+
+struct is_fd_result_header {
+	u32 offset;
+	u32 count;
+	u32 index;
+	u32 curr_index;
+	u32 width;
+	u32 height;
+};
+
+struct is_af_info {
+	u16 mode;
+	u32 af_state;
+	u32 af_lock_state;
+	u32 ae_lock_state;
+	u32 awb_lock_state;
+	u16 pos_x;
+	u16 pos_y;
+	u16 prev_pos_x;
+	u16 prev_pos_y;
+	u16 use_af;
+};
+
+struct fimc_is_firmware {
+	const struct firmware *f_w;
+
+	dma_addr_t paddr;
+	void *vaddr;
+	unsigned int size;
+
+	char info[FIMC_IS_FW_INFO_LEN + 1];
+	char version[FIMC_IS_FW_VER_LEN + 1];
+	char setfile_info[FIMC_IS_SETFILE_INFO_LEN + 1];
+	u8 state;
+};
+
+struct fimc_is_memory {
+	/* physical base address */
+	dma_addr_t paddr;
+	/* virtual base address */
+	void *vaddr;
+	/* total length */
+	unsigned int size;
+};
+
+#define FIMC_IS_I2H_MAX_ARGS	12
+
+struct i2h_cmd {
+	u32 cmd;
+	u32 sensor_id;
+	u16 num_args;
+	u32 args[FIMC_IS_I2H_MAX_ARGS];
+};
+
+struct h2i_cmd {
+	u16 cmd_type;
+	u32 entry_id;
+};
+
+#define FIMC_IS_DEBUG_MSG	0x3f
+#define FIMC_IS_DEBUG_LEVEL	3
+
+struct fimc_is_setfile {
+	const struct firmware *info;
+	unsigned int state;
+	unsigned int size;
+	u32 sub_index;
+	u32 base;
+};
+
+struct chain_config {
+	struct global_param	global;
+	struct sensor_param	sensor;
+	struct isp_param	isp;
+	struct drc_param	drc;
+	struct fd_param		fd;
+
+	unsigned long		p_region_index[2];
+};
+
+/**
+ * struct fimc_is - fimc-is data structure
+ * @pdev: pointer to FIMC-IS platform device
+ * @pctrl: pointer to pinctrl structure for this device
+ * @v4l2_dev: pointer to top the level v4l2_device
+ * @alloc_ctx: videobuf2 memory allocator context
+ * @lock: mutex serializing video device and the subdev operations
+ * @slock: spinlock protecting this data structure and the hw registers
+ * @clocks: FIMC-LITE gate clock
+ * @regs: MCUCTL mmapped registers region
+ * @pmu_regs: PMU ISP mmapped registers region
+ * @irq_queue: interrupt handling waitqueue
+ * @lpm: low power mode flag
+ * @state: internal driver's state flags
+ */
+struct fimc_is {
+	struct platform_device		*pdev;
+	struct pinctrl			*pctrl;
+	struct v4l2_device		*v4l2_dev;
+
+	struct fimc_is_firmware		fw;
+	struct fimc_is_memory		memory;
+	struct firmware			*f_w;
+
+	struct fimc_isp			isp;
+	struct fimc_is_sensor		sensor[FIMC_IS_SENSORS_NUM];
+	struct fimc_is_setfile		setfile;
+
+	struct vb2_alloc_ctx		*alloc_ctx;
+	struct v4l2_ctrl_handler	ctrl_handler;
+
+	struct mutex			lock;
+	spinlock_t			slock;
+
+	struct clk			*clocks[ISS_CLKS_MAX];
+	void __iomem			*regs;
+	void __iomem			*pmu_regs;
+	int				irq;
+	wait_queue_head_t		irq_queue;
+	u8				lpm;
+
+	unsigned long			state;
+	unsigned int			sensor_index;
+
+	struct i2h_cmd			i2h_cmd;
+	struct h2i_cmd			h2i_cmd;
+	struct is_fd_result_header	fd_header;
+
+	struct chain_config		config[IS_SC_MAX];
+	unsigned			config_index;
+
+	struct is_region		*is_p_region;
+	dma_addr_t			is_dma_p_region;
+	struct is_share_region		*is_shared_region;
+	struct is_af_info		af;
+
+	struct dentry			*debugfs_entry;
+};
+
+static inline struct fimc_is *fimc_isp_to_is(struct fimc_isp *isp)
+{
+	return container_of(isp, struct fimc_is, isp);
+}
+
+static inline struct chain_config *__get_curr_is_config(struct fimc_is *is)
+{
+	return &is->config[is->config_index];
+}
+
+static inline void fimc_is_mem_barrier(void)
+{
+	mb();
+}
+
+static inline void fimc_is_set_param_bit(struct fimc_is *is, int num)
+{
+	struct chain_config *cfg = &is->config[is->config_index];
+
+	set_bit(num, &cfg->p_region_index[0]);
+}
+
+static inline void fimc_is_set_param_ctrl_cmd(struct fimc_is *is, int cmd)
+{
+	is->is_p_region->parameter.isp.control.cmd = cmd;
+}
+
+static inline void mcuctl_write(u32 v, struct fimc_is *is, unsigned int offset)
+{
+	writel(v, is->regs + offset);
+}
+
+static inline u32 mcuctl_read(struct fimc_is *is, unsigned int offset)
+{
+	return readl(is->regs + offset);
+}
+
+static inline void pmuisp_write(u32 v, struct fimc_is *is, unsigned int offset)
+{
+	writel(v, is->pmu_regs + offset);
+}
+
+static inline u32 pmuisp_read(struct fimc_is *is, unsigned int offset)
+{
+	return readl(is->pmu_regs + offset);
+}
+
+int fimc_is_wait_event(struct fimc_is *is, unsigned long bit,
+		       unsigned int state, unsigned int timeout);
+int fimc_is_cpu_set_power(struct fimc_is *is, int on);
+int fimc_is_start_firmware(struct fimc_is *is);
+int fimc_is_hw_initialize(struct fimc_is *is);
+void fimc_is_log_dump(const char *level, const void *buf, size_t len);
+
+#endif /* FIMC_IS_H_ */
diff --git a/drivers/media/platform/exynos4-is/fimc-isp-video.c b/drivers/media/platform/exynos4-is/fimc-isp-video.c
new file mode 100644
index 0000000..6e66484
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-isp-video.c
@@ -0,0 +1,660 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * FIMC-IS ISP video input and video output DMA interface driver
+ *
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ * Author: Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * The hardware handling code derived from a driver written by
+ * Younghwan Joo <yhwan.joo@samsung.com>.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/bitops.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/printk.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/videobuf2-dma-contig.h>
+#include <media/exynos-fimc.h>
+
+#include "common.h"
+#include "media-dev.h"
+#include "fimc-is.h"
+#include "fimc-isp-video.h"
+#include "fimc-is-param.h"
+
+static int isp_video_capture_queue_setup(struct vb2_queue *vq,
+			const void *parg,
+			unsigned int *num_buffers, unsigned int *num_planes,
+			unsigned int sizes[], void *allocators[])
+{
+	const struct v4l2_format *pfmt = parg;
+	struct fimc_isp *isp = vb2_get_drv_priv(vq);
+	struct v4l2_pix_format_mplane *vid_fmt = &isp->video_capture.pixfmt;
+	const struct v4l2_pix_format_mplane *pixm = NULL;
+	const struct fimc_fmt *fmt;
+	unsigned int wh, i;
+
+	if (pfmt) {
+		pixm = &pfmt->fmt.pix_mp;
+		fmt = fimc_isp_find_format(&pixm->pixelformat, NULL, -1);
+		wh = pixm->width * pixm->height;
+	} else {
+		fmt = isp->video_capture.format;
+		wh = vid_fmt->width * vid_fmt->height;
+	}
+
+	if (fmt == NULL)
+		return -EINVAL;
+
+	*num_buffers = clamp_t(u32, *num_buffers, FIMC_ISP_REQ_BUFS_MIN,
+						FIMC_ISP_REQ_BUFS_MAX);
+	*num_planes = fmt->memplanes;
+
+	for (i = 0; i < fmt->memplanes; i++) {
+		unsigned int size = (wh * fmt->depth[i]) / 8;
+		if (pixm)
+			sizes[i] = max(size, pixm->plane_fmt[i].sizeimage);
+		else
+			sizes[i] = size;
+		allocators[i] = isp->alloc_ctx;
+	}
+
+	return 0;
+}
+
+static inline struct param_dma_output *__get_isp_dma2(struct fimc_is *is)
+{
+	return &__get_curr_is_config(is)->isp.dma2_output;
+}
+
+static int isp_video_capture_start_streaming(struct vb2_queue *q,
+						unsigned int count)
+{
+	struct fimc_isp *isp = vb2_get_drv_priv(q);
+	struct fimc_is *is = fimc_isp_to_is(isp);
+	struct param_dma_output *dma = __get_isp_dma2(is);
+	struct fimc_is_video *video = &isp->video_capture;
+	int ret;
+
+	if (!test_bit(ST_ISP_VID_CAP_BUF_PREP, &isp->state) ||
+	    test_bit(ST_ISP_VID_CAP_STREAMING, &isp->state))
+		return 0;
+
+
+	dma->cmd = DMA_OUTPUT_COMMAND_ENABLE;
+	dma->notify_dma_done = DMA_OUTPUT_NOTIFY_DMA_DONE_ENABLE;
+	dma->buffer_address = is->is_dma_p_region +
+				DMA2_OUTPUT_ADDR_ARRAY_OFFS;
+	dma->buffer_number = video->reqbufs_count;
+	dma->dma_out_mask = video->buf_mask;
+
+	isp_dbg(2, &video->ve.vdev,
+		"buf_count: %d, planes: %d, dma addr table: %#x\n",
+		video->buf_count, video->format->memplanes,
+		dma->buffer_address);
+
+	fimc_is_mem_barrier();
+
+	fimc_is_set_param_bit(is, PARAM_ISP_DMA2_OUTPUT);
+	__fimc_is_hw_update_param(is, PARAM_ISP_DMA2_OUTPUT);
+
+	ret = fimc_is_itf_s_param(is, false);
+	if (ret < 0)
+		return ret;
+
+	ret = fimc_pipeline_call(&video->ve, set_stream, 1);
+	if (ret < 0)
+		return ret;
+
+	set_bit(ST_ISP_VID_CAP_STREAMING, &isp->state);
+	return ret;
+}
+
+static void isp_video_capture_stop_streaming(struct vb2_queue *q)
+{
+	struct fimc_isp *isp = vb2_get_drv_priv(q);
+	struct fimc_is *is = fimc_isp_to_is(isp);
+	struct param_dma_output *dma = __get_isp_dma2(is);
+	int ret;
+
+	ret = fimc_pipeline_call(&isp->video_capture.ve, set_stream, 0);
+	if (ret < 0)
+		return;
+
+	dma->cmd = DMA_OUTPUT_COMMAND_DISABLE;
+	dma->notify_dma_done = DMA_OUTPUT_NOTIFY_DMA_DONE_DISABLE;
+	dma->buffer_number = 0;
+	dma->buffer_address = 0;
+	dma->dma_out_mask = 0;
+
+	fimc_is_set_param_bit(is, PARAM_ISP_DMA2_OUTPUT);
+	__fimc_is_hw_update_param(is, PARAM_ISP_DMA2_OUTPUT);
+
+	ret = fimc_is_itf_s_param(is, false);
+	if (ret < 0)
+		dev_warn(&is->pdev->dev, "%s: DMA stop failed\n", __func__);
+
+	fimc_is_hw_set_isp_buf_mask(is, 0);
+
+	clear_bit(ST_ISP_VID_CAP_BUF_PREP, &isp->state);
+	clear_bit(ST_ISP_VID_CAP_STREAMING, &isp->state);
+
+	isp->video_capture.buf_count = 0;
+}
+
+static int isp_video_capture_buffer_prepare(struct vb2_buffer *vb)
+{
+	struct fimc_isp *isp = vb2_get_drv_priv(vb->vb2_queue);
+	struct fimc_is_video *video = &isp->video_capture;
+	int i;
+
+	if (video->format == NULL)
+		return -EINVAL;
+
+	for (i = 0; i < video->format->memplanes; i++) {
+		unsigned long size = video->pixfmt.plane_fmt[i].sizeimage;
+
+		if (vb2_plane_size(vb, i) < size) {
+			v4l2_err(&video->ve.vdev,
+				 "User buffer too small (%ld < %ld)\n",
+				 vb2_plane_size(vb, i), size);
+			return -EINVAL;
+		}
+		vb2_set_plane_payload(vb, i, size);
+	}
+
+	/* Check if we get one of the already known buffers. */
+	if (test_bit(ST_ISP_VID_CAP_BUF_PREP, &isp->state)) {
+		dma_addr_t dma_addr = vb2_dma_contig_plane_dma_addr(vb, 0);
+		int i;
+
+		for (i = 0; i < video->buf_count; i++)
+			if (video->buffers[i]->dma_addr[0] == dma_addr)
+				return 0;
+		return -ENXIO;
+	}
+
+	return 0;
+}
+
+static void isp_video_capture_buffer_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct fimc_isp *isp = vb2_get_drv_priv(vb->vb2_queue);
+	struct fimc_is_video *video = &isp->video_capture;
+	struct fimc_is *is = fimc_isp_to_is(isp);
+	struct isp_video_buf *ivb = to_isp_video_buf(vbuf);
+	unsigned long flags;
+	unsigned int i;
+
+	if (test_bit(ST_ISP_VID_CAP_BUF_PREP, &isp->state)) {
+		spin_lock_irqsave(&is->slock, flags);
+		video->buf_mask |= BIT(ivb->index);
+		spin_unlock_irqrestore(&is->slock, flags);
+	} else {
+		unsigned int num_planes = video->format->memplanes;
+
+		ivb->index = video->buf_count;
+		video->buffers[ivb->index] = ivb;
+
+		for (i = 0; i < num_planes; i++) {
+			int buf_index = ivb->index * num_planes + i;
+
+			ivb->dma_addr[i] = vb2_dma_contig_plane_dma_addr(vb, i);
+			is->is_p_region->shared[32 + buf_index] =
+							ivb->dma_addr[i];
+
+			isp_dbg(2, &video->ve.vdev,
+				"dma_buf %pad (%d/%d/%d) addr: %pad\n",
+				&buf_index, ivb->index, i, vb->index,
+				&ivb->dma_addr[i]);
+		}
+
+		if (++video->buf_count < video->reqbufs_count)
+			return;
+
+		video->buf_mask = (1UL << video->buf_count) - 1;
+		set_bit(ST_ISP_VID_CAP_BUF_PREP, &isp->state);
+	}
+
+	if (!test_bit(ST_ISP_VID_CAP_STREAMING, &isp->state))
+		isp_video_capture_start_streaming(vb->vb2_queue, 0);
+}
+
+/*
+ * FIMC-IS ISP input and output DMA interface interrupt handler.
+ * Locking: called with is->slock spinlock held.
+ */
+void fimc_isp_video_irq_handler(struct fimc_is *is)
+{
+	struct fimc_is_video *video = &is->isp.video_capture;
+	struct vb2_v4l2_buffer *vbuf;
+	int buf_index;
+
+	/* TODO: Ensure the DMA is really stopped in stop_streaming callback */
+	if (!test_bit(ST_ISP_VID_CAP_STREAMING, &is->isp.state))
+		return;
+
+	buf_index = (is->i2h_cmd.args[1] - 1) % video->buf_count;
+	vbuf = &video->buffers[buf_index]->vb;
+
+	v4l2_get_timestamp(&vbuf->timestamp);
+	vb2_buffer_done(&vbuf->vb2_buf, VB2_BUF_STATE_DONE);
+
+	video->buf_mask &= ~BIT(buf_index);
+	fimc_is_hw_set_isp_buf_mask(is, video->buf_mask);
+}
+
+static const struct vb2_ops isp_video_capture_qops = {
+	.queue_setup	 = isp_video_capture_queue_setup,
+	.buf_prepare	 = isp_video_capture_buffer_prepare,
+	.buf_queue	 = isp_video_capture_buffer_queue,
+	.wait_prepare	 = vb2_ops_wait_prepare,
+	.wait_finish	 = vb2_ops_wait_finish,
+	.start_streaming = isp_video_capture_start_streaming,
+	.stop_streaming	 = isp_video_capture_stop_streaming,
+};
+
+static int isp_video_open(struct file *file)
+{
+	struct fimc_isp *isp = video_drvdata(file);
+	struct exynos_video_entity *ve = &isp->video_capture.ve;
+	struct media_entity *me = &ve->vdev.entity;
+	int ret;
+
+	if (mutex_lock_interruptible(&isp->video_lock))
+		return -ERESTARTSYS;
+
+	ret = v4l2_fh_open(file);
+	if (ret < 0)
+		goto unlock;
+
+	ret = pm_runtime_get_sync(&isp->pdev->dev);
+	if (ret < 0)
+		goto rel_fh;
+
+	if (v4l2_fh_is_singular_file(file)) {
+		mutex_lock(&me->parent->graph_mutex);
+
+		ret = fimc_pipeline_call(ve, open, me, true);
+
+		/* Mark the video pipeline as in use. */
+		if (ret == 0)
+			me->use_count++;
+
+		mutex_unlock(&me->parent->graph_mutex);
+	}
+	if (!ret)
+		goto unlock;
+rel_fh:
+	v4l2_fh_release(file);
+unlock:
+	mutex_unlock(&isp->video_lock);
+	return ret;
+}
+
+static int isp_video_release(struct file *file)
+{
+	struct fimc_isp *isp = video_drvdata(file);
+	struct fimc_is_video *ivc = &isp->video_capture;
+	struct media_entity *entity = &ivc->ve.vdev.entity;
+	struct media_device *mdev = entity->parent;
+
+	mutex_lock(&isp->video_lock);
+
+	if (v4l2_fh_is_singular_file(file) && ivc->streaming) {
+		media_entity_pipeline_stop(entity);
+		ivc->streaming = 0;
+	}
+
+	vb2_fop_release(file);
+
+	if (v4l2_fh_is_singular_file(file)) {
+		fimc_pipeline_call(&ivc->ve, close);
+
+		mutex_lock(&mdev->graph_mutex);
+		entity->use_count--;
+		mutex_unlock(&mdev->graph_mutex);
+	}
+
+	pm_runtime_put(&isp->pdev->dev);
+	mutex_unlock(&isp->video_lock);
+
+	return 0;
+}
+
+static const struct v4l2_file_operations isp_video_fops = {
+	.owner		= THIS_MODULE,
+	.open		= isp_video_open,
+	.release	= isp_video_release,
+	.poll		= vb2_fop_poll,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap		= vb2_fop_mmap,
+};
+
+/*
+ * Video node ioctl operations
+ */
+static int isp_video_querycap(struct file *file, void *priv,
+					struct v4l2_capability *cap)
+{
+	struct fimc_isp *isp = video_drvdata(file);
+
+	__fimc_vidioc_querycap(&isp->pdev->dev, cap, V4L2_CAP_STREAMING);
+	return 0;
+}
+
+static int isp_video_enum_fmt_mplane(struct file *file, void *priv,
+					struct v4l2_fmtdesc *f)
+{
+	const struct fimc_fmt *fmt;
+
+	if (f->index >= FIMC_ISP_NUM_FORMATS)
+		return -EINVAL;
+
+	fmt = fimc_isp_find_format(NULL, NULL, f->index);
+	if (WARN_ON(fmt == NULL))
+		return -EINVAL;
+
+	strlcpy(f->description, fmt->name, sizeof(f->description));
+	f->pixelformat = fmt->fourcc;
+
+	return 0;
+}
+
+static int isp_video_g_fmt_mplane(struct file *file, void *fh,
+					struct v4l2_format *f)
+{
+	struct fimc_isp *isp = video_drvdata(file);
+
+	f->fmt.pix_mp = isp->video_capture.pixfmt;
+	return 0;
+}
+
+static void __isp_video_try_fmt(struct fimc_isp *isp,
+				struct v4l2_pix_format_mplane *pixm,
+				const struct fimc_fmt **fmt)
+{
+	*fmt = fimc_isp_find_format(&pixm->pixelformat, NULL, 2);
+
+	pixm->colorspace = V4L2_COLORSPACE_SRGB;
+	pixm->field = V4L2_FIELD_NONE;
+	pixm->num_planes = (*fmt)->memplanes;
+	pixm->pixelformat = (*fmt)->fourcc;
+	/*
+	 * TODO: double check with the docmentation these width/height
+	 * constraints are correct.
+	 */
+	v4l_bound_align_image(&pixm->width, FIMC_ISP_SOURCE_WIDTH_MIN,
+			      FIMC_ISP_SOURCE_WIDTH_MAX, 3,
+			      &pixm->height, FIMC_ISP_SOURCE_HEIGHT_MIN,
+			      FIMC_ISP_SOURCE_HEIGHT_MAX, 0, 0);
+}
+
+static int isp_video_try_fmt_mplane(struct file *file, void *fh,
+					struct v4l2_format *f)
+{
+	struct fimc_isp *isp = video_drvdata(file);
+
+	__isp_video_try_fmt(isp, &f->fmt.pix_mp, NULL);
+	return 0;
+}
+
+static int isp_video_s_fmt_mplane(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct fimc_isp *isp = video_drvdata(file);
+	struct fimc_is *is = fimc_isp_to_is(isp);
+	struct v4l2_pix_format_mplane *pixm = &f->fmt.pix_mp;
+	const struct fimc_fmt *ifmt = NULL;
+	struct param_dma_output *dma = __get_isp_dma2(is);
+
+	__isp_video_try_fmt(isp, pixm, &ifmt);
+
+	if (WARN_ON(ifmt == NULL))
+		return -EINVAL;
+
+	dma->format = DMA_OUTPUT_FORMAT_BAYER;
+	dma->order = DMA_OUTPUT_ORDER_GB_BG;
+	dma->plane = ifmt->memplanes;
+	dma->bitwidth = ifmt->depth[0];
+	dma->width = pixm->width;
+	dma->height = pixm->height;
+
+	fimc_is_mem_barrier();
+
+	isp->video_capture.format = ifmt;
+	isp->video_capture.pixfmt = *pixm;
+
+	return 0;
+}
+
+/*
+ * Check for source/sink format differences at each link.
+ * Return 0 if the formats match or -EPIPE otherwise.
+ */
+static int isp_video_pipeline_validate(struct fimc_isp *isp)
+{
+	struct v4l2_subdev *sd = &isp->subdev;
+	struct v4l2_subdev_format sink_fmt, src_fmt;
+	struct media_pad *pad;
+	int ret;
+
+	while (1) {
+		/* Retrieve format at the sink pad */
+		pad = &sd->entity.pads[0];
+		if (!(pad->flags & MEDIA_PAD_FL_SINK))
+			break;
+		sink_fmt.pad = pad->index;
+		sink_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+		ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &sink_fmt);
+		if (ret < 0 && ret != -ENOIOCTLCMD)
+			return -EPIPE;
+
+		/* Retrieve format at the source pad */
+		pad = media_entity_remote_pad(pad);
+		if (pad == NULL ||
+		    media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+			break;
+
+		sd = media_entity_to_v4l2_subdev(pad->entity);
+		src_fmt.pad = pad->index;
+		src_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+		ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &src_fmt);
+		if (ret < 0 && ret != -ENOIOCTLCMD)
+			return -EPIPE;
+
+		if (src_fmt.format.width != sink_fmt.format.width ||
+		    src_fmt.format.height != sink_fmt.format.height ||
+		    src_fmt.format.code != sink_fmt.format.code)
+			return -EPIPE;
+	}
+
+	return 0;
+}
+
+static int isp_video_streamon(struct file *file, void *priv,
+				      enum v4l2_buf_type type)
+{
+	struct fimc_isp *isp = video_drvdata(file);
+	struct exynos_video_entity *ve = &isp->video_capture.ve;
+	struct media_entity *me = &ve->vdev.entity;
+	int ret;
+
+	ret = media_entity_pipeline_start(me, &ve->pipe->mp);
+	if (ret < 0)
+		return ret;
+
+	ret = isp_video_pipeline_validate(isp);
+	if (ret < 0)
+		goto p_stop;
+
+	ret = vb2_ioctl_streamon(file, priv, type);
+	if (ret < 0)
+		goto p_stop;
+
+	isp->video_capture.streaming = 1;
+	return 0;
+p_stop:
+	media_entity_pipeline_stop(me);
+	return ret;
+}
+
+static int isp_video_streamoff(struct file *file, void *priv,
+					enum v4l2_buf_type type)
+{
+	struct fimc_isp *isp = video_drvdata(file);
+	struct fimc_is_video *video = &isp->video_capture;
+	int ret;
+
+	ret = vb2_ioctl_streamoff(file, priv, type);
+	if (ret < 0)
+		return ret;
+
+	media_entity_pipeline_stop(&video->ve.vdev.entity);
+	video->streaming = 0;
+	return 0;
+}
+
+static int isp_video_reqbufs(struct file *file, void *priv,
+				struct v4l2_requestbuffers *rb)
+{
+	struct fimc_isp *isp = video_drvdata(file);
+	int ret;
+
+	ret = vb2_ioctl_reqbufs(file, priv, rb);
+	if (ret < 0)
+		return ret;
+
+	if (rb->count && rb->count < FIMC_ISP_REQ_BUFS_MIN) {
+		rb->count = 0;
+		vb2_ioctl_reqbufs(file, priv, rb);
+		ret = -ENOMEM;
+	}
+
+	isp->video_capture.reqbufs_count = rb->count;
+	return ret;
+}
+
+static const struct v4l2_ioctl_ops isp_video_ioctl_ops = {
+	.vidioc_querycap		= isp_video_querycap,
+	.vidioc_enum_fmt_vid_cap_mplane	= isp_video_enum_fmt_mplane,
+	.vidioc_try_fmt_vid_cap_mplane	= isp_video_try_fmt_mplane,
+	.vidioc_s_fmt_vid_cap_mplane	= isp_video_s_fmt_mplane,
+	.vidioc_g_fmt_vid_cap_mplane	= isp_video_g_fmt_mplane,
+	.vidioc_reqbufs			= isp_video_reqbufs,
+	.vidioc_querybuf		= vb2_ioctl_querybuf,
+	.vidioc_prepare_buf		= vb2_ioctl_prepare_buf,
+	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
+	.vidioc_qbuf			= vb2_ioctl_qbuf,
+	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
+	.vidioc_streamon		= isp_video_streamon,
+	.vidioc_streamoff		= isp_video_streamoff,
+};
+
+int fimc_isp_video_device_register(struct fimc_isp *isp,
+				   struct v4l2_device *v4l2_dev,
+				   enum v4l2_buf_type type)
+{
+	struct vb2_queue *q = &isp->video_capture.vb_queue;
+	struct fimc_is_video *iv;
+	struct video_device *vdev;
+	int ret;
+
+	if (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		iv = &isp->video_capture;
+	else
+		return -ENOSYS;
+
+	mutex_init(&isp->video_lock);
+	INIT_LIST_HEAD(&iv->pending_buf_q);
+	INIT_LIST_HEAD(&iv->active_buf_q);
+	iv->format = fimc_isp_find_format(NULL, NULL, 0);
+	iv->pixfmt.width = IS_DEFAULT_WIDTH;
+	iv->pixfmt.height = IS_DEFAULT_HEIGHT;
+	iv->pixfmt.pixelformat = iv->format->fourcc;
+	iv->pixfmt.colorspace = V4L2_COLORSPACE_SRGB;
+	iv->reqbufs_count = 0;
+
+	memset(q, 0, sizeof(*q));
+	q->type = type;
+	q->io_modes = VB2_MMAP | VB2_USERPTR;
+	q->ops = &isp_video_capture_qops;
+	q->mem_ops = &vb2_dma_contig_memops;
+	q->buf_struct_size = sizeof(struct isp_video_buf);
+	q->drv_priv = isp;
+	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	q->lock = &isp->video_lock;
+
+	ret = vb2_queue_init(q);
+	if (ret < 0)
+		return ret;
+
+	vdev = &iv->ve.vdev;
+	memset(vdev, 0, sizeof(*vdev));
+	snprintf(vdev->name, sizeof(vdev->name), "fimc-is-isp.%s",
+			type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE ?
+			"capture" : "output");
+	vdev->queue = q;
+	vdev->fops = &isp_video_fops;
+	vdev->ioctl_ops = &isp_video_ioctl_ops;
+	vdev->v4l2_dev = v4l2_dev;
+	vdev->minor = -1;
+	vdev->release = video_device_release_empty;
+	vdev->lock = &isp->video_lock;
+
+	iv->pad.flags = MEDIA_PAD_FL_SINK;
+	ret = media_entity_init(&vdev->entity, 1, &iv->pad, 0);
+	if (ret < 0)
+		return ret;
+
+	video_set_drvdata(vdev, isp);
+
+	ret = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
+	if (ret < 0) {
+		media_entity_cleanup(&vdev->entity);
+		return ret;
+	}
+
+	v4l2_info(v4l2_dev, "Registered %s as /dev/%s\n",
+		  vdev->name, video_device_node_name(vdev));
+
+	return 0;
+}
+
+void fimc_isp_video_device_unregister(struct fimc_isp *isp,
+				      enum v4l2_buf_type type)
+{
+	struct exynos_video_entity *ve;
+
+	if (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		ve = &isp->video_capture.ve;
+	else
+		return;
+
+	mutex_lock(&isp->video_lock);
+
+	if (video_is_registered(&ve->vdev)) {
+		video_unregister_device(&ve->vdev);
+		media_entity_cleanup(&ve->vdev.entity);
+		ve->pipe = NULL;
+	}
+
+	mutex_unlock(&isp->video_lock);
+}
diff --git a/drivers/media/platform/exynos4-is/fimc-isp-video.h b/drivers/media/platform/exynos4-is/fimc-isp-video.h
new file mode 100644
index 0000000..f79a1b3
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-isp-video.h
@@ -0,0 +1,44 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ * Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#ifndef FIMC_ISP_VIDEO__
+#define FIMC_ISP_VIDEO__
+
+#include <media/videobuf2-v4l2.h>
+#include "fimc-isp.h"
+
+#ifdef CONFIG_VIDEO_EXYNOS4_ISP_DMA_CAPTURE
+int fimc_isp_video_device_register(struct fimc_isp *isp,
+				struct v4l2_device *v4l2_dev,
+				enum v4l2_buf_type type);
+
+void fimc_isp_video_device_unregister(struct fimc_isp *isp,
+				enum v4l2_buf_type type);
+
+void fimc_isp_video_irq_handler(struct fimc_is *is);
+#else
+static inline void fimc_isp_video_irq_handler(struct fimc_is *is)
+{
+}
+
+static inline int fimc_isp_video_device_register(struct fimc_isp *isp,
+						struct v4l2_device *v4l2_dev,
+						enum v4l2_buf_type type)
+{
+	return 0;
+}
+
+void fimc_isp_video_device_unregister(struct fimc_isp *isp,
+				enum v4l2_buf_type type)
+{
+}
+#endif /* !CONFIG_VIDEO_EXYNOS4_ISP_DMA_CAPTURE */
+
+#endif /* FIMC_ISP_VIDEO__ */
diff --git a/drivers/media/platform/exynos4-is/fimc-isp.c b/drivers/media/platform/exynos4-is/fimc-isp.c
new file mode 100644
index 0000000..5d78f57
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-isp.c
@@ -0,0 +1,786 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ *
+ * Authors: Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *          Younghwan Joo <yhwan.joo@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#define pr_fmt(fmt) "%s:%d " fmt, __func__, __LINE__
+
+#include <linux/device.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/printk.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <media/v4l2-device.h>
+
+#include "media-dev.h"
+#include "fimc-isp-video.h"
+#include "fimc-is-command.h"
+#include "fimc-is-param.h"
+#include "fimc-is-regs.h"
+#include "fimc-is.h"
+
+int fimc_isp_debug;
+module_param_named(debug_isp, fimc_isp_debug, int, S_IRUGO | S_IWUSR);
+
+static const struct fimc_fmt fimc_isp_formats[FIMC_ISP_NUM_FORMATS] = {
+	{
+		.name		= "RAW8 (GRBG)",
+		.fourcc		= V4L2_PIX_FMT_SGRBG8,
+		.depth		= { 8 },
+		.color		= FIMC_FMT_RAW8,
+		.memplanes	= 1,
+		.mbus_code	= MEDIA_BUS_FMT_SGRBG8_1X8,
+	}, {
+		.name		= "RAW10 (GRBG)",
+		.fourcc		= V4L2_PIX_FMT_SGRBG10,
+		.depth		= { 10 },
+		.color		= FIMC_FMT_RAW10,
+		.memplanes	= 1,
+		.mbus_code	= MEDIA_BUS_FMT_SGRBG10_1X10,
+	}, {
+		.name		= "RAW12 (GRBG)",
+		.fourcc		= V4L2_PIX_FMT_SGRBG12,
+		.depth		= { 12 },
+		.color		= FIMC_FMT_RAW12,
+		.memplanes	= 1,
+		.mbus_code	= MEDIA_BUS_FMT_SGRBG12_1X12,
+	},
+};
+
+/**
+ * fimc_isp_find_format - lookup color format by fourcc or media bus code
+ * @pixelformat: fourcc to match, ignored if null
+ * @mbus_code: media bus code to match, ignored if null
+ * @index: index to the fimc_isp_formats array, ignored if negative
+ */
+const struct fimc_fmt *fimc_isp_find_format(const u32 *pixelformat,
+					const u32 *mbus_code, int index)
+{
+	const struct fimc_fmt *fmt, *def_fmt = NULL;
+	unsigned int i;
+	int id = 0;
+
+	if (index >= (int)ARRAY_SIZE(fimc_isp_formats))
+		return NULL;
+
+	for (i = 0; i < ARRAY_SIZE(fimc_isp_formats); ++i) {
+		fmt = &fimc_isp_formats[i];
+		if (pixelformat && fmt->fourcc == *pixelformat)
+			return fmt;
+		if (mbus_code && fmt->mbus_code == *mbus_code)
+			return fmt;
+		if (index == id)
+			def_fmt = fmt;
+		id++;
+	}
+	return def_fmt;
+}
+
+void fimc_isp_irq_handler(struct fimc_is *is)
+{
+	is->i2h_cmd.args[0] = mcuctl_read(is, MCUCTL_REG_ISSR(20));
+	is->i2h_cmd.args[1] = mcuctl_read(is, MCUCTL_REG_ISSR(21));
+
+	fimc_is_fw_clear_irq1(is, FIMC_IS_INT_FRAME_DONE_ISP);
+	fimc_isp_video_irq_handler(is);
+
+	wake_up(&is->irq_queue);
+}
+
+/* Capture subdev media entity operations */
+static int fimc_is_link_setup(struct media_entity *entity,
+				const struct media_pad *local,
+				const struct media_pad *remote, u32 flags)
+{
+	return 0;
+}
+
+static const struct media_entity_operations fimc_is_subdev_media_ops = {
+	.link_setup = fimc_is_link_setup,
+};
+
+static int fimc_is_subdev_enum_mbus_code(struct v4l2_subdev *sd,
+				struct v4l2_subdev_pad_config *cfg,
+				struct v4l2_subdev_mbus_code_enum *code)
+{
+	const struct fimc_fmt *fmt;
+
+	fmt = fimc_isp_find_format(NULL, NULL, code->index);
+	if (!fmt)
+		return -EINVAL;
+	code->code = fmt->mbus_code;
+	return 0;
+}
+
+static int fimc_isp_subdev_get_fmt(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_pad_config *cfg,
+				   struct v4l2_subdev_format *fmt)
+{
+	struct fimc_isp *isp = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *mf = &fmt->format;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		*mf = *v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
+		return 0;
+	}
+
+	mf->colorspace = V4L2_COLORSPACE_SRGB;
+
+	mutex_lock(&isp->subdev_lock);
+
+	if (fmt->pad == FIMC_ISP_SD_PAD_SINK) {
+		/* ISP OTF input image format */
+		*mf = isp->sink_fmt;
+	} else {
+		/* ISP OTF output image format */
+		*mf = isp->src_fmt;
+
+		if (fmt->pad == FIMC_ISP_SD_PAD_SRC_FIFO) {
+			mf->colorspace = V4L2_COLORSPACE_JPEG;
+			mf->code = MEDIA_BUS_FMT_YUV10_1X30;
+		}
+	}
+
+	mutex_unlock(&isp->subdev_lock);
+
+	isp_dbg(1, sd, "%s: pad%d: fmt: 0x%x, %dx%d\n", __func__,
+		fmt->pad, mf->code, mf->width, mf->height);
+
+	return 0;
+}
+
+static void __isp_subdev_try_format(struct fimc_isp *isp,
+				    struct v4l2_subdev_pad_config *cfg,
+				    struct v4l2_subdev_format *fmt)
+{
+	struct v4l2_mbus_framefmt *mf = &fmt->format;
+	struct v4l2_mbus_framefmt *format;
+
+	mf->colorspace = V4L2_COLORSPACE_SRGB;
+
+	if (fmt->pad == FIMC_ISP_SD_PAD_SINK) {
+		v4l_bound_align_image(&mf->width, FIMC_ISP_SINK_WIDTH_MIN,
+				FIMC_ISP_SINK_WIDTH_MAX, 0,
+				&mf->height, FIMC_ISP_SINK_HEIGHT_MIN,
+				FIMC_ISP_SINK_HEIGHT_MAX, 0, 0);
+		mf->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	} else {
+		if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
+			format = v4l2_subdev_get_try_format(&isp->subdev, cfg,
+						FIMC_ISP_SD_PAD_SINK);
+		else
+			format = &isp->sink_fmt;
+
+		/* Allow changing format only on sink pad */
+		mf->width = format->width - FIMC_ISP_CAC_MARGIN_WIDTH;
+		mf->height = format->height - FIMC_ISP_CAC_MARGIN_HEIGHT;
+
+		if (fmt->pad == FIMC_ISP_SD_PAD_SRC_FIFO) {
+			mf->code = MEDIA_BUS_FMT_YUV10_1X30;
+			mf->colorspace = V4L2_COLORSPACE_JPEG;
+		} else {
+			mf->code = format->code;
+		}
+	}
+}
+
+static int fimc_isp_subdev_set_fmt(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_pad_config *cfg,
+				   struct v4l2_subdev_format *fmt)
+{
+	struct fimc_isp *isp = v4l2_get_subdevdata(sd);
+	struct fimc_is *is = fimc_isp_to_is(isp);
+	struct v4l2_mbus_framefmt *mf = &fmt->format;
+	int ret = 0;
+
+	isp_dbg(1, sd, "%s: pad%d: code: 0x%x, %dx%d\n",
+		 __func__, fmt->pad, mf->code, mf->width, mf->height);
+
+	mutex_lock(&isp->subdev_lock);
+	__isp_subdev_try_format(isp, cfg, fmt);
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		mf = v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
+		*mf = fmt->format;
+
+		/* Propagate format to the source pads */
+		if (fmt->pad == FIMC_ISP_SD_PAD_SINK) {
+			struct v4l2_subdev_format format = *fmt;
+			unsigned int pad;
+
+			for (pad = FIMC_ISP_SD_PAD_SRC_FIFO;
+					pad < FIMC_ISP_SD_PADS_NUM; pad++) {
+				format.pad = pad;
+				__isp_subdev_try_format(isp, cfg, &format);
+				mf = v4l2_subdev_get_try_format(sd, cfg, pad);
+				*mf = format.format;
+			}
+		}
+	} else {
+		if (sd->entity.stream_count == 0) {
+			if (fmt->pad == FIMC_ISP_SD_PAD_SINK) {
+				struct v4l2_subdev_format format = *fmt;
+
+				isp->sink_fmt = *mf;
+
+				format.pad = FIMC_ISP_SD_PAD_SRC_DMA;
+				__isp_subdev_try_format(isp, cfg, &format);
+
+				isp->src_fmt = format.format;
+				__is_set_frame_size(is, &isp->src_fmt);
+			} else {
+				isp->src_fmt = *mf;
+			}
+		} else {
+			ret = -EBUSY;
+		}
+	}
+
+	mutex_unlock(&isp->subdev_lock);
+	return ret;
+}
+
+static int fimc_isp_subdev_s_stream(struct v4l2_subdev *sd, int on)
+{
+	struct fimc_isp *isp = v4l2_get_subdevdata(sd);
+	struct fimc_is *is = fimc_isp_to_is(isp);
+	int ret;
+
+	isp_dbg(1, sd, "%s: on: %d\n", __func__, on);
+
+	if (!test_bit(IS_ST_INIT_DONE, &is->state))
+		return -EBUSY;
+
+	fimc_is_mem_barrier();
+
+	if (on) {
+		if (__get_pending_param_count(is)) {
+			ret = fimc_is_itf_s_param(is, true);
+			if (ret < 0)
+				return ret;
+		}
+
+		isp_dbg(1, sd, "changing mode to %d\n", is->config_index);
+
+		ret = fimc_is_itf_mode_change(is);
+		if (ret)
+			return -EINVAL;
+
+		clear_bit(IS_ST_STREAM_ON, &is->state);
+		fimc_is_hw_stream_on(is);
+		ret = fimc_is_wait_event(is, IS_ST_STREAM_ON, 1,
+					 FIMC_IS_CONFIG_TIMEOUT);
+		if (ret < 0) {
+			v4l2_err(sd, "stream on timeout\n");
+			return ret;
+		}
+	} else {
+		clear_bit(IS_ST_STREAM_OFF, &is->state);
+		fimc_is_hw_stream_off(is);
+		ret = fimc_is_wait_event(is, IS_ST_STREAM_OFF, 1,
+					 FIMC_IS_CONFIG_TIMEOUT);
+		if (ret < 0) {
+			v4l2_err(sd, "stream off timeout\n");
+			return ret;
+		}
+		is->setfile.sub_index = 0;
+	}
+
+	return 0;
+}
+
+static int fimc_isp_subdev_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct fimc_isp *isp = v4l2_get_subdevdata(sd);
+	struct fimc_is *is = fimc_isp_to_is(isp);
+	int ret = 0;
+
+	pr_debug("on: %d\n", on);
+
+	if (on) {
+		ret = pm_runtime_get_sync(&is->pdev->dev);
+		if (ret < 0)
+			return ret;
+		set_bit(IS_ST_PWR_ON, &is->state);
+
+		ret = fimc_is_start_firmware(is);
+		if (ret < 0) {
+			v4l2_err(sd, "firmware booting failed\n");
+			pm_runtime_put(&is->pdev->dev);
+			return ret;
+		}
+		set_bit(IS_ST_PWR_SUBIP_ON, &is->state);
+
+		ret = fimc_is_hw_initialize(is);
+	} else {
+		/* Close sensor */
+		if (!test_bit(IS_ST_PWR_ON, &is->state)) {
+			fimc_is_hw_close_sensor(is, 0);
+
+			ret = fimc_is_wait_event(is, IS_ST_OPEN_SENSOR, 0,
+						 FIMC_IS_CONFIG_TIMEOUT);
+			if (ret < 0) {
+				v4l2_err(sd, "sensor close timeout\n");
+				return ret;
+			}
+		}
+
+		/* SUB IP power off */
+		if (test_bit(IS_ST_PWR_SUBIP_ON, &is->state)) {
+			fimc_is_hw_subip_power_off(is);
+			ret = fimc_is_wait_event(is, IS_ST_PWR_SUBIP_ON, 0,
+						 FIMC_IS_CONFIG_TIMEOUT);
+			if (ret < 0) {
+				v4l2_err(sd, "sub-IP power off timeout\n");
+				return ret;
+			}
+		}
+
+		fimc_is_cpu_set_power(is, 0);
+		pm_runtime_put_sync(&is->pdev->dev);
+
+		clear_bit(IS_ST_PWR_ON, &is->state);
+		clear_bit(IS_ST_INIT_DONE, &is->state);
+		is->state = 0;
+		is->config[is->config_index].p_region_index[0] = 0;
+		is->config[is->config_index].p_region_index[1] = 0;
+		set_bit(IS_ST_IDLE, &is->state);
+		wmb();
+	}
+
+	return ret;
+}
+
+static int fimc_isp_subdev_open(struct v4l2_subdev *sd,
+				struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_mbus_framefmt fmt;
+	struct v4l2_mbus_framefmt *format;
+
+	format = v4l2_subdev_get_try_format(sd, fh->pad, FIMC_ISP_SD_PAD_SINK);
+
+	fmt.colorspace = V4L2_COLORSPACE_SRGB;
+	fmt.code = fimc_isp_formats[0].mbus_code;
+	fmt.width = DEFAULT_PREVIEW_STILL_WIDTH + FIMC_ISP_CAC_MARGIN_WIDTH;
+	fmt.height = DEFAULT_PREVIEW_STILL_HEIGHT + FIMC_ISP_CAC_MARGIN_HEIGHT;
+	fmt.field = V4L2_FIELD_NONE;
+	*format = fmt;
+
+	format = v4l2_subdev_get_try_format(sd, fh->pad, FIMC_ISP_SD_PAD_SRC_FIFO);
+	fmt.width = DEFAULT_PREVIEW_STILL_WIDTH;
+	fmt.height = DEFAULT_PREVIEW_STILL_HEIGHT;
+	*format = fmt;
+
+	format = v4l2_subdev_get_try_format(sd, fh->pad, FIMC_ISP_SD_PAD_SRC_DMA);
+	*format = fmt;
+
+	return 0;
+}
+
+static int fimc_isp_subdev_registered(struct v4l2_subdev *sd)
+{
+	struct fimc_isp *isp = v4l2_get_subdevdata(sd);
+	int ret;
+
+	/* Use pipeline object allocated by the media device. */
+	isp->video_capture.ve.pipe = v4l2_get_subdev_hostdata(sd);
+
+	ret = fimc_isp_video_device_register(isp, sd->v4l2_dev,
+			V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+	if (ret < 0)
+		isp->video_capture.ve.pipe = NULL;
+
+	return ret;
+}
+
+static void fimc_isp_subdev_unregistered(struct v4l2_subdev *sd)
+{
+	struct fimc_isp *isp = v4l2_get_subdevdata(sd);
+
+	fimc_isp_video_device_unregister(isp,
+			V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+}
+
+static const struct v4l2_subdev_internal_ops fimc_is_subdev_internal_ops = {
+	.registered = fimc_isp_subdev_registered,
+	.unregistered = fimc_isp_subdev_unregistered,
+	.open = fimc_isp_subdev_open,
+};
+
+static const struct v4l2_subdev_pad_ops fimc_is_subdev_pad_ops = {
+	.enum_mbus_code = fimc_is_subdev_enum_mbus_code,
+	.get_fmt = fimc_isp_subdev_get_fmt,
+	.set_fmt = fimc_isp_subdev_set_fmt,
+};
+
+static const struct v4l2_subdev_video_ops fimc_is_subdev_video_ops = {
+	.s_stream = fimc_isp_subdev_s_stream,
+};
+
+static const struct v4l2_subdev_core_ops fimc_is_core_ops = {
+	.s_power = fimc_isp_subdev_s_power,
+};
+
+static struct v4l2_subdev_ops fimc_is_subdev_ops = {
+	.core = &fimc_is_core_ops,
+	.video = &fimc_is_subdev_video_ops,
+	.pad = &fimc_is_subdev_pad_ops,
+};
+
+static int __ctrl_set_white_balance(struct fimc_is *is, int value)
+{
+	switch (value) {
+	case V4L2_WHITE_BALANCE_AUTO:
+		__is_set_isp_awb(is, ISP_AWB_COMMAND_AUTO, 0);
+		break;
+	case V4L2_WHITE_BALANCE_DAYLIGHT:
+		__is_set_isp_awb(is, ISP_AWB_COMMAND_ILLUMINATION,
+					ISP_AWB_ILLUMINATION_DAYLIGHT);
+		break;
+	case V4L2_WHITE_BALANCE_CLOUDY:
+		__is_set_isp_awb(is, ISP_AWB_COMMAND_ILLUMINATION,
+					ISP_AWB_ILLUMINATION_CLOUDY);
+		break;
+	case V4L2_WHITE_BALANCE_INCANDESCENT:
+		__is_set_isp_awb(is, ISP_AWB_COMMAND_ILLUMINATION,
+					ISP_AWB_ILLUMINATION_TUNGSTEN);
+		break;
+	case V4L2_WHITE_BALANCE_FLUORESCENT:
+		__is_set_isp_awb(is, ISP_AWB_COMMAND_ILLUMINATION,
+					ISP_AWB_ILLUMINATION_FLUORESCENT);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int __ctrl_set_aewb_lock(struct fimc_is *is,
+				      struct v4l2_ctrl *ctrl)
+{
+	bool awb_lock = ctrl->val & V4L2_LOCK_WHITE_BALANCE;
+	bool ae_lock = ctrl->val & V4L2_LOCK_EXPOSURE;
+	struct isp_param *isp = &is->is_p_region->parameter.isp;
+	int cmd, ret;
+
+	cmd = ae_lock ? ISP_AA_COMMAND_STOP : ISP_AA_COMMAND_START;
+	isp->aa.cmd = cmd;
+	isp->aa.target = ISP_AA_TARGET_AE;
+	fimc_is_set_param_bit(is, PARAM_ISP_AA);
+	is->af.ae_lock_state = ae_lock;
+	wmb();
+
+	ret = fimc_is_itf_s_param(is, false);
+	if (ret < 0)
+		return ret;
+
+	cmd = awb_lock ? ISP_AA_COMMAND_STOP : ISP_AA_COMMAND_START;
+	isp->aa.cmd = cmd;
+	isp->aa.target = ISP_AA_TARGET_AE;
+	fimc_is_set_param_bit(is, PARAM_ISP_AA);
+	is->af.awb_lock_state = awb_lock;
+	wmb();
+
+	return fimc_is_itf_s_param(is, false);
+}
+
+/* Supported manual ISO values */
+static const s64 iso_qmenu[] = {
+	50, 100, 200, 400, 800,
+};
+
+static int __ctrl_set_iso(struct fimc_is *is, int value)
+{
+	unsigned int idx, iso;
+
+	if (value == V4L2_ISO_SENSITIVITY_AUTO) {
+		__is_set_isp_iso(is, ISP_ISO_COMMAND_AUTO, 0);
+		return 0;
+	}
+	idx = is->isp.ctrls.iso->val;
+	if (idx >= ARRAY_SIZE(iso_qmenu))
+		return -EINVAL;
+
+	iso = iso_qmenu[idx];
+	__is_set_isp_iso(is, ISP_ISO_COMMAND_MANUAL, iso);
+	return 0;
+}
+
+static int __ctrl_set_metering(struct fimc_is *is, unsigned int value)
+{
+	unsigned int val;
+
+	switch (value) {
+	case V4L2_EXPOSURE_METERING_AVERAGE:
+		val = ISP_METERING_COMMAND_AVERAGE;
+		break;
+	case V4L2_EXPOSURE_METERING_CENTER_WEIGHTED:
+		val = ISP_METERING_COMMAND_CENTER;
+		break;
+	case V4L2_EXPOSURE_METERING_SPOT:
+		val = ISP_METERING_COMMAND_SPOT;
+		break;
+	case V4L2_EXPOSURE_METERING_MATRIX:
+		val = ISP_METERING_COMMAND_MATRIX;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	__is_set_isp_metering(is, IS_METERING_CONFIG_CMD, val);
+	return 0;
+}
+
+static int __ctrl_set_afc(struct fimc_is *is, int value)
+{
+	switch (value) {
+	case V4L2_CID_POWER_LINE_FREQUENCY_DISABLED:
+		__is_set_isp_afc(is, ISP_AFC_COMMAND_DISABLE, 0);
+		break;
+	case V4L2_CID_POWER_LINE_FREQUENCY_50HZ:
+		__is_set_isp_afc(is, ISP_AFC_COMMAND_MANUAL, 50);
+		break;
+	case V4L2_CID_POWER_LINE_FREQUENCY_60HZ:
+		__is_set_isp_afc(is, ISP_AFC_COMMAND_MANUAL, 60);
+		break;
+	case V4L2_CID_POWER_LINE_FREQUENCY_AUTO:
+		__is_set_isp_afc(is, ISP_AFC_COMMAND_AUTO, 0);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int __ctrl_set_image_effect(struct fimc_is *is, int value)
+{
+	static const u8 effects[][2] = {
+		{ V4L2_COLORFX_NONE,	 ISP_IMAGE_EFFECT_DISABLE },
+		{ V4L2_COLORFX_BW,	 ISP_IMAGE_EFFECT_MONOCHROME },
+		{ V4L2_COLORFX_SEPIA,	 ISP_IMAGE_EFFECT_SEPIA },
+		{ V4L2_COLORFX_NEGATIVE, ISP_IMAGE_EFFECT_NEGATIVE_MONO },
+		{ 16 /* TODO */,	 ISP_IMAGE_EFFECT_NEGATIVE_COLOR },
+	};
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(effects); i++) {
+		if (effects[i][0] != value)
+			continue;
+
+		__is_set_isp_effect(is, effects[i][1]);
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int fimc_is_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct fimc_isp *isp = ctrl_to_fimc_isp(ctrl);
+	struct fimc_is *is = fimc_isp_to_is(isp);
+	bool set_param = true;
+	int ret = 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_CONTRAST:
+		__is_set_isp_adjust(is, ISP_ADJUST_COMMAND_MANUAL_CONTRAST,
+				    ctrl->val);
+		break;
+
+	case V4L2_CID_SATURATION:
+		__is_set_isp_adjust(is, ISP_ADJUST_COMMAND_MANUAL_SATURATION,
+				    ctrl->val);
+		break;
+
+	case V4L2_CID_SHARPNESS:
+		__is_set_isp_adjust(is, ISP_ADJUST_COMMAND_MANUAL_SHARPNESS,
+				    ctrl->val);
+		break;
+
+	case V4L2_CID_EXPOSURE_ABSOLUTE:
+		__is_set_isp_adjust(is, ISP_ADJUST_COMMAND_MANUAL_EXPOSURE,
+				    ctrl->val);
+		break;
+
+	case V4L2_CID_BRIGHTNESS:
+		__is_set_isp_adjust(is, ISP_ADJUST_COMMAND_MANUAL_BRIGHTNESS,
+				    ctrl->val);
+		break;
+
+	case V4L2_CID_HUE:
+		__is_set_isp_adjust(is, ISP_ADJUST_COMMAND_MANUAL_HUE,
+				    ctrl->val);
+		break;
+
+	case V4L2_CID_EXPOSURE_METERING:
+		ret = __ctrl_set_metering(is, ctrl->val);
+		break;
+
+	case V4L2_CID_AUTO_N_PRESET_WHITE_BALANCE:
+		ret = __ctrl_set_white_balance(is, ctrl->val);
+		break;
+
+	case V4L2_CID_3A_LOCK:
+		ret = __ctrl_set_aewb_lock(is, ctrl);
+		set_param = false;
+		break;
+
+	case V4L2_CID_ISO_SENSITIVITY_AUTO:
+		ret = __ctrl_set_iso(is, ctrl->val);
+		break;
+
+	case V4L2_CID_POWER_LINE_FREQUENCY:
+		ret = __ctrl_set_afc(is, ctrl->val);
+		break;
+
+	case V4L2_CID_COLORFX:
+		__ctrl_set_image_effect(is, ctrl->val);
+		break;
+
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	if (ret < 0) {
+		v4l2_err(&isp->subdev, "Failed to set control: %s (%d)\n",
+						ctrl->name, ctrl->val);
+		return ret;
+	}
+
+	if (set_param && test_bit(IS_ST_STREAM_ON, &is->state))
+		return fimc_is_itf_s_param(is, true);
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops fimc_isp_ctrl_ops = {
+	.s_ctrl	= fimc_is_s_ctrl,
+};
+
+static void __isp_subdev_set_default_format(struct fimc_isp *isp)
+{
+	struct fimc_is *is = fimc_isp_to_is(isp);
+
+	isp->sink_fmt.width = DEFAULT_PREVIEW_STILL_WIDTH +
+				FIMC_ISP_CAC_MARGIN_WIDTH;
+	isp->sink_fmt.height = DEFAULT_PREVIEW_STILL_HEIGHT +
+				FIMC_ISP_CAC_MARGIN_HEIGHT;
+	isp->sink_fmt.code = MEDIA_BUS_FMT_SGRBG10_1X10;
+
+	isp->src_fmt.width = DEFAULT_PREVIEW_STILL_WIDTH;
+	isp->src_fmt.height = DEFAULT_PREVIEW_STILL_HEIGHT;
+	isp->src_fmt.code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	__is_set_frame_size(is, &isp->src_fmt);
+}
+
+int fimc_isp_subdev_create(struct fimc_isp *isp)
+{
+	const struct v4l2_ctrl_ops *ops = &fimc_isp_ctrl_ops;
+	struct v4l2_ctrl_handler *handler = &isp->ctrls.handler;
+	struct v4l2_subdev *sd = &isp->subdev;
+	struct fimc_isp_ctrls *ctrls = &isp->ctrls;
+	int ret;
+
+	mutex_init(&isp->subdev_lock);
+
+	v4l2_subdev_init(sd, &fimc_is_subdev_ops);
+
+	sd->owner = THIS_MODULE;
+	sd->grp_id = GRP_ID_FIMC_IS;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	snprintf(sd->name, sizeof(sd->name), "FIMC-IS-ISP");
+
+	isp->subdev_pads[FIMC_ISP_SD_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+	isp->subdev_pads[FIMC_ISP_SD_PAD_SRC_FIFO].flags = MEDIA_PAD_FL_SOURCE;
+	isp->subdev_pads[FIMC_ISP_SD_PAD_SRC_DMA].flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_init(&sd->entity, FIMC_ISP_SD_PADS_NUM,
+				isp->subdev_pads, 0);
+	if (ret)
+		return ret;
+
+	v4l2_ctrl_handler_init(handler, 20);
+
+	ctrls->saturation = v4l2_ctrl_new_std(handler, ops, V4L2_CID_SATURATION,
+						-2, 2, 1, 0);
+	ctrls->brightness = v4l2_ctrl_new_std(handler, ops, V4L2_CID_BRIGHTNESS,
+						-4, 4, 1, 0);
+	ctrls->contrast = v4l2_ctrl_new_std(handler, ops, V4L2_CID_CONTRAST,
+						-2, 2, 1, 0);
+	ctrls->sharpness = v4l2_ctrl_new_std(handler, ops, V4L2_CID_SHARPNESS,
+						-2, 2, 1, 0);
+	ctrls->hue = v4l2_ctrl_new_std(handler, ops, V4L2_CID_HUE,
+						-2, 2, 1, 0);
+
+	ctrls->auto_wb = v4l2_ctrl_new_std_menu(handler, ops,
+					V4L2_CID_AUTO_N_PRESET_WHITE_BALANCE,
+					8, ~0x14e, V4L2_WHITE_BALANCE_AUTO);
+
+	ctrls->exposure = v4l2_ctrl_new_std(handler, ops,
+					V4L2_CID_EXPOSURE_ABSOLUTE,
+					-4, 4, 1, 0);
+
+	ctrls->exp_metering = v4l2_ctrl_new_std_menu(handler, ops,
+					V4L2_CID_EXPOSURE_METERING, 3,
+					~0xf, V4L2_EXPOSURE_METERING_AVERAGE);
+
+	v4l2_ctrl_new_std_menu(handler, ops, V4L2_CID_POWER_LINE_FREQUENCY,
+					V4L2_CID_POWER_LINE_FREQUENCY_AUTO, 0,
+					V4L2_CID_POWER_LINE_FREQUENCY_AUTO);
+	/* ISO sensitivity */
+	ctrls->auto_iso = v4l2_ctrl_new_std_menu(handler, ops,
+			V4L2_CID_ISO_SENSITIVITY_AUTO, 1, 0,
+			V4L2_ISO_SENSITIVITY_AUTO);
+
+	ctrls->iso = v4l2_ctrl_new_int_menu(handler, ops,
+			V4L2_CID_ISO_SENSITIVITY, ARRAY_SIZE(iso_qmenu) - 1,
+			ARRAY_SIZE(iso_qmenu)/2 - 1, iso_qmenu);
+
+	ctrls->aewb_lock = v4l2_ctrl_new_std(handler, ops,
+					V4L2_CID_3A_LOCK, 0, 0x3, 0, 0);
+
+	/* TODO: Add support for NEGATIVE_COLOR option */
+	ctrls->colorfx = v4l2_ctrl_new_std_menu(handler, ops, V4L2_CID_COLORFX,
+			V4L2_COLORFX_SET_CBCR + 1, ~0x1000f, V4L2_COLORFX_NONE);
+
+	if (handler->error) {
+		media_entity_cleanup(&sd->entity);
+		return handler->error;
+	}
+
+	v4l2_ctrl_auto_cluster(2, &ctrls->auto_iso,
+			V4L2_ISO_SENSITIVITY_MANUAL, false);
+
+	sd->ctrl_handler = handler;
+	sd->internal_ops = &fimc_is_subdev_internal_ops;
+	sd->entity.ops = &fimc_is_subdev_media_ops;
+	v4l2_set_subdevdata(sd, isp);
+
+	__isp_subdev_set_default_format(isp);
+
+	return 0;
+}
+
+void fimc_isp_subdev_destroy(struct fimc_isp *isp)
+{
+	struct v4l2_subdev *sd = &isp->subdev;
+
+	v4l2_device_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(&isp->ctrls.handler);
+	v4l2_set_subdevdata(sd, NULL);
+}
diff --git a/drivers/media/platform/exynos4-is/fimc-isp.h b/drivers/media/platform/exynos4-is/fimc-isp.h
new file mode 100644
index 0000000..c2d25df
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-isp.h
@@ -0,0 +1,195 @@
+/*
+ * Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver
+ *
+ * Copyright (C) 2013 Samsung Electronics Co., Ltd.
+ *
+ * Authors: Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *          Younghwan Joo <yhwan.joo@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#ifndef FIMC_ISP_H_
+#define FIMC_ISP_H_
+
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/spinlock.h>
+#include <linux/types.h>
+#include <linux/videodev2.h>
+
+#include <media/media-entity.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-mediabus.h>
+#include <media/exynos-fimc.h>
+
+extern int fimc_isp_debug;
+
+#define isp_dbg(level, dev, fmt, arg...) \
+	v4l2_dbg(level, fimc_isp_debug, dev, fmt, ## arg)
+
+/* FIXME: revisit these constraints */
+#define FIMC_ISP_SINK_WIDTH_MIN		(16 + 8)
+#define FIMC_ISP_SINK_HEIGHT_MIN	(12 + 8)
+#define FIMC_ISP_SOURCE_WIDTH_MIN	8
+#define FIMC_ISP_SOURCE_HEIGHT_MIN	8
+#define FIMC_ISP_CAC_MARGIN_WIDTH	16
+#define FIMC_ISP_CAC_MARGIN_HEIGHT	12
+
+#define FIMC_ISP_SINK_WIDTH_MAX		(4000 - 16)
+#define FIMC_ISP_SINK_HEIGHT_MAX	(4000 + 12)
+#define FIMC_ISP_SOURCE_WIDTH_MAX	4000
+#define FIMC_ISP_SOURCE_HEIGHT_MAX	4000
+
+#define FIMC_ISP_NUM_FORMATS		3
+#define FIMC_ISP_REQ_BUFS_MIN		2
+#define FIMC_ISP_REQ_BUFS_MAX		32
+
+#define FIMC_ISP_SD_PAD_SINK		0
+#define FIMC_ISP_SD_PAD_SRC_FIFO	1
+#define FIMC_ISP_SD_PAD_SRC_DMA		2
+#define FIMC_ISP_SD_PADS_NUM		3
+#define FIMC_ISP_MAX_PLANES		1
+
+/**
+ * struct fimc_isp_frame - source/target frame properties
+ * @width: full image width
+ * @height: full image height
+ * @rect: crop/composition rectangle
+ */
+struct fimc_isp_frame {
+	u16 width;
+	u16 height;
+	struct v4l2_rect rect;
+};
+
+struct fimc_isp_ctrls {
+	struct v4l2_ctrl_handler handler;
+
+	/* Auto white balance */
+	struct v4l2_ctrl *auto_wb;
+	/* Auto ISO control cluster */
+	struct {
+		struct v4l2_ctrl *auto_iso;
+		struct v4l2_ctrl *iso;
+	};
+	/* Adjust - contrast */
+	struct v4l2_ctrl *contrast;
+	/* Adjust - saturation */
+	struct v4l2_ctrl *saturation;
+	/* Adjust - sharpness */
+	struct v4l2_ctrl *sharpness;
+	/* Adjust - brightness */
+	struct v4l2_ctrl *brightness;
+	/* Adjust - hue */
+	struct v4l2_ctrl *hue;
+
+	/* Auto/manual exposure */
+	struct v4l2_ctrl *auto_exp;
+	/* Manual exposure value */
+	struct v4l2_ctrl *exposure;
+	/* AE/AWB lock/unlock */
+	struct v4l2_ctrl *aewb_lock;
+	/* Exposure metering mode */
+	struct v4l2_ctrl *exp_metering;
+	/* AFC */
+	struct v4l2_ctrl *afc;
+	/* ISP image effect */
+	struct v4l2_ctrl *colorfx;
+};
+
+struct isp_video_buf {
+	struct vb2_v4l2_buffer vb;
+	dma_addr_t dma_addr[FIMC_ISP_MAX_PLANES];
+	unsigned int index;
+};
+
+#define to_isp_video_buf(_b) container_of(_b, struct isp_video_buf, vb)
+
+#define FIMC_ISP_MAX_BUFS	4
+
+/**
+ * struct fimc_is_video - fimc-is video device structure
+ * @vdev: video_device structure
+ * @type: video device type (CAPTURE/OUTPUT)
+ * @pad: video device media (sink) pad
+ * @pending_buf_q: pending buffers queue head
+ * @active_buf_q: a queue head of buffers scheduled in hardware
+ * @vb_queue: vb2 buffer queue
+ * @active_buf_count: number of video buffers scheduled in hardware
+ * @frame_count: counter of frames dequeued to user space
+ * @reqbufs_count: number of buffers requested with REQBUFS ioctl
+ * @format: current pixel format
+ */
+struct fimc_is_video {
+	struct exynos_video_entity ve;
+	enum v4l2_buf_type	type;
+	struct media_pad	pad;
+	struct list_head	pending_buf_q;
+	struct list_head	active_buf_q;
+	struct vb2_queue	vb_queue;
+	unsigned int		reqbufs_count;
+	unsigned int		buf_count;
+	unsigned int		buf_mask;
+	unsigned int		frame_count;
+	int			streaming;
+	struct isp_video_buf	*buffers[FIMC_ISP_MAX_BUFS];
+	const struct fimc_fmt	*format;
+	struct v4l2_pix_format_mplane pixfmt;
+};
+
+/* struct fimc_isp:state bit definitions */
+#define ST_ISP_VID_CAP_BUF_PREP		0
+#define ST_ISP_VID_CAP_STREAMING	1
+
+/**
+ * struct fimc_isp - FIMC-IS ISP data structure
+ * @pdev: pointer to FIMC-IS platform device
+ * @alloc_ctx: videobuf2 memory allocator context
+ * @subdev: ISP v4l2_subdev
+ * @subdev_pads: the ISP subdev media pads
+ * @test_pattern: test pattern controls
+ * @ctrls: v4l2 controls structure
+ * @video_lock: mutex serializing video device and the subdev operations
+ * @cac_margin_x: horizontal CAC margin in pixels
+ * @cac_margin_y: vertical CAC margin in pixels
+ * @state: driver state flags
+ * @video_capture: the ISP block video capture device
+ */
+struct fimc_isp {
+	struct platform_device		*pdev;
+	struct vb2_alloc_ctx		*alloc_ctx;
+	struct v4l2_subdev		subdev;
+	struct media_pad		subdev_pads[FIMC_ISP_SD_PADS_NUM];
+	struct v4l2_mbus_framefmt	src_fmt;
+	struct v4l2_mbus_framefmt	sink_fmt;
+	struct v4l2_ctrl		*test_pattern;
+	struct fimc_isp_ctrls		ctrls;
+
+	struct mutex			video_lock;
+	struct mutex			subdev_lock;
+
+	unsigned int			cac_margin_x;
+	unsigned int			cac_margin_y;
+
+	unsigned long			state;
+
+	struct fimc_is_video		video_capture;
+};
+
+#define ctrl_to_fimc_isp(_ctrl) \
+	container_of(ctrl->handler, struct fimc_isp, ctrls.handler)
+
+struct fimc_is;
+
+int fimc_isp_subdev_create(struct fimc_isp *isp);
+void fimc_isp_subdev_destroy(struct fimc_isp *isp);
+void fimc_isp_irq_handler(struct fimc_is *is);
+int fimc_is_create_controls(struct fimc_isp *isp);
+int fimc_is_delete_controls(struct fimc_isp *isp);
+const struct fimc_fmt *fimc_isp_find_format(const u32 *pixelformat,
+					const u32 *mbus_code, int index);
+#endif /* FIMC_ISP_H_ */
diff --git a/drivers/media/platform/exynos4-is/fimc-lite-reg.c b/drivers/media/platform/exynos4-is/fimc-lite-reg.c
new file mode 100644
index 0000000..0477716
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-lite-reg.c
@@ -0,0 +1,349 @@
+/*
+ * Register interface file for EXYNOS FIMC-LITE (camera interface) driver
+ *
+ * Copyright (C) 2012 Samsung Electronics Co., Ltd.
+ * Author: Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+*/
+
+#include <linux/bitops.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <media/exynos-fimc.h>
+
+#include "fimc-lite-reg.h"
+#include "fimc-lite.h"
+#include "fimc-core.h"
+
+#define FLITE_RESET_TIMEOUT 50 /* in ms */
+
+void flite_hw_reset(struct fimc_lite *dev)
+{
+	unsigned long end = jiffies + msecs_to_jiffies(FLITE_RESET_TIMEOUT);
+	u32 cfg;
+
+	cfg = readl(dev->regs + FLITE_REG_CIGCTRL);
+	cfg |= FLITE_REG_CIGCTRL_SWRST_REQ;
+	writel(cfg, dev->regs + FLITE_REG_CIGCTRL);
+
+	while (time_is_after_jiffies(end)) {
+		cfg = readl(dev->regs + FLITE_REG_CIGCTRL);
+		if (cfg & FLITE_REG_CIGCTRL_SWRST_RDY)
+			break;
+		usleep_range(1000, 5000);
+	}
+
+	cfg |= FLITE_REG_CIGCTRL_SWRST;
+	writel(cfg, dev->regs + FLITE_REG_CIGCTRL);
+}
+
+void flite_hw_clear_pending_irq(struct fimc_lite *dev)
+{
+	u32 cfg = readl(dev->regs + FLITE_REG_CISTATUS);
+	cfg &= ~FLITE_REG_CISTATUS_IRQ_CAM;
+	writel(cfg, dev->regs + FLITE_REG_CISTATUS);
+}
+
+u32 flite_hw_get_interrupt_source(struct fimc_lite *dev)
+{
+	u32 intsrc = readl(dev->regs + FLITE_REG_CISTATUS);
+	return intsrc & FLITE_REG_CISTATUS_IRQ_MASK;
+}
+
+void flite_hw_clear_last_capture_end(struct fimc_lite *dev)
+{
+
+	u32 cfg = readl(dev->regs + FLITE_REG_CISTATUS2);
+	cfg &= ~FLITE_REG_CISTATUS2_LASTCAPEND;
+	writel(cfg, dev->regs + FLITE_REG_CISTATUS2);
+}
+
+void flite_hw_set_interrupt_mask(struct fimc_lite *dev)
+{
+	u32 cfg, intsrc;
+
+	/* Select interrupts to be enabled for each output mode */
+	if (atomic_read(&dev->out_path) == FIMC_IO_DMA) {
+		intsrc = FLITE_REG_CIGCTRL_IRQ_OVFEN |
+			 FLITE_REG_CIGCTRL_IRQ_LASTEN |
+			 FLITE_REG_CIGCTRL_IRQ_STARTEN |
+			 FLITE_REG_CIGCTRL_IRQ_ENDEN;
+	} else {
+		/* An output to the FIMC-IS */
+		intsrc = FLITE_REG_CIGCTRL_IRQ_OVFEN |
+			 FLITE_REG_CIGCTRL_IRQ_LASTEN;
+	}
+
+	cfg = readl(dev->regs + FLITE_REG_CIGCTRL);
+	cfg |= FLITE_REG_CIGCTRL_IRQ_DISABLE_MASK;
+	cfg &= ~intsrc;
+	writel(cfg, dev->regs + FLITE_REG_CIGCTRL);
+}
+
+void flite_hw_capture_start(struct fimc_lite *dev)
+{
+	u32 cfg = readl(dev->regs + FLITE_REG_CIIMGCPT);
+	cfg |= FLITE_REG_CIIMGCPT_IMGCPTEN;
+	writel(cfg, dev->regs + FLITE_REG_CIIMGCPT);
+}
+
+void flite_hw_capture_stop(struct fimc_lite *dev)
+{
+	u32 cfg = readl(dev->regs + FLITE_REG_CIIMGCPT);
+	cfg &= ~FLITE_REG_CIIMGCPT_IMGCPTEN;
+	writel(cfg, dev->regs + FLITE_REG_CIIMGCPT);
+}
+
+/*
+ * Test pattern (color bars) enable/disable. External sensor
+ * pixel clock must be active for the test pattern to work.
+ */
+void flite_hw_set_test_pattern(struct fimc_lite *dev, bool on)
+{
+	u32 cfg = readl(dev->regs + FLITE_REG_CIGCTRL);
+	if (on)
+		cfg |= FLITE_REG_CIGCTRL_TEST_PATTERN_COLORBAR;
+	else
+		cfg &= ~FLITE_REG_CIGCTRL_TEST_PATTERN_COLORBAR;
+	writel(cfg, dev->regs + FLITE_REG_CIGCTRL);
+}
+
+static const u32 src_pixfmt_map[8][3] = {
+	{ MEDIA_BUS_FMT_YUYV8_2X8, FLITE_REG_CISRCSIZE_ORDER422_IN_YCBYCR,
+	  FLITE_REG_CIGCTRL_YUV422_1P },
+	{ MEDIA_BUS_FMT_YVYU8_2X8, FLITE_REG_CISRCSIZE_ORDER422_IN_YCRYCB,
+	  FLITE_REG_CIGCTRL_YUV422_1P },
+	{ MEDIA_BUS_FMT_UYVY8_2X8, FLITE_REG_CISRCSIZE_ORDER422_IN_CBYCRY,
+	  FLITE_REG_CIGCTRL_YUV422_1P },
+	{ MEDIA_BUS_FMT_VYUY8_2X8, FLITE_REG_CISRCSIZE_ORDER422_IN_CRYCBY,
+	  FLITE_REG_CIGCTRL_YUV422_1P },
+	{ MEDIA_BUS_FMT_SGRBG8_1X8, 0, FLITE_REG_CIGCTRL_RAW8 },
+	{ MEDIA_BUS_FMT_SGRBG10_1X10, 0, FLITE_REG_CIGCTRL_RAW10 },
+	{ MEDIA_BUS_FMT_SGRBG12_1X12, 0, FLITE_REG_CIGCTRL_RAW12 },
+	{ MEDIA_BUS_FMT_JPEG_1X8, 0, FLITE_REG_CIGCTRL_USER(1) },
+};
+
+/* Set camera input pixel format and resolution */
+void flite_hw_set_source_format(struct fimc_lite *dev, struct flite_frame *f)
+{
+	u32 pixelcode = f->fmt->mbus_code;
+	int i = ARRAY_SIZE(src_pixfmt_map);
+	u32 cfg;
+
+	while (--i) {
+		if (src_pixfmt_map[i][0] == pixelcode)
+			break;
+	}
+
+	if (i == 0 && src_pixfmt_map[i][0] != pixelcode) {
+		v4l2_err(&dev->ve.vdev,
+			 "Unsupported pixel code, falling back to %#08x\n",
+			 src_pixfmt_map[i][0]);
+	}
+
+	cfg = readl(dev->regs + FLITE_REG_CIGCTRL);
+	cfg &= ~FLITE_REG_CIGCTRL_FMT_MASK;
+	cfg |= src_pixfmt_map[i][2];
+	writel(cfg, dev->regs + FLITE_REG_CIGCTRL);
+
+	cfg = readl(dev->regs + FLITE_REG_CISRCSIZE);
+	cfg &= ~(FLITE_REG_CISRCSIZE_ORDER422_MASK |
+		 FLITE_REG_CISRCSIZE_SIZE_CAM_MASK);
+	cfg |= (f->f_width << 16) | f->f_height;
+	cfg |= src_pixfmt_map[i][1];
+	writel(cfg, dev->regs + FLITE_REG_CISRCSIZE);
+}
+
+/* Set the camera host input window offsets (cropping) */
+void flite_hw_set_window_offset(struct fimc_lite *dev, struct flite_frame *f)
+{
+	u32 hoff2, voff2;
+	u32 cfg;
+
+	cfg = readl(dev->regs + FLITE_REG_CIWDOFST);
+	cfg &= ~FLITE_REG_CIWDOFST_OFST_MASK;
+	cfg |= (f->rect.left << 16) | f->rect.top;
+	cfg |= FLITE_REG_CIWDOFST_WINOFSEN;
+	writel(cfg, dev->regs + FLITE_REG_CIWDOFST);
+
+	hoff2 = f->f_width - f->rect.width - f->rect.left;
+	voff2 = f->f_height - f->rect.height - f->rect.top;
+
+	cfg = (hoff2 << 16) | voff2;
+	writel(cfg, dev->regs + FLITE_REG_CIWDOFST2);
+}
+
+/* Select camera port (A, B) */
+static void flite_hw_set_camera_port(struct fimc_lite *dev, int id)
+{
+	u32 cfg = readl(dev->regs + FLITE_REG_CIGENERAL);
+	if (id == 0)
+		cfg &= ~FLITE_REG_CIGENERAL_CAM_B;
+	else
+		cfg |= FLITE_REG_CIGENERAL_CAM_B;
+	writel(cfg, dev->regs + FLITE_REG_CIGENERAL);
+}
+
+/* Select serial or parallel bus, camera port (A,B) and set signals polarity */
+void flite_hw_set_camera_bus(struct fimc_lite *dev,
+			     struct fimc_source_info *si)
+{
+	u32 cfg = readl(dev->regs + FLITE_REG_CIGCTRL);
+	unsigned int flags = si->flags;
+
+	if (si->sensor_bus_type != FIMC_BUS_TYPE_MIPI_CSI2) {
+		cfg &= ~(FLITE_REG_CIGCTRL_SELCAM_MIPI |
+			 FLITE_REG_CIGCTRL_INVPOLPCLK |
+			 FLITE_REG_CIGCTRL_INVPOLVSYNC |
+			 FLITE_REG_CIGCTRL_INVPOLHREF);
+
+		if (flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
+			cfg |= FLITE_REG_CIGCTRL_INVPOLPCLK;
+
+		if (flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
+			cfg |= FLITE_REG_CIGCTRL_INVPOLVSYNC;
+
+		if (flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)
+			cfg |= FLITE_REG_CIGCTRL_INVPOLHREF;
+	} else {
+		cfg |= FLITE_REG_CIGCTRL_SELCAM_MIPI;
+	}
+
+	writel(cfg, dev->regs + FLITE_REG_CIGCTRL);
+
+	flite_hw_set_camera_port(dev, si->mux_id);
+}
+
+static void flite_hw_set_pack12(struct fimc_lite *dev, int on)
+{
+	u32 cfg = readl(dev->regs + FLITE_REG_CIODMAFMT);
+
+	cfg &= ~FLITE_REG_CIODMAFMT_PACK12;
+
+	if (on)
+		cfg |= FLITE_REG_CIODMAFMT_PACK12;
+
+	writel(cfg, dev->regs + FLITE_REG_CIODMAFMT);
+}
+
+static void flite_hw_set_out_order(struct fimc_lite *dev, struct flite_frame *f)
+{
+	static const u32 pixcode[4][2] = {
+		{ MEDIA_BUS_FMT_YUYV8_2X8, FLITE_REG_CIODMAFMT_YCBYCR },
+		{ MEDIA_BUS_FMT_YVYU8_2X8, FLITE_REG_CIODMAFMT_YCRYCB },
+		{ MEDIA_BUS_FMT_UYVY8_2X8, FLITE_REG_CIODMAFMT_CBYCRY },
+		{ MEDIA_BUS_FMT_VYUY8_2X8, FLITE_REG_CIODMAFMT_CRYCBY },
+	};
+	u32 cfg = readl(dev->regs + FLITE_REG_CIODMAFMT);
+	int i = ARRAY_SIZE(pixcode);
+
+	while (--i)
+		if (pixcode[i][0] == f->fmt->mbus_code)
+			break;
+	cfg &= ~FLITE_REG_CIODMAFMT_YCBCR_ORDER_MASK;
+	writel(cfg | pixcode[i][1], dev->regs + FLITE_REG_CIODMAFMT);
+}
+
+void flite_hw_set_dma_window(struct fimc_lite *dev, struct flite_frame *f)
+{
+	u32 cfg;
+
+	/* Maximum output pixel size */
+	cfg = readl(dev->regs + FLITE_REG_CIOCAN);
+	cfg &= ~FLITE_REG_CIOCAN_MASK;
+	cfg = (f->f_height << 16) | f->f_width;
+	writel(cfg, dev->regs + FLITE_REG_CIOCAN);
+
+	/* DMA offsets */
+	cfg = readl(dev->regs + FLITE_REG_CIOOFF);
+	cfg &= ~FLITE_REG_CIOOFF_MASK;
+	cfg |= (f->rect.top << 16) | f->rect.left;
+	writel(cfg, dev->regs + FLITE_REG_CIOOFF);
+}
+
+void flite_hw_set_dma_buffer(struct fimc_lite *dev, struct flite_buffer *buf)
+{
+	unsigned int index;
+	u32 cfg;
+
+	if (dev->dd->max_dma_bufs == 1)
+		index = 0;
+	else
+		index = buf->index;
+
+	if (index == 0)
+		writel(buf->paddr, dev->regs + FLITE_REG_CIOSA);
+	else
+		writel(buf->paddr, dev->regs + FLITE_REG_CIOSAN(index - 1));
+
+	cfg = readl(dev->regs + FLITE_REG_CIFCNTSEQ);
+	cfg |= BIT(index);
+	writel(cfg, dev->regs + FLITE_REG_CIFCNTSEQ);
+}
+
+void flite_hw_mask_dma_buffer(struct fimc_lite *dev, u32 index)
+{
+	u32 cfg;
+
+	if (dev->dd->max_dma_bufs == 1)
+		index = 0;
+
+	cfg = readl(dev->regs + FLITE_REG_CIFCNTSEQ);
+	cfg &= ~BIT(index);
+	writel(cfg, dev->regs + FLITE_REG_CIFCNTSEQ);
+}
+
+/* Enable/disable output DMA, set output pixel size and offsets (composition) */
+void flite_hw_set_output_dma(struct fimc_lite *dev, struct flite_frame *f,
+			     bool enable)
+{
+	u32 cfg = readl(dev->regs + FLITE_REG_CIGCTRL);
+
+	if (!enable) {
+		cfg |= FLITE_REG_CIGCTRL_ODMA_DISABLE;
+		writel(cfg, dev->regs + FLITE_REG_CIGCTRL);
+		return;
+	}
+
+	cfg &= ~FLITE_REG_CIGCTRL_ODMA_DISABLE;
+	writel(cfg, dev->regs + FLITE_REG_CIGCTRL);
+
+	flite_hw_set_out_order(dev, f);
+	flite_hw_set_dma_window(dev, f);
+	flite_hw_set_pack12(dev, 0);
+}
+
+void flite_hw_dump_regs(struct fimc_lite *dev, const char *label)
+{
+	struct {
+		u32 offset;
+		const char * const name;
+	} registers[] = {
+		{ 0x00, "CISRCSIZE" },
+		{ 0x04, "CIGCTRL" },
+		{ 0x08, "CIIMGCPT" },
+		{ 0x0c, "CICPTSEQ" },
+		{ 0x10, "CIWDOFST" },
+		{ 0x14, "CIWDOFST2" },
+		{ 0x18, "CIODMAFMT" },
+		{ 0x20, "CIOCAN" },
+		{ 0x24, "CIOOFF" },
+		{ 0x30, "CIOSA" },
+		{ 0x40, "CISTATUS" },
+		{ 0x44, "CISTATUS2" },
+		{ 0xf0, "CITHOLD" },
+		{ 0xfc, "CIGENERAL" },
+	};
+	u32 i;
+
+	v4l2_info(&dev->subdev, "--- %s ---\n", label);
+
+	for (i = 0; i < ARRAY_SIZE(registers); i++) {
+		u32 cfg = readl(dev->regs + registers[i].offset);
+		v4l2_info(&dev->subdev, "%9s: 0x%08x\n",
+			  registers[i].name, cfg);
+	}
+}
diff --git a/drivers/media/platform/exynos4-is/fimc-lite-reg.h b/drivers/media/platform/exynos4-is/fimc-lite-reg.h
new file mode 100644
index 0000000..10a7d7b
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-lite-reg.h
@@ -0,0 +1,156 @@
+/*
+ * Copyright (C) 2012 Samsung Electronics Co., Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef FIMC_LITE_REG_H_
+#define FIMC_LITE_REG_H_
+
+#include "fimc-lite.h"
+
+/* Camera Source size */
+#define FLITE_REG_CISRCSIZE			0x00
+#define FLITE_REG_CISRCSIZE_ORDER422_IN_YCBYCR	(0 << 14)
+#define FLITE_REG_CISRCSIZE_ORDER422_IN_YCRYCB	(1 << 14)
+#define FLITE_REG_CISRCSIZE_ORDER422_IN_CBYCRY	(2 << 14)
+#define FLITE_REG_CISRCSIZE_ORDER422_IN_CRYCBY	(3 << 14)
+#define FLITE_REG_CISRCSIZE_ORDER422_MASK	(0x3 << 14)
+#define FLITE_REG_CISRCSIZE_SIZE_CAM_MASK	(0x3fff << 16 | 0x3fff)
+
+/* Global control */
+#define FLITE_REG_CIGCTRL			0x04
+#define FLITE_REG_CIGCTRL_YUV422_1P		(0x1e << 24)
+#define FLITE_REG_CIGCTRL_RAW8			(0x2a << 24)
+#define FLITE_REG_CIGCTRL_RAW10			(0x2b << 24)
+#define FLITE_REG_CIGCTRL_RAW12			(0x2c << 24)
+#define FLITE_REG_CIGCTRL_RAW14			(0x2d << 24)
+/* User defined formats. x = 0...15 */
+#define FLITE_REG_CIGCTRL_USER(x)		((0x30 + x - 1) << 24)
+#define FLITE_REG_CIGCTRL_FMT_MASK		(0x3f << 24)
+#define FLITE_REG_CIGCTRL_SHADOWMASK_DISABLE	(1 << 21)
+#define FLITE_REG_CIGCTRL_ODMA_DISABLE		(1 << 20)
+#define FLITE_REG_CIGCTRL_SWRST_REQ		(1 << 19)
+#define FLITE_REG_CIGCTRL_SWRST_RDY		(1 << 18)
+#define FLITE_REG_CIGCTRL_SWRST			(1 << 17)
+#define FLITE_REG_CIGCTRL_TEST_PATTERN_COLORBAR	(1 << 15)
+#define FLITE_REG_CIGCTRL_INVPOLPCLK		(1 << 14)
+#define FLITE_REG_CIGCTRL_INVPOLVSYNC		(1 << 13)
+#define FLITE_REG_CIGCTRL_INVPOLHREF		(1 << 12)
+/* Interrupts mask bits (1 disables an interrupt) */
+#define FLITE_REG_CIGCTRL_IRQ_LASTEN		(1 << 8)
+#define FLITE_REG_CIGCTRL_IRQ_ENDEN		(1 << 7)
+#define FLITE_REG_CIGCTRL_IRQ_STARTEN		(1 << 6)
+#define FLITE_REG_CIGCTRL_IRQ_OVFEN		(1 << 5)
+#define FLITE_REG_CIGCTRL_IRQ_DISABLE_MASK	(0xf << 5)
+#define FLITE_REG_CIGCTRL_SELCAM_MIPI		(1 << 3)
+
+/* Image Capture Enable */
+#define FLITE_REG_CIIMGCPT			0x08
+#define FLITE_REG_CIIMGCPT_IMGCPTEN		(1 << 31)
+#define FLITE_REG_CIIMGCPT_CPT_FREN		(1 << 25)
+#define FLITE_REG_CIIMGCPT_CPT_MOD_FRCNT	(1 << 18)
+#define FLITE_REG_CIIMGCPT_CPT_MOD_FREN		(0 << 18)
+
+/* Capture Sequence */
+#define FLITE_REG_CICPTSEQ			0x0c
+
+/* Camera Window Offset */
+#define FLITE_REG_CIWDOFST			0x10
+#define FLITE_REG_CIWDOFST_WINOFSEN		(1 << 31)
+#define FLITE_REG_CIWDOFST_CLROVIY		(1 << 31)
+#define FLITE_REG_CIWDOFST_CLROVFICB		(1 << 15)
+#define FLITE_REG_CIWDOFST_CLROVFICR		(1 << 14)
+#define FLITE_REG_CIWDOFST_OFST_MASK		((0x1fff << 16) | 0x1fff)
+
+/* Camera Window Offset2 */
+#define FLITE_REG_CIWDOFST2			0x14
+
+/* Camera Output DMA Format */
+#define FLITE_REG_CIODMAFMT			0x18
+#define FLITE_REG_CIODMAFMT_RAW_CON		(1 << 15)
+#define FLITE_REG_CIODMAFMT_PACK12		(1 << 14)
+#define FLITE_REG_CIODMAFMT_YCBYCR		(0 << 4)
+#define FLITE_REG_CIODMAFMT_YCRYCB		(1 << 4)
+#define FLITE_REG_CIODMAFMT_CBYCRY		(2 << 4)
+#define FLITE_REG_CIODMAFMT_CRYCBY		(3 << 4)
+#define FLITE_REG_CIODMAFMT_YCBCR_ORDER_MASK	(0x3 << 4)
+
+/* Camera Output Canvas */
+#define FLITE_REG_CIOCAN			0x20
+#define FLITE_REG_CIOCAN_MASK			((0x3fff << 16) | 0x3fff)
+
+/* Camera Output DMA Offset */
+#define FLITE_REG_CIOOFF			0x24
+#define FLITE_REG_CIOOFF_MASK			((0x3fff << 16) | 0x3fff)
+
+/* Camera Output DMA Start Address */
+#define FLITE_REG_CIOSA				0x30
+
+/* Camera Status */
+#define FLITE_REG_CISTATUS			0x40
+#define FLITE_REG_CISTATUS_MIPI_VVALID		(1 << 22)
+#define FLITE_REG_CISTATUS_MIPI_HVALID		(1 << 21)
+#define FLITE_REG_CISTATUS_MIPI_DVALID		(1 << 20)
+#define FLITE_REG_CISTATUS_ITU_VSYNC		(1 << 14)
+#define FLITE_REG_CISTATUS_ITU_HREFF		(1 << 13)
+#define FLITE_REG_CISTATUS_OVFIY		(1 << 10)
+#define FLITE_REG_CISTATUS_OVFICB		(1 << 9)
+#define FLITE_REG_CISTATUS_OVFICR		(1 << 8)
+#define FLITE_REG_CISTATUS_IRQ_SRC_OVERFLOW	(1 << 7)
+#define FLITE_REG_CISTATUS_IRQ_SRC_LASTCAPEND	(1 << 6)
+#define FLITE_REG_CISTATUS_IRQ_SRC_FRMSTART	(1 << 5)
+#define FLITE_REG_CISTATUS_IRQ_SRC_FRMEND	(1 << 4)
+#define FLITE_REG_CISTATUS_IRQ_CAM		(1 << 0)
+#define FLITE_REG_CISTATUS_IRQ_MASK		(0xf << 4)
+
+/* Camera Status2 */
+#define FLITE_REG_CISTATUS2			0x44
+#define FLITE_REG_CISTATUS2_LASTCAPEND		(1 << 1)
+#define FLITE_REG_CISTATUS2_FRMEND		(1 << 0)
+
+/* Qos Threshold */
+#define FLITE_REG_CITHOLD			0xf0
+#define FLITE_REG_CITHOLD_W_QOS_EN		(1 << 30)
+
+/* Camera General Purpose */
+#define FLITE_REG_CIGENERAL			0xfc
+/* b0: 1 - camera B, 0 - camera A */
+#define FLITE_REG_CIGENERAL_CAM_B		(1 << 0)
+
+#define FLITE_REG_CIFCNTSEQ			0x100
+#define FLITE_REG_CIOSAN(x)			(0x200 + (4 * (x)))
+
+/* ----------------------------------------------------------------------------
+ * Function declarations
+ */
+void flite_hw_reset(struct fimc_lite *dev);
+void flite_hw_clear_pending_irq(struct fimc_lite *dev);
+u32 flite_hw_get_interrupt_source(struct fimc_lite *dev);
+void flite_hw_clear_last_capture_end(struct fimc_lite *dev);
+void flite_hw_set_interrupt_mask(struct fimc_lite *dev);
+void flite_hw_capture_start(struct fimc_lite *dev);
+void flite_hw_capture_stop(struct fimc_lite *dev);
+void flite_hw_set_camera_bus(struct fimc_lite *dev,
+			     struct fimc_source_info *s_info);
+void flite_hw_set_camera_polarity(struct fimc_lite *dev,
+				  struct fimc_source_info *cam);
+void flite_hw_set_window_offset(struct fimc_lite *dev, struct flite_frame *f);
+void flite_hw_set_source_format(struct fimc_lite *dev, struct flite_frame *f);
+
+void flite_hw_set_output_dma(struct fimc_lite *dev, struct flite_frame *f,
+			     bool enable);
+void flite_hw_set_dma_window(struct fimc_lite *dev, struct flite_frame *f);
+void flite_hw_set_test_pattern(struct fimc_lite *dev, bool on);
+void flite_hw_dump_regs(struct fimc_lite *dev, const char *label);
+void flite_hw_set_dma_buffer(struct fimc_lite *dev, struct flite_buffer *buf);
+void flite_hw_mask_dma_buffer(struct fimc_lite *dev, u32 index);
+
+static inline void flite_hw_set_dma_buf_mask(struct fimc_lite *dev, u32 mask)
+{
+	writel(mask, dev->regs + FLITE_REG_CIFCNTSEQ);
+}
+
+#endif /* FIMC_LITE_REG_H */
diff --git a/drivers/media/platform/exynos4-is/fimc-lite.c b/drivers/media/platform/exynos4-is/fimc-lite.c
new file mode 100644
index 0000000..60660c3
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-lite.c
@@ -0,0 +1,1726 @@
+/*
+ * Samsung EXYNOS FIMC-LITE (camera host interface) driver
+*
+ * Copyright (C) 2012 - 2013 Samsung Electronics Co., Ltd.
+ * Author: Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#define pr_fmt(fmt) "%s:%d " fmt, __func__, __LINE__
+
+#include <linux/bug.h>
+#include <linux/clk.h>
+#include <linux/device.h>
+#include <linux/errno.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/types.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/videobuf2-dma-contig.h>
+#include <media/exynos-fimc.h>
+
+#include "common.h"
+#include "fimc-core.h"
+#include "fimc-lite.h"
+#include "fimc-lite-reg.h"
+
+static int debug;
+module_param(debug, int, 0644);
+
+static const struct fimc_fmt fimc_lite_formats[] = {
+	{
+		.name		= "YUV 4:2:2 packed, YCbYCr",
+		.fourcc		= V4L2_PIX_FMT_YUYV,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+		.depth		= { 16 },
+		.color		= FIMC_FMT_YCBYCR422,
+		.memplanes	= 1,
+		.mbus_code	= MEDIA_BUS_FMT_YUYV8_2X8,
+		.flags		= FMT_FLAGS_YUV,
+	}, {
+		.name		= "YUV 4:2:2 packed, CbYCrY",
+		.fourcc		= V4L2_PIX_FMT_UYVY,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+		.depth		= { 16 },
+		.color		= FIMC_FMT_CBYCRY422,
+		.memplanes	= 1,
+		.mbus_code	= MEDIA_BUS_FMT_UYVY8_2X8,
+		.flags		= FMT_FLAGS_YUV,
+	}, {
+		.name		= "YUV 4:2:2 packed, CrYCbY",
+		.fourcc		= V4L2_PIX_FMT_VYUY,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+		.depth		= { 16 },
+		.color		= FIMC_FMT_CRYCBY422,
+		.memplanes	= 1,
+		.mbus_code	= MEDIA_BUS_FMT_VYUY8_2X8,
+		.flags		= FMT_FLAGS_YUV,
+	}, {
+		.name		= "YUV 4:2:2 packed, YCrYCb",
+		.fourcc		= V4L2_PIX_FMT_YVYU,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+		.depth		= { 16 },
+		.color		= FIMC_FMT_YCRYCB422,
+		.memplanes	= 1,
+		.mbus_code	= MEDIA_BUS_FMT_YVYU8_2X8,
+		.flags		= FMT_FLAGS_YUV,
+	}, {
+		.name		= "RAW8 (GRBG)",
+		.fourcc		= V4L2_PIX_FMT_SGRBG8,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.depth		= { 8 },
+		.color		= FIMC_FMT_RAW8,
+		.memplanes	= 1,
+		.mbus_code	= MEDIA_BUS_FMT_SGRBG8_1X8,
+		.flags		= FMT_FLAGS_RAW_BAYER,
+	}, {
+		.name		= "RAW10 (GRBG)",
+		.fourcc		= V4L2_PIX_FMT_SGRBG10,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.depth		= { 16 },
+		.color		= FIMC_FMT_RAW10,
+		.memplanes	= 1,
+		.mbus_code	= MEDIA_BUS_FMT_SGRBG10_1X10,
+		.flags		= FMT_FLAGS_RAW_BAYER,
+	}, {
+		.name		= "RAW12 (GRBG)",
+		.fourcc		= V4L2_PIX_FMT_SGRBG12,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.depth		= { 16 },
+		.color		= FIMC_FMT_RAW12,
+		.memplanes	= 1,
+		.mbus_code	= MEDIA_BUS_FMT_SGRBG12_1X12,
+		.flags		= FMT_FLAGS_RAW_BAYER,
+	},
+};
+
+/**
+ * fimc_lite_find_format - lookup fimc color format by fourcc or media bus code
+ * @pixelformat: fourcc to match, ignored if null
+ * @mbus_code: media bus code to match, ignored if null
+ * @mask: the color format flags to match
+ * @index: index to the fimc_lite_formats array, ignored if negative
+ */
+static const struct fimc_fmt *fimc_lite_find_format(const u32 *pixelformat,
+			const u32 *mbus_code, unsigned int mask, int index)
+{
+	const struct fimc_fmt *fmt, *def_fmt = NULL;
+	unsigned int i;
+	int id = 0;
+
+	if (index >= (int)ARRAY_SIZE(fimc_lite_formats))
+		return NULL;
+
+	for (i = 0; i < ARRAY_SIZE(fimc_lite_formats); ++i) {
+		fmt = &fimc_lite_formats[i];
+		if (mask && !(fmt->flags & mask))
+			continue;
+		if (pixelformat && fmt->fourcc == *pixelformat)
+			return fmt;
+		if (mbus_code && fmt->mbus_code == *mbus_code)
+			return fmt;
+		if (index == id)
+			def_fmt = fmt;
+		id++;
+	}
+	return def_fmt;
+}
+
+static int fimc_lite_hw_init(struct fimc_lite *fimc, bool isp_output)
+{
+	struct fimc_source_info *si;
+	unsigned long flags;
+
+	if (fimc->sensor == NULL)
+		return -ENXIO;
+
+	if (fimc->inp_frame.fmt == NULL || fimc->out_frame.fmt == NULL)
+		return -EINVAL;
+
+	/* Get sensor configuration data from the sensor subdev */
+	si = v4l2_get_subdev_hostdata(fimc->sensor);
+	if (!si)
+		return -EINVAL;
+
+	spin_lock_irqsave(&fimc->slock, flags);
+
+	flite_hw_set_camera_bus(fimc, si);
+	flite_hw_set_source_format(fimc, &fimc->inp_frame);
+	flite_hw_set_window_offset(fimc, &fimc->inp_frame);
+	flite_hw_set_dma_buf_mask(fimc, 0);
+	flite_hw_set_output_dma(fimc, &fimc->out_frame, !isp_output);
+	flite_hw_set_interrupt_mask(fimc);
+	flite_hw_set_test_pattern(fimc, fimc->test_pattern->val);
+
+	if (debug > 0)
+		flite_hw_dump_regs(fimc, __func__);
+
+	spin_unlock_irqrestore(&fimc->slock, flags);
+	return 0;
+}
+
+/*
+ * Reinitialize the driver so it is ready to start the streaming again.
+ * Set fimc->state to indicate stream off and the hardware shut down state.
+ * If not suspending (@suspend is false), return any buffers to videobuf2.
+ * Otherwise put any owned buffers onto the pending buffers queue, so they
+ * can be re-spun when the device is being resumed. Also perform FIMC
+ * software reset and disable streaming on the whole pipeline if required.
+ */
+static int fimc_lite_reinit(struct fimc_lite *fimc, bool suspend)
+{
+	struct flite_buffer *buf;
+	unsigned long flags;
+	bool streaming;
+
+	spin_lock_irqsave(&fimc->slock, flags);
+	streaming = fimc->state & (1 << ST_SENSOR_STREAM);
+
+	fimc->state &= ~(1 << ST_FLITE_RUN | 1 << ST_FLITE_OFF |
+			 1 << ST_FLITE_STREAM | 1 << ST_SENSOR_STREAM);
+	if (suspend)
+		fimc->state |= (1 << ST_FLITE_SUSPENDED);
+	else
+		fimc->state &= ~(1 << ST_FLITE_PENDING |
+				 1 << ST_FLITE_SUSPENDED);
+
+	/* Release unused buffers */
+	while (!suspend && !list_empty(&fimc->pending_buf_q)) {
+		buf = fimc_lite_pending_queue_pop(fimc);
+		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+	}
+	/* If suspending put unused buffers onto pending queue */
+	while (!list_empty(&fimc->active_buf_q)) {
+		buf = fimc_lite_active_queue_pop(fimc);
+		if (suspend)
+			fimc_lite_pending_queue_add(fimc, buf);
+		else
+			vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+	}
+
+	spin_unlock_irqrestore(&fimc->slock, flags);
+
+	flite_hw_reset(fimc);
+
+	if (!streaming)
+		return 0;
+
+	return fimc_pipeline_call(&fimc->ve, set_stream, 0);
+}
+
+static int fimc_lite_stop_capture(struct fimc_lite *fimc, bool suspend)
+{
+	unsigned long flags;
+
+	if (!fimc_lite_active(fimc))
+		return 0;
+
+	spin_lock_irqsave(&fimc->slock, flags);
+	set_bit(ST_FLITE_OFF, &fimc->state);
+	flite_hw_capture_stop(fimc);
+	spin_unlock_irqrestore(&fimc->slock, flags);
+
+	wait_event_timeout(fimc->irq_queue,
+			   !test_bit(ST_FLITE_OFF, &fimc->state),
+			   (2*HZ/10)); /* 200 ms */
+
+	return fimc_lite_reinit(fimc, suspend);
+}
+
+/* Must be called  with fimc.slock spinlock held. */
+static void fimc_lite_config_update(struct fimc_lite *fimc)
+{
+	flite_hw_set_window_offset(fimc, &fimc->inp_frame);
+	flite_hw_set_dma_window(fimc, &fimc->out_frame);
+	flite_hw_set_test_pattern(fimc, fimc->test_pattern->val);
+	clear_bit(ST_FLITE_CONFIG, &fimc->state);
+}
+
+static irqreturn_t flite_irq_handler(int irq, void *priv)
+{
+	struct fimc_lite *fimc = priv;
+	struct flite_buffer *vbuf;
+	unsigned long flags;
+	u32 intsrc;
+
+	spin_lock_irqsave(&fimc->slock, flags);
+
+	intsrc = flite_hw_get_interrupt_source(fimc);
+	flite_hw_clear_pending_irq(fimc);
+
+	if (test_and_clear_bit(ST_FLITE_OFF, &fimc->state)) {
+		wake_up(&fimc->irq_queue);
+		goto done;
+	}
+
+	if (intsrc & FLITE_REG_CISTATUS_IRQ_SRC_OVERFLOW) {
+		clear_bit(ST_FLITE_RUN, &fimc->state);
+		fimc->events.data_overflow++;
+	}
+
+	if (intsrc & FLITE_REG_CISTATUS_IRQ_SRC_LASTCAPEND) {
+		flite_hw_clear_last_capture_end(fimc);
+		clear_bit(ST_FLITE_STREAM, &fimc->state);
+		wake_up(&fimc->irq_queue);
+	}
+
+	if (atomic_read(&fimc->out_path) != FIMC_IO_DMA)
+		goto done;
+
+	if ((intsrc & FLITE_REG_CISTATUS_IRQ_SRC_FRMSTART) &&
+	    test_bit(ST_FLITE_RUN, &fimc->state) &&
+	    !list_empty(&fimc->pending_buf_q)) {
+		vbuf = fimc_lite_pending_queue_pop(fimc);
+		flite_hw_set_dma_buffer(fimc, vbuf);
+		fimc_lite_active_queue_add(fimc, vbuf);
+	}
+
+	if ((intsrc & FLITE_REG_CISTATUS_IRQ_SRC_FRMEND) &&
+	    test_bit(ST_FLITE_RUN, &fimc->state) &&
+	    !list_empty(&fimc->active_buf_q)) {
+		vbuf = fimc_lite_active_queue_pop(fimc);
+		v4l2_get_timestamp(&vbuf->vb.timestamp);
+		vbuf->vb.sequence = fimc->frame_count++;
+		flite_hw_mask_dma_buffer(fimc, vbuf->index);
+		vb2_buffer_done(&vbuf->vb.vb2_buf, VB2_BUF_STATE_DONE);
+	}
+
+	if (test_bit(ST_FLITE_CONFIG, &fimc->state))
+		fimc_lite_config_update(fimc);
+
+	if (list_empty(&fimc->pending_buf_q)) {
+		flite_hw_capture_stop(fimc);
+		clear_bit(ST_FLITE_STREAM, &fimc->state);
+	}
+done:
+	set_bit(ST_FLITE_RUN, &fimc->state);
+	spin_unlock_irqrestore(&fimc->slock, flags);
+	return IRQ_HANDLED;
+}
+
+static int start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct fimc_lite *fimc = q->drv_priv;
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&fimc->slock, flags);
+
+	fimc->buf_index = 0;
+	fimc->frame_count = 0;
+
+	spin_unlock_irqrestore(&fimc->slock, flags);
+
+	ret = fimc_lite_hw_init(fimc, false);
+	if (ret) {
+		fimc_lite_reinit(fimc, false);
+		return ret;
+	}
+
+	set_bit(ST_FLITE_PENDING, &fimc->state);
+
+	if (!list_empty(&fimc->active_buf_q) &&
+	    !test_and_set_bit(ST_FLITE_STREAM, &fimc->state)) {
+		flite_hw_capture_start(fimc);
+
+		if (!test_and_set_bit(ST_SENSOR_STREAM, &fimc->state))
+			fimc_pipeline_call(&fimc->ve, set_stream, 1);
+	}
+	if (debug > 0)
+		flite_hw_dump_regs(fimc, __func__);
+
+	return 0;
+}
+
+static void stop_streaming(struct vb2_queue *q)
+{
+	struct fimc_lite *fimc = q->drv_priv;
+
+	if (!fimc_lite_active(fimc))
+		return;
+
+	fimc_lite_stop_capture(fimc, false);
+}
+
+static int queue_setup(struct vb2_queue *vq, const void *parg,
+		       unsigned int *num_buffers, unsigned int *num_planes,
+		       unsigned int sizes[], void *allocators[])
+{
+	const struct v4l2_format *pfmt = parg;
+	const struct v4l2_pix_format_mplane *pixm = NULL;
+	struct fimc_lite *fimc = vq->drv_priv;
+	struct flite_frame *frame = &fimc->out_frame;
+	const struct fimc_fmt *fmt = frame->fmt;
+	unsigned long wh;
+	int i;
+
+	if (pfmt) {
+		pixm = &pfmt->fmt.pix_mp;
+		fmt = fimc_lite_find_format(&pixm->pixelformat, NULL, 0, -1);
+		wh = pixm->width * pixm->height;
+	} else {
+		wh = frame->f_width * frame->f_height;
+	}
+
+	if (fmt == NULL)
+		return -EINVAL;
+
+	*num_planes = fmt->memplanes;
+
+	for (i = 0; i < fmt->memplanes; i++) {
+		unsigned int size = (wh * fmt->depth[i]) / 8;
+		if (pixm)
+			sizes[i] = max(size, pixm->plane_fmt[i].sizeimage);
+		else
+			sizes[i] = size;
+		allocators[i] = fimc->alloc_ctx;
+	}
+
+	return 0;
+}
+
+static int buffer_prepare(struct vb2_buffer *vb)
+{
+	struct vb2_queue *vq = vb->vb2_queue;
+	struct fimc_lite *fimc = vq->drv_priv;
+	int i;
+
+	if (fimc->out_frame.fmt == NULL)
+		return -EINVAL;
+
+	for (i = 0; i < fimc->out_frame.fmt->memplanes; i++) {
+		unsigned long size = fimc->payload[i];
+
+		if (vb2_plane_size(vb, i) < size) {
+			v4l2_err(&fimc->ve.vdev,
+				 "User buffer too small (%ld < %ld)\n",
+				 vb2_plane_size(vb, i), size);
+			return -EINVAL;
+		}
+		vb2_set_plane_payload(vb, i, size);
+	}
+
+	return 0;
+}
+
+static void buffer_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct flite_buffer *buf
+		= container_of(vbuf, struct flite_buffer, vb);
+	struct fimc_lite *fimc = vb2_get_drv_priv(vb->vb2_queue);
+	unsigned long flags;
+
+	spin_lock_irqsave(&fimc->slock, flags);
+	buf->paddr = vb2_dma_contig_plane_dma_addr(vb, 0);
+
+	buf->index = fimc->buf_index++;
+	if (fimc->buf_index >= fimc->reqbufs_count)
+		fimc->buf_index = 0;
+
+	if (!test_bit(ST_FLITE_SUSPENDED, &fimc->state) &&
+	    !test_bit(ST_FLITE_STREAM, &fimc->state) &&
+	    list_empty(&fimc->active_buf_q)) {
+		flite_hw_set_dma_buffer(fimc, buf);
+		fimc_lite_active_queue_add(fimc, buf);
+	} else {
+		fimc_lite_pending_queue_add(fimc, buf);
+	}
+
+	if (vb2_is_streaming(&fimc->vb_queue) &&
+	    !list_empty(&fimc->pending_buf_q) &&
+	    !test_and_set_bit(ST_FLITE_STREAM, &fimc->state)) {
+		flite_hw_capture_start(fimc);
+		spin_unlock_irqrestore(&fimc->slock, flags);
+
+		if (!test_and_set_bit(ST_SENSOR_STREAM, &fimc->state))
+			fimc_pipeline_call(&fimc->ve, set_stream, 1);
+		return;
+	}
+	spin_unlock_irqrestore(&fimc->slock, flags);
+}
+
+static const struct vb2_ops fimc_lite_qops = {
+	.queue_setup	 = queue_setup,
+	.buf_prepare	 = buffer_prepare,
+	.buf_queue	 = buffer_queue,
+	.wait_prepare	 = vb2_ops_wait_prepare,
+	.wait_finish	 = vb2_ops_wait_finish,
+	.start_streaming = start_streaming,
+	.stop_streaming	 = stop_streaming,
+};
+
+static void fimc_lite_clear_event_counters(struct fimc_lite *fimc)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&fimc->slock, flags);
+	memset(&fimc->events, 0, sizeof(fimc->events));
+	spin_unlock_irqrestore(&fimc->slock, flags);
+}
+
+static int fimc_lite_open(struct file *file)
+{
+	struct fimc_lite *fimc = video_drvdata(file);
+	struct media_entity *me = &fimc->ve.vdev.entity;
+	int ret;
+
+	mutex_lock(&fimc->lock);
+	if (atomic_read(&fimc->out_path) != FIMC_IO_DMA) {
+		ret = -EBUSY;
+		goto unlock;
+	}
+
+	set_bit(ST_FLITE_IN_USE, &fimc->state);
+	ret = pm_runtime_get_sync(&fimc->pdev->dev);
+	if (ret < 0)
+		goto unlock;
+
+	ret = v4l2_fh_open(file);
+	if (ret < 0)
+		goto err_pm;
+
+	if (!v4l2_fh_is_singular_file(file) ||
+	    atomic_read(&fimc->out_path) != FIMC_IO_DMA)
+		goto unlock;
+
+	mutex_lock(&me->parent->graph_mutex);
+
+	ret = fimc_pipeline_call(&fimc->ve, open, me, true);
+
+	/* Mark video pipeline ending at this video node as in use. */
+	if (ret == 0)
+		me->use_count++;
+
+	mutex_unlock(&me->parent->graph_mutex);
+
+	if (!ret) {
+		fimc_lite_clear_event_counters(fimc);
+		goto unlock;
+	}
+
+	v4l2_fh_release(file);
+err_pm:
+	pm_runtime_put_sync(&fimc->pdev->dev);
+	clear_bit(ST_FLITE_IN_USE, &fimc->state);
+unlock:
+	mutex_unlock(&fimc->lock);
+	return ret;
+}
+
+static int fimc_lite_release(struct file *file)
+{
+	struct fimc_lite *fimc = video_drvdata(file);
+	struct media_entity *entity = &fimc->ve.vdev.entity;
+
+	mutex_lock(&fimc->lock);
+
+	if (v4l2_fh_is_singular_file(file) &&
+	    atomic_read(&fimc->out_path) == FIMC_IO_DMA) {
+		if (fimc->streaming) {
+			media_entity_pipeline_stop(entity);
+			fimc->streaming = false;
+		}
+		fimc_lite_stop_capture(fimc, false);
+		fimc_pipeline_call(&fimc->ve, close);
+		clear_bit(ST_FLITE_IN_USE, &fimc->state);
+
+		mutex_lock(&entity->parent->graph_mutex);
+		entity->use_count--;
+		mutex_unlock(&entity->parent->graph_mutex);
+	}
+
+	_vb2_fop_release(file, NULL);
+	pm_runtime_put(&fimc->pdev->dev);
+	clear_bit(ST_FLITE_SUSPENDED, &fimc->state);
+
+	mutex_unlock(&fimc->lock);
+	return 0;
+}
+
+static const struct v4l2_file_operations fimc_lite_fops = {
+	.owner		= THIS_MODULE,
+	.open		= fimc_lite_open,
+	.release	= fimc_lite_release,
+	.poll		= vb2_fop_poll,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap		= vb2_fop_mmap,
+};
+
+/*
+ * Format and crop negotiation helpers
+ */
+
+static const struct fimc_fmt *fimc_lite_subdev_try_fmt(struct fimc_lite *fimc,
+					struct v4l2_subdev_pad_config *cfg,
+					struct v4l2_subdev_format *format)
+{
+	struct flite_drvdata *dd = fimc->dd;
+	struct v4l2_mbus_framefmt *mf = &format->format;
+	const struct fimc_fmt *fmt = NULL;
+
+	if (format->pad == FLITE_SD_PAD_SINK) {
+		v4l_bound_align_image(&mf->width, 8, dd->max_width,
+				ffs(dd->out_width_align) - 1,
+				&mf->height, 0, dd->max_height, 0, 0);
+
+		fmt = fimc_lite_find_format(NULL, &mf->code, 0, 0);
+		if (WARN_ON(!fmt))
+			return NULL;
+
+		mf->colorspace = fmt->colorspace;
+		mf->code = fmt->mbus_code;
+	} else {
+		struct flite_frame *sink = &fimc->inp_frame;
+		struct v4l2_mbus_framefmt *sink_fmt;
+		struct v4l2_rect *rect;
+
+		if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+			sink_fmt = v4l2_subdev_get_try_format(&fimc->subdev, cfg,
+						FLITE_SD_PAD_SINK);
+
+			mf->code = sink_fmt->code;
+			mf->colorspace = sink_fmt->colorspace;
+
+			rect = v4l2_subdev_get_try_crop(&fimc->subdev, cfg,
+						FLITE_SD_PAD_SINK);
+		} else {
+			mf->code = sink->fmt->mbus_code;
+			mf->colorspace = sink->fmt->colorspace;
+			rect = &sink->rect;
+		}
+
+		/* Allow changing format only on sink pad */
+		mf->width = rect->width;
+		mf->height = rect->height;
+	}
+
+	mf->field = V4L2_FIELD_NONE;
+
+	v4l2_dbg(1, debug, &fimc->subdev, "code: %#x (%d), %dx%d\n",
+		 mf->code, mf->colorspace, mf->width, mf->height);
+
+	return fmt;
+}
+
+static void fimc_lite_try_crop(struct fimc_lite *fimc, struct v4l2_rect *r)
+{
+	struct flite_frame *frame = &fimc->inp_frame;
+
+	v4l_bound_align_image(&r->width, 0, frame->f_width, 0,
+			      &r->height, 0, frame->f_height, 0, 0);
+
+	/* Adjust left/top if cropping rectangle got out of bounds */
+	r->left = clamp_t(u32, r->left, 0, frame->f_width - r->width);
+	r->left = round_down(r->left, fimc->dd->win_hor_offs_align);
+	r->top  = clamp_t(u32, r->top, 0, frame->f_height - r->height);
+
+	v4l2_dbg(1, debug, &fimc->subdev, "(%d,%d)/%dx%d, sink fmt: %dx%d\n",
+		 r->left, r->top, r->width, r->height,
+		 frame->f_width, frame->f_height);
+}
+
+static void fimc_lite_try_compose(struct fimc_lite *fimc, struct v4l2_rect *r)
+{
+	struct flite_frame *frame = &fimc->out_frame;
+	struct v4l2_rect *crop_rect = &fimc->inp_frame.rect;
+
+	/* Scaling is not supported so we enforce compose rectangle size
+	   same as size of the sink crop rectangle. */
+	r->width = crop_rect->width;
+	r->height = crop_rect->height;
+
+	/* Adjust left/top if the composing rectangle got out of bounds */
+	r->left = clamp_t(u32, r->left, 0, frame->f_width - r->width);
+	r->left = round_down(r->left, fimc->dd->out_hor_offs_align);
+	r->top  = clamp_t(u32, r->top, 0, fimc->out_frame.f_height - r->height);
+
+	v4l2_dbg(1, debug, &fimc->subdev, "(%d,%d)/%dx%d, source fmt: %dx%d\n",
+		 r->left, r->top, r->width, r->height,
+		 frame->f_width, frame->f_height);
+}
+
+/*
+ * Video node ioctl operations
+ */
+static int fimc_lite_querycap(struct file *file, void *priv,
+					struct v4l2_capability *cap)
+{
+	struct fimc_lite *fimc = video_drvdata(file);
+
+	strlcpy(cap->driver, FIMC_LITE_DRV_NAME, sizeof(cap->driver));
+	strlcpy(cap->card, FIMC_LITE_DRV_NAME, sizeof(cap->card));
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
+					dev_name(&fimc->pdev->dev));
+
+	cap->device_caps = V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	return 0;
+}
+
+static int fimc_lite_enum_fmt_mplane(struct file *file, void *priv,
+				     struct v4l2_fmtdesc *f)
+{
+	const struct fimc_fmt *fmt;
+
+	if (f->index >= ARRAY_SIZE(fimc_lite_formats))
+		return -EINVAL;
+
+	fmt = &fimc_lite_formats[f->index];
+	strlcpy(f->description, fmt->name, sizeof(f->description));
+	f->pixelformat = fmt->fourcc;
+
+	return 0;
+}
+
+static int fimc_lite_g_fmt_mplane(struct file *file, void *fh,
+				  struct v4l2_format *f)
+{
+	struct fimc_lite *fimc = video_drvdata(file);
+	struct v4l2_pix_format_mplane *pixm = &f->fmt.pix_mp;
+	struct v4l2_plane_pix_format *plane_fmt = &pixm->plane_fmt[0];
+	struct flite_frame *frame = &fimc->out_frame;
+	const struct fimc_fmt *fmt = frame->fmt;
+
+	plane_fmt->bytesperline = (frame->f_width * fmt->depth[0]) / 8;
+	plane_fmt->sizeimage = plane_fmt->bytesperline * frame->f_height;
+
+	pixm->num_planes = fmt->memplanes;
+	pixm->pixelformat = fmt->fourcc;
+	pixm->width = frame->f_width;
+	pixm->height = frame->f_height;
+	pixm->field = V4L2_FIELD_NONE;
+	pixm->colorspace = fmt->colorspace;
+	return 0;
+}
+
+static int fimc_lite_try_fmt(struct fimc_lite *fimc,
+			     struct v4l2_pix_format_mplane *pixm,
+			     const struct fimc_fmt **ffmt)
+{
+	u32 bpl = pixm->plane_fmt[0].bytesperline;
+	struct flite_drvdata *dd = fimc->dd;
+	const struct fimc_fmt *inp_fmt = fimc->inp_frame.fmt;
+	const struct fimc_fmt *fmt;
+
+	if (WARN_ON(inp_fmt == NULL))
+		return -EINVAL;
+	/*
+	 * We allow some flexibility only for YUV formats. In case of raw
+	 * raw Bayer the FIMC-LITE's output format must match its camera
+	 * interface input format.
+	 */
+	if (inp_fmt->flags & FMT_FLAGS_YUV)
+		fmt = fimc_lite_find_format(&pixm->pixelformat, NULL,
+						inp_fmt->flags, 0);
+	else
+		fmt = inp_fmt;
+
+	if (WARN_ON(fmt == NULL))
+		return -EINVAL;
+	if (ffmt)
+		*ffmt = fmt;
+	v4l_bound_align_image(&pixm->width, 8, dd->max_width,
+			      ffs(dd->out_width_align) - 1,
+			      &pixm->height, 0, dd->max_height, 0, 0);
+
+	if ((bpl == 0 || ((bpl * 8) / fmt->depth[0]) < pixm->width))
+		pixm->plane_fmt[0].bytesperline = (pixm->width *
+						   fmt->depth[0]) / 8;
+
+	if (pixm->plane_fmt[0].sizeimage == 0)
+		pixm->plane_fmt[0].sizeimage = (pixm->width * pixm->height *
+						fmt->depth[0]) / 8;
+	pixm->num_planes = fmt->memplanes;
+	pixm->pixelformat = fmt->fourcc;
+	pixm->colorspace = fmt->colorspace;
+	pixm->field = V4L2_FIELD_NONE;
+	return 0;
+}
+
+static int fimc_lite_try_fmt_mplane(struct file *file, void *fh,
+				    struct v4l2_format *f)
+{
+	struct fimc_lite *fimc = video_drvdata(file);
+	return fimc_lite_try_fmt(fimc, &f->fmt.pix_mp, NULL);
+}
+
+static int fimc_lite_s_fmt_mplane(struct file *file, void *priv,
+				  struct v4l2_format *f)
+{
+	struct v4l2_pix_format_mplane *pixm = &f->fmt.pix_mp;
+	struct fimc_lite *fimc = video_drvdata(file);
+	struct flite_frame *frame = &fimc->out_frame;
+	const struct fimc_fmt *fmt = NULL;
+	int ret;
+
+	if (vb2_is_busy(&fimc->vb_queue))
+		return -EBUSY;
+
+	ret = fimc_lite_try_fmt(fimc, &f->fmt.pix_mp, &fmt);
+	if (ret < 0)
+		return ret;
+
+	frame->fmt = fmt;
+	fimc->payload[0] = max((pixm->width * pixm->height * fmt->depth[0]) / 8,
+			       pixm->plane_fmt[0].sizeimage);
+	frame->f_width = pixm->width;
+	frame->f_height = pixm->height;
+
+	return 0;
+}
+
+static int fimc_pipeline_validate(struct fimc_lite *fimc)
+{
+	struct v4l2_subdev *sd = &fimc->subdev;
+	struct v4l2_subdev_format sink_fmt, src_fmt;
+	struct media_pad *pad;
+	int ret;
+
+	while (1) {
+		/* Retrieve format at the sink pad */
+		pad = &sd->entity.pads[0];
+		if (!(pad->flags & MEDIA_PAD_FL_SINK))
+			break;
+		/* Don't call FIMC subdev operation to avoid nested locking */
+		if (sd == &fimc->subdev) {
+			struct flite_frame *ff = &fimc->out_frame;
+			sink_fmt.format.width = ff->f_width;
+			sink_fmt.format.height = ff->f_height;
+			sink_fmt.format.code = fimc->inp_frame.fmt->mbus_code;
+		} else {
+			sink_fmt.pad = pad->index;
+			sink_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+			ret = v4l2_subdev_call(sd, pad, get_fmt, NULL,
+					       &sink_fmt);
+			if (ret < 0 && ret != -ENOIOCTLCMD)
+				return -EPIPE;
+		}
+		/* Retrieve format at the source pad */
+		pad = media_entity_remote_pad(pad);
+		if (pad == NULL ||
+		    media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+			break;
+
+		sd = media_entity_to_v4l2_subdev(pad->entity);
+		src_fmt.pad = pad->index;
+		src_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+		ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &src_fmt);
+		if (ret < 0 && ret != -ENOIOCTLCMD)
+			return -EPIPE;
+
+		if (src_fmt.format.width != sink_fmt.format.width ||
+		    src_fmt.format.height != sink_fmt.format.height ||
+		    src_fmt.format.code != sink_fmt.format.code)
+			return -EPIPE;
+	}
+	return 0;
+}
+
+static int fimc_lite_streamon(struct file *file, void *priv,
+			      enum v4l2_buf_type type)
+{
+	struct fimc_lite *fimc = video_drvdata(file);
+	struct media_entity *entity = &fimc->ve.vdev.entity;
+	int ret;
+
+	if (fimc_lite_active(fimc))
+		return -EBUSY;
+
+	ret = media_entity_pipeline_start(entity, &fimc->ve.pipe->mp);
+	if (ret < 0)
+		return ret;
+
+	ret = fimc_pipeline_validate(fimc);
+	if (ret < 0)
+		goto err_p_stop;
+
+	fimc->sensor = fimc_find_remote_sensor(&fimc->subdev.entity);
+
+	ret = vb2_ioctl_streamon(file, priv, type);
+	if (!ret) {
+		fimc->streaming = true;
+		return ret;
+	}
+
+err_p_stop:
+	media_entity_pipeline_stop(entity);
+	return 0;
+}
+
+static int fimc_lite_streamoff(struct file *file, void *priv,
+			       enum v4l2_buf_type type)
+{
+	struct fimc_lite *fimc = video_drvdata(file);
+	int ret;
+
+	ret = vb2_ioctl_streamoff(file, priv, type);
+	if (ret < 0)
+		return ret;
+
+	media_entity_pipeline_stop(&fimc->ve.vdev.entity);
+	fimc->streaming = false;
+	return 0;
+}
+
+static int fimc_lite_reqbufs(struct file *file, void *priv,
+			     struct v4l2_requestbuffers *reqbufs)
+{
+	struct fimc_lite *fimc = video_drvdata(file);
+	int ret;
+
+	reqbufs->count = max_t(u32, FLITE_REQ_BUFS_MIN, reqbufs->count);
+	ret = vb2_ioctl_reqbufs(file, priv, reqbufs);
+	if (!ret)
+		fimc->reqbufs_count = reqbufs->count;
+
+	return ret;
+}
+
+/* Return 1 if rectangle a is enclosed in rectangle b, or 0 otherwise. */
+static int enclosed_rectangle(struct v4l2_rect *a, struct v4l2_rect *b)
+{
+	if (a->left < b->left || a->top < b->top)
+		return 0;
+	if (a->left + a->width > b->left + b->width)
+		return 0;
+	if (a->top + a->height > b->top + b->height)
+		return 0;
+
+	return 1;
+}
+
+static int fimc_lite_g_selection(struct file *file, void *fh,
+				 struct v4l2_selection *sel)
+{
+	struct fimc_lite *fimc = video_drvdata(file);
+	struct flite_frame *f = &fimc->out_frame;
+
+	if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		return -EINVAL;
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = f->f_width;
+		sel->r.height = f->f_height;
+		return 0;
+
+	case V4L2_SEL_TGT_COMPOSE:
+		sel->r = f->rect;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int fimc_lite_s_selection(struct file *file, void *fh,
+				 struct v4l2_selection *sel)
+{
+	struct fimc_lite *fimc = video_drvdata(file);
+	struct flite_frame *f = &fimc->out_frame;
+	struct v4l2_rect rect = sel->r;
+	unsigned long flags;
+
+	if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE ||
+	    sel->target != V4L2_SEL_TGT_COMPOSE)
+		return -EINVAL;
+
+	fimc_lite_try_compose(fimc, &rect);
+
+	if ((sel->flags & V4L2_SEL_FLAG_LE) &&
+	    !enclosed_rectangle(&rect, &sel->r))
+		return -ERANGE;
+
+	if ((sel->flags & V4L2_SEL_FLAG_GE) &&
+	    !enclosed_rectangle(&sel->r, &rect))
+		return -ERANGE;
+
+	sel->r = rect;
+	spin_lock_irqsave(&fimc->slock, flags);
+	f->rect = rect;
+	set_bit(ST_FLITE_CONFIG, &fimc->state);
+	spin_unlock_irqrestore(&fimc->slock, flags);
+
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops fimc_lite_ioctl_ops = {
+	.vidioc_querycap		= fimc_lite_querycap,
+	.vidioc_enum_fmt_vid_cap_mplane	= fimc_lite_enum_fmt_mplane,
+	.vidioc_try_fmt_vid_cap_mplane	= fimc_lite_try_fmt_mplane,
+	.vidioc_s_fmt_vid_cap_mplane	= fimc_lite_s_fmt_mplane,
+	.vidioc_g_fmt_vid_cap_mplane	= fimc_lite_g_fmt_mplane,
+	.vidioc_g_selection		= fimc_lite_g_selection,
+	.vidioc_s_selection		= fimc_lite_s_selection,
+	.vidioc_reqbufs			= fimc_lite_reqbufs,
+	.vidioc_querybuf		= vb2_ioctl_querybuf,
+	.vidioc_prepare_buf		= vb2_ioctl_prepare_buf,
+	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
+	.vidioc_qbuf			= vb2_ioctl_qbuf,
+	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
+	.vidioc_streamon		= fimc_lite_streamon,
+	.vidioc_streamoff		= fimc_lite_streamoff,
+};
+
+/* Capture subdev media entity operations */
+static int fimc_lite_link_setup(struct media_entity *entity,
+				const struct media_pad *local,
+				const struct media_pad *remote, u32 flags)
+{
+	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+	struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
+	unsigned int remote_ent_type = media_entity_type(remote->entity);
+	int ret = 0;
+
+	if (WARN_ON(fimc == NULL))
+		return 0;
+
+	v4l2_dbg(1, debug, sd, "%s: %s --> %s, flags: 0x%x. source_id: 0x%x\n",
+		 __func__, remote->entity->name, local->entity->name,
+		 flags, fimc->source_subdev_grp_id);
+
+	switch (local->index) {
+	case FLITE_SD_PAD_SINK:
+		if (remote_ent_type != MEDIA_ENT_T_V4L2_SUBDEV) {
+			ret = -EINVAL;
+			break;
+		}
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (fimc->source_subdev_grp_id == 0)
+				fimc->source_subdev_grp_id = sd->grp_id;
+			else
+				ret = -EBUSY;
+		} else {
+			fimc->source_subdev_grp_id = 0;
+			fimc->sensor = NULL;
+		}
+		break;
+
+	case FLITE_SD_PAD_SOURCE_DMA:
+		if (!(flags & MEDIA_LNK_FL_ENABLED))
+			atomic_set(&fimc->out_path, FIMC_IO_NONE);
+		else if (remote_ent_type == MEDIA_ENT_T_DEVNODE)
+			atomic_set(&fimc->out_path, FIMC_IO_DMA);
+		else
+			ret = -EINVAL;
+		break;
+
+	case FLITE_SD_PAD_SOURCE_ISP:
+		if (!(flags & MEDIA_LNK_FL_ENABLED))
+			atomic_set(&fimc->out_path, FIMC_IO_NONE);
+		else if (remote_ent_type == MEDIA_ENT_T_V4L2_SUBDEV)
+			atomic_set(&fimc->out_path, FIMC_IO_ISP);
+		else
+			ret = -EINVAL;
+		break;
+
+	default:
+		v4l2_err(sd, "Invalid pad index\n");
+		ret = -EINVAL;
+	}
+	mb();
+
+	return ret;
+}
+
+static const struct media_entity_operations fimc_lite_subdev_media_ops = {
+	.link_setup = fimc_lite_link_setup,
+};
+
+static int fimc_lite_subdev_enum_mbus_code(struct v4l2_subdev *sd,
+					   struct v4l2_subdev_pad_config *cfg,
+					   struct v4l2_subdev_mbus_code_enum *code)
+{
+	const struct fimc_fmt *fmt;
+
+	fmt = fimc_lite_find_format(NULL, NULL, 0, code->index);
+	if (!fmt)
+		return -EINVAL;
+	code->code = fmt->mbus_code;
+	return 0;
+}
+
+static struct v4l2_mbus_framefmt *__fimc_lite_subdev_get_try_fmt(
+		struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg, unsigned int pad)
+{
+	if (pad != FLITE_SD_PAD_SINK)
+		pad = FLITE_SD_PAD_SOURCE_DMA;
+
+	return v4l2_subdev_get_try_format(sd, cfg, pad);
+}
+
+static int fimc_lite_subdev_get_fmt(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_pad_config *cfg,
+				    struct v4l2_subdev_format *fmt)
+{
+	struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *mf = &fmt->format;
+	struct flite_frame *f = &fimc->inp_frame;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		mf = __fimc_lite_subdev_get_try_fmt(sd, cfg, fmt->pad);
+		fmt->format = *mf;
+		return 0;
+	}
+
+	mutex_lock(&fimc->lock);
+	mf->colorspace = f->fmt->colorspace;
+	mf->code = f->fmt->mbus_code;
+
+	if (fmt->pad == FLITE_SD_PAD_SINK) {
+		/* full camera input frame size */
+		mf->width = f->f_width;
+		mf->height = f->f_height;
+	} else {
+		/* crop size */
+		mf->width = f->rect.width;
+		mf->height = f->rect.height;
+	}
+	mutex_unlock(&fimc->lock);
+	return 0;
+}
+
+static int fimc_lite_subdev_set_fmt(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_pad_config *cfg,
+				    struct v4l2_subdev_format *fmt)
+{
+	struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *mf = &fmt->format;
+	struct flite_frame *sink = &fimc->inp_frame;
+	struct flite_frame *source = &fimc->out_frame;
+	const struct fimc_fmt *ffmt;
+
+	v4l2_dbg(1, debug, sd, "pad%d: code: 0x%x, %dx%d\n",
+		 fmt->pad, mf->code, mf->width, mf->height);
+
+	mutex_lock(&fimc->lock);
+
+	if ((atomic_read(&fimc->out_path) == FIMC_IO_ISP &&
+	    sd->entity.stream_count > 0) ||
+	    (atomic_read(&fimc->out_path) == FIMC_IO_DMA &&
+	    vb2_is_busy(&fimc->vb_queue))) {
+		mutex_unlock(&fimc->lock);
+		return -EBUSY;
+	}
+
+	ffmt = fimc_lite_subdev_try_fmt(fimc, cfg, fmt);
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		struct v4l2_mbus_framefmt *src_fmt;
+
+		mf = __fimc_lite_subdev_get_try_fmt(sd, cfg, fmt->pad);
+		*mf = fmt->format;
+
+		if (fmt->pad == FLITE_SD_PAD_SINK) {
+			unsigned int pad = FLITE_SD_PAD_SOURCE_DMA;
+			src_fmt = __fimc_lite_subdev_get_try_fmt(sd, cfg, pad);
+			*src_fmt = *mf;
+		}
+
+		mutex_unlock(&fimc->lock);
+		return 0;
+	}
+
+	if (fmt->pad == FLITE_SD_PAD_SINK) {
+		sink->f_width = mf->width;
+		sink->f_height = mf->height;
+		sink->fmt = ffmt;
+		/* Set sink crop rectangle */
+		sink->rect.width = mf->width;
+		sink->rect.height = mf->height;
+		sink->rect.left = 0;
+		sink->rect.top = 0;
+		/* Reset source format and crop rectangle */
+		source->rect = sink->rect;
+		source->f_width = mf->width;
+		source->f_height = mf->height;
+	}
+
+	mutex_unlock(&fimc->lock);
+	return 0;
+}
+
+static int fimc_lite_subdev_get_selection(struct v4l2_subdev *sd,
+					  struct v4l2_subdev_pad_config *cfg,
+					  struct v4l2_subdev_selection *sel)
+{
+	struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
+	struct flite_frame *f = &fimc->inp_frame;
+
+	if ((sel->target != V4L2_SEL_TGT_CROP &&
+	     sel->target != V4L2_SEL_TGT_CROP_BOUNDS) ||
+	     sel->pad != FLITE_SD_PAD_SINK)
+		return -EINVAL;
+
+	if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
+		sel->r = *v4l2_subdev_get_try_crop(sd, cfg, sel->pad);
+		return 0;
+	}
+
+	mutex_lock(&fimc->lock);
+	if (sel->target == V4L2_SEL_TGT_CROP) {
+		sel->r = f->rect;
+	} else {
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = f->f_width;
+		sel->r.height = f->f_height;
+	}
+	mutex_unlock(&fimc->lock);
+
+	v4l2_dbg(1, debug, sd, "%s: (%d,%d) %dx%d, f_w: %d, f_h: %d\n",
+		 __func__, f->rect.left, f->rect.top, f->rect.width,
+		 f->rect.height, f->f_width, f->f_height);
+
+	return 0;
+}
+
+static int fimc_lite_subdev_set_selection(struct v4l2_subdev *sd,
+					  struct v4l2_subdev_pad_config *cfg,
+					  struct v4l2_subdev_selection *sel)
+{
+	struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
+	struct flite_frame *f = &fimc->inp_frame;
+	int ret = 0;
+
+	if (sel->target != V4L2_SEL_TGT_CROP || sel->pad != FLITE_SD_PAD_SINK)
+		return -EINVAL;
+
+	mutex_lock(&fimc->lock);
+	fimc_lite_try_crop(fimc, &sel->r);
+
+	if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
+		*v4l2_subdev_get_try_crop(sd, cfg, sel->pad) = sel->r;
+	} else {
+		unsigned long flags;
+		spin_lock_irqsave(&fimc->slock, flags);
+		f->rect = sel->r;
+		/* Same crop rectangle on the source pad */
+		fimc->out_frame.rect = sel->r;
+		set_bit(ST_FLITE_CONFIG, &fimc->state);
+		spin_unlock_irqrestore(&fimc->slock, flags);
+	}
+	mutex_unlock(&fimc->lock);
+
+	v4l2_dbg(1, debug, sd, "%s: (%d,%d) %dx%d, f_w: %d, f_h: %d\n",
+		 __func__, f->rect.left, f->rect.top, f->rect.width,
+		 f->rect.height, f->f_width, f->f_height);
+
+	return ret;
+}
+
+static int fimc_lite_subdev_s_stream(struct v4l2_subdev *sd, int on)
+{
+	struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
+	unsigned long flags;
+	int ret;
+
+	/*
+	 * Find sensor subdev linked to FIMC-LITE directly or through
+	 * MIPI-CSIS. This is required for configuration where FIMC-LITE
+	 * is used as a subdev only and feeds data internally to FIMC-IS.
+	 * The pipeline links are protected through entity.stream_count
+	 * so there is no need to take the media graph mutex here.
+	 */
+	fimc->sensor = fimc_find_remote_sensor(&sd->entity);
+
+	if (atomic_read(&fimc->out_path) != FIMC_IO_ISP)
+		return -ENOIOCTLCMD;
+
+	mutex_lock(&fimc->lock);
+	if (on) {
+		flite_hw_reset(fimc);
+		ret = fimc_lite_hw_init(fimc, true);
+		if (!ret) {
+			spin_lock_irqsave(&fimc->slock, flags);
+			flite_hw_capture_start(fimc);
+			spin_unlock_irqrestore(&fimc->slock, flags);
+		}
+	} else {
+		set_bit(ST_FLITE_OFF, &fimc->state);
+
+		spin_lock_irqsave(&fimc->slock, flags);
+		flite_hw_capture_stop(fimc);
+		spin_unlock_irqrestore(&fimc->slock, flags);
+
+		ret = wait_event_timeout(fimc->irq_queue,
+				!test_bit(ST_FLITE_OFF, &fimc->state),
+				msecs_to_jiffies(200));
+		if (ret == 0)
+			v4l2_err(sd, "s_stream(0) timeout\n");
+		clear_bit(ST_FLITE_RUN, &fimc->state);
+	}
+
+	mutex_unlock(&fimc->lock);
+	return ret;
+}
+
+static int fimc_lite_log_status(struct v4l2_subdev *sd)
+{
+	struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
+
+	flite_hw_dump_regs(fimc, __func__);
+	return 0;
+}
+
+static int fimc_lite_subdev_registered(struct v4l2_subdev *sd)
+{
+	struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
+	struct vb2_queue *q = &fimc->vb_queue;
+	struct video_device *vfd = &fimc->ve.vdev;
+	int ret;
+
+	memset(vfd, 0, sizeof(*vfd));
+	atomic_set(&fimc->out_path, FIMC_IO_DMA);
+
+	snprintf(vfd->name, sizeof(vfd->name), "fimc-lite.%d.capture",
+		 fimc->index);
+
+	vfd->fops = &fimc_lite_fops;
+	vfd->ioctl_ops = &fimc_lite_ioctl_ops;
+	vfd->v4l2_dev = sd->v4l2_dev;
+	vfd->minor = -1;
+	vfd->release = video_device_release_empty;
+	vfd->queue = q;
+	fimc->reqbufs_count = 0;
+
+	INIT_LIST_HEAD(&fimc->pending_buf_q);
+	INIT_LIST_HEAD(&fimc->active_buf_q);
+
+	memset(q, 0, sizeof(*q));
+	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	q->io_modes = VB2_MMAP | VB2_USERPTR;
+	q->ops = &fimc_lite_qops;
+	q->mem_ops = &vb2_dma_contig_memops;
+	q->buf_struct_size = sizeof(struct flite_buffer);
+	q->drv_priv = fimc;
+	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	q->lock = &fimc->lock;
+
+	ret = vb2_queue_init(q);
+	if (ret < 0)
+		return ret;
+
+	fimc->vd_pad.flags = MEDIA_PAD_FL_SINK;
+	ret = media_entity_init(&vfd->entity, 1, &fimc->vd_pad, 0);
+	if (ret < 0)
+		return ret;
+
+	video_set_drvdata(vfd, fimc);
+	fimc->ve.pipe = v4l2_get_subdev_hostdata(sd);
+
+	ret = video_register_device(vfd, VFL_TYPE_GRABBER, -1);
+	if (ret < 0) {
+		media_entity_cleanup(&vfd->entity);
+		fimc->ve.pipe = NULL;
+		return ret;
+	}
+
+	v4l2_info(sd->v4l2_dev, "Registered %s as /dev/%s\n",
+		  vfd->name, video_device_node_name(vfd));
+	return 0;
+}
+
+static void fimc_lite_subdev_unregistered(struct v4l2_subdev *sd)
+{
+	struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
+
+	if (fimc == NULL)
+		return;
+
+	mutex_lock(&fimc->lock);
+
+	if (video_is_registered(&fimc->ve.vdev)) {
+		video_unregister_device(&fimc->ve.vdev);
+		media_entity_cleanup(&fimc->ve.vdev.entity);
+		fimc->ve.pipe = NULL;
+	}
+
+	mutex_unlock(&fimc->lock);
+}
+
+static const struct v4l2_subdev_internal_ops fimc_lite_subdev_internal_ops = {
+	.registered = fimc_lite_subdev_registered,
+	.unregistered = fimc_lite_subdev_unregistered,
+};
+
+static const struct v4l2_subdev_pad_ops fimc_lite_subdev_pad_ops = {
+	.enum_mbus_code = fimc_lite_subdev_enum_mbus_code,
+	.get_selection = fimc_lite_subdev_get_selection,
+	.set_selection = fimc_lite_subdev_set_selection,
+	.get_fmt = fimc_lite_subdev_get_fmt,
+	.set_fmt = fimc_lite_subdev_set_fmt,
+};
+
+static const struct v4l2_subdev_video_ops fimc_lite_subdev_video_ops = {
+	.s_stream = fimc_lite_subdev_s_stream,
+};
+
+static const struct v4l2_subdev_core_ops fimc_lite_core_ops = {
+	.log_status = fimc_lite_log_status,
+};
+
+static struct v4l2_subdev_ops fimc_lite_subdev_ops = {
+	.core = &fimc_lite_core_ops,
+	.video = &fimc_lite_subdev_video_ops,
+	.pad = &fimc_lite_subdev_pad_ops,
+};
+
+static int fimc_lite_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct fimc_lite *fimc = container_of(ctrl->handler, struct fimc_lite,
+					      ctrl_handler);
+	set_bit(ST_FLITE_CONFIG, &fimc->state);
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops fimc_lite_ctrl_ops = {
+	.s_ctrl	= fimc_lite_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config fimc_lite_ctrl = {
+	.ops	= &fimc_lite_ctrl_ops,
+	.id	= V4L2_CTRL_CLASS_USER | 0x1001,
+	.type	= V4L2_CTRL_TYPE_BOOLEAN,
+	.name	= "Test Pattern 640x480",
+	.step	= 1,
+};
+
+static void fimc_lite_set_default_config(struct fimc_lite *fimc)
+{
+	struct flite_frame *sink = &fimc->inp_frame;
+	struct flite_frame *source = &fimc->out_frame;
+
+	sink->fmt = &fimc_lite_formats[0];
+	sink->f_width = FLITE_DEFAULT_WIDTH;
+	sink->f_height = FLITE_DEFAULT_HEIGHT;
+
+	sink->rect.width = FLITE_DEFAULT_WIDTH;
+	sink->rect.height = FLITE_DEFAULT_HEIGHT;
+	sink->rect.left = 0;
+	sink->rect.top = 0;
+
+	*source = *sink;
+}
+
+static int fimc_lite_create_capture_subdev(struct fimc_lite *fimc)
+{
+	struct v4l2_ctrl_handler *handler = &fimc->ctrl_handler;
+	struct v4l2_subdev *sd = &fimc->subdev;
+	int ret;
+
+	v4l2_subdev_init(sd, &fimc_lite_subdev_ops);
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	snprintf(sd->name, sizeof(sd->name), "FIMC-LITE.%d", fimc->index);
+
+	fimc->subdev_pads[FLITE_SD_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+	fimc->subdev_pads[FLITE_SD_PAD_SOURCE_DMA].flags = MEDIA_PAD_FL_SOURCE;
+	fimc->subdev_pads[FLITE_SD_PAD_SOURCE_ISP].flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_init(&sd->entity, FLITE_SD_PADS_NUM,
+				fimc->subdev_pads, 0);
+	if (ret)
+		return ret;
+
+	v4l2_ctrl_handler_init(handler, 1);
+	fimc->test_pattern = v4l2_ctrl_new_custom(handler, &fimc_lite_ctrl,
+						  NULL);
+	if (handler->error) {
+		media_entity_cleanup(&sd->entity);
+		return handler->error;
+	}
+
+	sd->ctrl_handler = handler;
+	sd->internal_ops = &fimc_lite_subdev_internal_ops;
+	sd->entity.ops = &fimc_lite_subdev_media_ops;
+	sd->owner = THIS_MODULE;
+	v4l2_set_subdevdata(sd, fimc);
+
+	return 0;
+}
+
+static void fimc_lite_unregister_capture_subdev(struct fimc_lite *fimc)
+{
+	struct v4l2_subdev *sd = &fimc->subdev;
+
+	v4l2_device_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(&fimc->ctrl_handler);
+	v4l2_set_subdevdata(sd, NULL);
+}
+
+static void fimc_lite_clk_put(struct fimc_lite *fimc)
+{
+	if (IS_ERR(fimc->clock))
+		return;
+
+	clk_unprepare(fimc->clock);
+	clk_put(fimc->clock);
+	fimc->clock = ERR_PTR(-EINVAL);
+}
+
+static int fimc_lite_clk_get(struct fimc_lite *fimc)
+{
+	int ret;
+
+	fimc->clock = clk_get(&fimc->pdev->dev, FLITE_CLK_NAME);
+	if (IS_ERR(fimc->clock))
+		return PTR_ERR(fimc->clock);
+
+	ret = clk_prepare(fimc->clock);
+	if (ret < 0) {
+		clk_put(fimc->clock);
+		fimc->clock = ERR_PTR(-EINVAL);
+	}
+	return ret;
+}
+
+static const struct of_device_id flite_of_match[];
+
+static int fimc_lite_probe(struct platform_device *pdev)
+{
+	struct flite_drvdata *drv_data = NULL;
+	struct device *dev = &pdev->dev;
+	const struct of_device_id *of_id;
+	struct fimc_lite *fimc;
+	struct resource *res;
+	int ret;
+
+	if (!dev->of_node)
+		return -ENODEV;
+
+	fimc = devm_kzalloc(dev, sizeof(*fimc), GFP_KERNEL);
+	if (!fimc)
+		return -ENOMEM;
+
+	of_id = of_match_node(flite_of_match, dev->of_node);
+	if (of_id)
+		drv_data = (struct flite_drvdata *)of_id->data;
+	fimc->index = of_alias_get_id(dev->of_node, "fimc-lite");
+
+	if (!drv_data || fimc->index >= drv_data->num_instances ||
+						fimc->index < 0) {
+		dev_err(dev, "Wrong %s node alias\n",
+					dev->of_node->full_name);
+		return -EINVAL;
+	}
+
+	fimc->dd = drv_data;
+	fimc->pdev = pdev;
+
+	init_waitqueue_head(&fimc->irq_queue);
+	spin_lock_init(&fimc->slock);
+	mutex_init(&fimc->lock);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	fimc->regs = devm_ioremap_resource(dev, res);
+	if (IS_ERR(fimc->regs))
+		return PTR_ERR(fimc->regs);
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (res == NULL) {
+		dev_err(dev, "Failed to get IRQ resource\n");
+		return -ENXIO;
+	}
+
+	ret = fimc_lite_clk_get(fimc);
+	if (ret)
+		return ret;
+
+	ret = devm_request_irq(dev, res->start, flite_irq_handler,
+			       0, dev_name(dev), fimc);
+	if (ret) {
+		dev_err(dev, "Failed to install irq (%d)\n", ret);
+		goto err_clk_put;
+	}
+
+	/* The video node will be created within the subdev's registered() op */
+	ret = fimc_lite_create_capture_subdev(fimc);
+	if (ret)
+		goto err_clk_put;
+
+	platform_set_drvdata(pdev, fimc);
+	pm_runtime_enable(dev);
+
+	if (!pm_runtime_enabled(dev)) {
+		ret = clk_enable(fimc->clock);
+		if (ret < 0)
+			goto err_sd;
+	}
+
+	fimc->alloc_ctx = vb2_dma_contig_init_ctx(dev);
+	if (IS_ERR(fimc->alloc_ctx)) {
+		ret = PTR_ERR(fimc->alloc_ctx);
+		goto err_clk_dis;
+	}
+
+	fimc_lite_set_default_config(fimc);
+
+	dev_dbg(dev, "FIMC-LITE.%d registered successfully\n",
+		fimc->index);
+	return 0;
+
+err_clk_dis:
+	if (!pm_runtime_enabled(dev))
+		clk_disable(fimc->clock);
+err_sd:
+	fimc_lite_unregister_capture_subdev(fimc);
+err_clk_put:
+	fimc_lite_clk_put(fimc);
+	return ret;
+}
+
+#ifdef CONFIG_PM
+static int fimc_lite_runtime_resume(struct device *dev)
+{
+	struct fimc_lite *fimc = dev_get_drvdata(dev);
+
+	clk_enable(fimc->clock);
+	return 0;
+}
+
+static int fimc_lite_runtime_suspend(struct device *dev)
+{
+	struct fimc_lite *fimc = dev_get_drvdata(dev);
+
+	clk_disable(fimc->clock);
+	return 0;
+}
+#endif
+
+#ifdef CONFIG_PM_SLEEP
+static int fimc_lite_resume(struct device *dev)
+{
+	struct fimc_lite *fimc = dev_get_drvdata(dev);
+	struct flite_buffer *buf;
+	unsigned long flags;
+	int i;
+
+	spin_lock_irqsave(&fimc->slock, flags);
+	if (!test_and_clear_bit(ST_LPM, &fimc->state) ||
+	    !test_bit(ST_FLITE_IN_USE, &fimc->state)) {
+		spin_unlock_irqrestore(&fimc->slock, flags);
+		return 0;
+	}
+	flite_hw_reset(fimc);
+	spin_unlock_irqrestore(&fimc->slock, flags);
+
+	if (!test_and_clear_bit(ST_FLITE_SUSPENDED, &fimc->state))
+		return 0;
+
+	INIT_LIST_HEAD(&fimc->active_buf_q);
+	fimc_pipeline_call(&fimc->ve, open,
+			   &fimc->ve.vdev.entity, false);
+	fimc_lite_hw_init(fimc, atomic_read(&fimc->out_path) == FIMC_IO_ISP);
+	clear_bit(ST_FLITE_SUSPENDED, &fimc->state);
+
+	for (i = 0; i < fimc->reqbufs_count; i++) {
+		if (list_empty(&fimc->pending_buf_q))
+			break;
+		buf = fimc_lite_pending_queue_pop(fimc);
+		buffer_queue(&buf->vb.vb2_buf);
+	}
+	return 0;
+}
+
+static int fimc_lite_suspend(struct device *dev)
+{
+	struct fimc_lite *fimc = dev_get_drvdata(dev);
+	bool suspend = test_bit(ST_FLITE_IN_USE, &fimc->state);
+	int ret;
+
+	if (test_and_set_bit(ST_LPM, &fimc->state))
+		return 0;
+
+	ret = fimc_lite_stop_capture(fimc, suspend);
+	if (ret < 0 || !fimc_lite_active(fimc))
+		return ret;
+
+	return fimc_pipeline_call(&fimc->ve, close);
+}
+#endif /* CONFIG_PM_SLEEP */
+
+static int fimc_lite_remove(struct platform_device *pdev)
+{
+	struct fimc_lite *fimc = platform_get_drvdata(pdev);
+	struct device *dev = &pdev->dev;
+
+	pm_runtime_disable(dev);
+	pm_runtime_set_suspended(dev);
+	fimc_lite_unregister_capture_subdev(fimc);
+	vb2_dma_contig_cleanup_ctx(fimc->alloc_ctx);
+	fimc_lite_clk_put(fimc);
+
+	dev_info(dev, "Driver unloaded\n");
+	return 0;
+}
+
+static const struct dev_pm_ops fimc_lite_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(fimc_lite_suspend, fimc_lite_resume)
+	SET_RUNTIME_PM_OPS(fimc_lite_runtime_suspend, fimc_lite_runtime_resume,
+			   NULL)
+};
+
+/* EXYNOS4212, EXYNOS4412 */
+static struct flite_drvdata fimc_lite_drvdata_exynos4 = {
+	.max_width		= 8192,
+	.max_height		= 8192,
+	.out_width_align	= 8,
+	.win_hor_offs_align	= 2,
+	.out_hor_offs_align	= 8,
+	.max_dma_bufs		= 1,
+	.num_instances		= 2,
+};
+
+/* EXYNOS5250 */
+static struct flite_drvdata fimc_lite_drvdata_exynos5 = {
+	.max_width		= 8192,
+	.max_height		= 8192,
+	.out_width_align	= 8,
+	.win_hor_offs_align	= 2,
+	.out_hor_offs_align	= 8,
+	.max_dma_bufs		= 32,
+	.num_instances		= 3,
+};
+
+static const struct of_device_id flite_of_match[] = {
+	{
+		.compatible = "samsung,exynos4212-fimc-lite",
+		.data = &fimc_lite_drvdata_exynos4,
+	},
+	{
+		.compatible = "samsung,exynos5250-fimc-lite",
+		.data = &fimc_lite_drvdata_exynos5,
+	},
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, flite_of_match);
+
+static struct platform_driver fimc_lite_driver = {
+	.probe		= fimc_lite_probe,
+	.remove		= fimc_lite_remove,
+	.driver = {
+		.of_match_table = flite_of_match,
+		.name		= FIMC_LITE_DRV_NAME,
+		.pm		= &fimc_lite_pm_ops,
+	}
+};
+module_platform_driver(fimc_lite_driver);
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:" FIMC_LITE_DRV_NAME);
diff --git a/drivers/media/platform/exynos4-is/fimc-lite.h b/drivers/media/platform/exynos4-is/fimc-lite.h
new file mode 100644
index 0000000..b302305
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-lite.h
@@ -0,0 +1,227 @@
+/*
+ * Copyright (C) 2012 Samsung Electronics Co., Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef FIMC_LITE_H_
+#define FIMC_LITE_H_
+
+#include <linux/sizes.h>
+#include <linux/io.h>
+#include <linux/irqreturn.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/spinlock.h>
+#include <linux/types.h>
+#include <linux/videodev2.h>
+
+#include <media/media-entity.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-mediabus.h>
+#include <media/exynos-fimc.h>
+
+#define FIMC_LITE_DRV_NAME	"exynos-fimc-lite"
+#define FLITE_CLK_NAME		"flite"
+#define FIMC_LITE_MAX_DEVS	3
+#define FLITE_REQ_BUFS_MIN	2
+#define FLITE_DEFAULT_WIDTH	640
+#define FLITE_DEFAULT_HEIGHT	480
+
+/* Bit index definitions for struct fimc_lite::state */
+enum {
+	ST_FLITE_LPM,
+	ST_FLITE_PENDING,
+	ST_FLITE_RUN,
+	ST_FLITE_STREAM,
+	ST_FLITE_SUSPENDED,
+	ST_FLITE_OFF,
+	ST_FLITE_IN_USE,
+	ST_FLITE_CONFIG,
+	ST_SENSOR_STREAM,
+};
+
+#define FLITE_SD_PAD_SINK	0
+#define FLITE_SD_PAD_SOURCE_DMA	1
+#define FLITE_SD_PAD_SOURCE_ISP	2
+#define FLITE_SD_PADS_NUM	3
+
+/**
+ * struct flite_drvdata - FIMC-LITE IP variant data structure
+ * @max_width: maximum camera interface input width in pixels
+ * @max_height: maximum camera interface input height in pixels
+ * @out_width_align: minimum output width alignment in pixels
+ * @win_hor_offs_align: minimum camera interface crop window horizontal
+ * 			offset alignment in pixels
+ * @out_hor_offs_align: minimum output DMA compose rectangle horizontal
+ * 			offset alignment in pixels
+ * @max_dma_bufs: number of output DMA buffer start address registers
+ * @num_instances: total number of FIMC-LITE IP instances available
+ */
+struct flite_drvdata {
+	unsigned short max_width;
+	unsigned short max_height;
+	unsigned short out_width_align;
+	unsigned short win_hor_offs_align;
+	unsigned short out_hor_offs_align;
+	unsigned short max_dma_bufs;
+	unsigned short num_instances;
+};
+
+struct fimc_lite_events {
+	unsigned int data_overflow;
+};
+
+#define FLITE_MAX_PLANES	1
+
+/**
+ * struct flite_frame - source/target frame properties
+ * @f_width: full pixel width
+ * @f_height: full pixel height
+ * @rect: crop/composition rectangle
+ * @fmt: pointer to pixel format description data structure
+ */
+struct flite_frame {
+	u16 f_width;
+	u16 f_height;
+	struct v4l2_rect rect;
+	const struct fimc_fmt *fmt;
+};
+
+/**
+ * struct flite_buffer - video buffer structure
+ * @vb:    vb2 buffer
+ * @list:  list head for the buffers queue
+ * @paddr: DMA buffer start address
+ * @index: DMA start address register's index
+ */
+struct flite_buffer {
+	struct vb2_v4l2_buffer vb;
+	struct list_head list;
+	dma_addr_t paddr;
+	unsigned short index;
+};
+
+/**
+ * struct fimc_lite - fimc lite structure
+ * @pdev: pointer to FIMC-LITE platform device
+ * @dd: SoC specific driver data structure
+ * @ve: exynos video device entity structure
+ * @v4l2_dev: pointer to top the level v4l2_device
+ * @fh: v4l2 file handle
+ * @alloc_ctx: videobuf2 memory allocator context
+ * @subdev: FIMC-LITE subdev
+ * @vd_pad: media (sink) pad for the capture video node
+ * @subdev_pads: the subdev media pads
+ * @sensor: sensor subdev attached to FIMC-LITE directly or through MIPI-CSIS
+ * @ctrl_handler: v4l2 control handler
+ * @test_pattern: test pattern controls
+ * @index: FIMC-LITE platform device index
+ * @pipeline: video capture pipeline data structure
+ * @pipeline_ops: media pipeline ops for the video node driver
+ * @slock: spinlock protecting this data structure and the hw registers
+ * @lock: mutex serializing video device and the subdev operations
+ * @clock: FIMC-LITE gate clock
+ * @regs: memory mapped io registers
+ * @irq_queue: interrupt handler waitqueue
+ * @payload: image size in bytes (w x h x bpp)
+ * @inp_frame: camera input frame structure
+ * @out_frame: DMA output frame structure
+ * @out_path: output data path (DMA or FIFO)
+ * @source_subdev_grp_id: source subdev group id
+ * @state: driver state flags
+ * @pending_buf_q: pending buffers queue head
+ * @active_buf_q: the queue head of buffers scheduled in hardware
+ * @vb_queue: vb2 buffers queue
+ * @buf_index: helps to keep track of the DMA start address register index
+ * @active_buf_count: number of video buffers scheduled in hardware
+ * @frame_count: the captured frames counter
+ * @reqbufs_count: the number of buffers requested with REQBUFS ioctl
+ */
+struct fimc_lite {
+	struct platform_device	*pdev;
+	struct flite_drvdata	*dd;
+	struct exynos_video_entity ve;
+	struct v4l2_device	*v4l2_dev;
+	struct v4l2_fh		fh;
+	struct vb2_alloc_ctx	*alloc_ctx;
+	struct v4l2_subdev	subdev;
+	struct media_pad	vd_pad;
+	struct media_pad	subdev_pads[FLITE_SD_PADS_NUM];
+	struct v4l2_subdev	*sensor;
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct v4l2_ctrl	*test_pattern;
+	int			index;
+
+	struct mutex		lock;
+	spinlock_t		slock;
+
+	struct clk		*clock;
+	void __iomem		*regs;
+	wait_queue_head_t	irq_queue;
+
+	unsigned long		payload[FLITE_MAX_PLANES];
+	struct flite_frame	inp_frame;
+	struct flite_frame	out_frame;
+	atomic_t		out_path;
+	unsigned int		source_subdev_grp_id;
+
+	unsigned long		state;
+	struct list_head	pending_buf_q;
+	struct list_head	active_buf_q;
+	struct vb2_queue	vb_queue;
+	unsigned short		buf_index;
+	unsigned int		frame_count;
+	unsigned int		reqbufs_count;
+
+	struct fimc_lite_events	events;
+	bool			streaming;
+};
+
+static inline bool fimc_lite_active(struct fimc_lite *fimc)
+{
+	unsigned long flags;
+	bool ret;
+
+	spin_lock_irqsave(&fimc->slock, flags);
+	ret = fimc->state & (1 << ST_FLITE_RUN) ||
+		fimc->state & (1 << ST_FLITE_PENDING);
+	spin_unlock_irqrestore(&fimc->slock, flags);
+	return ret;
+}
+
+static inline void fimc_lite_active_queue_add(struct fimc_lite *dev,
+					 struct flite_buffer *buf)
+{
+	list_add_tail(&buf->list, &dev->active_buf_q);
+}
+
+static inline struct flite_buffer *fimc_lite_active_queue_pop(
+					struct fimc_lite *dev)
+{
+	struct flite_buffer *buf = list_entry(dev->active_buf_q.next,
+					      struct flite_buffer, list);
+	list_del(&buf->list);
+	return buf;
+}
+
+static inline void fimc_lite_pending_queue_add(struct fimc_lite *dev,
+					struct flite_buffer *buf)
+{
+	list_add_tail(&buf->list, &dev->pending_buf_q);
+}
+
+static inline struct flite_buffer *fimc_lite_pending_queue_pop(
+					struct fimc_lite *dev)
+{
+	struct flite_buffer *buf = list_entry(dev->pending_buf_q.next,
+					      struct flite_buffer, list);
+	list_del(&buf->list);
+	return buf;
+}
+
+#endif /* FIMC_LITE_H_ */
diff --git a/drivers/media/platform/exynos4-is/fimc-m2m.c b/drivers/media/platform/exynos4-is/fimc-m2m.c
new file mode 100644
index 0000000..4d1d64a
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-m2m.c
@@ -0,0 +1,773 @@
+/*
+ * Samsung S5P/EXYNOS4 SoC series FIMC (video postprocessor) driver
+ *
+ * Copyright (C) 2012 - 2013 Samsung Electronics Co., Ltd.
+ * Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published
+ * by the Free Software Foundation, either version 2 of the License,
+ * or (at your option) any later version.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/bug.h>
+#include <linux/interrupt.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/list.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/clk.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "common.h"
+#include "fimc-core.h"
+#include "fimc-reg.h"
+#include "media-dev.h"
+
+static unsigned int get_m2m_fmt_flags(unsigned int stream_type)
+{
+	if (stream_type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		return FMT_FLAGS_M2M_IN;
+	else
+		return FMT_FLAGS_M2M_OUT;
+}
+
+void fimc_m2m_job_finish(struct fimc_ctx *ctx, int vb_state)
+{
+	struct vb2_v4l2_buffer *src_vb, *dst_vb;
+
+	if (!ctx || !ctx->fh.m2m_ctx)
+		return;
+
+	src_vb = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+	dst_vb = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
+
+	if (src_vb && dst_vb) {
+		v4l2_m2m_buf_done(src_vb, vb_state);
+		v4l2_m2m_buf_done(dst_vb, vb_state);
+		v4l2_m2m_job_finish(ctx->fimc_dev->m2m.m2m_dev,
+				    ctx->fh.m2m_ctx);
+	}
+}
+
+/* Complete the transaction which has been scheduled for execution. */
+static int fimc_m2m_shutdown(struct fimc_ctx *ctx)
+{
+	struct fimc_dev *fimc = ctx->fimc_dev;
+	int ret;
+
+	if (!fimc_m2m_pending(fimc))
+		return 0;
+
+	fimc_ctx_state_set(FIMC_CTX_SHUT, ctx);
+
+	ret = wait_event_timeout(fimc->irq_queue,
+			   !fimc_ctx_state_is_set(FIMC_CTX_SHUT, ctx),
+			   FIMC_SHUTDOWN_TIMEOUT);
+
+	return ret == 0 ? -ETIMEDOUT : ret;
+}
+
+static int start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct fimc_ctx *ctx = q->drv_priv;
+	int ret;
+
+	ret = pm_runtime_get_sync(&ctx->fimc_dev->pdev->dev);
+	return ret > 0 ? 0 : ret;
+}
+
+static void stop_streaming(struct vb2_queue *q)
+{
+	struct fimc_ctx *ctx = q->drv_priv;
+	int ret;
+
+	ret = fimc_m2m_shutdown(ctx);
+	if (ret == -ETIMEDOUT)
+		fimc_m2m_job_finish(ctx, VB2_BUF_STATE_ERROR);
+
+	pm_runtime_put(&ctx->fimc_dev->pdev->dev);
+}
+
+static void fimc_device_run(void *priv)
+{
+	struct vb2_v4l2_buffer *src_vb, *dst_vb;
+	struct fimc_ctx *ctx = priv;
+	struct fimc_frame *sf, *df;
+	struct fimc_dev *fimc;
+	unsigned long flags;
+	int ret;
+
+	if (WARN(!ctx, "Null context\n"))
+		return;
+
+	fimc = ctx->fimc_dev;
+	spin_lock_irqsave(&fimc->slock, flags);
+
+	set_bit(ST_M2M_PEND, &fimc->state);
+	sf = &ctx->s_frame;
+	df = &ctx->d_frame;
+
+	if (ctx->state & FIMC_PARAMS) {
+		/* Prepare the DMA offsets for scaler */
+		fimc_prepare_dma_offset(ctx, sf);
+		fimc_prepare_dma_offset(ctx, df);
+	}
+
+	src_vb = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+	ret = fimc_prepare_addr(ctx, &src_vb->vb2_buf, sf, &sf->paddr);
+	if (ret)
+		goto dma_unlock;
+
+	dst_vb = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
+	ret = fimc_prepare_addr(ctx, &dst_vb->vb2_buf, df, &df->paddr);
+	if (ret)
+		goto dma_unlock;
+
+	dst_vb->timestamp = src_vb->timestamp;
+	dst_vb->flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+	dst_vb->flags |=
+		src_vb->flags & V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+
+	/* Reconfigure hardware if the context has changed. */
+	if (fimc->m2m.ctx != ctx) {
+		ctx->state |= FIMC_PARAMS;
+		fimc->m2m.ctx = ctx;
+	}
+
+	if (ctx->state & FIMC_PARAMS) {
+		fimc_set_yuv_order(ctx);
+		fimc_hw_set_input_path(ctx);
+		fimc_hw_set_in_dma(ctx);
+		ret = fimc_set_scaler_info(ctx);
+		if (ret)
+			goto dma_unlock;
+		fimc_hw_set_prescaler(ctx);
+		fimc_hw_set_mainscaler(ctx);
+		fimc_hw_set_target_format(ctx);
+		fimc_hw_set_rotation(ctx);
+		fimc_hw_set_effect(ctx);
+		fimc_hw_set_out_dma(ctx);
+		if (fimc->drv_data->alpha_color)
+			fimc_hw_set_rgb_alpha(ctx);
+		fimc_hw_set_output_path(ctx);
+	}
+	fimc_hw_set_input_addr(fimc, &sf->paddr);
+	fimc_hw_set_output_addr(fimc, &df->paddr, -1);
+
+	fimc_activate_capture(ctx);
+	ctx->state &= (FIMC_CTX_M2M | FIMC_CTX_CAP);
+	fimc_hw_activate_input_dma(fimc, true);
+
+dma_unlock:
+	spin_unlock_irqrestore(&fimc->slock, flags);
+}
+
+static void fimc_job_abort(void *priv)
+{
+	fimc_m2m_shutdown(priv);
+}
+
+static int fimc_queue_setup(struct vb2_queue *vq, const void *parg,
+			    unsigned int *num_buffers, unsigned int *num_planes,
+			    unsigned int sizes[], void *allocators[])
+{
+	struct fimc_ctx *ctx = vb2_get_drv_priv(vq);
+	struct fimc_frame *f;
+	int i;
+
+	f = ctx_get_frame(ctx, vq->type);
+	if (IS_ERR(f))
+		return PTR_ERR(f);
+	/*
+	 * Return number of non-contiguous planes (plane buffers)
+	 * depending on the configured color format.
+	 */
+	if (!f->fmt)
+		return -EINVAL;
+
+	*num_planes = f->fmt->memplanes;
+	for (i = 0; i < f->fmt->memplanes; i++) {
+		sizes[i] = f->payload[i];
+		allocators[i] = ctx->fimc_dev->alloc_ctx;
+	}
+	return 0;
+}
+
+static int fimc_buf_prepare(struct vb2_buffer *vb)
+{
+	struct fimc_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct fimc_frame *frame;
+	int i;
+
+	frame = ctx_get_frame(ctx, vb->vb2_queue->type);
+	if (IS_ERR(frame))
+		return PTR_ERR(frame);
+
+	for (i = 0; i < frame->fmt->memplanes; i++)
+		vb2_set_plane_payload(vb, i, frame->payload[i]);
+
+	return 0;
+}
+
+static void fimc_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct fimc_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
+}
+
+static struct vb2_ops fimc_qops = {
+	.queue_setup	 = fimc_queue_setup,
+	.buf_prepare	 = fimc_buf_prepare,
+	.buf_queue	 = fimc_buf_queue,
+	.wait_prepare	 = vb2_ops_wait_prepare,
+	.wait_finish	 = vb2_ops_wait_finish,
+	.stop_streaming	 = stop_streaming,
+	.start_streaming = start_streaming,
+};
+
+/*
+ * V4L2 ioctl handlers
+ */
+static int fimc_m2m_querycap(struct file *file, void *fh,
+				     struct v4l2_capability *cap)
+{
+	struct fimc_dev *fimc = video_drvdata(file);
+	unsigned int caps;
+
+	/*
+	 * This is only a mem-to-mem video device. The capture and output
+	 * device capability flags are left only for backward compatibility
+	 * and are scheduled for removal.
+	 */
+	caps = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_M2M_MPLANE |
+		V4L2_CAP_VIDEO_CAPTURE_MPLANE | V4L2_CAP_VIDEO_OUTPUT_MPLANE;
+
+	__fimc_vidioc_querycap(&fimc->pdev->dev, cap, caps);
+	return 0;
+}
+
+static int fimc_m2m_enum_fmt_mplane(struct file *file, void *priv,
+				    struct v4l2_fmtdesc *f)
+{
+	struct fimc_fmt *fmt;
+
+	fmt = fimc_find_format(NULL, NULL, get_m2m_fmt_flags(f->type),
+			       f->index);
+	if (!fmt)
+		return -EINVAL;
+
+	strncpy(f->description, fmt->name, sizeof(f->description) - 1);
+	f->pixelformat = fmt->fourcc;
+	return 0;
+}
+
+static int fimc_m2m_g_fmt_mplane(struct file *file, void *fh,
+				 struct v4l2_format *f)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(fh);
+	struct fimc_frame *frame = ctx_get_frame(ctx, f->type);
+
+	if (IS_ERR(frame))
+		return PTR_ERR(frame);
+
+	__fimc_get_format(frame, f);
+	return 0;
+}
+
+static int fimc_try_fmt_mplane(struct fimc_ctx *ctx, struct v4l2_format *f)
+{
+	struct fimc_dev *fimc = ctx->fimc_dev;
+	const struct fimc_variant *variant = fimc->variant;
+	struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp;
+	struct fimc_fmt *fmt;
+	u32 max_w, mod_x, mod_y;
+
+	if (!IS_M2M(f->type))
+		return -EINVAL;
+
+	fmt = fimc_find_format(&pix->pixelformat, NULL,
+			       get_m2m_fmt_flags(f->type), 0);
+	if (WARN(fmt == NULL, "Pixel format lookup failed"))
+		return -EINVAL;
+
+	if (pix->field == V4L2_FIELD_ANY)
+		pix->field = V4L2_FIELD_NONE;
+	else if (pix->field != V4L2_FIELD_NONE)
+		return -EINVAL;
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		max_w = variant->pix_limit->scaler_dis_w;
+		mod_x = ffs(variant->min_inp_pixsize) - 1;
+	} else {
+		max_w = variant->pix_limit->out_rot_dis_w;
+		mod_x = ffs(variant->min_out_pixsize) - 1;
+	}
+
+	if (tiled_fmt(fmt)) {
+		mod_x = 6; /* 64 x 32 pixels tile */
+		mod_y = 5;
+	} else {
+		if (variant->min_vsize_align == 1)
+			mod_y = fimc_fmt_is_rgb(fmt->color) ? 0 : 1;
+		else
+			mod_y = ffs(variant->min_vsize_align) - 1;
+	}
+
+	v4l_bound_align_image(&pix->width, 16, max_w, mod_x,
+		&pix->height, 8, variant->pix_limit->scaler_dis_w, mod_y, 0);
+
+	fimc_adjust_mplane_format(fmt, pix->width, pix->height, &f->fmt.pix_mp);
+	return 0;
+}
+
+static int fimc_m2m_try_fmt_mplane(struct file *file, void *fh,
+				   struct v4l2_format *f)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(fh);
+	return fimc_try_fmt_mplane(ctx, f);
+}
+
+static void __set_frame_format(struct fimc_frame *frame, struct fimc_fmt *fmt,
+			       struct v4l2_pix_format_mplane *pixm)
+{
+	int i;
+
+	for (i = 0; i < fmt->memplanes; i++) {
+		frame->bytesperline[i] = pixm->plane_fmt[i].bytesperline;
+		frame->payload[i] = pixm->plane_fmt[i].sizeimage;
+	}
+
+	frame->f_width = pixm->width;
+	frame->f_height	= pixm->height;
+	frame->o_width = pixm->width;
+	frame->o_height = pixm->height;
+	frame->width = pixm->width;
+	frame->height = pixm->height;
+	frame->offs_h = 0;
+	frame->offs_v = 0;
+	frame->fmt = fmt;
+}
+
+static int fimc_m2m_s_fmt_mplane(struct file *file, void *fh,
+				 struct v4l2_format *f)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(fh);
+	struct fimc_dev *fimc = ctx->fimc_dev;
+	struct fimc_fmt *fmt;
+	struct vb2_queue *vq;
+	struct fimc_frame *frame;
+	int ret;
+
+	ret = fimc_try_fmt_mplane(ctx, f);
+	if (ret)
+		return ret;
+
+	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
+
+	if (vb2_is_busy(vq)) {
+		v4l2_err(&fimc->m2m.vfd, "queue (%d) busy\n", f->type);
+		return -EBUSY;
+	}
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		frame = &ctx->s_frame;
+	else
+		frame = &ctx->d_frame;
+
+	fmt = fimc_find_format(&f->fmt.pix_mp.pixelformat, NULL,
+			       get_m2m_fmt_flags(f->type), 0);
+	if (!fmt)
+		return -EINVAL;
+
+	__set_frame_format(frame, fmt, &f->fmt.pix_mp);
+
+	/* Update RGB Alpha control state and value range */
+	fimc_alpha_ctrl_update(ctx);
+
+	return 0;
+}
+
+static int fimc_m2m_cropcap(struct file *file, void *fh,
+			    struct v4l2_cropcap *cr)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(fh);
+	struct fimc_frame *frame;
+
+	frame = ctx_get_frame(ctx, cr->type);
+	if (IS_ERR(frame))
+		return PTR_ERR(frame);
+
+	cr->bounds.left = 0;
+	cr->bounds.top = 0;
+	cr->bounds.width = frame->o_width;
+	cr->bounds.height = frame->o_height;
+	cr->defrect = cr->bounds;
+
+	return 0;
+}
+
+static int fimc_m2m_g_crop(struct file *file, void *fh, struct v4l2_crop *cr)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(fh);
+	struct fimc_frame *frame;
+
+	frame = ctx_get_frame(ctx, cr->type);
+	if (IS_ERR(frame))
+		return PTR_ERR(frame);
+
+	cr->c.left = frame->offs_h;
+	cr->c.top = frame->offs_v;
+	cr->c.width = frame->width;
+	cr->c.height = frame->height;
+
+	return 0;
+}
+
+static int fimc_m2m_try_crop(struct fimc_ctx *ctx, struct v4l2_crop *cr)
+{
+	struct fimc_dev *fimc = ctx->fimc_dev;
+	struct fimc_frame *f;
+	u32 min_size, halign, depth = 0;
+	int i;
+
+	if (cr->c.top < 0 || cr->c.left < 0) {
+		v4l2_err(&fimc->m2m.vfd,
+			"doesn't support negative values for top & left\n");
+		return -EINVAL;
+	}
+	if (cr->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		f = &ctx->d_frame;
+	else if (cr->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		f = &ctx->s_frame;
+	else
+		return -EINVAL;
+
+	min_size = (f == &ctx->s_frame) ?
+		fimc->variant->min_inp_pixsize : fimc->variant->min_out_pixsize;
+
+	/* Get pixel alignment constraints. */
+	if (fimc->variant->min_vsize_align == 1)
+		halign = fimc_fmt_is_rgb(f->fmt->color) ? 0 : 1;
+	else
+		halign = ffs(fimc->variant->min_vsize_align) - 1;
+
+	for (i = 0; i < f->fmt->memplanes; i++)
+		depth += f->fmt->depth[i];
+
+	v4l_bound_align_image(&cr->c.width, min_size, f->o_width,
+			      ffs(min_size) - 1,
+			      &cr->c.height, min_size, f->o_height,
+			      halign, 64/(ALIGN(depth, 8)));
+
+	/* adjust left/top if cropping rectangle is out of bounds */
+	if (cr->c.left + cr->c.width > f->o_width)
+		cr->c.left = f->o_width - cr->c.width;
+	if (cr->c.top + cr->c.height > f->o_height)
+		cr->c.top = f->o_height - cr->c.height;
+
+	cr->c.left = round_down(cr->c.left, min_size);
+	cr->c.top  = round_down(cr->c.top, fimc->variant->hor_offs_align);
+
+	dbg("l:%d, t:%d, w:%d, h:%d, f_w: %d, f_h: %d",
+	    cr->c.left, cr->c.top, cr->c.width, cr->c.height,
+	    f->f_width, f->f_height);
+
+	return 0;
+}
+
+static int fimc_m2m_s_crop(struct file *file, void *fh, const struct v4l2_crop *crop)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(fh);
+	struct fimc_dev *fimc = ctx->fimc_dev;
+	struct v4l2_crop cr = *crop;
+	struct fimc_frame *f;
+	int ret;
+
+	ret = fimc_m2m_try_crop(ctx, &cr);
+	if (ret)
+		return ret;
+
+	f = (cr.type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) ?
+		&ctx->s_frame : &ctx->d_frame;
+
+	/* Check to see if scaling ratio is within supported range */
+	if (cr.type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		ret = fimc_check_scaler_ratio(ctx, cr.c.width,
+				cr.c.height, ctx->d_frame.width,
+				ctx->d_frame.height, ctx->rotation);
+	} else {
+		ret = fimc_check_scaler_ratio(ctx, ctx->s_frame.width,
+				ctx->s_frame.height, cr.c.width,
+				cr.c.height, ctx->rotation);
+	}
+	if (ret) {
+		v4l2_err(&fimc->m2m.vfd, "Out of scaler range\n");
+		return -EINVAL;
+	}
+
+	f->offs_h = cr.c.left;
+	f->offs_v = cr.c.top;
+	f->width  = cr.c.width;
+	f->height = cr.c.height;
+
+	fimc_ctx_state_set(FIMC_PARAMS, ctx);
+
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops fimc_m2m_ioctl_ops = {
+	.vidioc_querycap		= fimc_m2m_querycap,
+	.vidioc_enum_fmt_vid_cap_mplane	= fimc_m2m_enum_fmt_mplane,
+	.vidioc_enum_fmt_vid_out_mplane	= fimc_m2m_enum_fmt_mplane,
+	.vidioc_g_fmt_vid_cap_mplane	= fimc_m2m_g_fmt_mplane,
+	.vidioc_g_fmt_vid_out_mplane	= fimc_m2m_g_fmt_mplane,
+	.vidioc_try_fmt_vid_cap_mplane	= fimc_m2m_try_fmt_mplane,
+	.vidioc_try_fmt_vid_out_mplane	= fimc_m2m_try_fmt_mplane,
+	.vidioc_s_fmt_vid_cap_mplane	= fimc_m2m_s_fmt_mplane,
+	.vidioc_s_fmt_vid_out_mplane	= fimc_m2m_s_fmt_mplane,
+	.vidioc_reqbufs			= v4l2_m2m_ioctl_reqbufs,
+	.vidioc_querybuf		= v4l2_m2m_ioctl_querybuf,
+	.vidioc_qbuf			= v4l2_m2m_ioctl_qbuf,
+	.vidioc_dqbuf			= v4l2_m2m_ioctl_dqbuf,
+	.vidioc_expbuf			= v4l2_m2m_ioctl_expbuf,
+	.vidioc_streamon		= v4l2_m2m_ioctl_streamon,
+	.vidioc_streamoff		= v4l2_m2m_ioctl_streamoff,
+	.vidioc_g_crop			= fimc_m2m_g_crop,
+	.vidioc_s_crop			= fimc_m2m_s_crop,
+	.vidioc_cropcap			= fimc_m2m_cropcap
+
+};
+
+static int queue_init(void *priv, struct vb2_queue *src_vq,
+		      struct vb2_queue *dst_vq)
+{
+	struct fimc_ctx *ctx = priv;
+	int ret;
+
+	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+	src_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
+	src_vq->drv_priv = ctx;
+	src_vq->ops = &fimc_qops;
+	src_vq->mem_ops = &vb2_dma_contig_memops;
+	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	src_vq->lock = &ctx->fimc_dev->lock;
+
+	ret = vb2_queue_init(src_vq);
+	if (ret)
+		return ret;
+
+	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	dst_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
+	dst_vq->drv_priv = ctx;
+	dst_vq->ops = &fimc_qops;
+	dst_vq->mem_ops = &vb2_dma_contig_memops;
+	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	dst_vq->lock = &ctx->fimc_dev->lock;
+
+	return vb2_queue_init(dst_vq);
+}
+
+static int fimc_m2m_set_default_format(struct fimc_ctx *ctx)
+{
+	struct v4l2_pix_format_mplane pixm = {
+		.pixelformat	= V4L2_PIX_FMT_RGB32,
+		.width		= 800,
+		.height		= 600,
+		.plane_fmt[0]	= {
+			.bytesperline = 800 * 4,
+			.sizeimage = 800 * 4 * 600,
+		},
+	};
+	struct fimc_fmt *fmt;
+
+	fmt = fimc_find_format(&pixm.pixelformat, NULL, FMT_FLAGS_M2M, 0);
+	if (!fmt)
+		return -EINVAL;
+
+	__set_frame_format(&ctx->s_frame, fmt, &pixm);
+	__set_frame_format(&ctx->d_frame, fmt, &pixm);
+
+	return 0;
+}
+
+static int fimc_m2m_open(struct file *file)
+{
+	struct fimc_dev *fimc = video_drvdata(file);
+	struct fimc_ctx *ctx;
+	int ret = -EBUSY;
+
+	pr_debug("pid: %d, state: %#lx\n", task_pid_nr(current), fimc->state);
+
+	if (mutex_lock_interruptible(&fimc->lock))
+		return -ERESTARTSYS;
+	/*
+	 * Don't allow simultaneous open() of the mem-to-mem and the
+	 * capture video node that belong to same FIMC IP instance.
+	 */
+	if (test_bit(ST_CAPT_BUSY, &fimc->state))
+		goto unlock;
+
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx) {
+		ret = -ENOMEM;
+		goto unlock;
+	}
+	v4l2_fh_init(&ctx->fh, &fimc->m2m.vfd);
+	ctx->fimc_dev = fimc;
+
+	/* Default color format */
+	ctx->s_frame.fmt = fimc_get_format(0);
+	ctx->d_frame.fmt = fimc_get_format(0);
+
+	ret = fimc_ctrls_create(ctx);
+	if (ret)
+		goto error_fh;
+
+	/* Use separate control handler per file handle */
+	ctx->fh.ctrl_handler = &ctx->ctrls.handler;
+	file->private_data = &ctx->fh;
+	v4l2_fh_add(&ctx->fh);
+
+	/* Setup the device context for memory-to-memory mode */
+	ctx->state = FIMC_CTX_M2M;
+	ctx->flags = 0;
+	ctx->in_path = FIMC_IO_DMA;
+	ctx->out_path = FIMC_IO_DMA;
+	ctx->scaler.enabled = 1;
+
+	ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(fimc->m2m.m2m_dev, ctx, queue_init);
+	if (IS_ERR(ctx->fh.m2m_ctx)) {
+		ret = PTR_ERR(ctx->fh.m2m_ctx);
+		goto error_c;
+	}
+
+	if (fimc->m2m.refcnt++ == 0)
+		set_bit(ST_M2M_RUN, &fimc->state);
+
+	ret = fimc_m2m_set_default_format(ctx);
+	if (ret < 0)
+		goto error_m2m_ctx;
+
+	mutex_unlock(&fimc->lock);
+	return 0;
+
+error_m2m_ctx:
+	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
+error_c:
+	fimc_ctrls_delete(ctx);
+error_fh:
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	kfree(ctx);
+unlock:
+	mutex_unlock(&fimc->lock);
+	return ret;
+}
+
+static int fimc_m2m_release(struct file *file)
+{
+	struct fimc_ctx *ctx = fh_to_ctx(file->private_data);
+	struct fimc_dev *fimc = ctx->fimc_dev;
+
+	dbg("pid: %d, state: 0x%lx, refcnt= %d",
+		task_pid_nr(current), fimc->state, fimc->m2m.refcnt);
+
+	mutex_lock(&fimc->lock);
+
+	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
+	fimc_ctrls_delete(ctx);
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+
+	if (--fimc->m2m.refcnt <= 0)
+		clear_bit(ST_M2M_RUN, &fimc->state);
+	kfree(ctx);
+
+	mutex_unlock(&fimc->lock);
+	return 0;
+}
+
+static const struct v4l2_file_operations fimc_m2m_fops = {
+	.owner		= THIS_MODULE,
+	.open		= fimc_m2m_open,
+	.release	= fimc_m2m_release,
+	.poll		= v4l2_m2m_fop_poll,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap		= v4l2_m2m_fop_mmap,
+};
+
+static struct v4l2_m2m_ops m2m_ops = {
+	.device_run	= fimc_device_run,
+	.job_abort	= fimc_job_abort,
+};
+
+int fimc_register_m2m_device(struct fimc_dev *fimc,
+			     struct v4l2_device *v4l2_dev)
+{
+	struct video_device *vfd = &fimc->m2m.vfd;
+	int ret;
+
+	fimc->v4l2_dev = v4l2_dev;
+
+	memset(vfd, 0, sizeof(*vfd));
+	vfd->fops = &fimc_m2m_fops;
+	vfd->ioctl_ops = &fimc_m2m_ioctl_ops;
+	vfd->v4l2_dev = v4l2_dev;
+	vfd->minor = -1;
+	vfd->release = video_device_release_empty;
+	vfd->lock = &fimc->lock;
+	vfd->vfl_dir = VFL_DIR_M2M;
+
+	snprintf(vfd->name, sizeof(vfd->name), "fimc.%d.m2m", fimc->id);
+	video_set_drvdata(vfd, fimc);
+
+	fimc->m2m.m2m_dev = v4l2_m2m_init(&m2m_ops);
+	if (IS_ERR(fimc->m2m.m2m_dev)) {
+		v4l2_err(v4l2_dev, "failed to initialize v4l2-m2m device\n");
+		return PTR_ERR(fimc->m2m.m2m_dev);
+	}
+
+	ret = media_entity_init(&vfd->entity, 0, NULL, 0);
+	if (ret)
+		goto err_me;
+
+	ret = video_register_device(vfd, VFL_TYPE_GRABBER, -1);
+	if (ret)
+		goto err_vd;
+
+	v4l2_info(v4l2_dev, "Registered %s as /dev/%s\n",
+		  vfd->name, video_device_node_name(vfd));
+	return 0;
+
+err_vd:
+	media_entity_cleanup(&vfd->entity);
+err_me:
+	v4l2_m2m_release(fimc->m2m.m2m_dev);
+	return ret;
+}
+
+void fimc_unregister_m2m_device(struct fimc_dev *fimc)
+{
+	if (!fimc)
+		return;
+
+	if (fimc->m2m.m2m_dev)
+		v4l2_m2m_release(fimc->m2m.m2m_dev);
+
+	if (video_is_registered(&fimc->m2m.vfd)) {
+		video_unregister_device(&fimc->m2m.vfd);
+		media_entity_cleanup(&fimc->m2m.vfd.entity);
+	}
+}
diff --git a/drivers/media/platform/exynos4-is/fimc-reg.c b/drivers/media/platform/exynos4-is/fimc-reg.c
new file mode 100644
index 0000000..df0cbcb
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-reg.c
@@ -0,0 +1,842 @@
+/*
+ * Register interface file for Samsung Camera Interface (FIMC) driver
+ *
+ * Copyright (C) 2010 - 2013 Samsung Electronics Co., Ltd.
+ * Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+*/
+
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/regmap.h>
+
+#include <media/exynos-fimc.h>
+#include "media-dev.h"
+
+#include "fimc-reg.h"
+#include "fimc-core.h"
+
+void fimc_hw_reset(struct fimc_dev *dev)
+{
+	u32 cfg;
+
+	cfg = readl(dev->regs + FIMC_REG_CISRCFMT);
+	cfg |= FIMC_REG_CISRCFMT_ITU601_8BIT;
+	writel(cfg, dev->regs + FIMC_REG_CISRCFMT);
+
+	/* Software reset. */
+	cfg = readl(dev->regs + FIMC_REG_CIGCTRL);
+	cfg |= (FIMC_REG_CIGCTRL_SWRST | FIMC_REG_CIGCTRL_IRQ_LEVEL);
+	writel(cfg, dev->regs + FIMC_REG_CIGCTRL);
+	udelay(10);
+
+	cfg = readl(dev->regs + FIMC_REG_CIGCTRL);
+	cfg &= ~FIMC_REG_CIGCTRL_SWRST;
+	writel(cfg, dev->regs + FIMC_REG_CIGCTRL);
+
+	if (dev->drv_data->out_buf_count > 4)
+		fimc_hw_set_dma_seq(dev, 0xF);
+}
+
+static u32 fimc_hw_get_in_flip(struct fimc_ctx *ctx)
+{
+	u32 flip = FIMC_REG_MSCTRL_FLIP_NORMAL;
+
+	if (ctx->hflip)
+		flip = FIMC_REG_MSCTRL_FLIP_Y_MIRROR;
+	if (ctx->vflip)
+		flip = FIMC_REG_MSCTRL_FLIP_X_MIRROR;
+
+	if (ctx->rotation <= 90)
+		return flip;
+
+	return (flip ^ FIMC_REG_MSCTRL_FLIP_180) & FIMC_REG_MSCTRL_FLIP_180;
+}
+
+static u32 fimc_hw_get_target_flip(struct fimc_ctx *ctx)
+{
+	u32 flip = FIMC_REG_CITRGFMT_FLIP_NORMAL;
+
+	if (ctx->hflip)
+		flip |= FIMC_REG_CITRGFMT_FLIP_Y_MIRROR;
+	if (ctx->vflip)
+		flip |= FIMC_REG_CITRGFMT_FLIP_X_MIRROR;
+
+	if (ctx->rotation <= 90)
+		return flip;
+
+	return (flip ^ FIMC_REG_CITRGFMT_FLIP_180) & FIMC_REG_CITRGFMT_FLIP_180;
+}
+
+void fimc_hw_set_rotation(struct fimc_ctx *ctx)
+{
+	u32 cfg, flip;
+	struct fimc_dev *dev = ctx->fimc_dev;
+
+	cfg = readl(dev->regs + FIMC_REG_CITRGFMT);
+	cfg &= ~(FIMC_REG_CITRGFMT_INROT90 | FIMC_REG_CITRGFMT_OUTROT90 |
+		 FIMC_REG_CITRGFMT_FLIP_180);
+
+	/*
+	 * The input and output rotator cannot work simultaneously.
+	 * Use the output rotator in output DMA mode or the input rotator
+	 * in direct fifo output mode.
+	 */
+	if (ctx->rotation == 90 || ctx->rotation == 270) {
+		if (ctx->out_path == FIMC_IO_LCDFIFO)
+			cfg |= FIMC_REG_CITRGFMT_INROT90;
+		else
+			cfg |= FIMC_REG_CITRGFMT_OUTROT90;
+	}
+
+	if (ctx->out_path == FIMC_IO_DMA) {
+		cfg |= fimc_hw_get_target_flip(ctx);
+		writel(cfg, dev->regs + FIMC_REG_CITRGFMT);
+	} else {
+		/* LCD FIFO path */
+		flip = readl(dev->regs + FIMC_REG_MSCTRL);
+		flip &= ~FIMC_REG_MSCTRL_FLIP_MASK;
+		flip |= fimc_hw_get_in_flip(ctx);
+		writel(flip, dev->regs + FIMC_REG_MSCTRL);
+	}
+}
+
+void fimc_hw_set_target_format(struct fimc_ctx *ctx)
+{
+	u32 cfg;
+	struct fimc_dev *dev = ctx->fimc_dev;
+	struct fimc_frame *frame = &ctx->d_frame;
+
+	dbg("w= %d, h= %d color: %d", frame->width,
+	    frame->height, frame->fmt->color);
+
+	cfg = readl(dev->regs + FIMC_REG_CITRGFMT);
+	cfg &= ~(FIMC_REG_CITRGFMT_FMT_MASK | FIMC_REG_CITRGFMT_HSIZE_MASK |
+		 FIMC_REG_CITRGFMT_VSIZE_MASK);
+
+	switch (frame->fmt->color) {
+	case FIMC_FMT_RGB444...FIMC_FMT_RGB888:
+		cfg |= FIMC_REG_CITRGFMT_RGB;
+		break;
+	case FIMC_FMT_YCBCR420:
+		cfg |= FIMC_REG_CITRGFMT_YCBCR420;
+		break;
+	case FIMC_FMT_YCBYCR422...FIMC_FMT_CRYCBY422:
+		if (frame->fmt->colplanes == 1)
+			cfg |= FIMC_REG_CITRGFMT_YCBCR422_1P;
+		else
+			cfg |= FIMC_REG_CITRGFMT_YCBCR422;
+		break;
+	default:
+		break;
+	}
+
+	if (ctx->rotation == 90 || ctx->rotation == 270)
+		cfg |= (frame->height << 16) | frame->width;
+	else
+		cfg |= (frame->width << 16) | frame->height;
+
+	writel(cfg, dev->regs + FIMC_REG_CITRGFMT);
+
+	cfg = readl(dev->regs + FIMC_REG_CITAREA);
+	cfg &= ~FIMC_REG_CITAREA_MASK;
+	cfg |= (frame->width * frame->height);
+	writel(cfg, dev->regs + FIMC_REG_CITAREA);
+}
+
+static void fimc_hw_set_out_dma_size(struct fimc_ctx *ctx)
+{
+	struct fimc_dev *dev = ctx->fimc_dev;
+	struct fimc_frame *frame = &ctx->d_frame;
+	u32 cfg;
+
+	cfg = (frame->f_height << 16) | frame->f_width;
+	writel(cfg, dev->regs + FIMC_REG_ORGOSIZE);
+
+	/* Select color space conversion equation (HD/SD size).*/
+	cfg = readl(dev->regs + FIMC_REG_CIGCTRL);
+	if (frame->f_width >= 1280) /* HD */
+		cfg |= FIMC_REG_CIGCTRL_CSC_ITU601_709;
+	else	/* SD */
+		cfg &= ~FIMC_REG_CIGCTRL_CSC_ITU601_709;
+	writel(cfg, dev->regs + FIMC_REG_CIGCTRL);
+
+}
+
+void fimc_hw_set_out_dma(struct fimc_ctx *ctx)
+{
+	struct fimc_dev *dev = ctx->fimc_dev;
+	struct fimc_frame *frame = &ctx->d_frame;
+	struct fimc_dma_offset *offset = &frame->dma_offset;
+	struct fimc_fmt *fmt = frame->fmt;
+	u32 cfg;
+
+	/* Set the input dma offsets. */
+	cfg = (offset->y_v << 16) | offset->y_h;
+	writel(cfg, dev->regs + FIMC_REG_CIOYOFF);
+
+	cfg = (offset->cb_v << 16) | offset->cb_h;
+	writel(cfg, dev->regs + FIMC_REG_CIOCBOFF);
+
+	cfg = (offset->cr_v << 16) | offset->cr_h;
+	writel(cfg, dev->regs + FIMC_REG_CIOCROFF);
+
+	fimc_hw_set_out_dma_size(ctx);
+
+	/* Configure chroma components order. */
+	cfg = readl(dev->regs + FIMC_REG_CIOCTRL);
+
+	cfg &= ~(FIMC_REG_CIOCTRL_ORDER2P_MASK |
+		 FIMC_REG_CIOCTRL_ORDER422_MASK |
+		 FIMC_REG_CIOCTRL_YCBCR_PLANE_MASK |
+		 FIMC_REG_CIOCTRL_RGB16FMT_MASK);
+
+	if (fmt->colplanes == 1)
+		cfg |= ctx->out_order_1p;
+	else if (fmt->colplanes == 2)
+		cfg |= ctx->out_order_2p | FIMC_REG_CIOCTRL_YCBCR_2PLANE;
+	else if (fmt->colplanes == 3)
+		cfg |= FIMC_REG_CIOCTRL_YCBCR_3PLANE;
+
+	if (fmt->color == FIMC_FMT_RGB565)
+		cfg |= FIMC_REG_CIOCTRL_RGB565;
+	else if (fmt->color == FIMC_FMT_RGB555)
+		cfg |= FIMC_REG_CIOCTRL_ARGB1555;
+	else if (fmt->color == FIMC_FMT_RGB444)
+		cfg |= FIMC_REG_CIOCTRL_ARGB4444;
+
+	writel(cfg, dev->regs + FIMC_REG_CIOCTRL);
+}
+
+static void fimc_hw_en_autoload(struct fimc_dev *dev, int enable)
+{
+	u32 cfg = readl(dev->regs + FIMC_REG_ORGISIZE);
+	if (enable)
+		cfg |= FIMC_REG_CIREAL_ISIZE_AUTOLOAD_EN;
+	else
+		cfg &= ~FIMC_REG_CIREAL_ISIZE_AUTOLOAD_EN;
+	writel(cfg, dev->regs + FIMC_REG_ORGISIZE);
+}
+
+void fimc_hw_en_lastirq(struct fimc_dev *dev, int enable)
+{
+	u32 cfg = readl(dev->regs + FIMC_REG_CIOCTRL);
+	if (enable)
+		cfg |= FIMC_REG_CIOCTRL_LASTIRQ_ENABLE;
+	else
+		cfg &= ~FIMC_REG_CIOCTRL_LASTIRQ_ENABLE;
+	writel(cfg, dev->regs + FIMC_REG_CIOCTRL);
+}
+
+void fimc_hw_set_prescaler(struct fimc_ctx *ctx)
+{
+	struct fimc_dev *dev =  ctx->fimc_dev;
+	struct fimc_scaler *sc = &ctx->scaler;
+	u32 cfg, shfactor;
+
+	shfactor = 10 - (sc->hfactor + sc->vfactor);
+	cfg = shfactor << 28;
+
+	cfg |= (sc->pre_hratio << 16) | sc->pre_vratio;
+	writel(cfg, dev->regs + FIMC_REG_CISCPRERATIO);
+
+	cfg = (sc->pre_dst_width << 16) | sc->pre_dst_height;
+	writel(cfg, dev->regs + FIMC_REG_CISCPREDST);
+}
+
+static void fimc_hw_set_scaler(struct fimc_ctx *ctx)
+{
+	struct fimc_dev *dev = ctx->fimc_dev;
+	struct fimc_scaler *sc = &ctx->scaler;
+	struct fimc_frame *src_frame = &ctx->s_frame;
+	struct fimc_frame *dst_frame = &ctx->d_frame;
+
+	u32 cfg = readl(dev->regs + FIMC_REG_CISCCTRL);
+
+	cfg &= ~(FIMC_REG_CISCCTRL_CSCR2Y_WIDE | FIMC_REG_CISCCTRL_CSCY2R_WIDE |
+		 FIMC_REG_CISCCTRL_SCALEUP_H | FIMC_REG_CISCCTRL_SCALEUP_V |
+		 FIMC_REG_CISCCTRL_SCALERBYPASS | FIMC_REG_CISCCTRL_ONE2ONE |
+		 FIMC_REG_CISCCTRL_INRGB_FMT_MASK | FIMC_REG_CISCCTRL_OUTRGB_FMT_MASK |
+		 FIMC_REG_CISCCTRL_INTERLACE | FIMC_REG_CISCCTRL_RGB_EXT);
+
+	if (!(ctx->flags & FIMC_COLOR_RANGE_NARROW))
+		cfg |= (FIMC_REG_CISCCTRL_CSCR2Y_WIDE |
+			FIMC_REG_CISCCTRL_CSCY2R_WIDE);
+
+	if (!sc->enabled)
+		cfg |= FIMC_REG_CISCCTRL_SCALERBYPASS;
+
+	if (sc->scaleup_h)
+		cfg |= FIMC_REG_CISCCTRL_SCALEUP_H;
+
+	if (sc->scaleup_v)
+		cfg |= FIMC_REG_CISCCTRL_SCALEUP_V;
+
+	if (sc->copy_mode)
+		cfg |= FIMC_REG_CISCCTRL_ONE2ONE;
+
+	if (ctx->in_path == FIMC_IO_DMA) {
+		switch (src_frame->fmt->color) {
+		case FIMC_FMT_RGB565:
+			cfg |= FIMC_REG_CISCCTRL_INRGB_FMT_RGB565;
+			break;
+		case FIMC_FMT_RGB666:
+			cfg |= FIMC_REG_CISCCTRL_INRGB_FMT_RGB666;
+			break;
+		case FIMC_FMT_RGB888:
+			cfg |= FIMC_REG_CISCCTRL_INRGB_FMT_RGB888;
+			break;
+		}
+	}
+
+	if (ctx->out_path == FIMC_IO_DMA) {
+		u32 color = dst_frame->fmt->color;
+
+		if (color >= FIMC_FMT_RGB444 && color <= FIMC_FMT_RGB565)
+			cfg |= FIMC_REG_CISCCTRL_OUTRGB_FMT_RGB565;
+		else if (color == FIMC_FMT_RGB666)
+			cfg |= FIMC_REG_CISCCTRL_OUTRGB_FMT_RGB666;
+		else if (color == FIMC_FMT_RGB888)
+			cfg |= FIMC_REG_CISCCTRL_OUTRGB_FMT_RGB888;
+	} else {
+		cfg |= FIMC_REG_CISCCTRL_OUTRGB_FMT_RGB888;
+
+		if (ctx->flags & FIMC_SCAN_MODE_INTERLACED)
+			cfg |= FIMC_REG_CISCCTRL_INTERLACE;
+	}
+
+	writel(cfg, dev->regs + FIMC_REG_CISCCTRL);
+}
+
+void fimc_hw_set_mainscaler(struct fimc_ctx *ctx)
+{
+	struct fimc_dev *dev = ctx->fimc_dev;
+	const struct fimc_variant *variant = dev->variant;
+	struct fimc_scaler *sc = &ctx->scaler;
+	u32 cfg;
+
+	dbg("main_hratio= 0x%X  main_vratio= 0x%X",
+	    sc->main_hratio, sc->main_vratio);
+
+	fimc_hw_set_scaler(ctx);
+
+	cfg = readl(dev->regs + FIMC_REG_CISCCTRL);
+	cfg &= ~(FIMC_REG_CISCCTRL_MHRATIO_MASK |
+		 FIMC_REG_CISCCTRL_MVRATIO_MASK);
+
+	if (variant->has_mainscaler_ext) {
+		cfg |= FIMC_REG_CISCCTRL_MHRATIO_EXT(sc->main_hratio);
+		cfg |= FIMC_REG_CISCCTRL_MVRATIO_EXT(sc->main_vratio);
+		writel(cfg, dev->regs + FIMC_REG_CISCCTRL);
+
+		cfg = readl(dev->regs + FIMC_REG_CIEXTEN);
+
+		cfg &= ~(FIMC_REG_CIEXTEN_MVRATIO_EXT_MASK |
+			 FIMC_REG_CIEXTEN_MHRATIO_EXT_MASK);
+		cfg |= FIMC_REG_CIEXTEN_MHRATIO_EXT(sc->main_hratio);
+		cfg |= FIMC_REG_CIEXTEN_MVRATIO_EXT(sc->main_vratio);
+		writel(cfg, dev->regs + FIMC_REG_CIEXTEN);
+	} else {
+		cfg |= FIMC_REG_CISCCTRL_MHRATIO(sc->main_hratio);
+		cfg |= FIMC_REG_CISCCTRL_MVRATIO(sc->main_vratio);
+		writel(cfg, dev->regs + FIMC_REG_CISCCTRL);
+	}
+}
+
+void fimc_hw_enable_capture(struct fimc_ctx *ctx)
+{
+	struct fimc_dev *dev = ctx->fimc_dev;
+	u32 cfg;
+
+	cfg = readl(dev->regs + FIMC_REG_CIIMGCPT);
+	cfg |= FIMC_REG_CIIMGCPT_CPT_FREN_ENABLE;
+
+	if (ctx->scaler.enabled)
+		cfg |= FIMC_REG_CIIMGCPT_IMGCPTEN_SC;
+	else
+		cfg &= FIMC_REG_CIIMGCPT_IMGCPTEN_SC;
+
+	cfg |= FIMC_REG_CIIMGCPT_IMGCPTEN;
+	writel(cfg, dev->regs + FIMC_REG_CIIMGCPT);
+}
+
+void fimc_hw_disable_capture(struct fimc_dev *dev)
+{
+	u32 cfg = readl(dev->regs + FIMC_REG_CIIMGCPT);
+	cfg &= ~(FIMC_REG_CIIMGCPT_IMGCPTEN |
+		 FIMC_REG_CIIMGCPT_IMGCPTEN_SC);
+	writel(cfg, dev->regs + FIMC_REG_CIIMGCPT);
+}
+
+void fimc_hw_set_effect(struct fimc_ctx *ctx)
+{
+	struct fimc_dev *dev = ctx->fimc_dev;
+	struct fimc_effect *effect = &ctx->effect;
+	u32 cfg = 0;
+
+	if (effect->type != FIMC_REG_CIIMGEFF_FIN_BYPASS) {
+		cfg |= FIMC_REG_CIIMGEFF_IE_SC_AFTER |
+			FIMC_REG_CIIMGEFF_IE_ENABLE;
+		cfg |= effect->type;
+		if (effect->type == FIMC_REG_CIIMGEFF_FIN_ARBITRARY)
+			cfg |= (effect->pat_cb << 13) | effect->pat_cr;
+	}
+
+	writel(cfg, dev->regs + FIMC_REG_CIIMGEFF);
+}
+
+void fimc_hw_set_rgb_alpha(struct fimc_ctx *ctx)
+{
+	struct fimc_dev *dev = ctx->fimc_dev;
+	struct fimc_frame *frame = &ctx->d_frame;
+	u32 cfg;
+
+	if (!(frame->fmt->flags & FMT_HAS_ALPHA))
+		return;
+
+	cfg = readl(dev->regs + FIMC_REG_CIOCTRL);
+	cfg &= ~FIMC_REG_CIOCTRL_ALPHA_OUT_MASK;
+	cfg |= (frame->alpha << 4);
+	writel(cfg, dev->regs + FIMC_REG_CIOCTRL);
+}
+
+static void fimc_hw_set_in_dma_size(struct fimc_ctx *ctx)
+{
+	struct fimc_dev *dev = ctx->fimc_dev;
+	struct fimc_frame *frame = &ctx->s_frame;
+	u32 cfg_o = 0;
+	u32 cfg_r = 0;
+
+	if (FIMC_IO_LCDFIFO == ctx->out_path)
+		cfg_r |= FIMC_REG_CIREAL_ISIZE_AUTOLOAD_EN;
+
+	cfg_o |= (frame->f_height << 16) | frame->f_width;
+	cfg_r |= (frame->height << 16) | frame->width;
+
+	writel(cfg_o, dev->regs + FIMC_REG_ORGISIZE);
+	writel(cfg_r, dev->regs + FIMC_REG_CIREAL_ISIZE);
+}
+
+void fimc_hw_set_in_dma(struct fimc_ctx *ctx)
+{
+	struct fimc_dev *dev = ctx->fimc_dev;
+	struct fimc_frame *frame = &ctx->s_frame;
+	struct fimc_dma_offset *offset = &frame->dma_offset;
+	u32 cfg;
+
+	/* Set the pixel offsets. */
+	cfg = (offset->y_v << 16) | offset->y_h;
+	writel(cfg, dev->regs + FIMC_REG_CIIYOFF);
+
+	cfg = (offset->cb_v << 16) | offset->cb_h;
+	writel(cfg, dev->regs + FIMC_REG_CIICBOFF);
+
+	cfg = (offset->cr_v << 16) | offset->cr_h;
+	writel(cfg, dev->regs + FIMC_REG_CIICROFF);
+
+	/* Input original and real size. */
+	fimc_hw_set_in_dma_size(ctx);
+
+	/* Use DMA autoload only in FIFO mode. */
+	fimc_hw_en_autoload(dev, ctx->out_path == FIMC_IO_LCDFIFO);
+
+	/* Set the input DMA to process single frame only. */
+	cfg = readl(dev->regs + FIMC_REG_MSCTRL);
+	cfg &= ~(FIMC_REG_MSCTRL_INFORMAT_MASK
+		 | FIMC_REG_MSCTRL_IN_BURST_COUNT_MASK
+		 | FIMC_REG_MSCTRL_INPUT_MASK
+		 | FIMC_REG_MSCTRL_C_INT_IN_MASK
+		 | FIMC_REG_MSCTRL_2P_IN_ORDER_MASK
+		 | FIMC_REG_MSCTRL_ORDER422_MASK);
+
+	cfg |= (FIMC_REG_MSCTRL_IN_BURST_COUNT(4)
+		| FIMC_REG_MSCTRL_INPUT_MEMORY
+		| FIMC_REG_MSCTRL_FIFO_CTRL_FULL);
+
+	switch (frame->fmt->color) {
+	case FIMC_FMT_RGB565...FIMC_FMT_RGB888:
+		cfg |= FIMC_REG_MSCTRL_INFORMAT_RGB;
+		break;
+	case FIMC_FMT_YCBCR420:
+		cfg |= FIMC_REG_MSCTRL_INFORMAT_YCBCR420;
+
+		if (frame->fmt->colplanes == 2)
+			cfg |= ctx->in_order_2p | FIMC_REG_MSCTRL_C_INT_IN_2PLANE;
+		else
+			cfg |= FIMC_REG_MSCTRL_C_INT_IN_3PLANE;
+
+		break;
+	case FIMC_FMT_YCBYCR422...FIMC_FMT_CRYCBY422:
+		if (frame->fmt->colplanes == 1) {
+			cfg |= ctx->in_order_1p
+				| FIMC_REG_MSCTRL_INFORMAT_YCBCR422_1P;
+		} else {
+			cfg |= FIMC_REG_MSCTRL_INFORMAT_YCBCR422;
+
+			if (frame->fmt->colplanes == 2)
+				cfg |= ctx->in_order_2p
+					| FIMC_REG_MSCTRL_C_INT_IN_2PLANE;
+			else
+				cfg |= FIMC_REG_MSCTRL_C_INT_IN_3PLANE;
+		}
+		break;
+	default:
+		break;
+	}
+
+	writel(cfg, dev->regs + FIMC_REG_MSCTRL);
+
+	/* Input/output DMA linear/tiled mode. */
+	cfg = readl(dev->regs + FIMC_REG_CIDMAPARAM);
+	cfg &= ~FIMC_REG_CIDMAPARAM_TILE_MASK;
+
+	if (tiled_fmt(ctx->s_frame.fmt))
+		cfg |= FIMC_REG_CIDMAPARAM_R_64X32;
+
+	if (tiled_fmt(ctx->d_frame.fmt))
+		cfg |= FIMC_REG_CIDMAPARAM_W_64X32;
+
+	writel(cfg, dev->regs + FIMC_REG_CIDMAPARAM);
+}
+
+
+void fimc_hw_set_input_path(struct fimc_ctx *ctx)
+{
+	struct fimc_dev *dev = ctx->fimc_dev;
+
+	u32 cfg = readl(dev->regs + FIMC_REG_MSCTRL);
+	cfg &= ~FIMC_REG_MSCTRL_INPUT_MASK;
+
+	if (ctx->in_path == FIMC_IO_DMA)
+		cfg |= FIMC_REG_MSCTRL_INPUT_MEMORY;
+	else
+		cfg |= FIMC_REG_MSCTRL_INPUT_EXTCAM;
+
+	writel(cfg, dev->regs + FIMC_REG_MSCTRL);
+}
+
+void fimc_hw_set_output_path(struct fimc_ctx *ctx)
+{
+	struct fimc_dev *dev = ctx->fimc_dev;
+
+	u32 cfg = readl(dev->regs + FIMC_REG_CISCCTRL);
+	cfg &= ~FIMC_REG_CISCCTRL_LCDPATHEN_FIFO;
+	if (ctx->out_path == FIMC_IO_LCDFIFO)
+		cfg |= FIMC_REG_CISCCTRL_LCDPATHEN_FIFO;
+	writel(cfg, dev->regs + FIMC_REG_CISCCTRL);
+}
+
+void fimc_hw_set_input_addr(struct fimc_dev *dev, struct fimc_addr *paddr)
+{
+	u32 cfg = readl(dev->regs + FIMC_REG_CIREAL_ISIZE);
+	cfg |= FIMC_REG_CIREAL_ISIZE_ADDR_CH_DIS;
+	writel(cfg, dev->regs + FIMC_REG_CIREAL_ISIZE);
+
+	writel(paddr->y, dev->regs + FIMC_REG_CIIYSA(0));
+	writel(paddr->cb, dev->regs + FIMC_REG_CIICBSA(0));
+	writel(paddr->cr, dev->regs + FIMC_REG_CIICRSA(0));
+
+	cfg &= ~FIMC_REG_CIREAL_ISIZE_ADDR_CH_DIS;
+	writel(cfg, dev->regs + FIMC_REG_CIREAL_ISIZE);
+}
+
+void fimc_hw_set_output_addr(struct fimc_dev *dev,
+			     struct fimc_addr *paddr, int index)
+{
+	int i = (index == -1) ? 0 : index;
+	do {
+		writel(paddr->y, dev->regs + FIMC_REG_CIOYSA(i));
+		writel(paddr->cb, dev->regs + FIMC_REG_CIOCBSA(i));
+		writel(paddr->cr, dev->regs + FIMC_REG_CIOCRSA(i));
+		dbg("dst_buf[%d]: 0x%X, cb: 0x%X, cr: 0x%X",
+		    i, paddr->y, paddr->cb, paddr->cr);
+	} while (index == -1 && ++i < FIMC_MAX_OUT_BUFS);
+}
+
+int fimc_hw_set_camera_polarity(struct fimc_dev *fimc,
+				struct fimc_source_info *cam)
+{
+	u32 cfg = readl(fimc->regs + FIMC_REG_CIGCTRL);
+
+	cfg &= ~(FIMC_REG_CIGCTRL_INVPOLPCLK | FIMC_REG_CIGCTRL_INVPOLVSYNC |
+		 FIMC_REG_CIGCTRL_INVPOLHREF | FIMC_REG_CIGCTRL_INVPOLHSYNC |
+		 FIMC_REG_CIGCTRL_INVPOLFIELD);
+
+	if (cam->flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
+		cfg |= FIMC_REG_CIGCTRL_INVPOLPCLK;
+
+	if (cam->flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
+		cfg |= FIMC_REG_CIGCTRL_INVPOLVSYNC;
+
+	if (cam->flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)
+		cfg |= FIMC_REG_CIGCTRL_INVPOLHREF;
+
+	if (cam->flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)
+		cfg |= FIMC_REG_CIGCTRL_INVPOLHSYNC;
+
+	if (cam->flags & V4L2_MBUS_FIELD_EVEN_LOW)
+		cfg |= FIMC_REG_CIGCTRL_INVPOLFIELD;
+
+	writel(cfg, fimc->regs + FIMC_REG_CIGCTRL);
+
+	return 0;
+}
+
+struct mbus_pixfmt_desc {
+	u32 pixelcode;
+	u32 cisrcfmt;
+	u16 bus_width;
+};
+
+static const struct mbus_pixfmt_desc pix_desc[] = {
+	{ MEDIA_BUS_FMT_YUYV8_2X8, FIMC_REG_CISRCFMT_ORDER422_YCBYCR, 8 },
+	{ MEDIA_BUS_FMT_YVYU8_2X8, FIMC_REG_CISRCFMT_ORDER422_YCRYCB, 8 },
+	{ MEDIA_BUS_FMT_VYUY8_2X8, FIMC_REG_CISRCFMT_ORDER422_CRYCBY, 8 },
+	{ MEDIA_BUS_FMT_UYVY8_2X8, FIMC_REG_CISRCFMT_ORDER422_CBYCRY, 8 },
+};
+
+int fimc_hw_set_camera_source(struct fimc_dev *fimc,
+			      struct fimc_source_info *source)
+{
+	struct fimc_vid_cap *vc = &fimc->vid_cap;
+	struct fimc_frame *f = &vc->ctx->s_frame;
+	u32 bus_width, cfg = 0;
+	int i;
+
+	switch (source->fimc_bus_type) {
+	case FIMC_BUS_TYPE_ITU_601:
+	case FIMC_BUS_TYPE_ITU_656:
+		for (i = 0; i < ARRAY_SIZE(pix_desc); i++) {
+			if (vc->ci_fmt.code == pix_desc[i].pixelcode) {
+				cfg = pix_desc[i].cisrcfmt;
+				bus_width = pix_desc[i].bus_width;
+				break;
+			}
+		}
+
+		if (i == ARRAY_SIZE(pix_desc)) {
+			v4l2_err(&vc->ve.vdev,
+				 "Camera color format not supported: %d\n",
+				 vc->ci_fmt.code);
+			return -EINVAL;
+		}
+
+		if (source->fimc_bus_type == FIMC_BUS_TYPE_ITU_601) {
+			if (bus_width == 8)
+				cfg |= FIMC_REG_CISRCFMT_ITU601_8BIT;
+			else if (bus_width == 16)
+				cfg |= FIMC_REG_CISRCFMT_ITU601_16BIT;
+		} /* else defaults to ITU-R BT.656 8-bit */
+		break;
+	case FIMC_BUS_TYPE_MIPI_CSI2:
+		if (fimc_fmt_is_user_defined(f->fmt->color))
+			cfg |= FIMC_REG_CISRCFMT_ITU601_8BIT;
+		break;
+	default:
+	case FIMC_BUS_TYPE_ISP_WRITEBACK:
+		/* Anything to do here ? */
+		break;
+	}
+
+	cfg |= (f->o_width << 16) | f->o_height;
+	writel(cfg, fimc->regs + FIMC_REG_CISRCFMT);
+	return 0;
+}
+
+void fimc_hw_set_camera_offset(struct fimc_dev *fimc, struct fimc_frame *f)
+{
+	u32 hoff2, voff2;
+
+	u32 cfg = readl(fimc->regs + FIMC_REG_CIWDOFST);
+
+	cfg &= ~(FIMC_REG_CIWDOFST_HOROFF_MASK | FIMC_REG_CIWDOFST_VEROFF_MASK);
+	cfg |=  FIMC_REG_CIWDOFST_OFF_EN |
+		(f->offs_h << 16) | f->offs_v;
+
+	writel(cfg, fimc->regs + FIMC_REG_CIWDOFST);
+
+	/* See CIWDOFSTn register description in the datasheet for details. */
+	hoff2 = f->o_width - f->width - f->offs_h;
+	voff2 = f->o_height - f->height - f->offs_v;
+	cfg = (hoff2 << 16) | voff2;
+	writel(cfg, fimc->regs + FIMC_REG_CIWDOFST2);
+}
+
+int fimc_hw_set_camera_type(struct fimc_dev *fimc,
+			    struct fimc_source_info *source)
+{
+	struct fimc_vid_cap *vid_cap = &fimc->vid_cap;
+	u32 csis_data_alignment = 32;
+	u32 cfg, tmp;
+
+	cfg = readl(fimc->regs + FIMC_REG_CIGCTRL);
+
+	/* Select ITU B interface, disable Writeback path and test pattern. */
+	cfg &= ~(FIMC_REG_CIGCTRL_TESTPAT_MASK | FIMC_REG_CIGCTRL_SELCAM_ITU_A |
+		FIMC_REG_CIGCTRL_SELCAM_MIPI | FIMC_REG_CIGCTRL_CAMIF_SELWB |
+		FIMC_REG_CIGCTRL_SELCAM_MIPI_A | FIMC_REG_CIGCTRL_CAM_JPEG |
+		FIMC_REG_CIGCTRL_SELWB_A);
+
+	switch (source->fimc_bus_type) {
+	case FIMC_BUS_TYPE_MIPI_CSI2:
+		cfg |= FIMC_REG_CIGCTRL_SELCAM_MIPI;
+
+		if (source->mux_id == 0)
+			cfg |= FIMC_REG_CIGCTRL_SELCAM_MIPI_A;
+
+		/* TODO: add remaining supported formats. */
+		switch (vid_cap->ci_fmt.code) {
+		case MEDIA_BUS_FMT_VYUY8_2X8:
+			tmp = FIMC_REG_CSIIMGFMT_YCBCR422_8BIT;
+			break;
+		case MEDIA_BUS_FMT_JPEG_1X8:
+		case MEDIA_BUS_FMT_S5C_UYVY_JPEG_1X8:
+			tmp = FIMC_REG_CSIIMGFMT_USER(1);
+			cfg |= FIMC_REG_CIGCTRL_CAM_JPEG;
+			break;
+		default:
+			v4l2_err(&vid_cap->ve.vdev,
+				 "Not supported camera pixel format: %#x\n",
+				 vid_cap->ci_fmt.code);
+			return -EINVAL;
+		}
+		tmp |= (csis_data_alignment == 32) << 8;
+
+		writel(tmp, fimc->regs + FIMC_REG_CSIIMGFMT);
+		break;
+	case FIMC_BUS_TYPE_ITU_601...FIMC_BUS_TYPE_ITU_656:
+		if (source->mux_id == 0) /* ITU-A, ITU-B: 0, 1 */
+			cfg |= FIMC_REG_CIGCTRL_SELCAM_ITU_A;
+		break;
+	case FIMC_BUS_TYPE_LCD_WRITEBACK_A:
+		cfg |= FIMC_REG_CIGCTRL_CAMIF_SELWB;
+		/* fall through */
+	case FIMC_BUS_TYPE_ISP_WRITEBACK:
+		if (fimc->variant->has_isp_wb)
+			cfg |= FIMC_REG_CIGCTRL_CAMIF_SELWB;
+		else
+			WARN_ONCE(1, "ISP Writeback input is not supported\n");
+		break;
+	default:
+		v4l2_err(&vid_cap->ve.vdev,
+			 "Invalid FIMC bus type selected: %d\n",
+			 source->fimc_bus_type);
+		return -EINVAL;
+	}
+	writel(cfg, fimc->regs + FIMC_REG_CIGCTRL);
+
+	return 0;
+}
+
+void fimc_hw_clear_irq(struct fimc_dev *dev)
+{
+	u32 cfg = readl(dev->regs + FIMC_REG_CIGCTRL);
+	cfg |= FIMC_REG_CIGCTRL_IRQ_CLR;
+	writel(cfg, dev->regs + FIMC_REG_CIGCTRL);
+}
+
+void fimc_hw_enable_scaler(struct fimc_dev *dev, bool on)
+{
+	u32 cfg = readl(dev->regs + FIMC_REG_CISCCTRL);
+	if (on)
+		cfg |= FIMC_REG_CISCCTRL_SCALERSTART;
+	else
+		cfg &= ~FIMC_REG_CISCCTRL_SCALERSTART;
+	writel(cfg, dev->regs + FIMC_REG_CISCCTRL);
+}
+
+void fimc_hw_activate_input_dma(struct fimc_dev *dev, bool on)
+{
+	u32 cfg = readl(dev->regs + FIMC_REG_MSCTRL);
+	if (on)
+		cfg |= FIMC_REG_MSCTRL_ENVID;
+	else
+		cfg &= ~FIMC_REG_MSCTRL_ENVID;
+	writel(cfg, dev->regs + FIMC_REG_MSCTRL);
+}
+
+/* Return an index to the buffer actually being written. */
+s32 fimc_hw_get_frame_index(struct fimc_dev *dev)
+{
+	s32 reg;
+
+	if (dev->drv_data->cistatus2) {
+		reg = readl(dev->regs + FIMC_REG_CISTATUS2) & 0x3f;
+		return reg - 1;
+	}
+
+	reg = readl(dev->regs + FIMC_REG_CISTATUS);
+
+	return (reg & FIMC_REG_CISTATUS_FRAMECNT_MASK) >>
+		FIMC_REG_CISTATUS_FRAMECNT_SHIFT;
+}
+
+/* Return an index to the buffer being written previously. */
+s32 fimc_hw_get_prev_frame_index(struct fimc_dev *dev)
+{
+	s32 reg;
+
+	if (!dev->drv_data->cistatus2)
+		return -1;
+
+	reg = readl(dev->regs + FIMC_REG_CISTATUS2);
+	return ((reg >> 7) & 0x3f) - 1;
+}
+
+/* Locking: the caller holds fimc->slock */
+void fimc_activate_capture(struct fimc_ctx *ctx)
+{
+	fimc_hw_enable_scaler(ctx->fimc_dev, ctx->scaler.enabled);
+	fimc_hw_enable_capture(ctx);
+}
+
+void fimc_deactivate_capture(struct fimc_dev *fimc)
+{
+	fimc_hw_en_lastirq(fimc, true);
+	fimc_hw_disable_capture(fimc);
+	fimc_hw_enable_scaler(fimc, false);
+	fimc_hw_en_lastirq(fimc, false);
+}
+
+int fimc_hw_camblk_cfg_writeback(struct fimc_dev *fimc)
+{
+	struct regmap *map = fimc->sysreg;
+	unsigned int mask, val, camblk_cfg;
+	int ret;
+
+	if (map == NULL)
+		return 0;
+
+	ret = regmap_read(map, SYSREG_CAMBLK, &camblk_cfg);
+	if (ret < 0 || ((camblk_cfg & 0x00700000) >> 20 != 0x3))
+		return ret;
+
+	if (!WARN(fimc->id >= 3, "not supported id: %d\n", fimc->id))
+		val = 0x1 << (fimc->id + 20);
+	else
+		val = 0;
+
+	mask = SYSREG_CAMBLK_FIFORST_ISP | SYSREG_CAMBLK_ISPWB_FULL_EN;
+	ret = regmap_update_bits(map, SYSREG_CAMBLK, mask, val);
+	if (ret < 0)
+		return ret;
+
+	usleep_range(1000, 2000);
+
+	val |= SYSREG_CAMBLK_FIFORST_ISP;
+	ret = regmap_update_bits(map, SYSREG_CAMBLK, mask, val);
+	if (ret < 0)
+		return ret;
+
+	mask = SYSREG_ISPBLK_FIFORST_CAM_BLK;
+	ret = regmap_update_bits(map, SYSREG_ISPBLK, mask, ~mask);
+	if (ret < 0)
+		return ret;
+
+	usleep_range(1000, 2000);
+
+	return regmap_update_bits(map, SYSREG_ISPBLK, mask, mask);
+}
diff --git a/drivers/media/platform/exynos4-is/fimc-reg.h b/drivers/media/platform/exynos4-is/fimc-reg.h
new file mode 100644
index 0000000..6c97798
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/fimc-reg.h
@@ -0,0 +1,338 @@
+/*
+ * Samsung camera host interface (FIMC) registers definition
+ *
+ * Copyright (C) 2010 - 2012 Samsung Electronics Co., Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef FIMC_REG_H_
+#define FIMC_REG_H_
+
+#include "fimc-core.h"
+
+/* Input source format */
+#define FIMC_REG_CISRCFMT			0x00
+#define FIMC_REG_CISRCFMT_ITU601_8BIT		(1 << 31)
+#define FIMC_REG_CISRCFMT_ITU601_16BIT		(1 << 29)
+#define FIMC_REG_CISRCFMT_ORDER422_YCBYCR	(0 << 14)
+#define FIMC_REG_CISRCFMT_ORDER422_YCRYCB	(1 << 14)
+#define FIMC_REG_CISRCFMT_ORDER422_CBYCRY	(2 << 14)
+#define FIMC_REG_CISRCFMT_ORDER422_CRYCBY	(3 << 14)
+
+/* Window offset */
+#define FIMC_REG_CIWDOFST			0x04
+#define FIMC_REG_CIWDOFST_OFF_EN		(1 << 31)
+#define FIMC_REG_CIWDOFST_CLROVFIY		(1 << 30)
+#define FIMC_REG_CIWDOFST_CLROVRLB		(1 << 29)
+#define FIMC_REG_CIWDOFST_HOROFF_MASK		(0x7ff << 16)
+#define FIMC_REG_CIWDOFST_CLROVFICB		(1 << 15)
+#define FIMC_REG_CIWDOFST_CLROVFICR		(1 << 14)
+#define FIMC_REG_CIWDOFST_VEROFF_MASK		(0xfff << 0)
+
+/* Global control */
+#define FIMC_REG_CIGCTRL			0x08
+#define FIMC_REG_CIGCTRL_SWRST			(1 << 31)
+#define FIMC_REG_CIGCTRL_CAMRST_A		(1 << 30)
+#define FIMC_REG_CIGCTRL_SELCAM_ITU_A		(1 << 29)
+#define FIMC_REG_CIGCTRL_TESTPAT_NORMAL		(0 << 27)
+#define FIMC_REG_CIGCTRL_TESTPAT_COLOR_BAR	(1 << 27)
+#define FIMC_REG_CIGCTRL_TESTPAT_HOR_INC	(2 << 27)
+#define FIMC_REG_CIGCTRL_TESTPAT_VER_INC	(3 << 27)
+#define FIMC_REG_CIGCTRL_TESTPAT_MASK		(3 << 27)
+#define FIMC_REG_CIGCTRL_TESTPAT_SHIFT		27
+#define FIMC_REG_CIGCTRL_INVPOLPCLK		(1 << 26)
+#define FIMC_REG_CIGCTRL_INVPOLVSYNC		(1 << 25)
+#define FIMC_REG_CIGCTRL_INVPOLHREF		(1 << 24)
+#define FIMC_REG_CIGCTRL_IRQ_OVFEN		(1 << 22)
+#define FIMC_REG_CIGCTRL_HREF_MASK		(1 << 21)
+#define FIMC_REG_CIGCTRL_IRQ_LEVEL		(1 << 20)
+#define FIMC_REG_CIGCTRL_IRQ_CLR		(1 << 19)
+#define FIMC_REG_CIGCTRL_IRQ_ENABLE		(1 << 16)
+#define FIMC_REG_CIGCTRL_SHDW_DISABLE		(1 << 12)
+/* 0 - selects Writeback A (LCD), 1 - selects Writeback B (LCD/ISP) */
+#define FIMC_REG_CIGCTRL_SELWB_A		(1 << 10)
+#define FIMC_REG_CIGCTRL_CAM_JPEG		(1 << 8)
+#define FIMC_REG_CIGCTRL_SELCAM_MIPI_A		(1 << 7)
+#define FIMC_REG_CIGCTRL_CAMIF_SELWB		(1 << 6)
+/* 0 - ITU601; 1 - ITU709 */
+#define FIMC_REG_CIGCTRL_CSC_ITU601_709		(1 << 5)
+#define FIMC_REG_CIGCTRL_INVPOLHSYNC		(1 << 4)
+#define FIMC_REG_CIGCTRL_SELCAM_MIPI		(1 << 3)
+#define FIMC_REG_CIGCTRL_INVPOLFIELD		(1 << 1)
+#define FIMC_REG_CIGCTRL_INTERLACE		(1 << 0)
+
+/* Window offset 2 */
+#define FIMC_REG_CIWDOFST2			0x14
+#define FIMC_REG_CIWDOFST2_HOROFF_MASK		(0xfff << 16)
+#define FIMC_REG_CIWDOFST2_VEROFF_MASK		(0xfff << 0)
+
+/* Output DMA Y/Cb/Cr plane start addresses */
+#define FIMC_REG_CIOYSA(n)			(0x18 + (n) * 4)
+#define FIMC_REG_CIOCBSA(n)			(0x28 + (n) * 4)
+#define FIMC_REG_CIOCRSA(n)			(0x38 + (n) * 4)
+
+/* Target image format */
+#define FIMC_REG_CITRGFMT			0x48
+#define FIMC_REG_CITRGFMT_INROT90		(1 << 31)
+#define FIMC_REG_CITRGFMT_YCBCR420		(0 << 29)
+#define FIMC_REG_CITRGFMT_YCBCR422		(1 << 29)
+#define FIMC_REG_CITRGFMT_YCBCR422_1P		(2 << 29)
+#define FIMC_REG_CITRGFMT_RGB			(3 << 29)
+#define FIMC_REG_CITRGFMT_FMT_MASK		(3 << 29)
+#define FIMC_REG_CITRGFMT_HSIZE_MASK		(0xfff << 16)
+#define FIMC_REG_CITRGFMT_FLIP_SHIFT		14
+#define FIMC_REG_CITRGFMT_FLIP_NORMAL		(0 << 14)
+#define FIMC_REG_CITRGFMT_FLIP_X_MIRROR		(1 << 14)
+#define FIMC_REG_CITRGFMT_FLIP_Y_MIRROR		(2 << 14)
+#define FIMC_REG_CITRGFMT_FLIP_180		(3 << 14)
+#define FIMC_REG_CITRGFMT_FLIP_MASK		(3 << 14)
+#define FIMC_REG_CITRGFMT_OUTROT90		(1 << 13)
+#define FIMC_REG_CITRGFMT_VSIZE_MASK		(0xfff << 0)
+
+/* Output DMA control */
+#define FIMC_REG_CIOCTRL			0x4c
+#define FIMC_REG_CIOCTRL_ORDER422_MASK		(3 << 0)
+#define FIMC_REG_CIOCTRL_ORDER422_YCBYCR	(0 << 0)
+#define FIMC_REG_CIOCTRL_ORDER422_YCRYCB	(1 << 0)
+#define FIMC_REG_CIOCTRL_ORDER422_CBYCRY	(2 << 0)
+#define FIMC_REG_CIOCTRL_ORDER422_CRYCBY	(3 << 0)
+#define FIMC_REG_CIOCTRL_LASTIRQ_ENABLE		(1 << 2)
+#define FIMC_REG_CIOCTRL_YCBCR_3PLANE		(0 << 3)
+#define FIMC_REG_CIOCTRL_YCBCR_2PLANE		(1 << 3)
+#define FIMC_REG_CIOCTRL_YCBCR_PLANE_MASK	(1 << 3)
+#define FIMC_REG_CIOCTRL_ALPHA_OUT_MASK		(0xff << 4)
+#define FIMC_REG_CIOCTRL_RGB16FMT_MASK		(3 << 16)
+#define FIMC_REG_CIOCTRL_RGB565			(0 << 16)
+#define FIMC_REG_CIOCTRL_ARGB1555		(1 << 16)
+#define FIMC_REG_CIOCTRL_ARGB4444		(2 << 16)
+#define FIMC_REG_CIOCTRL_ORDER2P_SHIFT		24
+#define FIMC_REG_CIOCTRL_ORDER2P_MASK		(3 << 24)
+#define FIMC_REG_CIOCTRL_ORDER422_2P_LSB_CRCB	(0 << 24)
+
+/* Pre-scaler control 1 */
+#define FIMC_REG_CISCPRERATIO			0x50
+
+#define FIMC_REG_CISCPREDST			0x54
+
+/* Main scaler control */
+#define FIMC_REG_CISCCTRL			0x58
+#define FIMC_REG_CISCCTRL_SCALERBYPASS		(1 << 31)
+#define FIMC_REG_CISCCTRL_SCALEUP_H		(1 << 30)
+#define FIMC_REG_CISCCTRL_SCALEUP_V		(1 << 29)
+#define FIMC_REG_CISCCTRL_CSCR2Y_WIDE		(1 << 28)
+#define FIMC_REG_CISCCTRL_CSCY2R_WIDE		(1 << 27)
+#define FIMC_REG_CISCCTRL_LCDPATHEN_FIFO	(1 << 26)
+#define FIMC_REG_CISCCTRL_INTERLACE		(1 << 25)
+#define FIMC_REG_CISCCTRL_SCALERSTART		(1 << 15)
+#define FIMC_REG_CISCCTRL_INRGB_FMT_RGB565	(0 << 13)
+#define FIMC_REG_CISCCTRL_INRGB_FMT_RGB666	(1 << 13)
+#define FIMC_REG_CISCCTRL_INRGB_FMT_RGB888	(2 << 13)
+#define FIMC_REG_CISCCTRL_INRGB_FMT_MASK	(3 << 13)
+#define FIMC_REG_CISCCTRL_OUTRGB_FMT_RGB565	(0 << 11)
+#define FIMC_REG_CISCCTRL_OUTRGB_FMT_RGB666	(1 << 11)
+#define FIMC_REG_CISCCTRL_OUTRGB_FMT_RGB888	(2 << 11)
+#define FIMC_REG_CISCCTRL_OUTRGB_FMT_MASK	(3 << 11)
+#define FIMC_REG_CISCCTRL_RGB_EXT		(1 << 10)
+#define FIMC_REG_CISCCTRL_ONE2ONE		(1 << 9)
+#define FIMC_REG_CISCCTRL_MHRATIO(x)		((x) << 16)
+#define FIMC_REG_CISCCTRL_MVRATIO(x)		((x) << 0)
+#define FIMC_REG_CISCCTRL_MHRATIO_MASK		(0x1ff << 16)
+#define FIMC_REG_CISCCTRL_MVRATIO_MASK		(0x1ff << 0)
+#define FIMC_REG_CISCCTRL_MHRATIO_EXT(x)	(((x) >> 6) << 16)
+#define FIMC_REG_CISCCTRL_MVRATIO_EXT(x)	(((x) >> 6) << 0)
+
+/* Target area */
+#define FIMC_REG_CITAREA			0x5c
+#define FIMC_REG_CITAREA_MASK			0x0fffffff
+
+/* General status */
+#define FIMC_REG_CISTATUS			0x64
+#define FIMC_REG_CISTATUS_OVFIY			(1 << 31)
+#define FIMC_REG_CISTATUS_OVFICB		(1 << 30)
+#define FIMC_REG_CISTATUS_OVFICR		(1 << 29)
+#define FIMC_REG_CISTATUS_VSYNC			(1 << 28)
+#define FIMC_REG_CISTATUS_FRAMECNT_MASK		(3 << 26)
+#define FIMC_REG_CISTATUS_FRAMECNT_SHIFT	26
+#define FIMC_REG_CISTATUS_WINOFF_EN		(1 << 25)
+#define FIMC_REG_CISTATUS_IMGCPT_EN		(1 << 22)
+#define FIMC_REG_CISTATUS_IMGCPT_SCEN		(1 << 21)
+#define FIMC_REG_CISTATUS_VSYNC_A		(1 << 20)
+#define FIMC_REG_CISTATUS_VSYNC_B		(1 << 19)
+#define FIMC_REG_CISTATUS_OVRLB			(1 << 18)
+#define FIMC_REG_CISTATUS_FRAME_END		(1 << 17)
+#define FIMC_REG_CISTATUS_LASTCAPT_END		(1 << 16)
+#define FIMC_REG_CISTATUS_VVALID_A		(1 << 15)
+#define FIMC_REG_CISTATUS_VVALID_B		(1 << 14)
+
+/* Indexes to the last and the currently processed buffer. */
+#define FIMC_REG_CISTATUS2			0x68
+
+/* Image capture control */
+#define FIMC_REG_CIIMGCPT			0xc0
+#define FIMC_REG_CIIMGCPT_IMGCPTEN		(1 << 31)
+#define FIMC_REG_CIIMGCPT_IMGCPTEN_SC		(1 << 30)
+#define FIMC_REG_CIIMGCPT_CPT_FREN_ENABLE	(1 << 25)
+#define FIMC_REG_CIIMGCPT_CPT_FRMOD_CNT		(1 << 18)
+
+/* Frame capture sequence */
+#define FIMC_REG_CICPTSEQ			0xc4
+
+/* Image effect */
+#define FIMC_REG_CIIMGEFF			0xd0
+#define FIMC_REG_CIIMGEFF_IE_ENABLE		(1 << 30)
+#define FIMC_REG_CIIMGEFF_IE_SC_BEFORE		(0 << 29)
+#define FIMC_REG_CIIMGEFF_IE_SC_AFTER		(1 << 29)
+#define FIMC_REG_CIIMGEFF_FIN_BYPASS		(0 << 26)
+#define FIMC_REG_CIIMGEFF_FIN_ARBITRARY		(1 << 26)
+#define FIMC_REG_CIIMGEFF_FIN_NEGATIVE		(2 << 26)
+#define FIMC_REG_CIIMGEFF_FIN_ARTFREEZE		(3 << 26)
+#define FIMC_REG_CIIMGEFF_FIN_EMBOSSING		(4 << 26)
+#define FIMC_REG_CIIMGEFF_FIN_SILHOUETTE	(5 << 26)
+#define FIMC_REG_CIIMGEFF_FIN_MASK		(7 << 26)
+#define FIMC_REG_CIIMGEFF_PAT_CBCR_MASK		((0xff << 13) | 0xff)
+
+/* Input DMA Y/Cb/Cr plane start address 0/1 */
+#define FIMC_REG_CIIYSA(n)			(0xd4 + (n) * 0x70)
+#define FIMC_REG_CIICBSA(n)			(0xd8 + (n) * 0x70)
+#define FIMC_REG_CIICRSA(n)			(0xdc + (n) * 0x70)
+
+/* Real input DMA image size */
+#define FIMC_REG_CIREAL_ISIZE			0xf8
+#define FIMC_REG_CIREAL_ISIZE_AUTOLOAD_EN	(1 << 31)
+#define FIMC_REG_CIREAL_ISIZE_ADDR_CH_DIS	(1 << 30)
+
+/* Input DMA control */
+#define FIMC_REG_MSCTRL				0xfc
+#define FIMC_REG_MSCTRL_IN_BURST_COUNT_MASK	(0xf << 24)
+#define FIMC_REG_MSCTRL_2P_IN_ORDER_MASK	(3 << 16)
+#define FIMC_REG_MSCTRL_2P_IN_ORDER_SHIFT	16
+#define FIMC_REG_MSCTRL_C_INT_IN_3PLANE		(0 << 15)
+#define FIMC_REG_MSCTRL_C_INT_IN_2PLANE		(1 << 15)
+#define FIMC_REG_MSCTRL_C_INT_IN_MASK		(1 << 15)
+#define FIMC_REG_MSCTRL_FLIP_SHIFT		13
+#define FIMC_REG_MSCTRL_FLIP_MASK		(3 << 13)
+#define FIMC_REG_MSCTRL_FLIP_NORMAL		(0 << 13)
+#define FIMC_REG_MSCTRL_FLIP_X_MIRROR		(1 << 13)
+#define FIMC_REG_MSCTRL_FLIP_Y_MIRROR		(2 << 13)
+#define FIMC_REG_MSCTRL_FLIP_180		(3 << 13)
+#define FIMC_REG_MSCTRL_FIFO_CTRL_FULL		(1 << 12)
+#define FIMC_REG_MSCTRL_ORDER422_SHIFT		4
+#define FIMC_REG_MSCTRL_ORDER422_CRYCBY		(0 << 4)
+#define FIMC_REG_MSCTRL_ORDER422_YCRYCB		(1 << 4)
+#define FIMC_REG_MSCTRL_ORDER422_CBYCRY		(2 << 4)
+#define FIMC_REG_MSCTRL_ORDER422_YCBYCR		(3 << 4)
+#define FIMC_REG_MSCTRL_ORDER422_MASK		(3 << 4)
+#define FIMC_REG_MSCTRL_INPUT_EXTCAM		(0 << 3)
+#define FIMC_REG_MSCTRL_INPUT_MEMORY		(1 << 3)
+#define FIMC_REG_MSCTRL_INPUT_MASK		(1 << 3)
+#define FIMC_REG_MSCTRL_INFORMAT_YCBCR420	(0 << 1)
+#define FIMC_REG_MSCTRL_INFORMAT_YCBCR422	(1 << 1)
+#define FIMC_REG_MSCTRL_INFORMAT_YCBCR422_1P	(2 << 1)
+#define FIMC_REG_MSCTRL_INFORMAT_RGB		(3 << 1)
+#define FIMC_REG_MSCTRL_INFORMAT_MASK		(3 << 1)
+#define FIMC_REG_MSCTRL_ENVID			(1 << 0)
+#define FIMC_REG_MSCTRL_IN_BURST_COUNT(x)	((x) << 24)
+
+/* Output DMA Y/Cb/Cr offset */
+#define FIMC_REG_CIOYOFF			0x168
+#define FIMC_REG_CIOCBOFF			0x16c
+#define FIMC_REG_CIOCROFF			0x170
+
+/* Input DMA Y/Cb/Cr offset */
+#define FIMC_REG_CIIYOFF			0x174
+#define FIMC_REG_CIICBOFF			0x178
+#define FIMC_REG_CIICROFF			0x17c
+
+/* Input DMA original image size */
+#define FIMC_REG_ORGISIZE			0x180
+
+/* Output DMA original image size */
+#define FIMC_REG_ORGOSIZE			0x184
+
+/* Real output DMA image size (extension register) */
+#define FIMC_REG_CIEXTEN			0x188
+#define FIMC_REG_CIEXTEN_MHRATIO_EXT(x)		(((x) & 0x3f) << 10)
+#define FIMC_REG_CIEXTEN_MVRATIO_EXT(x)		((x) & 0x3f)
+#define FIMC_REG_CIEXTEN_MHRATIO_EXT_MASK	(0x3f << 10)
+#define FIMC_REG_CIEXTEN_MVRATIO_EXT_MASK	0x3f
+
+#define FIMC_REG_CIDMAPARAM			0x18c
+#define FIMC_REG_CIDMAPARAM_R_LINEAR		(0 << 29)
+#define FIMC_REG_CIDMAPARAM_R_64X32		(3 << 29)
+#define FIMC_REG_CIDMAPARAM_W_LINEAR		(0 << 13)
+#define FIMC_REG_CIDMAPARAM_W_64X32		(3 << 13)
+#define FIMC_REG_CIDMAPARAM_TILE_MASK		((3 << 29) | (3 << 13))
+
+/* MIPI CSI image format */
+#define FIMC_REG_CSIIMGFMT			0x194
+#define FIMC_REG_CSIIMGFMT_YCBCR422_8BIT	0x1e
+#define FIMC_REG_CSIIMGFMT_RAW8			0x2a
+#define FIMC_REG_CSIIMGFMT_RAW10		0x2b
+#define FIMC_REG_CSIIMGFMT_RAW12		0x2c
+/* User defined formats. x = 0...16. */
+#define FIMC_REG_CSIIMGFMT_USER(x)		(0x30 + x - 1)
+
+/* Output frame buffer sequence mask */
+#define FIMC_REG_CIFCNTSEQ			0x1fc
+
+/* SYSREG ISP Writeback register address offsets */
+#define SYSREG_ISPBLK				0x020c
+#define SYSREG_ISPBLK_FIFORST_CAM_BLK		(1 << 7)
+
+#define SYSREG_CAMBLK				0x0218
+#define SYSREG_CAMBLK_FIFORST_ISP		(1 << 15)
+#define SYSREG_CAMBLK_ISPWB_FULL_EN		(7 << 20)
+
+/*
+ * Function declarations
+ */
+void fimc_hw_reset(struct fimc_dev *fimc);
+void fimc_hw_set_rotation(struct fimc_ctx *ctx);
+void fimc_hw_set_target_format(struct fimc_ctx *ctx);
+void fimc_hw_set_out_dma(struct fimc_ctx *ctx);
+void fimc_hw_en_lastirq(struct fimc_dev *fimc, int enable);
+void fimc_hw_en_irq(struct fimc_dev *fimc, int enable);
+void fimc_hw_set_prescaler(struct fimc_ctx *ctx);
+void fimc_hw_set_mainscaler(struct fimc_ctx *ctx);
+void fimc_hw_enable_capture(struct fimc_ctx *ctx);
+void fimc_hw_set_effect(struct fimc_ctx *ctx);
+void fimc_hw_set_rgb_alpha(struct fimc_ctx *ctx);
+void fimc_hw_set_in_dma(struct fimc_ctx *ctx);
+void fimc_hw_set_input_path(struct fimc_ctx *ctx);
+void fimc_hw_set_output_path(struct fimc_ctx *ctx);
+void fimc_hw_set_input_addr(struct fimc_dev *fimc, struct fimc_addr *paddr);
+void fimc_hw_set_output_addr(struct fimc_dev *fimc, struct fimc_addr *paddr,
+			     int index);
+int fimc_hw_set_camera_source(struct fimc_dev *fimc,
+			      struct fimc_source_info *cam);
+void fimc_hw_set_camera_offset(struct fimc_dev *fimc, struct fimc_frame *f);
+int fimc_hw_set_camera_polarity(struct fimc_dev *fimc,
+				struct fimc_source_info *cam);
+int fimc_hw_set_camera_type(struct fimc_dev *fimc,
+			    struct fimc_source_info *cam);
+void fimc_hw_clear_irq(struct fimc_dev *dev);
+void fimc_hw_enable_scaler(struct fimc_dev *dev, bool on);
+void fimc_hw_activate_input_dma(struct fimc_dev *dev, bool on);
+void fimc_hw_disable_capture(struct fimc_dev *dev);
+s32 fimc_hw_get_frame_index(struct fimc_dev *dev);
+s32 fimc_hw_get_prev_frame_index(struct fimc_dev *dev);
+int fimc_hw_camblk_cfg_writeback(struct fimc_dev *fimc);
+void fimc_activate_capture(struct fimc_ctx *ctx);
+void fimc_deactivate_capture(struct fimc_dev *fimc);
+
+/**
+ * fimc_hw_set_dma_seq - configure output DMA buffer sequence
+ * @mask: bitmask for the DMA output buffer registers, set to 0 to skip buffer
+ * This function masks output DMA ring buffers, it allows to select which of
+ * the 32 available output buffer address registers will be used by the DMA
+ * engine.
+ */
+static inline void fimc_hw_set_dma_seq(struct fimc_dev *dev, u32 mask)
+{
+	writel(mask, dev->regs + FIMC_REG_CIFCNTSEQ);
+}
+
+#endif /* FIMC_REG_H_ */
diff --git a/drivers/media/platform/exynos4-is/media-dev.c b/drivers/media/platform/exynos4-is/media-dev.c
new file mode 100644
index 0000000..4f5586a
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/media-dev.c
@@ -0,0 +1,1499 @@
+/*
+ * S5P/EXYNOS4 SoC series camera host interface media device driver
+ *
+ * Copyright (C) 2011 - 2013 Samsung Electronics Co., Ltd.
+ * Author: Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published
+ * by the Free Software Foundation, either version 2 of the License,
+ * or (at your option) any later version.
+ */
+
+#include <linux/bug.h>
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/device.h>
+#include <linux/errno.h>
+#include <linux/i2c.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/of_device.h>
+#include <linux/of_graph.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-of.h>
+#include <media/media-device.h>
+#include <media/exynos-fimc.h>
+
+#include "media-dev.h"
+#include "fimc-core.h"
+#include "fimc-is.h"
+#include "fimc-lite.h"
+#include "mipi-csis.h"
+
+/* Set up image sensor subdev -> FIMC capture node notifications. */
+static void __setup_sensor_notification(struct fimc_md *fmd,
+					struct v4l2_subdev *sensor,
+					struct v4l2_subdev *fimc_sd)
+{
+	struct fimc_source_info *src_inf;
+	struct fimc_sensor_info *md_si;
+	unsigned long flags;
+
+	src_inf = v4l2_get_subdev_hostdata(sensor);
+	if (!src_inf || WARN_ON(fmd == NULL))
+		return;
+
+	md_si = source_to_sensor_info(src_inf);
+	spin_lock_irqsave(&fmd->slock, flags);
+	md_si->host = v4l2_get_subdevdata(fimc_sd);
+	spin_unlock_irqrestore(&fmd->slock, flags);
+}
+
+/**
+ * fimc_pipeline_prepare - update pipeline information with subdevice pointers
+ * @me: media entity terminating the pipeline
+ *
+ * Caller holds the graph mutex.
+ */
+static void fimc_pipeline_prepare(struct fimc_pipeline *p,
+					struct media_entity *me)
+{
+	struct fimc_md *fmd = entity_to_fimc_mdev(me);
+	struct v4l2_subdev *sd;
+	struct v4l2_subdev *sensor = NULL;
+	int i;
+
+	for (i = 0; i < IDX_MAX; i++)
+		p->subdevs[i] = NULL;
+
+	while (1) {
+		struct media_pad *pad = NULL;
+
+		/* Find remote source pad */
+		for (i = 0; i < me->num_pads; i++) {
+			struct media_pad *spad = &me->pads[i];
+			if (!(spad->flags & MEDIA_PAD_FL_SINK))
+				continue;
+			pad = media_entity_remote_pad(spad);
+			if (pad)
+				break;
+		}
+
+		if (pad == NULL ||
+		    media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+			break;
+		sd = media_entity_to_v4l2_subdev(pad->entity);
+
+		switch (sd->grp_id) {
+		case GRP_ID_SENSOR:
+			sensor = sd;
+			/* fall through */
+		case GRP_ID_FIMC_IS_SENSOR:
+			p->subdevs[IDX_SENSOR] = sd;
+			break;
+		case GRP_ID_CSIS:
+			p->subdevs[IDX_CSIS] = sd;
+			break;
+		case GRP_ID_FLITE:
+			p->subdevs[IDX_FLITE] = sd;
+			break;
+		case GRP_ID_FIMC:
+			p->subdevs[IDX_FIMC] = sd;
+			break;
+		case GRP_ID_FIMC_IS:
+			p->subdevs[IDX_IS_ISP] = sd;
+			break;
+		default:
+			break;
+		}
+		me = &sd->entity;
+		if (me->num_pads == 1)
+			break;
+	}
+
+	if (sensor && p->subdevs[IDX_FIMC])
+		__setup_sensor_notification(fmd, sensor, p->subdevs[IDX_FIMC]);
+}
+
+/**
+ * __subdev_set_power - change power state of a single subdev
+ * @sd: subdevice to change power state for
+ * @on: 1 to enable power or 0 to disable
+ *
+ * Return result of s_power subdev operation or -ENXIO if sd argument
+ * is NULL. Return 0 if the subdevice does not implement s_power.
+ */
+static int __subdev_set_power(struct v4l2_subdev *sd, int on)
+{
+	int *use_count;
+	int ret;
+
+	if (sd == NULL)
+		return -ENXIO;
+
+	use_count = &sd->entity.use_count;
+	if (on && (*use_count)++ > 0)
+		return 0;
+	else if (!on && (*use_count == 0 || --(*use_count) > 0))
+		return 0;
+	ret = v4l2_subdev_call(sd, core, s_power, on);
+
+	return ret != -ENOIOCTLCMD ? ret : 0;
+}
+
+/**
+ * fimc_pipeline_s_power - change power state of all pipeline subdevs
+ * @fimc: fimc device terminating the pipeline
+ * @state: true to power on, false to power off
+ *
+ * Needs to be called with the graph mutex held.
+ */
+static int fimc_pipeline_s_power(struct fimc_pipeline *p, bool on)
+{
+	static const u8 seq[2][IDX_MAX - 1] = {
+		{ IDX_IS_ISP, IDX_SENSOR, IDX_CSIS, IDX_FLITE },
+		{ IDX_CSIS, IDX_FLITE, IDX_SENSOR, IDX_IS_ISP },
+	};
+	int i, ret = 0;
+
+	if (p->subdevs[IDX_SENSOR] == NULL)
+		return -ENXIO;
+
+	for (i = 0; i < IDX_MAX - 1; i++) {
+		unsigned int idx = seq[on][i];
+
+		ret = __subdev_set_power(p->subdevs[idx], on);
+
+
+		if (ret < 0 && ret != -ENXIO)
+			goto error;
+	}
+	return 0;
+error:
+	for (; i >= 0; i--) {
+		unsigned int idx = seq[on][i];
+		__subdev_set_power(p->subdevs[idx], !on);
+	}
+	return ret;
+}
+
+/**
+ * __fimc_pipeline_open - update the pipeline information, enable power
+ *                        of all pipeline subdevs and the sensor clock
+ * @me: media entity to start graph walk with
+ * @prepare: true to walk the current pipeline and acquire all subdevs
+ *
+ * Called with the graph mutex held.
+ */
+static int __fimc_pipeline_open(struct exynos_media_pipeline *ep,
+				struct media_entity *me, bool prepare)
+{
+	struct fimc_md *fmd = entity_to_fimc_mdev(me);
+	struct fimc_pipeline *p = to_fimc_pipeline(ep);
+	struct v4l2_subdev *sd;
+	int ret;
+
+	if (WARN_ON(p == NULL || me == NULL))
+		return -EINVAL;
+
+	if (prepare)
+		fimc_pipeline_prepare(p, me);
+
+	sd = p->subdevs[IDX_SENSOR];
+	if (sd == NULL)
+		return -EINVAL;
+
+	/* Disable PXLASYNC clock if this pipeline includes FIMC-IS */
+	if (!IS_ERR(fmd->wbclk[CLK_IDX_WB_B]) && p->subdevs[IDX_IS_ISP]) {
+		ret = clk_prepare_enable(fmd->wbclk[CLK_IDX_WB_B]);
+		if (ret < 0)
+			return ret;
+	}
+
+	ret = fimc_pipeline_s_power(p, 1);
+	if (!ret)
+		return 0;
+
+	if (!IS_ERR(fmd->wbclk[CLK_IDX_WB_B]) && p->subdevs[IDX_IS_ISP])
+		clk_disable_unprepare(fmd->wbclk[CLK_IDX_WB_B]);
+
+	return ret;
+}
+
+/**
+ * __fimc_pipeline_close - disable the sensor clock and pipeline power
+ * @fimc: fimc device terminating the pipeline
+ *
+ * Disable power of all subdevs and turn the external sensor clock off.
+ */
+static int __fimc_pipeline_close(struct exynos_media_pipeline *ep)
+{
+	struct fimc_pipeline *p = to_fimc_pipeline(ep);
+	struct v4l2_subdev *sd = p ? p->subdevs[IDX_SENSOR] : NULL;
+	struct fimc_md *fmd;
+	int ret;
+
+	if (sd == NULL) {
+		pr_warn("%s(): No sensor subdev\n", __func__);
+		return 0;
+	}
+
+	ret = fimc_pipeline_s_power(p, 0);
+
+	fmd = entity_to_fimc_mdev(&sd->entity);
+
+	/* Disable PXLASYNC clock if this pipeline includes FIMC-IS */
+	if (!IS_ERR(fmd->wbclk[CLK_IDX_WB_B]) && p->subdevs[IDX_IS_ISP])
+		clk_disable_unprepare(fmd->wbclk[CLK_IDX_WB_B]);
+
+	return ret == -ENXIO ? 0 : ret;
+}
+
+/**
+ * __fimc_pipeline_s_stream - call s_stream() on pipeline subdevs
+ * @pipeline: video pipeline structure
+ * @on: passed as the s_stream() callback argument
+ */
+static int __fimc_pipeline_s_stream(struct exynos_media_pipeline *ep, bool on)
+{
+	static const u8 seq[2][IDX_MAX] = {
+		{ IDX_FIMC, IDX_SENSOR, IDX_IS_ISP, IDX_CSIS, IDX_FLITE },
+		{ IDX_CSIS, IDX_FLITE, IDX_FIMC, IDX_SENSOR, IDX_IS_ISP },
+	};
+	struct fimc_pipeline *p = to_fimc_pipeline(ep);
+	int i, ret = 0;
+
+	if (p->subdevs[IDX_SENSOR] == NULL)
+		return -ENODEV;
+
+	for (i = 0; i < IDX_MAX; i++) {
+		unsigned int idx = seq[on][i];
+
+		ret = v4l2_subdev_call(p->subdevs[idx], video, s_stream, on);
+
+		if (ret < 0 && ret != -ENOIOCTLCMD && ret != -ENODEV)
+			goto error;
+	}
+	return 0;
+error:
+	for (; i >= 0; i--) {
+		unsigned int idx = seq[on][i];
+		v4l2_subdev_call(p->subdevs[idx], video, s_stream, !on);
+	}
+	return ret;
+}
+
+/* Media pipeline operations for the FIMC/FIMC-LITE video device driver */
+static const struct exynos_media_pipeline_ops fimc_pipeline_ops = {
+	.open		= __fimc_pipeline_open,
+	.close		= __fimc_pipeline_close,
+	.set_stream	= __fimc_pipeline_s_stream,
+};
+
+static struct exynos_media_pipeline *fimc_md_pipeline_create(
+						struct fimc_md *fmd)
+{
+	struct fimc_pipeline *p;
+
+	p = kzalloc(sizeof(*p), GFP_KERNEL);
+	if (!p)
+		return NULL;
+
+	list_add_tail(&p->list, &fmd->pipelines);
+
+	p->ep.ops = &fimc_pipeline_ops;
+	return &p->ep;
+}
+
+static void fimc_md_pipelines_free(struct fimc_md *fmd)
+{
+	while (!list_empty(&fmd->pipelines)) {
+		struct fimc_pipeline *p;
+
+		p = list_entry(fmd->pipelines.next, typeof(*p), list);
+		list_del(&p->list);
+		kfree(p);
+	}
+}
+
+/* Parse port node and register as a sub-device any sensor specified there. */
+static int fimc_md_parse_port_node(struct fimc_md *fmd,
+				   struct device_node *port,
+				   unsigned int index)
+{
+	struct fimc_source_info *pd = &fmd->sensor[index].pdata;
+	struct device_node *rem, *ep, *np;
+	struct v4l2_of_endpoint endpoint;
+
+	/* Assume here a port node can have only one endpoint node. */
+	ep = of_get_next_child(port, NULL);
+	if (!ep)
+		return 0;
+
+	v4l2_of_parse_endpoint(ep, &endpoint);
+	if (WARN_ON(endpoint.base.port == 0) || index >= FIMC_MAX_SENSORS)
+		return -EINVAL;
+
+	pd->mux_id = (endpoint.base.port - 1) & 0x1;
+
+	rem = of_graph_get_remote_port_parent(ep);
+	of_node_put(ep);
+	if (rem == NULL) {
+		v4l2_info(&fmd->v4l2_dev, "Remote device at %s not found\n",
+							ep->full_name);
+		return 0;
+	}
+
+	if (fimc_input_is_parallel(endpoint.base.port)) {
+		if (endpoint.bus_type == V4L2_MBUS_PARALLEL)
+			pd->sensor_bus_type = FIMC_BUS_TYPE_ITU_601;
+		else
+			pd->sensor_bus_type = FIMC_BUS_TYPE_ITU_656;
+		pd->flags = endpoint.bus.parallel.flags;
+	} else if (fimc_input_is_mipi_csi(endpoint.base.port)) {
+		/*
+		 * MIPI CSI-2: only input mux selection and
+		 * the sensor's clock frequency is needed.
+		 */
+		pd->sensor_bus_type = FIMC_BUS_TYPE_MIPI_CSI2;
+	} else {
+		v4l2_err(&fmd->v4l2_dev, "Wrong port id (%u) at node %s\n",
+			 endpoint.base.port, rem->full_name);
+	}
+	/*
+	 * For FIMC-IS handled sensors, that are placed under i2c-isp device
+	 * node, FIMC is connected to the FIMC-IS through its ISP Writeback
+	 * input. Sensors are attached to the FIMC-LITE hostdata interface
+	 * directly or through MIPI-CSIS, depending on the external media bus
+	 * used. This needs to be handled in a more reliable way, not by just
+	 * checking parent's node name.
+	 */
+	np = of_get_parent(rem);
+
+	if (np && !of_node_cmp(np->name, "i2c-isp"))
+		pd->fimc_bus_type = FIMC_BUS_TYPE_ISP_WRITEBACK;
+	else
+		pd->fimc_bus_type = pd->sensor_bus_type;
+
+	if (WARN_ON(index >= ARRAY_SIZE(fmd->sensor)))
+		return -EINVAL;
+
+	fmd->sensor[index].asd.match_type = V4L2_ASYNC_MATCH_OF;
+	fmd->sensor[index].asd.match.of.node = rem;
+	fmd->async_subdevs[index] = &fmd->sensor[index].asd;
+
+	fmd->num_sensors++;
+
+	of_node_put(rem);
+	return 0;
+}
+
+/* Register all SoC external sub-devices */
+static int fimc_md_register_sensor_entities(struct fimc_md *fmd)
+{
+	struct device_node *parent = fmd->pdev->dev.of_node;
+	struct device_node *node, *ports;
+	int index = 0;
+	int ret;
+
+	/*
+	 * Runtime resume one of the FIMC entities to make sure
+	 * the sclk_cam clocks are not globally disabled.
+	 */
+	if (!fmd->pmf)
+		return -ENXIO;
+
+	ret = pm_runtime_get_sync(fmd->pmf);
+	if (ret < 0)
+		return ret;
+
+	fmd->num_sensors = 0;
+
+	/* Attach sensors linked to MIPI CSI-2 receivers */
+	for_each_available_child_of_node(parent, node) {
+		struct device_node *port;
+
+		if (of_node_cmp(node->name, "csis"))
+			continue;
+		/* The csis node can have only port subnode. */
+		port = of_get_next_child(node, NULL);
+		if (!port)
+			continue;
+
+		ret = fimc_md_parse_port_node(fmd, port, index);
+		if (ret < 0)
+			goto rpm_put;
+		index++;
+	}
+
+	/* Attach sensors listed in the parallel-ports node */
+	ports = of_get_child_by_name(parent, "parallel-ports");
+	if (!ports)
+		goto rpm_put;
+
+	for_each_child_of_node(ports, node) {
+		ret = fimc_md_parse_port_node(fmd, node, index);
+		if (ret < 0)
+			break;
+		index++;
+	}
+rpm_put:
+	pm_runtime_put(fmd->pmf);
+	return ret;
+}
+
+static int __of_get_csis_id(struct device_node *np)
+{
+	u32 reg = 0;
+
+	np = of_get_child_by_name(np, "port");
+	if (!np)
+		return -EINVAL;
+	of_property_read_u32(np, "reg", &reg);
+	return reg - FIMC_INPUT_MIPI_CSI2_0;
+}
+
+/*
+ * MIPI-CSIS, FIMC and FIMC-LITE platform devices registration.
+ */
+static int register_fimc_lite_entity(struct fimc_md *fmd,
+				     struct fimc_lite *fimc_lite)
+{
+	struct v4l2_subdev *sd;
+	struct exynos_media_pipeline *ep;
+	int ret;
+
+	if (WARN_ON(fimc_lite->index >= FIMC_LITE_MAX_DEVS ||
+		    fmd->fimc_lite[fimc_lite->index]))
+		return -EBUSY;
+
+	sd = &fimc_lite->subdev;
+	sd->grp_id = GRP_ID_FLITE;
+
+	ep = fimc_md_pipeline_create(fmd);
+	if (!ep)
+		return -ENOMEM;
+
+	v4l2_set_subdev_hostdata(sd, ep);
+
+	ret = v4l2_device_register_subdev(&fmd->v4l2_dev, sd);
+	if (!ret)
+		fmd->fimc_lite[fimc_lite->index] = fimc_lite;
+	else
+		v4l2_err(&fmd->v4l2_dev, "Failed to register FIMC.LITE%d\n",
+			 fimc_lite->index);
+	return ret;
+}
+
+static int register_fimc_entity(struct fimc_md *fmd, struct fimc_dev *fimc)
+{
+	struct v4l2_subdev *sd;
+	struct exynos_media_pipeline *ep;
+	int ret;
+
+	if (WARN_ON(fimc->id >= FIMC_MAX_DEVS || fmd->fimc[fimc->id]))
+		return -EBUSY;
+
+	sd = &fimc->vid_cap.subdev;
+	sd->grp_id = GRP_ID_FIMC;
+
+	ep = fimc_md_pipeline_create(fmd);
+	if (!ep)
+		return -ENOMEM;
+
+	v4l2_set_subdev_hostdata(sd, ep);
+
+	ret = v4l2_device_register_subdev(&fmd->v4l2_dev, sd);
+	if (!ret) {
+		if (!fmd->pmf && fimc->pdev)
+			fmd->pmf = &fimc->pdev->dev;
+		fmd->fimc[fimc->id] = fimc;
+		fimc->vid_cap.user_subdev_api = fmd->user_subdev_api;
+	} else {
+		v4l2_err(&fmd->v4l2_dev, "Failed to register FIMC.%d (%d)\n",
+			 fimc->id, ret);
+	}
+	return ret;
+}
+
+static int register_csis_entity(struct fimc_md *fmd,
+				struct platform_device *pdev,
+				struct v4l2_subdev *sd)
+{
+	struct device_node *node = pdev->dev.of_node;
+	int id, ret;
+
+	id = node ? __of_get_csis_id(node) : max(0, pdev->id);
+
+	if (WARN_ON(id < 0 || id >= CSIS_MAX_ENTITIES))
+		return -ENOENT;
+
+	if (WARN_ON(fmd->csis[id].sd))
+		return -EBUSY;
+
+	sd->grp_id = GRP_ID_CSIS;
+	ret = v4l2_device_register_subdev(&fmd->v4l2_dev, sd);
+	if (!ret)
+		fmd->csis[id].sd = sd;
+	else
+		v4l2_err(&fmd->v4l2_dev,
+			 "Failed to register MIPI-CSIS.%d (%d)\n", id, ret);
+	return ret;
+}
+
+static int register_fimc_is_entity(struct fimc_md *fmd, struct fimc_is *is)
+{
+	struct v4l2_subdev *sd = &is->isp.subdev;
+	struct exynos_media_pipeline *ep;
+	int ret;
+
+	/* Allocate pipeline object for the ISP capture video node. */
+	ep = fimc_md_pipeline_create(fmd);
+	if (!ep)
+		return -ENOMEM;
+
+	v4l2_set_subdev_hostdata(sd, ep);
+
+	ret = v4l2_device_register_subdev(&fmd->v4l2_dev, sd);
+	if (ret) {
+		v4l2_err(&fmd->v4l2_dev,
+			 "Failed to register FIMC-ISP (%d)\n", ret);
+		return ret;
+	}
+
+	fmd->fimc_is = is;
+	return 0;
+}
+
+static int fimc_md_register_platform_entity(struct fimc_md *fmd,
+					    struct platform_device *pdev,
+					    int plat_entity)
+{
+	struct device *dev = &pdev->dev;
+	int ret = -EPROBE_DEFER;
+	void *drvdata;
+
+	/* Lock to ensure dev->driver won't change. */
+	device_lock(dev);
+
+	if (!dev->driver || !try_module_get(dev->driver->owner))
+		goto dev_unlock;
+
+	drvdata = dev_get_drvdata(dev);
+	/* Some subdev didn't probe successfully id drvdata is NULL */
+	if (drvdata) {
+		switch (plat_entity) {
+		case IDX_FIMC:
+			ret = register_fimc_entity(fmd, drvdata);
+			break;
+		case IDX_FLITE:
+			ret = register_fimc_lite_entity(fmd, drvdata);
+			break;
+		case IDX_CSIS:
+			ret = register_csis_entity(fmd, pdev, drvdata);
+			break;
+		case IDX_IS_ISP:
+			ret = register_fimc_is_entity(fmd, drvdata);
+			break;
+		default:
+			ret = -ENODEV;
+		}
+	}
+
+	module_put(dev->driver->owner);
+dev_unlock:
+	device_unlock(dev);
+	if (ret == -EPROBE_DEFER)
+		dev_info(&fmd->pdev->dev, "deferring %s device registration\n",
+			dev_name(dev));
+	else if (ret < 0)
+		dev_err(&fmd->pdev->dev, "%s device registration failed (%d)\n",
+			dev_name(dev), ret);
+	return ret;
+}
+
+/* Register FIMC, FIMC-LITE and CSIS media entities */
+static int fimc_md_register_platform_entities(struct fimc_md *fmd,
+					      struct device_node *parent)
+{
+	struct device_node *node;
+	int ret = 0;
+
+	for_each_available_child_of_node(parent, node) {
+		struct platform_device *pdev;
+		int plat_entity = -1;
+
+		pdev = of_find_device_by_node(node);
+		if (!pdev)
+			continue;
+
+		/* If driver of any entity isn't ready try all again later. */
+		if (!strcmp(node->name, CSIS_OF_NODE_NAME))
+			plat_entity = IDX_CSIS;
+		else if	(!strcmp(node->name, FIMC_IS_OF_NODE_NAME))
+			plat_entity = IDX_IS_ISP;
+		else if (!strcmp(node->name, FIMC_LITE_OF_NODE_NAME))
+			plat_entity = IDX_FLITE;
+		else if	(!strcmp(node->name, FIMC_OF_NODE_NAME) &&
+			 !of_property_read_bool(node, "samsung,lcd-wb"))
+			plat_entity = IDX_FIMC;
+
+		if (plat_entity >= 0)
+			ret = fimc_md_register_platform_entity(fmd, pdev,
+							plat_entity);
+		put_device(&pdev->dev);
+		if (ret < 0)
+			break;
+	}
+
+	return ret;
+}
+
+static void fimc_md_unregister_entities(struct fimc_md *fmd)
+{
+	int i;
+
+	for (i = 0; i < FIMC_MAX_DEVS; i++) {
+		struct fimc_dev *dev = fmd->fimc[i];
+		if (dev == NULL)
+			continue;
+		v4l2_device_unregister_subdev(&dev->vid_cap.subdev);
+		dev->vid_cap.ve.pipe = NULL;
+		fmd->fimc[i] = NULL;
+	}
+	for (i = 0; i < FIMC_LITE_MAX_DEVS; i++) {
+		struct fimc_lite *dev = fmd->fimc_lite[i];
+		if (dev == NULL)
+			continue;
+		v4l2_device_unregister_subdev(&dev->subdev);
+		dev->ve.pipe = NULL;
+		fmd->fimc_lite[i] = NULL;
+	}
+	for (i = 0; i < CSIS_MAX_ENTITIES; i++) {
+		if (fmd->csis[i].sd == NULL)
+			continue;
+		v4l2_device_unregister_subdev(fmd->csis[i].sd);
+		fmd->csis[i].sd = NULL;
+	}
+
+	if (fmd->fimc_is)
+		v4l2_device_unregister_subdev(&fmd->fimc_is->isp.subdev);
+
+	v4l2_info(&fmd->v4l2_dev, "Unregistered all entities\n");
+}
+
+/**
+ * __fimc_md_create_fimc_links - create links to all FIMC entities
+ * @fmd: fimc media device
+ * @source: the source entity to create links to all fimc entities from
+ * @sensor: sensor subdev linked to FIMC[fimc_id] entity, may be null
+ * @pad: the source entity pad index
+ * @link_mask: bitmask of the fimc devices for which link should be enabled
+ */
+static int __fimc_md_create_fimc_sink_links(struct fimc_md *fmd,
+					    struct media_entity *source,
+					    struct v4l2_subdev *sensor,
+					    int pad, int link_mask)
+{
+	struct fimc_source_info *si = NULL;
+	struct media_entity *sink;
+	unsigned int flags = 0;
+	int i, ret = 0;
+
+	if (sensor) {
+		si = v4l2_get_subdev_hostdata(sensor);
+		/* Skip direct FIMC links in the logical FIMC-IS sensor path */
+		if (si && si->fimc_bus_type == FIMC_BUS_TYPE_ISP_WRITEBACK)
+			ret = 1;
+	}
+
+	for (i = 0; !ret && i < FIMC_MAX_DEVS; i++) {
+		if (!fmd->fimc[i])
+			continue;
+		/*
+		 * Some FIMC variants are not fitted with camera capture
+		 * interface. Skip creating a link from sensor for those.
+		 */
+		if (!fmd->fimc[i]->variant->has_cam_if)
+			continue;
+
+		flags = ((1 << i) & link_mask) ? MEDIA_LNK_FL_ENABLED : 0;
+
+		sink = &fmd->fimc[i]->vid_cap.subdev.entity;
+		ret = media_entity_create_link(source, pad, sink,
+					      FIMC_SD_PAD_SINK_CAM, flags);
+		if (ret)
+			return ret;
+
+		/* Notify FIMC capture subdev entity */
+		ret = media_entity_call(sink, link_setup, &sink->pads[0],
+					&source->pads[pad], flags);
+		if (ret)
+			break;
+
+		v4l2_info(&fmd->v4l2_dev, "created link [%s] %c> [%s]\n",
+			  source->name, flags ? '=' : '-', sink->name);
+	}
+
+	for (i = 0; i < FIMC_LITE_MAX_DEVS; i++) {
+		if (!fmd->fimc_lite[i])
+			continue;
+
+		sink = &fmd->fimc_lite[i]->subdev.entity;
+		ret = media_entity_create_link(source, pad, sink,
+					       FLITE_SD_PAD_SINK, 0);
+		if (ret)
+			return ret;
+
+		/* Notify FIMC-LITE subdev entity */
+		ret = media_entity_call(sink, link_setup, &sink->pads[0],
+					&source->pads[pad], 0);
+		if (ret)
+			break;
+
+		v4l2_info(&fmd->v4l2_dev, "created link [%s] -> [%s]\n",
+			  source->name, sink->name);
+	}
+	return 0;
+}
+
+/* Create links from FIMC-LITE source pads to other entities */
+static int __fimc_md_create_flite_source_links(struct fimc_md *fmd)
+{
+	struct media_entity *source, *sink;
+	int i, ret = 0;
+
+	for (i = 0; i < FIMC_LITE_MAX_DEVS; i++) {
+		struct fimc_lite *fimc = fmd->fimc_lite[i];
+
+		if (fimc == NULL)
+			continue;
+
+		source = &fimc->subdev.entity;
+		sink = &fimc->ve.vdev.entity;
+		/* FIMC-LITE's subdev and video node */
+		ret = media_entity_create_link(source, FLITE_SD_PAD_SOURCE_DMA,
+					       sink, 0, 0);
+		if (ret)
+			break;
+		/* Link from FIMC-LITE to IS-ISP subdev */
+		sink = &fmd->fimc_is->isp.subdev.entity;
+		ret = media_entity_create_link(source, FLITE_SD_PAD_SOURCE_ISP,
+					       sink, 0, 0);
+		if (ret)
+			break;
+	}
+
+	return ret;
+}
+
+/* Create FIMC-IS links */
+static int __fimc_md_create_fimc_is_links(struct fimc_md *fmd)
+{
+	struct fimc_isp *isp = &fmd->fimc_is->isp;
+	struct media_entity *source, *sink;
+	int i, ret;
+
+	source = &isp->subdev.entity;
+
+	for (i = 0; i < FIMC_MAX_DEVS; i++) {
+		if (fmd->fimc[i] == NULL)
+			continue;
+
+		/* Link from FIMC-IS-ISP subdev to FIMC */
+		sink = &fmd->fimc[i]->vid_cap.subdev.entity;
+		ret = media_entity_create_link(source, FIMC_ISP_SD_PAD_SRC_FIFO,
+					       sink, FIMC_SD_PAD_SINK_FIFO, 0);
+		if (ret)
+			return ret;
+	}
+
+	/* Link from FIMC-IS-ISP subdev to fimc-is-isp.capture video node */
+	sink = &isp->video_capture.ve.vdev.entity;
+
+	/* Skip this link if the fimc-is-isp video node driver isn't built-in */
+	if (sink->num_pads == 0)
+		return 0;
+
+	return media_entity_create_link(source, FIMC_ISP_SD_PAD_SRC_DMA,
+					sink, 0, 0);
+}
+
+/**
+ * fimc_md_create_links - create default links between registered entities
+ *
+ * Parallel interface sensor entities are connected directly to FIMC capture
+ * entities. The sensors using MIPI CSIS bus are connected through immutable
+ * link with CSI receiver entity specified by mux_id. Any registered CSIS
+ * entity has a link to each registered FIMC capture entity. Enabled links
+ * are created by default between each subsequent registered sensor and
+ * subsequent FIMC capture entity. The number of default active links is
+ * determined by the number of available sensors or FIMC entities,
+ * whichever is less.
+ */
+static int fimc_md_create_links(struct fimc_md *fmd)
+{
+	struct v4l2_subdev *csi_sensors[CSIS_MAX_ENTITIES] = { NULL };
+	struct v4l2_subdev *sensor, *csis;
+	struct fimc_source_info *pdata;
+	struct media_entity *source, *sink;
+	int i, pad, fimc_id = 0, ret = 0;
+	u32 flags, link_mask = 0;
+
+	for (i = 0; i < fmd->num_sensors; i++) {
+		if (fmd->sensor[i].subdev == NULL)
+			continue;
+
+		sensor = fmd->sensor[i].subdev;
+		pdata = v4l2_get_subdev_hostdata(sensor);
+		if (!pdata)
+			continue;
+
+		source = NULL;
+
+		switch (pdata->sensor_bus_type) {
+		case FIMC_BUS_TYPE_MIPI_CSI2:
+			if (WARN(pdata->mux_id >= CSIS_MAX_ENTITIES,
+				"Wrong CSI channel id: %d\n", pdata->mux_id))
+				return -EINVAL;
+
+			csis = fmd->csis[pdata->mux_id].sd;
+			if (WARN(csis == NULL,
+				 "MIPI-CSI interface specified "
+				 "but s5p-csis module is not loaded!\n"))
+				return -EINVAL;
+
+			pad = sensor->entity.num_pads - 1;
+			ret = media_entity_create_link(&sensor->entity, pad,
+					      &csis->entity, CSIS_PAD_SINK,
+					      MEDIA_LNK_FL_IMMUTABLE |
+					      MEDIA_LNK_FL_ENABLED);
+			if (ret)
+				return ret;
+
+			v4l2_info(&fmd->v4l2_dev, "created link [%s] => [%s]\n",
+				  sensor->entity.name, csis->entity.name);
+
+			source = NULL;
+			csi_sensors[pdata->mux_id] = sensor;
+			break;
+
+		case FIMC_BUS_TYPE_ITU_601...FIMC_BUS_TYPE_ITU_656:
+			source = &sensor->entity;
+			pad = 0;
+			break;
+
+		default:
+			v4l2_err(&fmd->v4l2_dev, "Wrong bus_type: %x\n",
+				 pdata->sensor_bus_type);
+			return -EINVAL;
+		}
+		if (source == NULL)
+			continue;
+
+		link_mask = 1 << fimc_id++;
+		ret = __fimc_md_create_fimc_sink_links(fmd, source, sensor,
+						       pad, link_mask);
+	}
+
+	for (i = 0; i < CSIS_MAX_ENTITIES; i++) {
+		if (fmd->csis[i].sd == NULL)
+			continue;
+
+		source = &fmd->csis[i].sd->entity;
+		pad = CSIS_PAD_SOURCE;
+		sensor = csi_sensors[i];
+
+		link_mask = 1 << fimc_id++;
+		ret = __fimc_md_create_fimc_sink_links(fmd, source, sensor,
+						       pad, link_mask);
+	}
+
+	/* Create immutable links between each FIMC's subdev and video node */
+	flags = MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED;
+	for (i = 0; i < FIMC_MAX_DEVS; i++) {
+		if (!fmd->fimc[i])
+			continue;
+
+		source = &fmd->fimc[i]->vid_cap.subdev.entity;
+		sink = &fmd->fimc[i]->vid_cap.ve.vdev.entity;
+
+		ret = media_entity_create_link(source, FIMC_SD_PAD_SOURCE,
+					      sink, 0, flags);
+		if (ret)
+			break;
+	}
+
+	ret = __fimc_md_create_flite_source_links(fmd);
+	if (ret < 0)
+		return ret;
+
+	if (fmd->use_isp)
+		ret = __fimc_md_create_fimc_is_links(fmd);
+
+	return ret;
+}
+
+/*
+ * The peripheral sensor and CAM_BLK (PIXELASYNCMx) clocks management.
+ */
+static void fimc_md_put_clocks(struct fimc_md *fmd)
+{
+	int i = FIMC_MAX_CAMCLKS;
+
+	while (--i >= 0) {
+		if (IS_ERR(fmd->camclk[i].clock))
+			continue;
+		clk_put(fmd->camclk[i].clock);
+		fmd->camclk[i].clock = ERR_PTR(-EINVAL);
+	}
+
+	/* Writeback (PIXELASYNCMx) clocks */
+	for (i = 0; i < FIMC_MAX_WBCLKS; i++) {
+		if (IS_ERR(fmd->wbclk[i]))
+			continue;
+		clk_put(fmd->wbclk[i]);
+		fmd->wbclk[i] = ERR_PTR(-EINVAL);
+	}
+}
+
+static int fimc_md_get_clocks(struct fimc_md *fmd)
+{
+	struct device *dev = &fmd->pdev->dev;
+	char clk_name[32];
+	struct clk *clock;
+	int i, ret = 0;
+
+	for (i = 0; i < FIMC_MAX_CAMCLKS; i++)
+		fmd->camclk[i].clock = ERR_PTR(-EINVAL);
+
+	for (i = 0; i < FIMC_MAX_CAMCLKS; i++) {
+		snprintf(clk_name, sizeof(clk_name), "sclk_cam%u", i);
+		clock = clk_get(dev, clk_name);
+
+		if (IS_ERR(clock)) {
+			dev_err(dev, "Failed to get clock: %s\n", clk_name);
+			ret = PTR_ERR(clock);
+			break;
+		}
+		fmd->camclk[i].clock = clock;
+	}
+	if (ret)
+		fimc_md_put_clocks(fmd);
+
+	if (!fmd->use_isp)
+		return 0;
+	/*
+	 * For now get only PIXELASYNCM1 clock (Writeback B/ISP),
+	 * leave PIXELASYNCM0 out for the LCD Writeback driver.
+	 */
+	fmd->wbclk[CLK_IDX_WB_A] = ERR_PTR(-EINVAL);
+
+	for (i = CLK_IDX_WB_B; i < FIMC_MAX_WBCLKS; i++) {
+		snprintf(clk_name, sizeof(clk_name), "pxl_async%u", i);
+		clock = clk_get(dev, clk_name);
+		if (IS_ERR(clock)) {
+			v4l2_err(&fmd->v4l2_dev, "Failed to get clock: %s\n",
+				  clk_name);
+			ret = PTR_ERR(clock);
+			break;
+		}
+		fmd->wbclk[i] = clock;
+	}
+	if (ret)
+		fimc_md_put_clocks(fmd);
+
+	return ret;
+}
+
+static int __fimc_md_modify_pipeline(struct media_entity *entity, bool enable)
+{
+	struct exynos_video_entity *ve;
+	struct fimc_pipeline *p;
+	struct video_device *vdev;
+	int ret;
+
+	vdev = media_entity_to_video_device(entity);
+	if (vdev->entity.use_count == 0)
+		return 0;
+
+	ve = vdev_to_exynos_video_entity(vdev);
+	p = to_fimc_pipeline(ve->pipe);
+	/*
+	 * Nothing to do if we are disabling the pipeline, some link
+	 * has been disconnected and p->subdevs array is cleared now.
+	 */
+	if (!enable && p->subdevs[IDX_SENSOR] == NULL)
+		return 0;
+
+	if (enable)
+		ret = __fimc_pipeline_open(ve->pipe, entity, true);
+	else
+		ret = __fimc_pipeline_close(ve->pipe);
+
+	if (ret == 0 && !enable)
+		memset(p->subdevs, 0, sizeof(p->subdevs));
+
+	return ret;
+}
+
+/* Locking: called with entity->parent->graph_mutex mutex held. */
+static int __fimc_md_modify_pipelines(struct media_entity *entity, bool enable)
+{
+	struct media_entity *entity_err = entity;
+	struct media_entity_graph graph;
+	int ret;
+
+	/*
+	 * Walk current graph and call the pipeline open/close routine for each
+	 * opened video node that belongs to the graph of entities connected
+	 * through active links. This is needed as we cannot power on/off the
+	 * subdevs in random order.
+	 */
+	media_entity_graph_walk_start(&graph, entity);
+
+	while ((entity = media_entity_graph_walk_next(&graph))) {
+		if (media_entity_type(entity) != MEDIA_ENT_T_DEVNODE)
+			continue;
+
+		ret  = __fimc_md_modify_pipeline(entity, enable);
+
+		if (ret < 0)
+			goto err;
+	}
+
+	return 0;
+ err:
+	media_entity_graph_walk_start(&graph, entity_err);
+
+	while ((entity_err = media_entity_graph_walk_next(&graph))) {
+		if (media_entity_type(entity_err) != MEDIA_ENT_T_DEVNODE)
+			continue;
+
+		__fimc_md_modify_pipeline(entity_err, !enable);
+
+		if (entity_err == entity)
+			break;
+	}
+
+	return ret;
+}
+
+static int fimc_md_link_notify(struct media_link *link, unsigned int flags,
+				unsigned int notification)
+{
+	struct media_entity *sink = link->sink->entity;
+	int ret = 0;
+
+	/* Before link disconnection */
+	if (notification == MEDIA_DEV_NOTIFY_PRE_LINK_CH) {
+		if (!(flags & MEDIA_LNK_FL_ENABLED))
+			ret = __fimc_md_modify_pipelines(sink, false);
+#if 0
+		else
+			/* TODO: Link state change validation */
+#endif
+	/* After link activation */
+	} else if (notification == MEDIA_DEV_NOTIFY_POST_LINK_CH &&
+		   (link->flags & MEDIA_LNK_FL_ENABLED)) {
+		ret = __fimc_md_modify_pipelines(sink, true);
+	}
+
+	return ret ? -EPIPE : 0;
+}
+
+static ssize_t fimc_md_sysfs_show(struct device *dev,
+				  struct device_attribute *attr, char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct fimc_md *fmd = platform_get_drvdata(pdev);
+
+	if (fmd->user_subdev_api)
+		return strlcpy(buf, "Sub-device API (sub-dev)\n", PAGE_SIZE);
+
+	return strlcpy(buf, "V4L2 video node only API (vid-dev)\n", PAGE_SIZE);
+}
+
+static ssize_t fimc_md_sysfs_store(struct device *dev,
+				   struct device_attribute *attr,
+				   const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct fimc_md *fmd = platform_get_drvdata(pdev);
+	bool subdev_api;
+	int i;
+
+	if (!strcmp(buf, "vid-dev\n"))
+		subdev_api = false;
+	else if (!strcmp(buf, "sub-dev\n"))
+		subdev_api = true;
+	else
+		return count;
+
+	fmd->user_subdev_api = subdev_api;
+	for (i = 0; i < FIMC_MAX_DEVS; i++)
+		if (fmd->fimc[i])
+			fmd->fimc[i]->vid_cap.user_subdev_api = subdev_api;
+	return count;
+}
+/*
+ * This device attribute is to select video pipeline configuration method.
+ * There are following valid values:
+ *  vid-dev - for V4L2 video node API only, subdevice will be configured
+ *  by the host driver.
+ *  sub-dev - for media controller API, subdevs must be configured in user
+ *  space before starting streaming.
+ */
+static DEVICE_ATTR(subdev_conf_mode, S_IWUSR | S_IRUGO,
+		   fimc_md_sysfs_show, fimc_md_sysfs_store);
+
+static int fimc_md_get_pinctrl(struct fimc_md *fmd)
+{
+	struct device *dev = &fmd->pdev->dev;
+	struct fimc_pinctrl *pctl = &fmd->pinctl;
+
+	pctl->pinctrl = devm_pinctrl_get(dev);
+	if (IS_ERR(pctl->pinctrl))
+		return PTR_ERR(pctl->pinctrl);
+
+	pctl->state_default = pinctrl_lookup_state(pctl->pinctrl,
+					PINCTRL_STATE_DEFAULT);
+	if (IS_ERR(pctl->state_default))
+		return PTR_ERR(pctl->state_default);
+
+	pctl->state_idle = pinctrl_lookup_state(pctl->pinctrl,
+					PINCTRL_STATE_IDLE);
+	return 0;
+}
+
+static int cam_clk_prepare(struct clk_hw *hw)
+{
+	struct cam_clk *camclk = to_cam_clk(hw);
+	int ret;
+
+	if (camclk->fmd->pmf == NULL)
+		return -ENODEV;
+
+	ret = pm_runtime_get_sync(camclk->fmd->pmf);
+	return ret < 0 ? ret : 0;
+}
+
+static void cam_clk_unprepare(struct clk_hw *hw)
+{
+	struct cam_clk *camclk = to_cam_clk(hw);
+
+	if (camclk->fmd->pmf == NULL)
+		return;
+
+	pm_runtime_put_sync(camclk->fmd->pmf);
+}
+
+static const struct clk_ops cam_clk_ops = {
+	.prepare = cam_clk_prepare,
+	.unprepare = cam_clk_unprepare,
+};
+
+static void fimc_md_unregister_clk_provider(struct fimc_md *fmd)
+{
+	struct cam_clk_provider *cp = &fmd->clk_provider;
+	unsigned int i;
+
+	if (cp->of_node)
+		of_clk_del_provider(cp->of_node);
+
+	for (i = 0; i < cp->num_clocks; i++)
+		clk_unregister(cp->clks[i]);
+}
+
+static int fimc_md_register_clk_provider(struct fimc_md *fmd)
+{
+	struct cam_clk_provider *cp = &fmd->clk_provider;
+	struct device *dev = &fmd->pdev->dev;
+	int i, ret;
+
+	for (i = 0; i < FIMC_MAX_CAMCLKS; i++) {
+		struct cam_clk *camclk = &cp->camclk[i];
+		struct clk_init_data init;
+		const char *p_name;
+
+		ret = of_property_read_string_index(dev->of_node,
+					"clock-output-names", i, &init.name);
+		if (ret < 0)
+			break;
+
+		p_name = __clk_get_name(fmd->camclk[i].clock);
+
+		/* It's safe since clk_register() will duplicate the string. */
+		init.parent_names = &p_name;
+		init.num_parents = 1;
+		init.ops = &cam_clk_ops;
+		init.flags = CLK_SET_RATE_PARENT;
+		camclk->hw.init = &init;
+		camclk->fmd = fmd;
+
+		cp->clks[i] = clk_register(NULL, &camclk->hw);
+		if (IS_ERR(cp->clks[i])) {
+			dev_err(dev, "failed to register clock: %s (%ld)\n",
+					init.name, PTR_ERR(cp->clks[i]));
+			ret = PTR_ERR(cp->clks[i]);
+			goto err;
+		}
+		cp->num_clocks++;
+	}
+
+	if (cp->num_clocks == 0) {
+		dev_warn(dev, "clk provider not registered\n");
+		return 0;
+	}
+
+	cp->clk_data.clks = cp->clks;
+	cp->clk_data.clk_num = cp->num_clocks;
+	cp->of_node = dev->of_node;
+	ret = of_clk_add_provider(dev->of_node, of_clk_src_onecell_get,
+				  &cp->clk_data);
+	if (ret == 0)
+		return 0;
+err:
+	fimc_md_unregister_clk_provider(fmd);
+	return ret;
+}
+
+static int subdev_notifier_bound(struct v4l2_async_notifier *notifier,
+				 struct v4l2_subdev *subdev,
+				 struct v4l2_async_subdev *asd)
+{
+	struct fimc_md *fmd = notifier_to_fimc_md(notifier);
+	struct fimc_sensor_info *si = NULL;
+	int i;
+
+	/* Find platform data for this sensor subdev */
+	for (i = 0; i < ARRAY_SIZE(fmd->sensor); i++)
+		if (fmd->sensor[i].asd.match.of.node == subdev->dev->of_node)
+			si = &fmd->sensor[i];
+
+	if (si == NULL)
+		return -EINVAL;
+
+	v4l2_set_subdev_hostdata(subdev, &si->pdata);
+
+	if (si->pdata.fimc_bus_type == FIMC_BUS_TYPE_ISP_WRITEBACK)
+		subdev->grp_id = GRP_ID_FIMC_IS_SENSOR;
+	else
+		subdev->grp_id = GRP_ID_SENSOR;
+
+	si->subdev = subdev;
+
+	v4l2_info(&fmd->v4l2_dev, "Registered sensor subdevice: %s (%d)\n",
+		  subdev->name, fmd->num_sensors);
+
+	fmd->num_sensors++;
+
+	return 0;
+}
+
+static int subdev_notifier_complete(struct v4l2_async_notifier *notifier)
+{
+	struct fimc_md *fmd = notifier_to_fimc_md(notifier);
+	int ret;
+
+	mutex_lock(&fmd->media_dev.graph_mutex);
+
+	ret = fimc_md_create_links(fmd);
+	if (ret < 0)
+		goto unlock;
+
+	ret = v4l2_device_register_subdev_nodes(&fmd->v4l2_dev);
+unlock:
+	mutex_unlock(&fmd->media_dev.graph_mutex);
+	return ret;
+}
+
+static int fimc_md_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct v4l2_device *v4l2_dev;
+	struct fimc_md *fmd;
+	int ret;
+
+	fmd = devm_kzalloc(dev, sizeof(*fmd), GFP_KERNEL);
+	if (!fmd)
+		return -ENOMEM;
+
+	spin_lock_init(&fmd->slock);
+	INIT_LIST_HEAD(&fmd->pipelines);
+	fmd->pdev = pdev;
+
+	strlcpy(fmd->media_dev.model, "SAMSUNG S5P FIMC",
+		sizeof(fmd->media_dev.model));
+	fmd->media_dev.link_notify = fimc_md_link_notify;
+	fmd->media_dev.dev = dev;
+
+	v4l2_dev = &fmd->v4l2_dev;
+	v4l2_dev->mdev = &fmd->media_dev;
+	v4l2_dev->notify = fimc_sensor_notify;
+	strlcpy(v4l2_dev->name, "s5p-fimc-md", sizeof(v4l2_dev->name));
+
+	fmd->use_isp = fimc_md_is_isp_available(dev->of_node);
+	fmd->user_subdev_api = true;
+
+	ret = v4l2_device_register(dev, &fmd->v4l2_dev);
+	if (ret < 0) {
+		v4l2_err(v4l2_dev, "Failed to register v4l2_device: %d\n", ret);
+		return ret;
+	}
+
+	ret = media_device_register(&fmd->media_dev);
+	if (ret < 0) {
+		v4l2_err(v4l2_dev, "Failed to register media device: %d\n", ret);
+		goto err_v4l2_dev;
+	}
+
+	ret = fimc_md_get_clocks(fmd);
+	if (ret)
+		goto err_md;
+
+	ret = fimc_md_get_pinctrl(fmd);
+	if (ret < 0) {
+		if (ret != EPROBE_DEFER)
+			dev_err(dev, "Failed to get pinctrl: %d\n", ret);
+		goto err_clk;
+	}
+
+	platform_set_drvdata(pdev, fmd);
+
+	/* Protect the media graph while we're registering entities */
+	mutex_lock(&fmd->media_dev.graph_mutex);
+
+	ret = fimc_md_register_platform_entities(fmd, dev->of_node);
+	if (ret) {
+		mutex_unlock(&fmd->media_dev.graph_mutex);
+		goto err_clk;
+	}
+
+	ret = fimc_md_register_sensor_entities(fmd);
+	if (ret) {
+		mutex_unlock(&fmd->media_dev.graph_mutex);
+		goto err_m_ent;
+	}
+
+	mutex_unlock(&fmd->media_dev.graph_mutex);
+
+	ret = device_create_file(&pdev->dev, &dev_attr_subdev_conf_mode);
+	if (ret)
+		goto err_m_ent;
+	/*
+	 * FIMC platform devices need to be registered before the sclk_cam
+	 * clocks provider, as one of these devices needs to be activated
+	 * to enable the clock.
+	 */
+	ret = fimc_md_register_clk_provider(fmd);
+	if (ret < 0) {
+		v4l2_err(v4l2_dev, "clock provider registration failed\n");
+		goto err_attr;
+	}
+
+	if (fmd->num_sensors > 0) {
+		fmd->subdev_notifier.subdevs = fmd->async_subdevs;
+		fmd->subdev_notifier.num_subdevs = fmd->num_sensors;
+		fmd->subdev_notifier.bound = subdev_notifier_bound;
+		fmd->subdev_notifier.complete = subdev_notifier_complete;
+		fmd->num_sensors = 0;
+
+		ret = v4l2_async_notifier_register(&fmd->v4l2_dev,
+						&fmd->subdev_notifier);
+		if (ret)
+			goto err_clk_p;
+	}
+
+	return 0;
+
+err_clk_p:
+	fimc_md_unregister_clk_provider(fmd);
+err_attr:
+	device_remove_file(&pdev->dev, &dev_attr_subdev_conf_mode);
+err_clk:
+	fimc_md_put_clocks(fmd);
+err_m_ent:
+	fimc_md_unregister_entities(fmd);
+err_md:
+	media_device_unregister(&fmd->media_dev);
+err_v4l2_dev:
+	v4l2_device_unregister(&fmd->v4l2_dev);
+	return ret;
+}
+
+static int fimc_md_remove(struct platform_device *pdev)
+{
+	struct fimc_md *fmd = platform_get_drvdata(pdev);
+
+	if (!fmd)
+		return 0;
+
+	fimc_md_unregister_clk_provider(fmd);
+	v4l2_async_notifier_unregister(&fmd->subdev_notifier);
+
+	v4l2_device_unregister(&fmd->v4l2_dev);
+	device_remove_file(&pdev->dev, &dev_attr_subdev_conf_mode);
+	fimc_md_unregister_entities(fmd);
+	fimc_md_pipelines_free(fmd);
+	media_device_unregister(&fmd->media_dev);
+	fimc_md_put_clocks(fmd);
+
+	return 0;
+}
+
+static const struct platform_device_id fimc_driver_ids[] __always_unused = {
+	{ .name = "s5p-fimc-md" },
+	{ },
+};
+MODULE_DEVICE_TABLE(platform, fimc_driver_ids);
+
+static const struct of_device_id fimc_md_of_match[] = {
+	{ .compatible = "samsung,fimc" },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, fimc_md_of_match);
+
+static struct platform_driver fimc_md_driver = {
+	.probe		= fimc_md_probe,
+	.remove		= fimc_md_remove,
+	.driver = {
+		.of_match_table = of_match_ptr(fimc_md_of_match),
+		.name		= "s5p-fimc-md",
+	}
+};
+
+static int __init fimc_md_init(void)
+{
+	int ret;
+
+	request_module("s5p-csis");
+	ret = fimc_register_driver();
+	if (ret)
+		return ret;
+
+	return platform_driver_register(&fimc_md_driver);
+}
+
+static void __exit fimc_md_exit(void)
+{
+	platform_driver_unregister(&fimc_md_driver);
+	fimc_unregister_driver();
+}
+
+module_init(fimc_md_init);
+module_exit(fimc_md_exit);
+
+MODULE_AUTHOR("Sylwester Nawrocki <s.nawrocki@samsung.com>");
+MODULE_DESCRIPTION("S5P FIMC camera host interface/video postprocessor driver");
+MODULE_LICENSE("GPL");
+MODULE_VERSION("2.0.1");
diff --git a/drivers/media/platform/exynos4-is/media-dev.h b/drivers/media/platform/exynos4-is/media-dev.h
new file mode 100644
index 0000000..0321454
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/media-dev.h
@@ -0,0 +1,210 @@
+/*
+ * Copyright (C) 2011 - 2012 Samsung Electronics Co., Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef FIMC_MDEVICE_H_
+#define FIMC_MDEVICE_H_
+
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/platform_device.h>
+#include <linux/mutex.h>
+#include <linux/of.h>
+#include <linux/pinctrl/consumer.h>
+#include <media/media-device.h>
+#include <media/media-entity.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+#include <media/exynos-fimc.h>
+
+#include "fimc-core.h"
+#include "fimc-lite.h"
+#include "mipi-csis.h"
+
+#define FIMC_OF_NODE_NAME	"fimc"
+#define FIMC_LITE_OF_NODE_NAME	"fimc-lite"
+#define FIMC_IS_OF_NODE_NAME	"fimc-is"
+#define CSIS_OF_NODE_NAME	"csis"
+
+#define PINCTRL_STATE_IDLE	"idle"
+
+#define FIMC_MAX_SENSORS	4
+#define FIMC_MAX_CAMCLKS	2
+#define DEFAULT_SENSOR_CLK_FREQ	24000000U
+
+/* LCD/ISP Writeback clocks (PIXELASYNCMx) */
+enum {
+	CLK_IDX_WB_A,
+	CLK_IDX_WB_B,
+	FIMC_MAX_WBCLKS
+};
+
+enum fimc_subdev_index {
+	IDX_SENSOR,
+	IDX_CSIS,
+	IDX_FLITE,
+	IDX_IS_ISP,
+	IDX_FIMC,
+	IDX_MAX,
+};
+
+/*
+ * This structure represents a chain of media entities, including a data
+ * source entity (e.g. an image sensor subdevice), a data capture entity
+ * - a video capture device node and any remaining entities.
+ */
+struct fimc_pipeline {
+	struct exynos_media_pipeline ep;
+	struct list_head list;
+	struct media_entity *vdev_entity;
+	struct v4l2_subdev *subdevs[IDX_MAX];
+};
+
+#define to_fimc_pipeline(_ep) container_of(_ep, struct fimc_pipeline, ep)
+
+struct fimc_csis_info {
+	struct v4l2_subdev *sd;
+	int id;
+};
+
+struct fimc_camclk_info {
+	struct clk *clock;
+	int use_count;
+	unsigned long frequency;
+};
+
+/**
+ * struct fimc_sensor_info - image data source subdev information
+ * @pdata: sensor's atrributes passed as media device's platform data
+ * @asd: asynchronous subdev registration data structure
+ * @subdev: image sensor v4l2 subdev
+ * @host: fimc device the sensor is currently linked to
+ *
+ * This data structure applies to image sensor and the writeback subdevs.
+ */
+struct fimc_sensor_info {
+	struct fimc_source_info pdata;
+	struct v4l2_async_subdev asd;
+	struct v4l2_subdev *subdev;
+	struct fimc_dev *host;
+};
+
+struct cam_clk {
+	struct clk_hw hw;
+	struct fimc_md *fmd;
+};
+#define to_cam_clk(_hw) container_of(_hw, struct cam_clk, hw)
+
+/**
+ * struct fimc_md - fimc media device information
+ * @csis: MIPI CSIS subdevs data
+ * @sensor: array of registered sensor subdevs
+ * @num_sensors: actual number of registered sensors
+ * @camclk: external sensor clock information
+ * @fimc: array of registered fimc devices
+ * @fimc_is: fimc-is data structure
+ * @use_isp: set to true when FIMC-IS subsystem is used
+ * @pmf: handle to the CAMCLK clock control FIMC helper device
+ * @media_dev: top level media device
+ * @v4l2_dev: top level v4l2_device holding up the subdevs
+ * @pdev: platform device this media device is hooked up into
+ * @pinctrl: camera port pinctrl handle
+ * @state_default: pinctrl default state handle
+ * @state_idle: pinctrl idle state handle
+ * @cam_clk_provider: CAMCLK clock provider structure
+ * @user_subdev_api: true if subdevs are not configured by the host driver
+ * @slock: spinlock protecting @sensor array
+ */
+struct fimc_md {
+	struct fimc_csis_info csis[CSIS_MAX_ENTITIES];
+	struct fimc_sensor_info sensor[FIMC_MAX_SENSORS];
+	int num_sensors;
+	struct fimc_camclk_info camclk[FIMC_MAX_CAMCLKS];
+	struct clk *wbclk[FIMC_MAX_WBCLKS];
+	struct fimc_lite *fimc_lite[FIMC_LITE_MAX_DEVS];
+	struct fimc_dev *fimc[FIMC_MAX_DEVS];
+	struct fimc_is *fimc_is;
+	bool use_isp;
+	struct device *pmf;
+	struct media_device media_dev;
+	struct v4l2_device v4l2_dev;
+	struct platform_device *pdev;
+
+	struct fimc_pinctrl {
+		struct pinctrl *pinctrl;
+		struct pinctrl_state *state_default;
+		struct pinctrl_state *state_idle;
+	} pinctl;
+
+	struct cam_clk_provider {
+		struct clk *clks[FIMC_MAX_CAMCLKS];
+		struct clk_onecell_data clk_data;
+		struct device_node *of_node;
+		struct cam_clk camclk[FIMC_MAX_CAMCLKS];
+		int num_clocks;
+	} clk_provider;
+
+	struct v4l2_async_notifier subdev_notifier;
+	struct v4l2_async_subdev *async_subdevs[FIMC_MAX_SENSORS];
+
+	bool user_subdev_api;
+	spinlock_t slock;
+	struct list_head pipelines;
+};
+
+static inline
+struct fimc_sensor_info *source_to_sensor_info(struct fimc_source_info *si)
+{
+	return container_of(si, struct fimc_sensor_info, pdata);
+}
+
+static inline struct fimc_md *entity_to_fimc_mdev(struct media_entity *me)
+{
+	return me->parent == NULL ? NULL :
+		container_of(me->parent, struct fimc_md, media_dev);
+}
+
+static inline struct fimc_md *notifier_to_fimc_md(struct v4l2_async_notifier *n)
+{
+	return container_of(n, struct fimc_md, subdev_notifier);
+}
+
+static inline void fimc_md_graph_lock(struct exynos_video_entity *ve)
+{
+	mutex_lock(&ve->vdev.entity.parent->graph_mutex);
+}
+
+static inline void fimc_md_graph_unlock(struct exynos_video_entity *ve)
+{
+	mutex_unlock(&ve->vdev.entity.parent->graph_mutex);
+}
+
+int fimc_md_set_camclk(struct v4l2_subdev *sd, bool on);
+
+#ifdef CONFIG_OF
+static inline bool fimc_md_is_isp_available(struct device_node *node)
+{
+	node = of_get_child_by_name(node, FIMC_IS_OF_NODE_NAME);
+	return node ? of_device_is_available(node) : false;
+}
+#else
+#define fimc_md_is_isp_available(node) (false)
+#endif /* CONFIG_OF */
+
+static inline struct v4l2_subdev *__fimc_md_get_subdev(
+				struct exynos_media_pipeline *ep,
+				unsigned int index)
+{
+	struct fimc_pipeline *p = to_fimc_pipeline(ep);
+
+	if (!p || index >= IDX_MAX)
+		return NULL;
+	else
+		return p->subdevs[index];
+}
+
+#endif
diff --git a/drivers/media/platform/exynos4-is/mipi-csis.c b/drivers/media/platform/exynos4-is/mipi-csis.c
new file mode 100644
index 0000000..4b85105
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/mipi-csis.c
@@ -0,0 +1,1053 @@
+/*
+ * Samsung S5P/EXYNOS SoC series MIPI-CSI receiver driver
+ *
+ * Copyright (C) 2011 - 2013 Samsung Electronics Co., Ltd.
+ * Author: Sylwester Nawrocki <s.nawrocki@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/errno.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/kernel.h>
+#include <linux/memory.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_graph.h>
+#include <linux/phy/phy.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/regulator/consumer.h>
+#include <linux/sizes.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/videodev2.h>
+#include <media/exynos-fimc.h>
+#include <media/v4l2-of.h>
+#include <media/v4l2-subdev.h>
+
+#include "mipi-csis.h"
+
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "Debug level (0-2)");
+
+/* Register map definition */
+
+/* CSIS global control */
+#define S5PCSIS_CTRL			0x00
+#define S5PCSIS_CTRL_DPDN_DEFAULT	(0 << 31)
+#define S5PCSIS_CTRL_DPDN_SWAP		(1 << 31)
+#define S5PCSIS_CTRL_ALIGN_32BIT	(1 << 20)
+#define S5PCSIS_CTRL_UPDATE_SHADOW	(1 << 16)
+#define S5PCSIS_CTRL_WCLK_EXTCLK	(1 << 8)
+#define S5PCSIS_CTRL_RESET		(1 << 4)
+#define S5PCSIS_CTRL_ENABLE		(1 << 0)
+
+/* D-PHY control */
+#define S5PCSIS_DPHYCTRL		0x04
+#define S5PCSIS_DPHYCTRL_HSS_MASK	(0x1f << 27)
+#define S5PCSIS_DPHYCTRL_ENABLE		(0x1f << 0)
+
+#define S5PCSIS_CONFIG			0x08
+#define S5PCSIS_CFG_FMT_YCBCR422_8BIT	(0x1e << 2)
+#define S5PCSIS_CFG_FMT_RAW8		(0x2a << 2)
+#define S5PCSIS_CFG_FMT_RAW10		(0x2b << 2)
+#define S5PCSIS_CFG_FMT_RAW12		(0x2c << 2)
+/* User defined formats, x = 1...4 */
+#define S5PCSIS_CFG_FMT_USER(x)		((0x30 + x - 1) << 2)
+#define S5PCSIS_CFG_FMT_MASK		(0x3f << 2)
+#define S5PCSIS_CFG_NR_LANE_MASK	3
+
+/* Interrupt mask */
+#define S5PCSIS_INTMSK			0x10
+#define S5PCSIS_INTMSK_EVEN_BEFORE	(1 << 31)
+#define S5PCSIS_INTMSK_EVEN_AFTER	(1 << 30)
+#define S5PCSIS_INTMSK_ODD_BEFORE	(1 << 29)
+#define S5PCSIS_INTMSK_ODD_AFTER	(1 << 28)
+#define S5PCSIS_INTMSK_FRAME_START	(1 << 27)
+#define S5PCSIS_INTMSK_FRAME_END	(1 << 26)
+#define S5PCSIS_INTMSK_ERR_SOT_HS	(1 << 12)
+#define S5PCSIS_INTMSK_ERR_LOST_FS	(1 << 5)
+#define S5PCSIS_INTMSK_ERR_LOST_FE	(1 << 4)
+#define S5PCSIS_INTMSK_ERR_OVER		(1 << 3)
+#define S5PCSIS_INTMSK_ERR_ECC		(1 << 2)
+#define S5PCSIS_INTMSK_ERR_CRC		(1 << 1)
+#define S5PCSIS_INTMSK_ERR_UNKNOWN	(1 << 0)
+#define S5PCSIS_INTMSK_EXYNOS4_EN_ALL	0xf000103f
+#define S5PCSIS_INTMSK_EXYNOS5_EN_ALL	0xfc00103f
+
+/* Interrupt source */
+#define S5PCSIS_INTSRC			0x14
+#define S5PCSIS_INTSRC_EVEN_BEFORE	(1 << 31)
+#define S5PCSIS_INTSRC_EVEN_AFTER	(1 << 30)
+#define S5PCSIS_INTSRC_EVEN		(0x3 << 30)
+#define S5PCSIS_INTSRC_ODD_BEFORE	(1 << 29)
+#define S5PCSIS_INTSRC_ODD_AFTER	(1 << 28)
+#define S5PCSIS_INTSRC_ODD		(0x3 << 28)
+#define S5PCSIS_INTSRC_NON_IMAGE_DATA	(0xf << 28)
+#define S5PCSIS_INTSRC_FRAME_START	(1 << 27)
+#define S5PCSIS_INTSRC_FRAME_END	(1 << 26)
+#define S5PCSIS_INTSRC_ERR_SOT_HS	(0xf << 12)
+#define S5PCSIS_INTSRC_ERR_LOST_FS	(1 << 5)
+#define S5PCSIS_INTSRC_ERR_LOST_FE	(1 << 4)
+#define S5PCSIS_INTSRC_ERR_OVER		(1 << 3)
+#define S5PCSIS_INTSRC_ERR_ECC		(1 << 2)
+#define S5PCSIS_INTSRC_ERR_CRC		(1 << 1)
+#define S5PCSIS_INTSRC_ERR_UNKNOWN	(1 << 0)
+#define S5PCSIS_INTSRC_ERRORS		0xf03f
+
+/* Pixel resolution */
+#define S5PCSIS_RESOL			0x2c
+#define CSIS_MAX_PIX_WIDTH		0xffff
+#define CSIS_MAX_PIX_HEIGHT		0xffff
+
+/* Non-image packet data buffers */
+#define S5PCSIS_PKTDATA_ODD		0x2000
+#define S5PCSIS_PKTDATA_EVEN		0x3000
+#define S5PCSIS_PKTDATA_SIZE		SZ_4K
+
+enum {
+	CSIS_CLK_MUX,
+	CSIS_CLK_GATE,
+};
+
+static char *csi_clock_name[] = {
+	[CSIS_CLK_MUX]  = "sclk_csis",
+	[CSIS_CLK_GATE] = "csis",
+};
+#define NUM_CSIS_CLOCKS	ARRAY_SIZE(csi_clock_name)
+#define DEFAULT_SCLK_CSIS_FREQ	166000000UL
+
+static const char * const csis_supply_name[] = {
+	"vddcore",  /* CSIS Core (1.0V, 1.1V or 1.2V) suppply */
+	"vddio",    /* CSIS I/O and PLL (1.8V) supply */
+};
+#define CSIS_NUM_SUPPLIES ARRAY_SIZE(csis_supply_name)
+
+enum {
+	ST_POWERED	= 1,
+	ST_STREAMING	= 2,
+	ST_SUSPENDED	= 4,
+};
+
+struct s5pcsis_event {
+	u32 mask;
+	const char * const name;
+	unsigned int counter;
+};
+
+static const struct s5pcsis_event s5pcsis_events[] = {
+	/* Errors */
+	{ S5PCSIS_INTSRC_ERR_SOT_HS,	"SOT Error" },
+	{ S5PCSIS_INTSRC_ERR_LOST_FS,	"Lost Frame Start Error" },
+	{ S5PCSIS_INTSRC_ERR_LOST_FE,	"Lost Frame End Error" },
+	{ S5PCSIS_INTSRC_ERR_OVER,	"FIFO Overflow Error" },
+	{ S5PCSIS_INTSRC_ERR_ECC,	"ECC Error" },
+	{ S5PCSIS_INTSRC_ERR_CRC,	"CRC Error" },
+	{ S5PCSIS_INTSRC_ERR_UNKNOWN,	"Unknown Error" },
+	/* Non-image data receive events */
+	{ S5PCSIS_INTSRC_EVEN_BEFORE,	"Non-image data before even frame" },
+	{ S5PCSIS_INTSRC_EVEN_AFTER,	"Non-image data after even frame" },
+	{ S5PCSIS_INTSRC_ODD_BEFORE,	"Non-image data before odd frame" },
+	{ S5PCSIS_INTSRC_ODD_AFTER,	"Non-image data after odd frame" },
+	/* Frame start/end */
+	{ S5PCSIS_INTSRC_FRAME_START,	"Frame Start" },
+	{ S5PCSIS_INTSRC_FRAME_END,	"Frame End" },
+};
+#define S5PCSIS_NUM_EVENTS ARRAY_SIZE(s5pcsis_events)
+
+struct csis_pktbuf {
+	u32 *data;
+	unsigned int len;
+};
+
+struct csis_drvdata {
+	/* Mask of all used interrupts in S5PCSIS_INTMSK register */
+	u32 interrupt_mask;
+};
+
+/**
+ * struct csis_state - the driver's internal state data structure
+ * @lock: mutex serializing the subdev and power management operations,
+ *        protecting @format and @flags members
+ * @pads: CSIS pads array
+ * @sd: v4l2_subdev associated with CSIS device instance
+ * @index: the hardware instance index
+ * @pdev: CSIS platform device
+ * @phy: pointer to the CSIS generic PHY
+ * @regs: mmaped I/O registers memory
+ * @supplies: CSIS regulator supplies
+ * @clock: CSIS clocks
+ * @irq: requested s5p-mipi-csis irq number
+ * @interrupt_mask: interrupt mask of the all used interrupts
+ * @flags: the state variable for power and streaming control
+ * @clock_frequency: device bus clock frequency
+ * @hs_settle: HS-RX settle time
+ * @num_lanes: number of MIPI-CSI data lanes used
+ * @max_num_lanes: maximum number of MIPI-CSI data lanes supported
+ * @wclk_ext: CSI wrapper clock: 0 - bus clock, 1 - external SCLK_CAM
+ * @csis_fmt: current CSIS pixel format
+ * @format: common media bus format for the source and sink pad
+ * @slock: spinlock protecting structure members below
+ * @pkt_buf: the frame embedded (non-image) data buffer
+ * @events: MIPI-CSIS event (error) counters
+ */
+struct csis_state {
+	struct mutex lock;
+	struct media_pad pads[CSIS_PADS_NUM];
+	struct v4l2_subdev sd;
+	u8 index;
+	struct platform_device *pdev;
+	struct phy *phy;
+	void __iomem *regs;
+	struct regulator_bulk_data supplies[CSIS_NUM_SUPPLIES];
+	struct clk *clock[NUM_CSIS_CLOCKS];
+	int irq;
+	u32 interrupt_mask;
+	u32 flags;
+
+	u32 clk_frequency;
+	u32 hs_settle;
+	u32 num_lanes;
+	u32 max_num_lanes;
+	u8 wclk_ext;
+
+	const struct csis_pix_format *csis_fmt;
+	struct v4l2_mbus_framefmt format;
+
+	spinlock_t slock;
+	struct csis_pktbuf pkt_buf;
+	struct s5pcsis_event events[S5PCSIS_NUM_EVENTS];
+};
+
+/**
+ * struct csis_pix_format - CSIS pixel format description
+ * @pix_width_alignment: horizontal pixel alignment, width will be
+ *                       multiple of 2^pix_width_alignment
+ * @code: corresponding media bus code
+ * @fmt_reg: S5PCSIS_CONFIG register value
+ * @data_alignment: MIPI-CSI data alignment in bits
+ */
+struct csis_pix_format {
+	unsigned int pix_width_alignment;
+	u32 code;
+	u32 fmt_reg;
+	u8 data_alignment;
+};
+
+static const struct csis_pix_format s5pcsis_formats[] = {
+	{
+		.code = MEDIA_BUS_FMT_VYUY8_2X8,
+		.fmt_reg = S5PCSIS_CFG_FMT_YCBCR422_8BIT,
+		.data_alignment = 32,
+	}, {
+		.code = MEDIA_BUS_FMT_JPEG_1X8,
+		.fmt_reg = S5PCSIS_CFG_FMT_USER(1),
+		.data_alignment = 32,
+	}, {
+		.code = MEDIA_BUS_FMT_S5C_UYVY_JPEG_1X8,
+		.fmt_reg = S5PCSIS_CFG_FMT_USER(1),
+		.data_alignment = 32,
+	}, {
+		.code = MEDIA_BUS_FMT_SGRBG8_1X8,
+		.fmt_reg = S5PCSIS_CFG_FMT_RAW8,
+		.data_alignment = 24,
+	}, {
+		.code = MEDIA_BUS_FMT_SGRBG10_1X10,
+		.fmt_reg = S5PCSIS_CFG_FMT_RAW10,
+		.data_alignment = 24,
+	}, {
+		.code = MEDIA_BUS_FMT_SGRBG12_1X12,
+		.fmt_reg = S5PCSIS_CFG_FMT_RAW12,
+		.data_alignment = 24,
+	}
+};
+
+#define s5pcsis_write(__csis, __r, __v) writel(__v, __csis->regs + __r)
+#define s5pcsis_read(__csis, __r) readl(__csis->regs + __r)
+
+static struct csis_state *sd_to_csis_state(struct v4l2_subdev *sdev)
+{
+	return container_of(sdev, struct csis_state, sd);
+}
+
+static const struct csis_pix_format *find_csis_format(
+	struct v4l2_mbus_framefmt *mf)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(s5pcsis_formats); i++)
+		if (mf->code == s5pcsis_formats[i].code)
+			return &s5pcsis_formats[i];
+	return NULL;
+}
+
+static void s5pcsis_enable_interrupts(struct csis_state *state, bool on)
+{
+	u32 val = s5pcsis_read(state, S5PCSIS_INTMSK);
+	if (on)
+		val |= state->interrupt_mask;
+	else
+		val &= ~state->interrupt_mask;
+	s5pcsis_write(state, S5PCSIS_INTMSK, val);
+}
+
+static void s5pcsis_reset(struct csis_state *state)
+{
+	u32 val = s5pcsis_read(state, S5PCSIS_CTRL);
+
+	s5pcsis_write(state, S5PCSIS_CTRL, val | S5PCSIS_CTRL_RESET);
+	udelay(10);
+}
+
+static void s5pcsis_system_enable(struct csis_state *state, int on)
+{
+	u32 val, mask;
+
+	val = s5pcsis_read(state, S5PCSIS_CTRL);
+	if (on)
+		val |= S5PCSIS_CTRL_ENABLE;
+	else
+		val &= ~S5PCSIS_CTRL_ENABLE;
+	s5pcsis_write(state, S5PCSIS_CTRL, val);
+
+	val = s5pcsis_read(state, S5PCSIS_DPHYCTRL);
+	val &= ~S5PCSIS_DPHYCTRL_ENABLE;
+	if (on) {
+		mask = (1 << (state->num_lanes + 1)) - 1;
+		val |= (mask & S5PCSIS_DPHYCTRL_ENABLE);
+	}
+	s5pcsis_write(state, S5PCSIS_DPHYCTRL, val);
+}
+
+/* Called with the state.lock mutex held */
+static void __s5pcsis_set_format(struct csis_state *state)
+{
+	struct v4l2_mbus_framefmt *mf = &state->format;
+	u32 val;
+
+	v4l2_dbg(1, debug, &state->sd, "fmt: %#x, %d x %d\n",
+		 mf->code, mf->width, mf->height);
+
+	/* Color format */
+	val = s5pcsis_read(state, S5PCSIS_CONFIG);
+	val = (val & ~S5PCSIS_CFG_FMT_MASK) | state->csis_fmt->fmt_reg;
+	s5pcsis_write(state, S5PCSIS_CONFIG, val);
+
+	/* Pixel resolution */
+	val = (mf->width << 16) | mf->height;
+	s5pcsis_write(state, S5PCSIS_RESOL, val);
+}
+
+static void s5pcsis_set_hsync_settle(struct csis_state *state, int settle)
+{
+	u32 val = s5pcsis_read(state, S5PCSIS_DPHYCTRL);
+
+	val = (val & ~S5PCSIS_DPHYCTRL_HSS_MASK) | (settle << 27);
+	s5pcsis_write(state, S5PCSIS_DPHYCTRL, val);
+}
+
+static void s5pcsis_set_params(struct csis_state *state)
+{
+	u32 val;
+
+	val = s5pcsis_read(state, S5PCSIS_CONFIG);
+	val = (val & ~S5PCSIS_CFG_NR_LANE_MASK) | (state->num_lanes - 1);
+	s5pcsis_write(state, S5PCSIS_CONFIG, val);
+
+	__s5pcsis_set_format(state);
+	s5pcsis_set_hsync_settle(state, state->hs_settle);
+
+	val = s5pcsis_read(state, S5PCSIS_CTRL);
+	if (state->csis_fmt->data_alignment == 32)
+		val |= S5PCSIS_CTRL_ALIGN_32BIT;
+	else /* 24-bits */
+		val &= ~S5PCSIS_CTRL_ALIGN_32BIT;
+
+	val &= ~S5PCSIS_CTRL_WCLK_EXTCLK;
+	if (state->wclk_ext)
+		val |= S5PCSIS_CTRL_WCLK_EXTCLK;
+	s5pcsis_write(state, S5PCSIS_CTRL, val);
+
+	/* Update the shadow register. */
+	val = s5pcsis_read(state, S5PCSIS_CTRL);
+	s5pcsis_write(state, S5PCSIS_CTRL, val | S5PCSIS_CTRL_UPDATE_SHADOW);
+}
+
+static void s5pcsis_clk_put(struct csis_state *state)
+{
+	int i;
+
+	for (i = 0; i < NUM_CSIS_CLOCKS; i++) {
+		if (IS_ERR(state->clock[i]))
+			continue;
+		clk_unprepare(state->clock[i]);
+		clk_put(state->clock[i]);
+		state->clock[i] = ERR_PTR(-EINVAL);
+	}
+}
+
+static int s5pcsis_clk_get(struct csis_state *state)
+{
+	struct device *dev = &state->pdev->dev;
+	int i, ret;
+
+	for (i = 0; i < NUM_CSIS_CLOCKS; i++)
+		state->clock[i] = ERR_PTR(-EINVAL);
+
+	for (i = 0; i < NUM_CSIS_CLOCKS; i++) {
+		state->clock[i] = clk_get(dev, csi_clock_name[i]);
+		if (IS_ERR(state->clock[i])) {
+			ret = PTR_ERR(state->clock[i]);
+			goto err;
+		}
+		ret = clk_prepare(state->clock[i]);
+		if (ret < 0) {
+			clk_put(state->clock[i]);
+			state->clock[i] = ERR_PTR(-EINVAL);
+			goto err;
+		}
+	}
+	return 0;
+err:
+	s5pcsis_clk_put(state);
+	dev_err(dev, "failed to get clock: %s\n", csi_clock_name[i]);
+	return ret;
+}
+
+static void dump_regs(struct csis_state *state, const char *label)
+{
+	struct {
+		u32 offset;
+		const char * const name;
+	} registers[] = {
+		{ 0x00, "CTRL" },
+		{ 0x04, "DPHYCTRL" },
+		{ 0x08, "CONFIG" },
+		{ 0x0c, "DPHYSTS" },
+		{ 0x10, "INTMSK" },
+		{ 0x2c, "RESOL" },
+		{ 0x38, "SDW_CONFIG" },
+	};
+	u32 i;
+
+	v4l2_info(&state->sd, "--- %s ---\n", label);
+
+	for (i = 0; i < ARRAY_SIZE(registers); i++) {
+		u32 cfg = s5pcsis_read(state, registers[i].offset);
+		v4l2_info(&state->sd, "%10s: 0x%08x\n", registers[i].name, cfg);
+	}
+}
+
+static void s5pcsis_start_stream(struct csis_state *state)
+{
+	s5pcsis_reset(state);
+	s5pcsis_set_params(state);
+	s5pcsis_system_enable(state, true);
+	s5pcsis_enable_interrupts(state, true);
+}
+
+static void s5pcsis_stop_stream(struct csis_state *state)
+{
+	s5pcsis_enable_interrupts(state, false);
+	s5pcsis_system_enable(state, false);
+}
+
+static void s5pcsis_clear_counters(struct csis_state *state)
+{
+	unsigned long flags;
+	int i;
+
+	spin_lock_irqsave(&state->slock, flags);
+	for (i = 0; i < S5PCSIS_NUM_EVENTS; i++)
+		state->events[i].counter = 0;
+	spin_unlock_irqrestore(&state->slock, flags);
+}
+
+static void s5pcsis_log_counters(struct csis_state *state, bool non_errors)
+{
+	int i = non_errors ? S5PCSIS_NUM_EVENTS : S5PCSIS_NUM_EVENTS - 4;
+	unsigned long flags;
+
+	spin_lock_irqsave(&state->slock, flags);
+
+	for (i--; i >= 0; i--) {
+		if (state->events[i].counter > 0 || debug)
+			v4l2_info(&state->sd, "%s events: %d\n",
+				  state->events[i].name,
+				  state->events[i].counter);
+	}
+	spin_unlock_irqrestore(&state->slock, flags);
+}
+
+/*
+ * V4L2 subdev operations
+ */
+static int s5pcsis_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct csis_state *state = sd_to_csis_state(sd);
+	struct device *dev = &state->pdev->dev;
+
+	if (on)
+		return pm_runtime_get_sync(dev);
+
+	return pm_runtime_put_sync(dev);
+}
+
+static int s5pcsis_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct csis_state *state = sd_to_csis_state(sd);
+	int ret = 0;
+
+	v4l2_dbg(1, debug, sd, "%s: %d, state: 0x%x\n",
+		 __func__, enable, state->flags);
+
+	if (enable) {
+		s5pcsis_clear_counters(state);
+		ret = pm_runtime_get_sync(&state->pdev->dev);
+		if (ret && ret != 1)
+			return ret;
+	}
+
+	mutex_lock(&state->lock);
+	if (enable) {
+		if (state->flags & ST_SUSPENDED) {
+			ret = -EBUSY;
+			goto unlock;
+		}
+		s5pcsis_start_stream(state);
+		state->flags |= ST_STREAMING;
+	} else {
+		s5pcsis_stop_stream(state);
+		state->flags &= ~ST_STREAMING;
+		if (debug > 0)
+			s5pcsis_log_counters(state, true);
+	}
+unlock:
+	mutex_unlock(&state->lock);
+	if (!enable)
+		pm_runtime_put(&state->pdev->dev);
+
+	return ret == 1 ? 0 : ret;
+}
+
+static int s5pcsis_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_pad_config *cfg,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index >= ARRAY_SIZE(s5pcsis_formats))
+		return -EINVAL;
+
+	code->code = s5pcsis_formats[code->index].code;
+	return 0;
+}
+
+static struct csis_pix_format const *s5pcsis_try_format(
+	struct v4l2_mbus_framefmt *mf)
+{
+	struct csis_pix_format const *csis_fmt;
+
+	csis_fmt = find_csis_format(mf);
+	if (csis_fmt == NULL)
+		csis_fmt = &s5pcsis_formats[0];
+
+	mf->code = csis_fmt->code;
+	v4l_bound_align_image(&mf->width, 1, CSIS_MAX_PIX_WIDTH,
+			      csis_fmt->pix_width_alignment,
+			      &mf->height, 1, CSIS_MAX_PIX_HEIGHT, 1,
+			      0);
+	return csis_fmt;
+}
+
+static struct v4l2_mbus_framefmt *__s5pcsis_get_format(
+		struct csis_state *state, struct v4l2_subdev_pad_config *cfg,
+		enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return cfg ? v4l2_subdev_get_try_format(&state->sd, cfg, 0) : NULL;
+
+	return &state->format;
+}
+
+static int s5pcsis_set_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_pad_config *cfg,
+			   struct v4l2_subdev_format *fmt)
+{
+	struct csis_state *state = sd_to_csis_state(sd);
+	struct csis_pix_format const *csis_fmt;
+	struct v4l2_mbus_framefmt *mf;
+
+	mf = __s5pcsis_get_format(state, cfg, fmt->which);
+
+	if (fmt->pad == CSIS_PAD_SOURCE) {
+		if (mf) {
+			mutex_lock(&state->lock);
+			fmt->format = *mf;
+			mutex_unlock(&state->lock);
+		}
+		return 0;
+	}
+	csis_fmt = s5pcsis_try_format(&fmt->format);
+	if (mf) {
+		mutex_lock(&state->lock);
+		*mf = fmt->format;
+		if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+			state->csis_fmt = csis_fmt;
+		mutex_unlock(&state->lock);
+	}
+	return 0;
+}
+
+static int s5pcsis_get_fmt(struct v4l2_subdev *sd, struct v4l2_subdev_pad_config *cfg,
+			   struct v4l2_subdev_format *fmt)
+{
+	struct csis_state *state = sd_to_csis_state(sd);
+	struct v4l2_mbus_framefmt *mf;
+
+	mf = __s5pcsis_get_format(state, cfg, fmt->which);
+	if (!mf)
+		return -EINVAL;
+
+	mutex_lock(&state->lock);
+	fmt->format = *mf;
+	mutex_unlock(&state->lock);
+	return 0;
+}
+
+static int s5pcsis_s_rx_buffer(struct v4l2_subdev *sd, void *buf,
+			       unsigned int *size)
+{
+	struct csis_state *state = sd_to_csis_state(sd);
+	unsigned long flags;
+
+	*size = min_t(unsigned int, *size, S5PCSIS_PKTDATA_SIZE);
+
+	spin_lock_irqsave(&state->slock, flags);
+	state->pkt_buf.data = buf;
+	state->pkt_buf.len = *size;
+	spin_unlock_irqrestore(&state->slock, flags);
+
+	return 0;
+}
+
+static int s5pcsis_log_status(struct v4l2_subdev *sd)
+{
+	struct csis_state *state = sd_to_csis_state(sd);
+
+	mutex_lock(&state->lock);
+	s5pcsis_log_counters(state, true);
+	if (debug && (state->flags & ST_POWERED))
+		dump_regs(state, __func__);
+	mutex_unlock(&state->lock);
+	return 0;
+}
+
+static int s5pcsis_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_mbus_framefmt *format = v4l2_subdev_get_try_format(sd, fh->pad, 0);
+
+	format->colorspace = V4L2_COLORSPACE_JPEG;
+	format->code = s5pcsis_formats[0].code;
+	format->width = S5PCSIS_DEF_PIX_WIDTH;
+	format->height = S5PCSIS_DEF_PIX_HEIGHT;
+	format->field = V4L2_FIELD_NONE;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_internal_ops s5pcsis_sd_internal_ops = {
+	.open = s5pcsis_open,
+};
+
+static struct v4l2_subdev_core_ops s5pcsis_core_ops = {
+	.s_power = s5pcsis_s_power,
+	.log_status = s5pcsis_log_status,
+};
+
+static struct v4l2_subdev_pad_ops s5pcsis_pad_ops = {
+	.enum_mbus_code = s5pcsis_enum_mbus_code,
+	.get_fmt = s5pcsis_get_fmt,
+	.set_fmt = s5pcsis_set_fmt,
+};
+
+static struct v4l2_subdev_video_ops s5pcsis_video_ops = {
+	.s_rx_buffer = s5pcsis_s_rx_buffer,
+	.s_stream = s5pcsis_s_stream,
+};
+
+static struct v4l2_subdev_ops s5pcsis_subdev_ops = {
+	.core = &s5pcsis_core_ops,
+	.pad = &s5pcsis_pad_ops,
+	.video = &s5pcsis_video_ops,
+};
+
+static irqreturn_t s5pcsis_irq_handler(int irq, void *dev_id)
+{
+	struct csis_state *state = dev_id;
+	struct csis_pktbuf *pktbuf = &state->pkt_buf;
+	unsigned long flags;
+	u32 status;
+
+	status = s5pcsis_read(state, S5PCSIS_INTSRC);
+	spin_lock_irqsave(&state->slock, flags);
+
+	if ((status & S5PCSIS_INTSRC_NON_IMAGE_DATA) && pktbuf->data) {
+		u32 offset;
+
+		if (status & S5PCSIS_INTSRC_EVEN)
+			offset = S5PCSIS_PKTDATA_EVEN;
+		else
+			offset = S5PCSIS_PKTDATA_ODD;
+
+		memcpy(pktbuf->data, (u8 __force *)state->regs + offset,
+		       pktbuf->len);
+		pktbuf->data = NULL;
+		rmb();
+	}
+
+	/* Update the event/error counters */
+	if ((status & S5PCSIS_INTSRC_ERRORS) || debug) {
+		int i;
+		for (i = 0; i < S5PCSIS_NUM_EVENTS; i++) {
+			if (!(status & state->events[i].mask))
+				continue;
+			state->events[i].counter++;
+			v4l2_dbg(2, debug, &state->sd, "%s: %d\n",
+				 state->events[i].name,
+				 state->events[i].counter);
+		}
+		v4l2_dbg(2, debug, &state->sd, "status: %08x\n", status);
+	}
+	spin_unlock_irqrestore(&state->slock, flags);
+
+	s5pcsis_write(state, S5PCSIS_INTSRC, status);
+	return IRQ_HANDLED;
+}
+
+static int s5pcsis_parse_dt(struct platform_device *pdev,
+			    struct csis_state *state)
+{
+	struct device_node *node = pdev->dev.of_node;
+	struct v4l2_of_endpoint endpoint;
+
+	if (of_property_read_u32(node, "clock-frequency",
+				 &state->clk_frequency))
+		state->clk_frequency = DEFAULT_SCLK_CSIS_FREQ;
+	if (of_property_read_u32(node, "bus-width",
+				 &state->max_num_lanes))
+		return -EINVAL;
+
+	node = of_graph_get_next_endpoint(node, NULL);
+	if (!node) {
+		dev_err(&pdev->dev, "No port node at %s\n",
+				pdev->dev.of_node->full_name);
+		return -EINVAL;
+	}
+	/* Get port node and validate MIPI-CSI channel id. */
+	v4l2_of_parse_endpoint(node, &endpoint);
+
+	state->index = endpoint.base.port - FIMC_INPUT_MIPI_CSI2_0;
+	if (state->index >= CSIS_MAX_ENTITIES)
+		return -ENXIO;
+
+	/* Get MIPI CSI-2 bus configration from the endpoint node. */
+	of_property_read_u32(node, "samsung,csis-hs-settle",
+					&state->hs_settle);
+	state->wclk_ext = of_property_read_bool(node,
+					"samsung,csis-wclk");
+
+	state->num_lanes = endpoint.bus.mipi_csi2.num_data_lanes;
+	of_node_put(node);
+
+	return 0;
+}
+
+static int s5pcsis_pm_resume(struct device *dev, bool runtime);
+static const struct of_device_id s5pcsis_of_match[];
+
+static int s5pcsis_probe(struct platform_device *pdev)
+{
+	const struct of_device_id *of_id;
+	const struct csis_drvdata *drv_data;
+	struct device *dev = &pdev->dev;
+	struct resource *mem_res;
+	struct csis_state *state;
+	int ret = -ENOMEM;
+	int i;
+
+	state = devm_kzalloc(dev, sizeof(*state), GFP_KERNEL);
+	if (!state)
+		return -ENOMEM;
+
+	mutex_init(&state->lock);
+	spin_lock_init(&state->slock);
+	state->pdev = pdev;
+
+	of_id = of_match_node(s5pcsis_of_match, dev->of_node);
+	if (WARN_ON(of_id == NULL))
+		return -EINVAL;
+
+	drv_data = of_id->data;
+	state->interrupt_mask = drv_data->interrupt_mask;
+
+	ret = s5pcsis_parse_dt(pdev, state);
+	if (ret < 0)
+		return ret;
+
+	if (state->num_lanes == 0 || state->num_lanes > state->max_num_lanes) {
+		dev_err(dev, "Unsupported number of data lanes: %d (max. %d)\n",
+			state->num_lanes, state->max_num_lanes);
+		return -EINVAL;
+	}
+
+	state->phy = devm_phy_get(dev, "csis");
+	if (IS_ERR(state->phy))
+		return PTR_ERR(state->phy);
+
+	mem_res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	state->regs = devm_ioremap_resource(dev, mem_res);
+	if (IS_ERR(state->regs))
+		return PTR_ERR(state->regs);
+
+	state->irq = platform_get_irq(pdev, 0);
+	if (state->irq < 0) {
+		dev_err(dev, "Failed to get irq\n");
+		return state->irq;
+	}
+
+	for (i = 0; i < CSIS_NUM_SUPPLIES; i++)
+		state->supplies[i].supply = csis_supply_name[i];
+
+	ret = devm_regulator_bulk_get(dev, CSIS_NUM_SUPPLIES,
+				 state->supplies);
+	if (ret)
+		return ret;
+
+	ret = s5pcsis_clk_get(state);
+	if (ret < 0)
+		return ret;
+
+	if (state->clk_frequency)
+		ret = clk_set_rate(state->clock[CSIS_CLK_MUX],
+				   state->clk_frequency);
+	else
+		dev_WARN(dev, "No clock frequency specified!\n");
+	if (ret < 0)
+		goto e_clkput;
+
+	ret = clk_enable(state->clock[CSIS_CLK_MUX]);
+	if (ret < 0)
+		goto e_clkput;
+
+	ret = devm_request_irq(dev, state->irq, s5pcsis_irq_handler,
+			       0, dev_name(dev), state);
+	if (ret) {
+		dev_err(dev, "Interrupt request failed\n");
+		goto e_clkdis;
+	}
+
+	v4l2_subdev_init(&state->sd, &s5pcsis_subdev_ops);
+	state->sd.owner = THIS_MODULE;
+	snprintf(state->sd.name, sizeof(state->sd.name), "%s.%d",
+		 CSIS_SUBDEV_NAME, state->index);
+	state->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	state->csis_fmt = &s5pcsis_formats[0];
+
+	state->format.code = s5pcsis_formats[0].code;
+	state->format.width = S5PCSIS_DEF_PIX_WIDTH;
+	state->format.height = S5PCSIS_DEF_PIX_HEIGHT;
+
+	state->pads[CSIS_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+	state->pads[CSIS_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_init(&state->sd.entity,
+				CSIS_PADS_NUM, state->pads, 0);
+	if (ret < 0)
+		goto e_clkdis;
+
+	/* This allows to retrieve the platform device id by the host driver */
+	v4l2_set_subdevdata(&state->sd, pdev);
+
+	/* .. and a pointer to the subdev. */
+	platform_set_drvdata(pdev, &state->sd);
+	memcpy(state->events, s5pcsis_events, sizeof(state->events));
+
+	pm_runtime_enable(dev);
+	if (!pm_runtime_enabled(dev)) {
+		ret = s5pcsis_pm_resume(dev, true);
+		if (ret < 0)
+			goto e_m_ent;
+	}
+
+	dev_info(&pdev->dev, "lanes: %d, hs_settle: %d, wclk: %d, freq: %u\n",
+		 state->num_lanes, state->hs_settle, state->wclk_ext,
+		 state->clk_frequency);
+	return 0;
+
+e_m_ent:
+	media_entity_cleanup(&state->sd.entity);
+e_clkdis:
+	clk_disable(state->clock[CSIS_CLK_MUX]);
+e_clkput:
+	s5pcsis_clk_put(state);
+	return ret;
+}
+
+static int s5pcsis_pm_suspend(struct device *dev, bool runtime)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct v4l2_subdev *sd = platform_get_drvdata(pdev);
+	struct csis_state *state = sd_to_csis_state(sd);
+	int ret = 0;
+
+	v4l2_dbg(1, debug, sd, "%s: flags: 0x%x\n",
+		 __func__, state->flags);
+
+	mutex_lock(&state->lock);
+	if (state->flags & ST_POWERED) {
+		s5pcsis_stop_stream(state);
+		ret = phy_power_off(state->phy);
+		if (ret)
+			goto unlock;
+		ret = regulator_bulk_disable(CSIS_NUM_SUPPLIES,
+					     state->supplies);
+		if (ret)
+			goto unlock;
+		clk_disable(state->clock[CSIS_CLK_GATE]);
+		state->flags &= ~ST_POWERED;
+		if (!runtime)
+			state->flags |= ST_SUSPENDED;
+	}
+ unlock:
+	mutex_unlock(&state->lock);
+	return ret ? -EAGAIN : 0;
+}
+
+static int s5pcsis_pm_resume(struct device *dev, bool runtime)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct v4l2_subdev *sd = platform_get_drvdata(pdev);
+	struct csis_state *state = sd_to_csis_state(sd);
+	int ret = 0;
+
+	v4l2_dbg(1, debug, sd, "%s: flags: 0x%x\n",
+		 __func__, state->flags);
+
+	mutex_lock(&state->lock);
+	if (!runtime && !(state->flags & ST_SUSPENDED))
+		goto unlock;
+
+	if (!(state->flags & ST_POWERED)) {
+		ret = regulator_bulk_enable(CSIS_NUM_SUPPLIES,
+					    state->supplies);
+		if (ret)
+			goto unlock;
+		ret = phy_power_on(state->phy);
+		if (!ret) {
+			state->flags |= ST_POWERED;
+		} else {
+			regulator_bulk_disable(CSIS_NUM_SUPPLIES,
+					       state->supplies);
+			goto unlock;
+		}
+		clk_enable(state->clock[CSIS_CLK_GATE]);
+	}
+	if (state->flags & ST_STREAMING)
+		s5pcsis_start_stream(state);
+
+	state->flags &= ~ST_SUSPENDED;
+ unlock:
+	mutex_unlock(&state->lock);
+	return ret ? -EAGAIN : 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int s5pcsis_suspend(struct device *dev)
+{
+	return s5pcsis_pm_suspend(dev, false);
+}
+
+static int s5pcsis_resume(struct device *dev)
+{
+	return s5pcsis_pm_resume(dev, false);
+}
+#endif
+
+#ifdef CONFIG_PM
+static int s5pcsis_runtime_suspend(struct device *dev)
+{
+	return s5pcsis_pm_suspend(dev, true);
+}
+
+static int s5pcsis_runtime_resume(struct device *dev)
+{
+	return s5pcsis_pm_resume(dev, true);
+}
+#endif
+
+static int s5pcsis_remove(struct platform_device *pdev)
+{
+	struct v4l2_subdev *sd = platform_get_drvdata(pdev);
+	struct csis_state *state = sd_to_csis_state(sd);
+
+	pm_runtime_disable(&pdev->dev);
+	s5pcsis_pm_suspend(&pdev->dev, true);
+	clk_disable(state->clock[CSIS_CLK_MUX]);
+	pm_runtime_set_suspended(&pdev->dev);
+	s5pcsis_clk_put(state);
+
+	media_entity_cleanup(&state->sd.entity);
+
+	return 0;
+}
+
+static const struct dev_pm_ops s5pcsis_pm_ops = {
+	SET_RUNTIME_PM_OPS(s5pcsis_runtime_suspend, s5pcsis_runtime_resume,
+			   NULL)
+	SET_SYSTEM_SLEEP_PM_OPS(s5pcsis_suspend, s5pcsis_resume)
+};
+
+static const struct csis_drvdata exynos4_csis_drvdata = {
+	.interrupt_mask = S5PCSIS_INTMSK_EXYNOS4_EN_ALL,
+};
+
+static const struct csis_drvdata exynos5_csis_drvdata = {
+	.interrupt_mask = S5PCSIS_INTMSK_EXYNOS5_EN_ALL,
+};
+
+static const struct of_device_id s5pcsis_of_match[] = {
+	{
+		.compatible = "samsung,s5pv210-csis",
+		.data = &exynos4_csis_drvdata,
+	}, {
+		.compatible = "samsung,exynos4210-csis",
+		.data = &exynos4_csis_drvdata,
+	}, {
+		.compatible = "samsung,exynos5250-csis",
+		.data = &exynos5_csis_drvdata,
+	},
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, s5pcsis_of_match);
+
+static struct platform_driver s5pcsis_driver = {
+	.probe		= s5pcsis_probe,
+	.remove		= s5pcsis_remove,
+	.driver		= {
+		.of_match_table = s5pcsis_of_match,
+		.name		= CSIS_DRIVER_NAME,
+		.pm		= &s5pcsis_pm_ops,
+	},
+};
+
+module_platform_driver(s5pcsis_driver);
+
+MODULE_AUTHOR("Sylwester Nawrocki <s.nawrocki@samsung.com>");
+MODULE_DESCRIPTION("Samsung S5P/EXYNOS SoC MIPI-CSI2 receiver driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/exynos4-is/mipi-csis.h b/drivers/media/platform/exynos4-is/mipi-csis.h
new file mode 100644
index 0000000..28c11c4
--- /dev/null
+++ b/drivers/media/platform/exynos4-is/mipi-csis.h
@@ -0,0 +1,26 @@
+/*
+ * Samsung S5P/EXYNOS4 SoC series MIPI-CSI receiver driver
+ *
+ * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#ifndef S5P_MIPI_CSIS_H_
+#define S5P_MIPI_CSIS_H_
+
+#define CSIS_DRIVER_NAME	"s5p-mipi-csis"
+#define CSIS_SUBDEV_NAME	CSIS_DRIVER_NAME
+#define CSIS_MAX_ENTITIES	2
+#define CSIS0_MAX_LANES		4
+#define CSIS1_MAX_LANES		2
+
+#define CSIS_PAD_SINK		0
+#define CSIS_PAD_SOURCE		1
+#define CSIS_PADS_NUM		2
+
+#define S5PCSIS_DEF_PIX_WIDTH	640
+#define S5PCSIS_DEF_PIX_HEIGHT	480
+
+#endif
diff --git a/drivers/media/platform/fsl-viu.c b/drivers/media/platform/fsl-viu.c
new file mode 100644
index 0000000..ae8c6b3
--- /dev/null
+++ b/drivers/media/platform/fsl-viu.c
@@ -0,0 +1,1615 @@
+/*
+ * Copyright 2008-2010 Freescale Semiconductor, Inc. All Rights Reserved.
+ *
+ *  Freescale VIU video driver
+ *
+ *  Authors: Hongjun Chen <hong-jun.chen@freescale.com>
+ *	     Porting to 2.6.35 by DENX Software Engineering,
+ *	     Anatolij Gustschin <agust@denx.de>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/clk.h>
+#include <linux/kernel.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/of_platform.h>
+#include <linux/slab.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-event.h>
+#include <media/videobuf-dma-contig.h>
+
+#define DRV_NAME		"fsl_viu"
+#define VIU_VERSION		"0.5.1"
+
+#define BUFFER_TIMEOUT		msecs_to_jiffies(500)  /* 0.5 seconds */
+
+#define	VIU_VID_MEM_LIMIT	4	/* Video memory limit, in Mb */
+
+/* I2C address of video decoder chip is 0x4A */
+#define VIU_VIDEO_DECODER_ADDR	0x25
+
+static int info_level;
+
+#define dprintk(level, fmt, arg...)					\
+	do {								\
+		if (level <= info_level)				\
+			printk(KERN_DEBUG "viu: " fmt , ## arg);	\
+	} while (0)
+
+/*
+ * Basic structures
+ */
+struct viu_fmt {
+	u32   fourcc;		/* v4l2 format id */
+	u32   pixelformat;
+	int   depth;
+};
+
+static struct viu_fmt formats[] = {
+	{
+		.fourcc		= V4L2_PIX_FMT_RGB565,
+		.pixelformat	= V4L2_PIX_FMT_RGB565,
+		.depth		= 16,
+	}, {
+		.fourcc		= V4L2_PIX_FMT_RGB32,
+		.pixelformat	= V4L2_PIX_FMT_RGB32,
+		.depth		= 32,
+	}
+};
+
+struct viu_dev;
+struct viu_buf;
+
+/* buffer for one video frame */
+struct viu_buf {
+	/* common v4l buffer stuff -- must be first */
+	struct videobuf_buffer vb;
+	struct viu_fmt *fmt;
+};
+
+struct viu_dmaqueue {
+	struct viu_dev		*dev;
+	struct list_head	active;
+	struct list_head	queued;
+	struct timer_list	timeout;
+};
+
+struct viu_status {
+	u32 field_irq;
+	u32 vsync_irq;
+	u32 hsync_irq;
+	u32 vstart_irq;
+	u32 dma_end_irq;
+	u32 error_irq;
+};
+
+struct viu_reg {
+	u32 status_cfg;
+	u32 luminance;
+	u32 chroma_r;
+	u32 chroma_g;
+	u32 chroma_b;
+	u32 field_base_addr;
+	u32 dma_inc;
+	u32 picture_count;
+	u32 req_alarm;
+	u32 alpha;
+} __attribute__ ((packed));
+
+struct viu_dev {
+	struct v4l2_device	v4l2_dev;
+	struct v4l2_ctrl_handler hdl;
+	struct mutex		lock;
+	spinlock_t		slock;
+	int			users;
+
+	struct device		*dev;
+	/* various device info */
+	struct video_device	*vdev;
+	struct viu_dmaqueue	vidq;
+	enum v4l2_field		capfield;
+	int			field;
+	int			first;
+	int			dma_done;
+
+	/* Hardware register area */
+	struct viu_reg		*vr;
+
+	/* Interrupt vector */
+	int			irq;
+	struct viu_status	irqs;
+
+	/* video overlay */
+	struct v4l2_framebuffer	ovbuf;
+	struct viu_fmt		*ovfmt;
+	unsigned int		ovenable;
+	enum v4l2_field		ovfield;
+
+	/* crop */
+	struct v4l2_rect	crop_current;
+
+	/* clock pointer */
+	struct clk		*clk;
+
+	/* decoder */
+	struct v4l2_subdev	*decoder;
+
+	v4l2_std_id		std;
+};
+
+struct viu_fh {
+	/* must remain the first field of this struct */
+	struct v4l2_fh		fh;
+	struct viu_dev		*dev;
+
+	/* video capture */
+	struct videobuf_queue	vb_vidq;
+	spinlock_t		vbq_lock; /* spinlock for the videobuf queue */
+
+	/* video overlay */
+	struct v4l2_window	win;
+	struct v4l2_clip	clips[1];
+
+	/* video capture */
+	struct viu_fmt		*fmt;
+	int			width, height, sizeimage;
+	enum v4l2_buf_type	type;
+};
+
+static struct viu_reg reg_val;
+
+/*
+ * Macro definitions of VIU registers
+ */
+
+/* STATUS_CONFIG register */
+enum status_config {
+	SOFT_RST		= 1 << 0,
+
+	ERR_MASK		= 0x0f << 4,	/* Error code mask */
+	ERR_NO			= 0x00,		/* No error */
+	ERR_DMA_V		= 0x01 << 4,	/* DMA in vertical active */
+	ERR_DMA_VB		= 0x02 << 4,	/* DMA in vertical blanking */
+	ERR_LINE_TOO_LONG	= 0x04 << 4,	/* Line too long */
+	ERR_TOO_MANG_LINES	= 0x05 << 4,	/* Too many lines in field */
+	ERR_LINE_TOO_SHORT	= 0x06 << 4,	/* Line too short */
+	ERR_NOT_ENOUGH_LINE	= 0x07 << 4,	/* Not enough lines in field */
+	ERR_FIFO_OVERFLOW	= 0x08 << 4,	/* FIFO overflow */
+	ERR_FIFO_UNDERFLOW	= 0x09 << 4,	/* FIFO underflow */
+	ERR_1bit_ECC		= 0x0a << 4,	/* One bit ECC error */
+	ERR_MORE_ECC		= 0x0b << 4,	/* Two/more bits ECC error */
+
+	INT_FIELD_EN		= 0x01 << 8,	/* Enable field interrupt */
+	INT_VSYNC_EN		= 0x01 << 9,	/* Enable vsync interrupt */
+	INT_HSYNC_EN		= 0x01 << 10,	/* Enable hsync interrupt */
+	INT_VSTART_EN		= 0x01 << 11,	/* Enable vstart interrupt */
+	INT_DMA_END_EN		= 0x01 << 12,	/* Enable DMA end interrupt */
+	INT_ERROR_EN		= 0x01 << 13,	/* Enable error interrupt */
+	INT_ECC_EN		= 0x01 << 14,	/* Enable ECC interrupt */
+
+	INT_FIELD_STATUS	= 0x01 << 16,	/* field interrupt status */
+	INT_VSYNC_STATUS	= 0x01 << 17,	/* vsync interrupt status */
+	INT_HSYNC_STATUS	= 0x01 << 18,	/* hsync interrupt status */
+	INT_VSTART_STATUS	= 0x01 << 19,	/* vstart interrupt status */
+	INT_DMA_END_STATUS	= 0x01 << 20,	/* DMA end interrupt status */
+	INT_ERROR_STATUS	= 0x01 << 21,	/* error interrupt status */
+
+	DMA_ACT			= 0x01 << 27,	/* Enable DMA transfer */
+	FIELD_NO		= 0x01 << 28,	/* Field number */
+	DITHER_ON		= 0x01 << 29,	/* Dithering is on */
+	ROUND_ON		= 0x01 << 30,	/* Round is on */
+	MODE_32BIT		= 0x01 << 31,	/* Data in RGBa888,
+						 * 0 in RGB565
+						 */
+};
+
+#define norm_maxw()	720
+#define norm_maxh()	576
+
+#define INT_ALL_STATUS	(INT_FIELD_STATUS | INT_VSYNC_STATUS | \
+			 INT_HSYNC_STATUS | INT_VSTART_STATUS | \
+			 INT_DMA_END_STATUS | INT_ERROR_STATUS)
+
+#define NUM_FORMATS	ARRAY_SIZE(formats)
+
+static irqreturn_t viu_intr(int irq, void *dev_id);
+
+struct viu_fmt *format_by_fourcc(int fourcc)
+{
+	int i;
+
+	for (i = 0; i < NUM_FORMATS; i++) {
+		if (formats[i].pixelformat == fourcc)
+			return formats + i;
+	}
+
+	dprintk(0, "unknown pixelformat:'%4.4s'\n", (char *)&fourcc);
+	return NULL;
+}
+
+void viu_start_dma(struct viu_dev *dev)
+{
+	struct viu_reg *vr = dev->vr;
+
+	dev->field = 0;
+
+	/* Enable DMA operation */
+	out_be32(&vr->status_cfg, SOFT_RST);
+	out_be32(&vr->status_cfg, INT_FIELD_EN);
+}
+
+void viu_stop_dma(struct viu_dev *dev)
+{
+	struct viu_reg *vr = dev->vr;
+	int cnt = 100;
+	u32 status_cfg;
+
+	out_be32(&vr->status_cfg, 0);
+
+	/* Clear pending interrupts */
+	status_cfg = in_be32(&vr->status_cfg);
+	if (status_cfg & 0x3f0000)
+		out_be32(&vr->status_cfg, status_cfg & 0x3f0000);
+
+	if (status_cfg & DMA_ACT) {
+		do {
+			status_cfg = in_be32(&vr->status_cfg);
+			if (status_cfg & INT_DMA_END_STATUS)
+				break;
+		} while (cnt--);
+
+		if (cnt < 0) {
+			/* timed out, issue soft reset */
+			out_be32(&vr->status_cfg, SOFT_RST);
+			out_be32(&vr->status_cfg, 0);
+		} else {
+			/* clear DMA_END and other pending irqs */
+			out_be32(&vr->status_cfg, status_cfg & 0x3f0000);
+		}
+	}
+
+	dev->field = 0;
+}
+
+static int restart_video_queue(struct viu_dmaqueue *vidq)
+{
+	struct viu_buf *buf, *prev;
+
+	dprintk(1, "%s vidq=0x%08lx\n", __func__, (unsigned long)vidq);
+	if (!list_empty(&vidq->active)) {
+		buf = list_entry(vidq->active.next, struct viu_buf, vb.queue);
+		dprintk(2, "restart_queue [%p/%d]: restart dma\n",
+			buf, buf->vb.i);
+
+		viu_stop_dma(vidq->dev);
+
+		/* cancel all outstanding capture requests */
+		list_for_each_entry_safe(buf, prev, &vidq->active, vb.queue) {
+			list_del(&buf->vb.queue);
+			buf->vb.state = VIDEOBUF_ERROR;
+			wake_up(&buf->vb.done);
+		}
+		mod_timer(&vidq->timeout, jiffies+BUFFER_TIMEOUT);
+		return 0;
+	}
+
+	prev = NULL;
+	for (;;) {
+		if (list_empty(&vidq->queued))
+			return 0;
+		buf = list_entry(vidq->queued.next, struct viu_buf, vb.queue);
+		if (prev == NULL) {
+			list_move_tail(&buf->vb.queue, &vidq->active);
+
+			dprintk(1, "Restarting video dma\n");
+			viu_stop_dma(vidq->dev);
+			viu_start_dma(vidq->dev);
+
+			buf->vb.state = VIDEOBUF_ACTIVE;
+			mod_timer(&vidq->timeout, jiffies+BUFFER_TIMEOUT);
+			dprintk(2, "[%p/%d] restart_queue - first active\n",
+				buf, buf->vb.i);
+
+		} else if (prev->vb.width  == buf->vb.width  &&
+			   prev->vb.height == buf->vb.height &&
+			   prev->fmt       == buf->fmt) {
+			list_move_tail(&buf->vb.queue, &vidq->active);
+			buf->vb.state = VIDEOBUF_ACTIVE;
+			dprintk(2, "[%p/%d] restart_queue - move to active\n",
+				buf, buf->vb.i);
+		} else {
+			return 0;
+		}
+		prev = buf;
+	}
+}
+
+static void viu_vid_timeout(unsigned long data)
+{
+	struct viu_dev *dev = (struct viu_dev *)data;
+	struct viu_buf *buf;
+	struct viu_dmaqueue *vidq = &dev->vidq;
+
+	while (!list_empty(&vidq->active)) {
+		buf = list_entry(vidq->active.next, struct viu_buf, vb.queue);
+		list_del(&buf->vb.queue);
+		buf->vb.state = VIDEOBUF_ERROR;
+		wake_up(&buf->vb.done);
+		dprintk(1, "viu/0: [%p/%d] timeout\n", buf, buf->vb.i);
+	}
+
+	restart_video_queue(vidq);
+}
+
+/*
+ * Videobuf operations
+ */
+static int buffer_setup(struct videobuf_queue *vq, unsigned int *count,
+			unsigned int *size)
+{
+	struct viu_fh *fh = vq->priv_data;
+
+	*size = fh->width * fh->height * fh->fmt->depth >> 3;
+	if (*count == 0)
+		*count = 32;
+
+	while (*size * *count > VIU_VID_MEM_LIMIT * 1024 * 1024)
+		(*count)--;
+
+	dprintk(1, "%s, count=%d, size=%d\n", __func__, *count, *size);
+	return 0;
+}
+
+static void free_buffer(struct videobuf_queue *vq, struct viu_buf *buf)
+{
+	struct videobuf_buffer *vb = &buf->vb;
+	void *vaddr = NULL;
+
+	BUG_ON(in_interrupt());
+
+	videobuf_waiton(vq, &buf->vb, 0, 0);
+
+	if (vq->int_ops && vq->int_ops->vaddr)
+		vaddr = vq->int_ops->vaddr(vb);
+
+	if (vaddr)
+		videobuf_dma_contig_free(vq, &buf->vb);
+
+	buf->vb.state = VIDEOBUF_NEEDS_INIT;
+}
+
+inline int buffer_activate(struct viu_dev *dev, struct viu_buf *buf)
+{
+	struct viu_reg *vr = dev->vr;
+	int bpp;
+
+	/* setup the DMA base address */
+	reg_val.field_base_addr = videobuf_to_dma_contig(&buf->vb);
+
+	dprintk(1, "buffer_activate [%p/%d]: dma addr 0x%lx\n",
+		buf, buf->vb.i, (unsigned long)reg_val.field_base_addr);
+
+	/* interlace is on by default, set horizontal DMA increment */
+	reg_val.status_cfg = 0;
+	bpp = buf->fmt->depth >> 3;
+	switch (bpp) {
+	case 2:
+		reg_val.status_cfg &= ~MODE_32BIT;
+		reg_val.dma_inc = buf->vb.width * 2;
+		break;
+	case 4:
+		reg_val.status_cfg |= MODE_32BIT;
+		reg_val.dma_inc = buf->vb.width * 4;
+		break;
+	default:
+		dprintk(0, "doesn't support color depth(%d)\n",
+			bpp * 8);
+		return -EINVAL;
+	}
+
+	/* setup picture_count register */
+	reg_val.picture_count = (buf->vb.height / 2) << 16 |
+				buf->vb.width;
+
+	reg_val.status_cfg |= DMA_ACT | INT_DMA_END_EN | INT_FIELD_EN;
+
+	buf->vb.state = VIDEOBUF_ACTIVE;
+	dev->capfield = buf->vb.field;
+
+	/* reset dma increment if needed */
+	if (!V4L2_FIELD_HAS_BOTH(buf->vb.field))
+		reg_val.dma_inc = 0;
+
+	out_be32(&vr->dma_inc, reg_val.dma_inc);
+	out_be32(&vr->picture_count, reg_val.picture_count);
+	out_be32(&vr->field_base_addr, reg_val.field_base_addr);
+	mod_timer(&dev->vidq.timeout, jiffies + BUFFER_TIMEOUT);
+	return 0;
+}
+
+static int buffer_prepare(struct videobuf_queue *vq,
+			  struct videobuf_buffer *vb,
+			  enum v4l2_field field)
+{
+	struct viu_fh  *fh  = vq->priv_data;
+	struct viu_buf *buf = container_of(vb, struct viu_buf, vb);
+	int rc;
+
+	BUG_ON(fh->fmt == NULL);
+
+	if (fh->width  < 48 || fh->width  > norm_maxw() ||
+	    fh->height < 32 || fh->height > norm_maxh())
+		return -EINVAL;
+	buf->vb.size = (fh->width * fh->height * fh->fmt->depth) >> 3;
+	if (buf->vb.baddr != 0 && buf->vb.bsize < buf->vb.size)
+		return -EINVAL;
+
+	if (buf->fmt       != fh->fmt	 ||
+	    buf->vb.width  != fh->width  ||
+	    buf->vb.height != fh->height ||
+	    buf->vb.field  != field) {
+		buf->fmt       = fh->fmt;
+		buf->vb.width  = fh->width;
+		buf->vb.height = fh->height;
+		buf->vb.field  = field;
+	}
+
+	if (buf->vb.state == VIDEOBUF_NEEDS_INIT) {
+		rc = videobuf_iolock(vq, &buf->vb, NULL);
+		if (rc != 0)
+			goto fail;
+
+		buf->vb.width  = fh->width;
+		buf->vb.height = fh->height;
+		buf->vb.field  = field;
+		buf->fmt       = fh->fmt;
+	}
+
+	buf->vb.state = VIDEOBUF_PREPARED;
+	return 0;
+
+fail:
+	free_buffer(vq, buf);
+	return rc;
+}
+
+static void buffer_queue(struct videobuf_queue *vq, struct videobuf_buffer *vb)
+{
+	struct viu_buf       *buf     = container_of(vb, struct viu_buf, vb);
+	struct viu_fh        *fh      = vq->priv_data;
+	struct viu_dev       *dev     = fh->dev;
+	struct viu_dmaqueue  *vidq    = &dev->vidq;
+	struct viu_buf       *prev;
+
+	if (!list_empty(&vidq->queued)) {
+		dprintk(1, "adding vb queue=0x%08lx\n",
+				(unsigned long)&buf->vb.queue);
+		dprintk(1, "vidq pointer 0x%p, queued 0x%p\n",
+				vidq, &vidq->queued);
+		dprintk(1, "dev %p, queued: self %p, next %p, head %p\n",
+			dev, &vidq->queued, vidq->queued.next,
+			vidq->queued.prev);
+		list_add_tail(&buf->vb.queue, &vidq->queued);
+		buf->vb.state = VIDEOBUF_QUEUED;
+		dprintk(2, "[%p/%d] buffer_queue - append to queued\n",
+			buf, buf->vb.i);
+	} else if (list_empty(&vidq->active)) {
+		dprintk(1, "adding vb active=0x%08lx\n",
+				(unsigned long)&buf->vb.queue);
+		list_add_tail(&buf->vb.queue, &vidq->active);
+		buf->vb.state = VIDEOBUF_ACTIVE;
+		mod_timer(&vidq->timeout, jiffies+BUFFER_TIMEOUT);
+		dprintk(2, "[%p/%d] buffer_queue - first active\n",
+			buf, buf->vb.i);
+
+		buffer_activate(dev, buf);
+	} else {
+		dprintk(1, "adding vb queue2=0x%08lx\n",
+				(unsigned long)&buf->vb.queue);
+		prev = list_entry(vidq->active.prev, struct viu_buf, vb.queue);
+		if (prev->vb.width  == buf->vb.width  &&
+		    prev->vb.height == buf->vb.height &&
+		    prev->fmt       == buf->fmt) {
+			list_add_tail(&buf->vb.queue, &vidq->active);
+			buf->vb.state = VIDEOBUF_ACTIVE;
+			dprintk(2, "[%p/%d] buffer_queue - append to active\n",
+				buf, buf->vb.i);
+		} else {
+			list_add_tail(&buf->vb.queue, &vidq->queued);
+			buf->vb.state = VIDEOBUF_QUEUED;
+			dprintk(2, "[%p/%d] buffer_queue - first queued\n",
+				buf, buf->vb.i);
+		}
+	}
+}
+
+static void buffer_release(struct videobuf_queue *vq,
+				struct videobuf_buffer *vb)
+{
+	struct viu_buf *buf  = container_of(vb, struct viu_buf, vb);
+	struct viu_fh  *fh   = vq->priv_data;
+	struct viu_dev *dev  = (struct viu_dev *)fh->dev;
+
+	viu_stop_dma(dev);
+	free_buffer(vq, buf);
+}
+
+static struct videobuf_queue_ops viu_video_qops = {
+	.buf_setup      = buffer_setup,
+	.buf_prepare    = buffer_prepare,
+	.buf_queue      = buffer_queue,
+	.buf_release    = buffer_release,
+};
+
+/*
+ * IOCTL vidioc handling
+ */
+static int vidioc_querycap(struct file *file, void *priv,
+			   struct v4l2_capability *cap)
+{
+	strcpy(cap->driver, "viu");
+	strcpy(cap->card, "viu");
+	strcpy(cap->bus_info, "platform:viu");
+	cap->device_caps =	V4L2_CAP_VIDEO_CAPTURE |
+				V4L2_CAP_STREAMING     |
+				V4L2_CAP_VIDEO_OVERLAY |
+				V4L2_CAP_READWRITE;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	return 0;
+}
+
+static int vidioc_enum_fmt(struct file *file, void  *priv,
+					struct v4l2_fmtdesc *f)
+{
+	int index = f->index;
+
+	if (f->index >= NUM_FORMATS)
+		return -EINVAL;
+
+	f->pixelformat = formats[index].fourcc;
+	return 0;
+}
+
+static int vidioc_g_fmt_cap(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct viu_fh *fh = priv;
+
+	f->fmt.pix.width        = fh->width;
+	f->fmt.pix.height       = fh->height;
+	f->fmt.pix.field        = fh->vb_vidq.field;
+	f->fmt.pix.pixelformat  = fh->fmt->pixelformat;
+	f->fmt.pix.bytesperline =
+			(f->fmt.pix.width * fh->fmt->depth) >> 3;
+	f->fmt.pix.sizeimage	= fh->sizeimage;
+	f->fmt.pix.colorspace	= V4L2_COLORSPACE_SMPTE170M;
+	return 0;
+}
+
+static int vidioc_try_fmt_cap(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct viu_fmt *fmt;
+	unsigned int maxw, maxh;
+
+	fmt = format_by_fourcc(f->fmt.pix.pixelformat);
+	if (!fmt) {
+		dprintk(1, "Fourcc format (0x%08x) invalid.",
+			f->fmt.pix.pixelformat);
+		return -EINVAL;
+	}
+
+	maxw  = norm_maxw();
+	maxh  = norm_maxh();
+
+	f->fmt.pix.field = V4L2_FIELD_INTERLACED;
+	if (f->fmt.pix.height < 32)
+		f->fmt.pix.height = 32;
+	if (f->fmt.pix.height > maxh)
+		f->fmt.pix.height = maxh;
+	if (f->fmt.pix.width < 48)
+		f->fmt.pix.width = 48;
+	if (f->fmt.pix.width > maxw)
+		f->fmt.pix.width = maxw;
+	f->fmt.pix.width &= ~0x03;
+	f->fmt.pix.bytesperline =
+		(f->fmt.pix.width * fmt->depth) >> 3;
+	f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
+
+	return 0;
+}
+
+static int vidioc_s_fmt_cap(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct viu_fh *fh = priv;
+	int ret;
+
+	ret = vidioc_try_fmt_cap(file, fh, f);
+	if (ret < 0)
+		return ret;
+
+	fh->fmt           = format_by_fourcc(f->fmt.pix.pixelformat);
+	fh->width         = f->fmt.pix.width;
+	fh->height        = f->fmt.pix.height;
+	fh->sizeimage     = f->fmt.pix.sizeimage;
+	fh->vb_vidq.field = f->fmt.pix.field;
+	fh->type          = f->type;
+	return 0;
+}
+
+static int vidioc_g_fmt_overlay(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct viu_fh *fh = priv;
+
+	f->fmt.win = fh->win;
+	return 0;
+}
+
+static int verify_preview(struct viu_dev *dev, struct v4l2_window *win)
+{
+	enum v4l2_field field;
+	int maxw, maxh;
+
+	if (dev->ovbuf.base == NULL)
+		return -EINVAL;
+	if (dev->ovfmt == NULL)
+		return -EINVAL;
+	if (win->w.width < 48 || win->w.height < 32)
+		return -EINVAL;
+
+	field = win->field;
+	maxw  = dev->crop_current.width;
+	maxh  = dev->crop_current.height;
+
+	if (field == V4L2_FIELD_ANY) {
+		field = (win->w.height > maxh/2)
+			? V4L2_FIELD_INTERLACED
+			: V4L2_FIELD_TOP;
+	}
+	switch (field) {
+	case V4L2_FIELD_TOP:
+	case V4L2_FIELD_BOTTOM:
+		maxh = maxh / 2;
+		break;
+	case V4L2_FIELD_INTERLACED:
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	win->field = field;
+	if (win->w.width > maxw)
+		win->w.width = maxw;
+	if (win->w.height > maxh)
+		win->w.height = maxh;
+	return 0;
+}
+
+inline void viu_activate_overlay(struct viu_reg *viu_reg)
+{
+	struct viu_reg *vr = viu_reg;
+
+	out_be32(&vr->field_base_addr, reg_val.field_base_addr);
+	out_be32(&vr->dma_inc, reg_val.dma_inc);
+	out_be32(&vr->picture_count, reg_val.picture_count);
+}
+
+static int viu_setup_preview(struct viu_dev *dev, struct viu_fh *fh)
+{
+	int bpp;
+
+	dprintk(1, "%s %dx%d\n", __func__,
+		fh->win.w.width, fh->win.w.height);
+
+	reg_val.status_cfg = 0;
+
+	/* setup window */
+	reg_val.picture_count = (fh->win.w.height / 2) << 16 |
+				fh->win.w.width;
+
+	/* setup color depth and dma increment */
+	bpp = dev->ovfmt->depth / 8;
+	switch (bpp) {
+	case 2:
+		reg_val.status_cfg &= ~MODE_32BIT;
+		reg_val.dma_inc = fh->win.w.width * 2;
+		break;
+	case 4:
+		reg_val.status_cfg |= MODE_32BIT;
+		reg_val.dma_inc = fh->win.w.width * 4;
+		break;
+	default:
+		dprintk(0, "device doesn't support color depth(%d)\n",
+			bpp * 8);
+		return -EINVAL;
+	}
+
+	dev->ovfield = fh->win.field;
+	if (!V4L2_FIELD_HAS_BOTH(dev->ovfield))
+		reg_val.dma_inc = 0;
+
+	reg_val.status_cfg |= DMA_ACT | INT_DMA_END_EN | INT_FIELD_EN;
+
+	/* setup the base address of the overlay buffer */
+	reg_val.field_base_addr = (u32)dev->ovbuf.base;
+
+	return 0;
+}
+
+static int vidioc_s_fmt_overlay(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct viu_fh  *fh  = priv;
+	struct viu_dev *dev = (struct viu_dev *)fh->dev;
+	unsigned long  flags;
+	int err;
+
+	err = verify_preview(dev, &f->fmt.win);
+	if (err)
+		return err;
+
+	fh->win = f->fmt.win;
+
+	spin_lock_irqsave(&dev->slock, flags);
+	viu_setup_preview(dev, fh);
+	spin_unlock_irqrestore(&dev->slock, flags);
+	return 0;
+}
+
+static int vidioc_try_fmt_overlay(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	return 0;
+}
+
+static int vidioc_overlay(struct file *file, void *priv, unsigned int on)
+{
+	struct viu_fh  *fh  = priv;
+	struct viu_dev *dev = (struct viu_dev *)fh->dev;
+	unsigned long  flags;
+
+	if (on) {
+		spin_lock_irqsave(&dev->slock, flags);
+		viu_activate_overlay(dev->vr);
+		dev->ovenable = 1;
+
+		/* start dma */
+		viu_start_dma(dev);
+		spin_unlock_irqrestore(&dev->slock, flags);
+	} else {
+		viu_stop_dma(dev);
+		dev->ovenable = 0;
+	}
+
+	return 0;
+}
+
+int vidioc_g_fbuf(struct file *file, void *priv, struct v4l2_framebuffer *arg)
+{
+	struct viu_fh  *fh = priv;
+	struct viu_dev *dev = fh->dev;
+	struct v4l2_framebuffer *fb = arg;
+
+	*fb = dev->ovbuf;
+	fb->capability = V4L2_FBUF_CAP_LIST_CLIPPING;
+	return 0;
+}
+
+int vidioc_s_fbuf(struct file *file, void *priv, const struct v4l2_framebuffer *arg)
+{
+	struct viu_fh  *fh = priv;
+	struct viu_dev *dev = fh->dev;
+	const struct v4l2_framebuffer *fb = arg;
+	struct viu_fmt *fmt;
+
+	if (!capable(CAP_SYS_ADMIN) && !capable(CAP_SYS_RAWIO))
+		return -EPERM;
+
+	/* check args */
+	fmt = format_by_fourcc(fb->fmt.pixelformat);
+	if (fmt == NULL)
+		return -EINVAL;
+
+	/* ok, accept it */
+	dev->ovbuf = *fb;
+	dev->ovfmt = fmt;
+	if (dev->ovbuf.fmt.bytesperline == 0) {
+		dev->ovbuf.fmt.bytesperline =
+			dev->ovbuf.fmt.width * fmt->depth / 8;
+	}
+	return 0;
+}
+
+static int vidioc_reqbufs(struct file *file, void *priv,
+				struct v4l2_requestbuffers *p)
+{
+	struct viu_fh *fh = priv;
+
+	return videobuf_reqbufs(&fh->vb_vidq, p);
+}
+
+static int vidioc_querybuf(struct file *file, void *priv,
+					struct v4l2_buffer *p)
+{
+	struct viu_fh *fh = priv;
+
+	return videobuf_querybuf(&fh->vb_vidq, p);
+}
+
+static int vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *p)
+{
+	struct viu_fh *fh = priv;
+
+	return videobuf_qbuf(&fh->vb_vidq, p);
+}
+
+static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *p)
+{
+	struct viu_fh *fh = priv;
+
+	return videobuf_dqbuf(&fh->vb_vidq, p,
+				file->f_flags & O_NONBLOCK);
+}
+
+static int vidioc_streamon(struct file *file, void *priv, enum v4l2_buf_type i)
+{
+	struct viu_fh *fh = priv;
+	struct viu_dev *dev = fh->dev;
+
+	if (fh->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+	if (fh->type != i)
+		return -EINVAL;
+
+	if (dev->ovenable)
+		dev->ovenable = 0;
+
+	viu_start_dma(fh->dev);
+
+	return videobuf_streamon(&fh->vb_vidq);
+}
+
+static int vidioc_streamoff(struct file *file, void *priv, enum v4l2_buf_type i)
+{
+	struct viu_fh  *fh = priv;
+
+	if (fh->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+	if (fh->type != i)
+		return -EINVAL;
+
+	viu_stop_dma(fh->dev);
+
+	return videobuf_streamoff(&fh->vb_vidq);
+}
+
+#define decoder_call(viu, o, f, args...) \
+	v4l2_subdev_call(viu->decoder, o, f, ##args)
+
+static int vidioc_querystd(struct file *file, void *priv, v4l2_std_id *std_id)
+{
+	struct viu_fh *fh = priv;
+
+	decoder_call(fh->dev, video, querystd, std_id);
+	return 0;
+}
+
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id id)
+{
+	struct viu_fh *fh = priv;
+
+	fh->dev->std = id;
+	decoder_call(fh->dev, video, s_std, id);
+	return 0;
+}
+
+static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *std_id)
+{
+	struct viu_fh *fh = priv;
+
+	*std_id = fh->dev->std;
+	return 0;
+}
+
+/* only one input in this driver */
+static int vidioc_enum_input(struct file *file, void *priv,
+					struct v4l2_input *inp)
+{
+	struct viu_fh *fh = priv;
+
+	if (inp->index != 0)
+		return -EINVAL;
+
+	inp->type = V4L2_INPUT_TYPE_CAMERA;
+	inp->std = fh->dev->vdev->tvnorms;
+	strcpy(inp->name, "Camera");
+	return 0;
+}
+
+static int vidioc_g_input(struct file *file, void *priv, unsigned int *i)
+{
+	*i = 0;
+	return 0;
+}
+
+static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
+{
+	struct viu_fh *fh = priv;
+
+	if (i)
+		return -EINVAL;
+
+	decoder_call(fh->dev, video, s_routing, i, 0, 0);
+	return 0;
+}
+
+inline void viu_activate_next_buf(struct viu_dev *dev,
+				struct viu_dmaqueue *viuq)
+{
+	struct viu_dmaqueue *vidq = viuq;
+	struct viu_buf *buf;
+
+	/* launch another DMA operation for an active/queued buffer */
+	if (!list_empty(&vidq->active)) {
+		buf = list_entry(vidq->active.next, struct viu_buf,
+					vb.queue);
+		dprintk(1, "start another queued buffer: 0x%p\n", buf);
+		buffer_activate(dev, buf);
+	} else if (!list_empty(&vidq->queued)) {
+		buf = list_entry(vidq->queued.next, struct viu_buf,
+					vb.queue);
+		list_del(&buf->vb.queue);
+
+		dprintk(1, "start another queued buffer: 0x%p\n", buf);
+		list_add_tail(&buf->vb.queue, &vidq->active);
+		buf->vb.state = VIDEOBUF_ACTIVE;
+		buffer_activate(dev, buf);
+	}
+}
+
+inline void viu_default_settings(struct viu_reg *viu_reg)
+{
+	struct viu_reg *vr = viu_reg;
+
+	out_be32(&vr->luminance, 0x9512A254);
+	out_be32(&vr->chroma_r, 0x03310000);
+	out_be32(&vr->chroma_g, 0x06600F38);
+	out_be32(&vr->chroma_b, 0x00000409);
+	out_be32(&vr->alpha, 0x000000ff);
+	out_be32(&vr->req_alarm, 0x00000090);
+	dprintk(1, "status reg: 0x%08x, field base: 0x%08x\n",
+		in_be32(&vr->status_cfg), in_be32(&vr->field_base_addr));
+}
+
+static void viu_overlay_intr(struct viu_dev *dev, u32 status)
+{
+	struct viu_reg *vr = dev->vr;
+
+	if (status & INT_DMA_END_STATUS)
+		dev->dma_done = 1;
+
+	if (status & INT_FIELD_STATUS) {
+		if (dev->dma_done) {
+			u32 addr = reg_val.field_base_addr;
+
+			dev->dma_done = 0;
+			if (status & FIELD_NO)
+				addr += reg_val.dma_inc;
+
+			out_be32(&vr->field_base_addr, addr);
+			out_be32(&vr->dma_inc, reg_val.dma_inc);
+			out_be32(&vr->status_cfg,
+				 (status & 0xffc0ffff) |
+				 (status & INT_ALL_STATUS) |
+				 reg_val.status_cfg);
+		} else if (status & INT_VSYNC_STATUS) {
+			out_be32(&vr->status_cfg,
+				 (status & 0xffc0ffff) |
+				 (status & INT_ALL_STATUS) |
+				 reg_val.status_cfg);
+		}
+	}
+}
+
+static void viu_capture_intr(struct viu_dev *dev, u32 status)
+{
+	struct viu_dmaqueue *vidq = &dev->vidq;
+	struct viu_reg *vr = dev->vr;
+	struct viu_buf *buf;
+	int field_num;
+	int need_two;
+	int dma_done = 0;
+
+	field_num = status & FIELD_NO;
+	need_two = V4L2_FIELD_HAS_BOTH(dev->capfield);
+
+	if (status & INT_DMA_END_STATUS) {
+		dma_done = 1;
+		if (((field_num == 0) && (dev->field == 0)) ||
+		    (field_num && (dev->field == 1)))
+			dev->field++;
+	}
+
+	if (status & INT_FIELD_STATUS) {
+		dprintk(1, "irq: field %d, done %d\n",
+			!!field_num, dma_done);
+		if (unlikely(dev->first)) {
+			if (field_num == 0) {
+				dev->first = 0;
+				dprintk(1, "activate first buf\n");
+				viu_activate_next_buf(dev, vidq);
+			} else
+				dprintk(1, "wait field 0\n");
+			return;
+		}
+
+		/* setup buffer address for next dma operation */
+		if (!list_empty(&vidq->active)) {
+			u32 addr = reg_val.field_base_addr;
+
+			if (field_num && need_two) {
+				addr += reg_val.dma_inc;
+				dprintk(1, "field 1, 0x%lx, dev field %d\n",
+					(unsigned long)addr, dev->field);
+			}
+			out_be32(&vr->field_base_addr, addr);
+			out_be32(&vr->dma_inc, reg_val.dma_inc);
+			out_be32(&vr->status_cfg,
+				 (status & 0xffc0ffff) |
+				 (status & INT_ALL_STATUS) |
+				 reg_val.status_cfg);
+			return;
+		}
+	}
+
+	if (dma_done && field_num && (dev->field == 2)) {
+		dev->field = 0;
+		buf = list_entry(vidq->active.next,
+				 struct viu_buf, vb.queue);
+		dprintk(1, "viu/0: [%p/%d] 0x%lx/0x%lx: dma complete\n",
+			buf, buf->vb.i,
+			(unsigned long)videobuf_to_dma_contig(&buf->vb),
+			(unsigned long)in_be32(&vr->field_base_addr));
+
+		if (waitqueue_active(&buf->vb.done)) {
+			list_del(&buf->vb.queue);
+			v4l2_get_timestamp(&buf->vb.ts);
+			buf->vb.state = VIDEOBUF_DONE;
+			buf->vb.field_count++;
+			wake_up(&buf->vb.done);
+		}
+		/* activate next dma buffer */
+		viu_activate_next_buf(dev, vidq);
+	}
+}
+
+static irqreturn_t viu_intr(int irq, void *dev_id)
+{
+	struct viu_dev *dev  = (struct viu_dev *)dev_id;
+	struct viu_reg *vr = dev->vr;
+	u32 status;
+	u32 error;
+
+	status = in_be32(&vr->status_cfg);
+
+	if (status & INT_ERROR_STATUS) {
+		dev->irqs.error_irq++;
+		error = status & ERR_MASK;
+		if (error)
+			dprintk(1, "Err: error(%d), times:%d!\n",
+				error >> 4, dev->irqs.error_irq);
+		/* Clear interrupt error bit and error flags */
+		out_be32(&vr->status_cfg,
+			 (status & 0xffc0ffff) | INT_ERROR_STATUS);
+	}
+
+	if (status & INT_DMA_END_STATUS) {
+		dev->irqs.dma_end_irq++;
+		dev->dma_done = 1;
+		dprintk(2, "VIU DMA end interrupt times: %d\n",
+					dev->irqs.dma_end_irq);
+	}
+
+	if (status & INT_HSYNC_STATUS)
+		dev->irqs.hsync_irq++;
+
+	if (status & INT_FIELD_STATUS) {
+		dev->irqs.field_irq++;
+		dprintk(2, "VIU field interrupt times: %d\n",
+					dev->irqs.field_irq);
+	}
+
+	if (status & INT_VSTART_STATUS)
+		dev->irqs.vstart_irq++;
+
+	if (status & INT_VSYNC_STATUS) {
+		dev->irqs.vsync_irq++;
+		dprintk(2, "VIU vsync interrupt times: %d\n",
+			dev->irqs.vsync_irq);
+	}
+
+	/* clear all pending irqs */
+	status = in_be32(&vr->status_cfg);
+	out_be32(&vr->status_cfg,
+		 (status & 0xffc0ffff) | (status & INT_ALL_STATUS));
+
+	if (dev->ovenable) {
+		viu_overlay_intr(dev, status);
+		return IRQ_HANDLED;
+	}
+
+	/* Capture mode */
+	viu_capture_intr(dev, status);
+	return IRQ_HANDLED;
+}
+
+/*
+ * File operations for the device
+ */
+static int viu_open(struct file *file)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct viu_dev *dev = video_get_drvdata(vdev);
+	struct viu_fh *fh;
+	struct viu_reg *vr;
+	int minor = vdev->minor;
+	u32 status_cfg;
+
+	dprintk(1, "viu: open (minor=%d)\n", minor);
+
+	dev->users++;
+	if (dev->users > 1) {
+		dev->users--;
+		return -EBUSY;
+	}
+
+	vr = dev->vr;
+
+	dprintk(1, "open minor=%d type=%s users=%d\n", minor,
+		v4l2_type_names[V4L2_BUF_TYPE_VIDEO_CAPTURE], dev->users);
+
+	if (mutex_lock_interruptible(&dev->lock)) {
+		dev->users--;
+		return -ERESTARTSYS;
+	}
+
+	/* allocate and initialize per filehandle data */
+	fh = kzalloc(sizeof(*fh), GFP_KERNEL);
+	if (!fh) {
+		dev->users--;
+		mutex_unlock(&dev->lock);
+		return -ENOMEM;
+	}
+
+	v4l2_fh_init(&fh->fh, vdev);
+	file->private_data = fh;
+	fh->dev = dev;
+
+	fh->type     = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	fh->fmt      = format_by_fourcc(V4L2_PIX_FMT_RGB32);
+	fh->width    = norm_maxw();
+	fh->height   = norm_maxh();
+	dev->crop_current.width  = fh->width;
+	dev->crop_current.height = fh->height;
+
+	dprintk(1, "Open: fh=0x%08lx, dev=0x%08lx, dev->vidq=0x%08lx\n",
+		(unsigned long)fh, (unsigned long)dev,
+		(unsigned long)&dev->vidq);
+	dprintk(1, "Open: list_empty queued=%d\n",
+		list_empty(&dev->vidq.queued));
+	dprintk(1, "Open: list_empty active=%d\n",
+		list_empty(&dev->vidq.active));
+
+	viu_default_settings(vr);
+
+	status_cfg = in_be32(&vr->status_cfg);
+	out_be32(&vr->status_cfg,
+		 status_cfg & ~(INT_VSYNC_EN | INT_HSYNC_EN |
+				INT_FIELD_EN | INT_VSTART_EN |
+				INT_DMA_END_EN | INT_ERROR_EN | INT_ECC_EN));
+
+	status_cfg = in_be32(&vr->status_cfg);
+	out_be32(&vr->status_cfg, status_cfg | INT_ALL_STATUS);
+
+	spin_lock_init(&fh->vbq_lock);
+	videobuf_queue_dma_contig_init(&fh->vb_vidq, &viu_video_qops,
+				       dev->dev, &fh->vbq_lock,
+				       fh->type, V4L2_FIELD_INTERLACED,
+				       sizeof(struct viu_buf), fh,
+				       &fh->dev->lock);
+	v4l2_fh_add(&fh->fh);
+	mutex_unlock(&dev->lock);
+	return 0;
+}
+
+static ssize_t viu_read(struct file *file, char __user *data, size_t count,
+			loff_t *ppos)
+{
+	struct viu_fh *fh = file->private_data;
+	struct viu_dev *dev = fh->dev;
+	int ret = 0;
+
+	dprintk(2, "%s\n", __func__);
+	if (dev->ovenable)
+		dev->ovenable = 0;
+
+	if (fh->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+		if (mutex_lock_interruptible(&dev->lock))
+			return -ERESTARTSYS;
+		viu_start_dma(dev);
+		ret = videobuf_read_stream(&fh->vb_vidq, data, count,
+				ppos, 0, file->f_flags & O_NONBLOCK);
+		mutex_unlock(&dev->lock);
+		return ret;
+	}
+	return 0;
+}
+
+static unsigned int viu_poll(struct file *file, struct poll_table_struct *wait)
+{
+	struct viu_fh *fh = file->private_data;
+	struct videobuf_queue *q = &fh->vb_vidq;
+	struct viu_dev *dev = fh->dev;
+	unsigned long req_events = poll_requested_events(wait);
+	unsigned int res = v4l2_ctrl_poll(file, wait);
+
+	if (V4L2_BUF_TYPE_VIDEO_CAPTURE != fh->type)
+		return POLLERR;
+
+	if (!(req_events & (POLLIN | POLLRDNORM)))
+		return res;
+
+	mutex_lock(&dev->lock);
+	res |= videobuf_poll_stream(file, q, wait);
+	mutex_unlock(&dev->lock);
+	return res;
+}
+
+static int viu_release(struct file *file)
+{
+	struct viu_fh *fh = file->private_data;
+	struct viu_dev *dev = fh->dev;
+	int minor = video_devdata(file)->minor;
+
+	mutex_lock(&dev->lock);
+	viu_stop_dma(dev);
+	videobuf_stop(&fh->vb_vidq);
+	videobuf_mmap_free(&fh->vb_vidq);
+	v4l2_fh_del(&fh->fh);
+	v4l2_fh_exit(&fh->fh);
+	mutex_unlock(&dev->lock);
+
+	kfree(fh);
+
+	dev->users--;
+	dprintk(1, "close (minor=%d, users=%d)\n",
+		minor, dev->users);
+	return 0;
+}
+
+void viu_reset(struct viu_reg *reg)
+{
+	out_be32(&reg->status_cfg, 0);
+	out_be32(&reg->luminance, 0x9512a254);
+	out_be32(&reg->chroma_r, 0x03310000);
+	out_be32(&reg->chroma_g, 0x06600f38);
+	out_be32(&reg->chroma_b, 0x00000409);
+	out_be32(&reg->field_base_addr, 0);
+	out_be32(&reg->dma_inc, 0);
+	out_be32(&reg->picture_count, 0x01e002d0);
+	out_be32(&reg->req_alarm, 0x00000090);
+	out_be32(&reg->alpha, 0x000000ff);
+}
+
+static int viu_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct viu_fh *fh = file->private_data;
+	struct viu_dev *dev = fh->dev;
+	int ret;
+
+	dprintk(1, "mmap called, vma=0x%08lx\n", (unsigned long)vma);
+
+	if (mutex_lock_interruptible(&dev->lock))
+		return -ERESTARTSYS;
+	ret = videobuf_mmap_mapper(&fh->vb_vidq, vma);
+	mutex_unlock(&dev->lock);
+
+	dprintk(1, "vma start=0x%08lx, size=%ld, ret=%d\n",
+		(unsigned long)vma->vm_start,
+		(unsigned long)vma->vm_end-(unsigned long)vma->vm_start,
+		ret);
+
+	return ret;
+}
+
+static struct v4l2_file_operations viu_fops = {
+	.owner		= THIS_MODULE,
+	.open		= viu_open,
+	.release	= viu_release,
+	.read		= viu_read,
+	.poll		= viu_poll,
+	.unlocked_ioctl	= video_ioctl2, /* V4L2 ioctl handler */
+	.mmap		= viu_mmap,
+};
+
+static const struct v4l2_ioctl_ops viu_ioctl_ops = {
+	.vidioc_querycap	= vidioc_querycap,
+	.vidioc_enum_fmt_vid_cap  = vidioc_enum_fmt,
+	.vidioc_g_fmt_vid_cap     = vidioc_g_fmt_cap,
+	.vidioc_try_fmt_vid_cap   = vidioc_try_fmt_cap,
+	.vidioc_s_fmt_vid_cap     = vidioc_s_fmt_cap,
+	.vidioc_enum_fmt_vid_overlay = vidioc_enum_fmt,
+	.vidioc_g_fmt_vid_overlay = vidioc_g_fmt_overlay,
+	.vidioc_try_fmt_vid_overlay = vidioc_try_fmt_overlay,
+	.vidioc_s_fmt_vid_overlay = vidioc_s_fmt_overlay,
+	.vidioc_overlay	      = vidioc_overlay,
+	.vidioc_g_fbuf	      = vidioc_g_fbuf,
+	.vidioc_s_fbuf	      = vidioc_s_fbuf,
+	.vidioc_reqbufs       = vidioc_reqbufs,
+	.vidioc_querybuf      = vidioc_querybuf,
+	.vidioc_qbuf          = vidioc_qbuf,
+	.vidioc_dqbuf         = vidioc_dqbuf,
+	.vidioc_g_std         = vidioc_g_std,
+	.vidioc_s_std         = vidioc_s_std,
+	.vidioc_querystd      = vidioc_querystd,
+	.vidioc_enum_input    = vidioc_enum_input,
+	.vidioc_g_input       = vidioc_g_input,
+	.vidioc_s_input       = vidioc_s_input,
+	.vidioc_streamon      = vidioc_streamon,
+	.vidioc_streamoff     = vidioc_streamoff,
+	.vidioc_log_status    = v4l2_ctrl_log_status,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+};
+
+static struct video_device viu_template = {
+	.name		= "FSL viu",
+	.fops		= &viu_fops,
+	.minor		= -1,
+	.ioctl_ops	= &viu_ioctl_ops,
+	.release	= video_device_release,
+
+	.tvnorms        = V4L2_STD_NTSC_M | V4L2_STD_PAL,
+};
+
+static int viu_of_probe(struct platform_device *op)
+{
+	struct viu_dev *viu_dev;
+	struct video_device *vdev;
+	struct resource r;
+	struct viu_reg __iomem *viu_regs;
+	struct i2c_adapter *ad;
+	int ret, viu_irq;
+	struct clk *clk;
+
+	ret = of_address_to_resource(op->dev.of_node, 0, &r);
+	if (ret) {
+		dev_err(&op->dev, "Can't parse device node resource\n");
+		return -ENODEV;
+	}
+
+	viu_irq = irq_of_parse_and_map(op->dev.of_node, 0);
+	if (viu_irq == NO_IRQ) {
+		dev_err(&op->dev, "Error while mapping the irq\n");
+		return -EINVAL;
+	}
+
+	/* request mem region */
+	if (!devm_request_mem_region(&op->dev, r.start,
+				     sizeof(struct viu_reg), DRV_NAME)) {
+		dev_err(&op->dev, "Error while requesting mem region\n");
+		ret = -EBUSY;
+		goto err;
+	}
+
+	/* remap registers */
+	viu_regs = devm_ioremap(&op->dev, r.start, sizeof(struct viu_reg));
+	if (!viu_regs) {
+		dev_err(&op->dev, "Can't map register set\n");
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	/* Prepare our private structure */
+	viu_dev = devm_kzalloc(&op->dev, sizeof(struct viu_dev), GFP_ATOMIC);
+	if (!viu_dev) {
+		dev_err(&op->dev, "Can't allocate private structure\n");
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	viu_dev->vr = viu_regs;
+	viu_dev->irq = viu_irq;
+	viu_dev->dev = &op->dev;
+
+	/* init video dma queues */
+	INIT_LIST_HEAD(&viu_dev->vidq.active);
+	INIT_LIST_HEAD(&viu_dev->vidq.queued);
+
+	snprintf(viu_dev->v4l2_dev.name,
+		 sizeof(viu_dev->v4l2_dev.name), "%s", "VIU");
+	ret = v4l2_device_register(viu_dev->dev, &viu_dev->v4l2_dev);
+	if (ret < 0) {
+		dev_err(&op->dev, "v4l2_device_register() failed: %d\n", ret);
+		goto err;
+	}
+
+	ad = i2c_get_adapter(0);
+
+	v4l2_ctrl_handler_init(&viu_dev->hdl, 5);
+	if (viu_dev->hdl.error) {
+		ret = viu_dev->hdl.error;
+		dev_err(&op->dev, "couldn't register control\n");
+		goto err_vdev;
+	}
+	/* This control handler will inherit the control(s) from the
+	   sub-device(s). */
+	viu_dev->v4l2_dev.ctrl_handler = &viu_dev->hdl;
+	viu_dev->decoder = v4l2_i2c_new_subdev(&viu_dev->v4l2_dev, ad,
+			"saa7113", VIU_VIDEO_DECODER_ADDR, NULL);
+
+	viu_dev->vidq.timeout.function = viu_vid_timeout;
+	viu_dev->vidq.timeout.data     = (unsigned long)viu_dev;
+	init_timer(&viu_dev->vidq.timeout);
+	viu_dev->std = V4L2_STD_NTSC_M;
+	viu_dev->first = 1;
+
+	/* Allocate memory for video device */
+	vdev = video_device_alloc();
+	if (vdev == NULL) {
+		ret = -ENOMEM;
+		goto err_vdev;
+	}
+
+	*vdev = viu_template;
+
+	vdev->v4l2_dev = &viu_dev->v4l2_dev;
+
+	viu_dev->vdev = vdev;
+
+	/* initialize locks */
+	mutex_init(&viu_dev->lock);
+	viu_dev->vdev->lock = &viu_dev->lock;
+	spin_lock_init(&viu_dev->slock);
+
+	video_set_drvdata(viu_dev->vdev, viu_dev);
+
+	mutex_lock(&viu_dev->lock);
+
+	ret = video_register_device(viu_dev->vdev, VFL_TYPE_GRABBER, -1);
+	if (ret < 0) {
+		video_device_release(viu_dev->vdev);
+		goto err_vdev;
+	}
+
+	/* enable VIU clock */
+	clk = devm_clk_get(&op->dev, "ipg");
+	if (IS_ERR(clk)) {
+		dev_err(&op->dev, "failed to lookup the clock!\n");
+		ret = PTR_ERR(clk);
+		goto err_clk;
+	}
+	ret = clk_prepare_enable(clk);
+	if (ret) {
+		dev_err(&op->dev, "failed to enable the clock!\n");
+		goto err_clk;
+	}
+	viu_dev->clk = clk;
+
+	/* reset VIU module */
+	viu_reset(viu_dev->vr);
+
+	/* install interrupt handler */
+	if (request_irq(viu_dev->irq, viu_intr, 0, "viu", (void *)viu_dev)) {
+		dev_err(&op->dev, "Request VIU IRQ failed.\n");
+		ret = -ENODEV;
+		goto err_irq;
+	}
+
+	mutex_unlock(&viu_dev->lock);
+
+	dev_info(&op->dev, "Freescale VIU Video Capture Board\n");
+	return ret;
+
+err_irq:
+	clk_disable_unprepare(viu_dev->clk);
+err_clk:
+	video_unregister_device(viu_dev->vdev);
+err_vdev:
+	v4l2_ctrl_handler_free(&viu_dev->hdl);
+	mutex_unlock(&viu_dev->lock);
+	i2c_put_adapter(ad);
+	v4l2_device_unregister(&viu_dev->v4l2_dev);
+err:
+	irq_dispose_mapping(viu_irq);
+	return ret;
+}
+
+static int viu_of_remove(struct platform_device *op)
+{
+	struct v4l2_device *v4l2_dev = dev_get_drvdata(&op->dev);
+	struct viu_dev *dev = container_of(v4l2_dev, struct viu_dev, v4l2_dev);
+	struct v4l2_subdev *sdev = list_entry(v4l2_dev->subdevs.next,
+					      struct v4l2_subdev, list);
+	struct i2c_client *client = v4l2_get_subdevdata(sdev);
+
+	free_irq(dev->irq, (void *)dev);
+	irq_dispose_mapping(dev->irq);
+
+	clk_disable_unprepare(dev->clk);
+
+	v4l2_ctrl_handler_free(&dev->hdl);
+	video_unregister_device(dev->vdev);
+	i2c_put_adapter(client->adapter);
+	v4l2_device_unregister(&dev->v4l2_dev);
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int viu_suspend(struct platform_device *op, pm_message_t state)
+{
+	struct v4l2_device *v4l2_dev = dev_get_drvdata(&op->dev);
+	struct viu_dev *dev = container_of(v4l2_dev, struct viu_dev, v4l2_dev);
+
+	clk_disable(dev->clk);
+	return 0;
+}
+
+static int viu_resume(struct platform_device *op)
+{
+	struct v4l2_device *v4l2_dev = dev_get_drvdata(&op->dev);
+	struct viu_dev *dev = container_of(v4l2_dev, struct viu_dev, v4l2_dev);
+
+	clk_enable(dev->clk);
+	return 0;
+}
+#endif
+
+/*
+ * Initialization and module stuff
+ */
+static const struct of_device_id mpc512x_viu_of_match[] = {
+	{
+		.compatible = "fsl,mpc5121-viu",
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(of, mpc512x_viu_of_match);
+
+static struct platform_driver viu_of_platform_driver = {
+	.probe = viu_of_probe,
+	.remove = viu_of_remove,
+#ifdef CONFIG_PM
+	.suspend = viu_suspend,
+	.resume = viu_resume,
+#endif
+	.driver = {
+		.name = DRV_NAME,
+		.of_match_table = mpc512x_viu_of_match,
+	},
+};
+
+module_platform_driver(viu_of_platform_driver);
+
+MODULE_DESCRIPTION("Freescale Video-In(VIU)");
+MODULE_AUTHOR("Hongjun Chen");
+MODULE_LICENSE("GPL");
+MODULE_VERSION(VIU_VERSION);
diff --git a/drivers/media/platform/m2m-deinterlace.c b/drivers/media/platform/m2m-deinterlace.c
new file mode 100644
index 0000000..29973f9
--- /dev/null
+++ b/drivers/media/platform/m2m-deinterlace.c
@@ -0,0 +1,1100 @@
+/*
+ * V4L2 deinterlacing support.
+ *
+ * Copyright (c) 2012 Vista Silicon S.L.
+ * Javier Martin <javier.martin@vista-silicon.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation; either version 2 of the
+ * License, or (at your option) any later version
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/dmaengine.h>
+#include <linux/platform_device.h>
+
+#include <media/v4l2-mem2mem.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-dma-contig.h>
+
+#define MEM2MEM_TEST_MODULE_NAME "mem2mem-deinterlace"
+
+MODULE_DESCRIPTION("mem2mem device which supports deinterlacing using dmaengine");
+MODULE_AUTHOR("Javier Martin <javier.martin@vista-silicon.com");
+MODULE_LICENSE("GPL");
+MODULE_VERSION("0.0.1");
+
+static bool debug;
+module_param(debug, bool, 0644);
+
+/* Flags that indicate a format can be used for capture/output */
+#define MEM2MEM_CAPTURE	(1 << 0)
+#define MEM2MEM_OUTPUT	(1 << 1)
+
+#define MEM2MEM_NAME		"m2m-deinterlace"
+
+#define dprintk(dev, fmt, arg...) \
+	v4l2_dbg(1, debug, &dev->v4l2_dev, "%s: " fmt, __func__, ## arg)
+
+struct deinterlace_fmt {
+	char	*name;
+	u32	fourcc;
+	/* Types the format can be used for */
+	u32	types;
+};
+
+static struct deinterlace_fmt formats[] = {
+	{
+		.name	= "YUV 4:2:0 Planar",
+		.fourcc	= V4L2_PIX_FMT_YUV420,
+		.types	= MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
+	},
+	{
+		.name	= "YUYV 4:2:2",
+		.fourcc	= V4L2_PIX_FMT_YUYV,
+		.types	= MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
+	},
+};
+
+#define NUM_FORMATS ARRAY_SIZE(formats)
+
+/* Per-queue, driver-specific private data */
+struct deinterlace_q_data {
+	unsigned int		width;
+	unsigned int		height;
+	unsigned int		sizeimage;
+	struct deinterlace_fmt	*fmt;
+	enum v4l2_field		field;
+};
+
+enum {
+	V4L2_M2M_SRC = 0,
+	V4L2_M2M_DST = 1,
+};
+
+enum {
+	YUV420_DMA_Y_ODD,
+	YUV420_DMA_Y_EVEN,
+	YUV420_DMA_U_ODD,
+	YUV420_DMA_U_EVEN,
+	YUV420_DMA_V_ODD,
+	YUV420_DMA_V_EVEN,
+	YUV420_DMA_Y_ODD_DOUBLING,
+	YUV420_DMA_U_ODD_DOUBLING,
+	YUV420_DMA_V_ODD_DOUBLING,
+	YUYV_DMA_ODD,
+	YUYV_DMA_EVEN,
+	YUYV_DMA_EVEN_DOUBLING,
+};
+
+/* Source and destination queue data */
+static struct deinterlace_q_data q_data[2];
+
+static struct deinterlace_q_data *get_q_data(enum v4l2_buf_type type)
+{
+	switch (type) {
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+		return &q_data[V4L2_M2M_SRC];
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		return &q_data[V4L2_M2M_DST];
+	default:
+		BUG();
+	}
+	return NULL;
+}
+
+static struct deinterlace_fmt *find_format(struct v4l2_format *f)
+{
+	struct deinterlace_fmt *fmt;
+	unsigned int k;
+
+	for (k = 0; k < NUM_FORMATS; k++) {
+		fmt = &formats[k];
+		if ((fmt->types & f->type) &&
+			(fmt->fourcc == f->fmt.pix.pixelformat))
+			break;
+	}
+
+	if (k == NUM_FORMATS)
+		return NULL;
+
+	return &formats[k];
+}
+
+struct deinterlace_dev {
+	struct v4l2_device	v4l2_dev;
+	struct video_device	vfd;
+
+	atomic_t		busy;
+	struct mutex		dev_mutex;
+	spinlock_t		irqlock;
+
+	struct dma_chan		*dma_chan;
+
+	struct v4l2_m2m_dev	*m2m_dev;
+	struct vb2_alloc_ctx	*alloc_ctx;
+};
+
+struct deinterlace_ctx {
+	struct deinterlace_dev	*dev;
+
+	/* Abort requested by m2m */
+	int			aborting;
+	enum v4l2_colorspace	colorspace;
+	dma_cookie_t		cookie;
+	struct v4l2_m2m_ctx	*m2m_ctx;
+	struct dma_interleaved_template *xt;
+};
+
+/*
+ * mem2mem callbacks
+ */
+static int deinterlace_job_ready(void *priv)
+{
+	struct deinterlace_ctx *ctx = priv;
+	struct deinterlace_dev *pcdev = ctx->dev;
+
+	if ((v4l2_m2m_num_src_bufs_ready(ctx->m2m_ctx) > 0)
+	    && (v4l2_m2m_num_dst_bufs_ready(ctx->m2m_ctx) > 0)
+	    && (atomic_read(&ctx->dev->busy) == 0)) {
+		dprintk(pcdev, "Task ready\n");
+		return 1;
+	}
+
+	dprintk(pcdev, "Task not ready to run\n");
+
+	return 0;
+}
+
+static void deinterlace_job_abort(void *priv)
+{
+	struct deinterlace_ctx *ctx = priv;
+	struct deinterlace_dev *pcdev = ctx->dev;
+
+	ctx->aborting = 1;
+
+	dprintk(pcdev, "Aborting task\n");
+
+	v4l2_m2m_job_finish(pcdev->m2m_dev, ctx->m2m_ctx);
+}
+
+static void deinterlace_lock(void *priv)
+{
+	struct deinterlace_ctx *ctx = priv;
+	struct deinterlace_dev *pcdev = ctx->dev;
+	mutex_lock(&pcdev->dev_mutex);
+}
+
+static void deinterlace_unlock(void *priv)
+{
+	struct deinterlace_ctx *ctx = priv;
+	struct deinterlace_dev *pcdev = ctx->dev;
+	mutex_unlock(&pcdev->dev_mutex);
+}
+
+static void dma_callback(void *data)
+{
+	struct deinterlace_ctx *curr_ctx = data;
+	struct deinterlace_dev *pcdev = curr_ctx->dev;
+	struct vb2_v4l2_buffer *src_vb, *dst_vb;
+
+	atomic_set(&pcdev->busy, 0);
+
+	src_vb = v4l2_m2m_src_buf_remove(curr_ctx->m2m_ctx);
+	dst_vb = v4l2_m2m_dst_buf_remove(curr_ctx->m2m_ctx);
+
+	dst_vb->timestamp = src_vb->timestamp;
+	dst_vb->flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+	dst_vb->flags |=
+		src_vb->flags & V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+	dst_vb->timecode = src_vb->timecode;
+
+	v4l2_m2m_buf_done(src_vb, VB2_BUF_STATE_DONE);
+	v4l2_m2m_buf_done(dst_vb, VB2_BUF_STATE_DONE);
+
+	v4l2_m2m_job_finish(pcdev->m2m_dev, curr_ctx->m2m_ctx);
+
+	dprintk(pcdev, "dma transfers completed.\n");
+}
+
+static void deinterlace_issue_dma(struct deinterlace_ctx *ctx, int op,
+				  int do_callback)
+{
+	struct deinterlace_q_data *s_q_data;
+	struct vb2_v4l2_buffer *src_buf, *dst_buf;
+	struct deinterlace_dev *pcdev = ctx->dev;
+	struct dma_chan *chan = pcdev->dma_chan;
+	struct dma_device *dmadev = chan->device;
+	struct dma_async_tx_descriptor *tx;
+	unsigned int s_width, s_height;
+	unsigned int s_size;
+	dma_addr_t p_in, p_out;
+	enum dma_ctrl_flags flags;
+
+	src_buf = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
+	dst_buf = v4l2_m2m_next_dst_buf(ctx->m2m_ctx);
+
+	s_q_data = get_q_data(V4L2_BUF_TYPE_VIDEO_OUTPUT);
+	s_width	= s_q_data->width;
+	s_height = s_q_data->height;
+	s_size = s_width * s_height;
+
+	p_in = (dma_addr_t)vb2_dma_contig_plane_dma_addr(&src_buf->vb2_buf, 0);
+	p_out = (dma_addr_t)vb2_dma_contig_plane_dma_addr(&dst_buf->vb2_buf,
+							  0);
+	if (!p_in || !p_out) {
+		v4l2_err(&pcdev->v4l2_dev,
+			 "Acquiring kernel pointers to buffers failed\n");
+		return;
+	}
+
+	switch (op) {
+	case YUV420_DMA_Y_ODD:
+		ctx->xt->numf = s_height / 2;
+		ctx->xt->sgl[0].size = s_width;
+		ctx->xt->sgl[0].icg = s_width;
+		ctx->xt->src_start = p_in;
+		ctx->xt->dst_start = p_out;
+		break;
+	case YUV420_DMA_Y_EVEN:
+		ctx->xt->numf = s_height / 2;
+		ctx->xt->sgl[0].size = s_width;
+		ctx->xt->sgl[0].icg = s_width;
+		ctx->xt->src_start = p_in + s_size / 2;
+		ctx->xt->dst_start = p_out + s_width;
+		break;
+	case YUV420_DMA_U_ODD:
+		ctx->xt->numf = s_height / 4;
+		ctx->xt->sgl[0].size = s_width / 2;
+		ctx->xt->sgl[0].icg = s_width / 2;
+		ctx->xt->src_start = p_in + s_size;
+		ctx->xt->dst_start = p_out + s_size;
+		break;
+	case YUV420_DMA_U_EVEN:
+		ctx->xt->numf = s_height / 4;
+		ctx->xt->sgl[0].size = s_width / 2;
+		ctx->xt->sgl[0].icg = s_width / 2;
+		ctx->xt->src_start = p_in + (9 * s_size) / 8;
+		ctx->xt->dst_start = p_out + s_size + s_width / 2;
+		break;
+	case YUV420_DMA_V_ODD:
+		ctx->xt->numf = s_height / 4;
+		ctx->xt->sgl[0].size = s_width / 2;
+		ctx->xt->sgl[0].icg = s_width / 2;
+		ctx->xt->src_start = p_in + (5 * s_size) / 4;
+		ctx->xt->dst_start = p_out + (5 * s_size) / 4;
+		break;
+	case YUV420_DMA_V_EVEN:
+		ctx->xt->numf = s_height / 4;
+		ctx->xt->sgl[0].size = s_width / 2;
+		ctx->xt->sgl[0].icg = s_width / 2;
+		ctx->xt->src_start = p_in + (11 * s_size) / 8;
+		ctx->xt->dst_start = p_out + (5 * s_size) / 4 + s_width / 2;
+		break;
+	case YUV420_DMA_Y_ODD_DOUBLING:
+		ctx->xt->numf = s_height / 2;
+		ctx->xt->sgl[0].size = s_width;
+		ctx->xt->sgl[0].icg = s_width;
+		ctx->xt->src_start = p_in;
+		ctx->xt->dst_start = p_out + s_width;
+		break;
+	case YUV420_DMA_U_ODD_DOUBLING:
+		ctx->xt->numf = s_height / 4;
+		ctx->xt->sgl[0].size = s_width / 2;
+		ctx->xt->sgl[0].icg = s_width / 2;
+		ctx->xt->src_start = p_in + s_size;
+		ctx->xt->dst_start = p_out + s_size + s_width / 2;
+		break;
+	case YUV420_DMA_V_ODD_DOUBLING:
+		ctx->xt->numf = s_height / 4;
+		ctx->xt->sgl[0].size = s_width / 2;
+		ctx->xt->sgl[0].icg = s_width / 2;
+		ctx->xt->src_start = p_in + (5 * s_size) / 4;
+		ctx->xt->dst_start = p_out + (5 * s_size) / 4 + s_width / 2;
+		break;
+	case YUYV_DMA_ODD:
+		ctx->xt->numf = s_height / 2;
+		ctx->xt->sgl[0].size = s_width * 2;
+		ctx->xt->sgl[0].icg = s_width * 2;
+		ctx->xt->src_start = p_in;
+		ctx->xt->dst_start = p_out;
+		break;
+	case YUYV_DMA_EVEN:
+		ctx->xt->numf = s_height / 2;
+		ctx->xt->sgl[0].size = s_width * 2;
+		ctx->xt->sgl[0].icg = s_width * 2;
+		ctx->xt->src_start = p_in + s_size;
+		ctx->xt->dst_start = p_out + s_width * 2;
+		break;
+	case YUYV_DMA_EVEN_DOUBLING:
+	default:
+		ctx->xt->numf = s_height / 2;
+		ctx->xt->sgl[0].size = s_width * 2;
+		ctx->xt->sgl[0].icg = s_width * 2;
+		ctx->xt->src_start = p_in;
+		ctx->xt->dst_start = p_out + s_width * 2;
+		break;
+	}
+
+	/* Common parameters for al transfers */
+	ctx->xt->frame_size = 1;
+	ctx->xt->dir = DMA_MEM_TO_MEM;
+	ctx->xt->src_sgl = false;
+	ctx->xt->dst_sgl = true;
+	flags = DMA_CTRL_ACK | DMA_PREP_INTERRUPT;
+
+	tx = dmadev->device_prep_interleaved_dma(chan, ctx->xt, flags);
+	if (tx == NULL) {
+		v4l2_warn(&pcdev->v4l2_dev, "DMA interleaved prep error\n");
+		return;
+	}
+
+	if (do_callback) {
+		tx->callback = dma_callback;
+		tx->callback_param = ctx;
+	}
+
+	ctx->cookie = dmaengine_submit(tx);
+	if (dma_submit_error(ctx->cookie)) {
+		v4l2_warn(&pcdev->v4l2_dev,
+			  "DMA submit error %d with src=0x%x dst=0x%x len=0x%x\n",
+			  ctx->cookie, (unsigned)p_in, (unsigned)p_out,
+			  s_size * 3/2);
+		return;
+	}
+
+	dma_async_issue_pending(chan);
+}
+
+static void deinterlace_device_run(void *priv)
+{
+	struct deinterlace_ctx *ctx = priv;
+	struct deinterlace_q_data *dst_q_data;
+
+	atomic_set(&ctx->dev->busy, 1);
+
+	dprintk(ctx->dev, "%s: DMA try issue.\n", __func__);
+
+	dst_q_data = get_q_data(V4L2_BUF_TYPE_VIDEO_CAPTURE);
+
+	/*
+	 * 4 possible field conversions are possible at the moment:
+	 *  V4L2_FIELD_SEQ_TB --> V4L2_FIELD_INTERLACED_TB:
+	 *	two separate fields in the same input buffer are interlaced
+	 * 	in the output buffer using weaving. Top field comes first.
+	 *  V4L2_FIELD_SEQ_TB --> V4L2_FIELD_NONE:
+	 * 	top field from the input buffer is copied to the output buffer
+	 * 	using line doubling. Bottom field from the input buffer is discarded.
+	 * V4L2_FIELD_SEQ_BT --> V4L2_FIELD_INTERLACED_BT:
+	 *	two separate fields in the same input buffer are interlaced
+	 * 	in the output buffer using weaving. Bottom field comes first.
+	 * V4L2_FIELD_SEQ_BT --> V4L2_FIELD_NONE:
+	 * 	bottom field from the input buffer is copied to the output buffer
+	 * 	using line doubling. Top field from the input buffer is discarded.
+	 */
+	switch (dst_q_data->fmt->fourcc) {
+	case V4L2_PIX_FMT_YUV420:
+		switch (dst_q_data->field) {
+		case V4L2_FIELD_INTERLACED_TB:
+		case V4L2_FIELD_INTERLACED_BT:
+			dprintk(ctx->dev, "%s: yuv420 interlaced tb.\n",
+				__func__);
+			deinterlace_issue_dma(ctx, YUV420_DMA_Y_ODD, 0);
+			deinterlace_issue_dma(ctx, YUV420_DMA_Y_EVEN, 0);
+			deinterlace_issue_dma(ctx, YUV420_DMA_U_ODD, 0);
+			deinterlace_issue_dma(ctx, YUV420_DMA_U_EVEN, 0);
+			deinterlace_issue_dma(ctx, YUV420_DMA_V_ODD, 0);
+			deinterlace_issue_dma(ctx, YUV420_DMA_V_EVEN, 1);
+			break;
+		case V4L2_FIELD_NONE:
+		default:
+			dprintk(ctx->dev, "%s: yuv420 interlaced line doubling.\n",
+				__func__);
+			deinterlace_issue_dma(ctx, YUV420_DMA_Y_ODD, 0);
+			deinterlace_issue_dma(ctx, YUV420_DMA_Y_ODD_DOUBLING, 0);
+			deinterlace_issue_dma(ctx, YUV420_DMA_U_ODD, 0);
+			deinterlace_issue_dma(ctx, YUV420_DMA_U_ODD_DOUBLING, 0);
+			deinterlace_issue_dma(ctx, YUV420_DMA_V_ODD, 0);
+			deinterlace_issue_dma(ctx, YUV420_DMA_V_ODD_DOUBLING, 1);
+			break;
+		}
+		break;
+	case V4L2_PIX_FMT_YUYV:
+	default:
+		switch (dst_q_data->field) {
+		case V4L2_FIELD_INTERLACED_TB:
+		case V4L2_FIELD_INTERLACED_BT:
+			dprintk(ctx->dev, "%s: yuyv interlaced_tb.\n",
+				__func__);
+			deinterlace_issue_dma(ctx, YUYV_DMA_ODD, 0);
+			deinterlace_issue_dma(ctx, YUYV_DMA_EVEN, 1);
+			break;
+		case V4L2_FIELD_NONE:
+		default:
+			dprintk(ctx->dev, "%s: yuyv interlaced line doubling.\n",
+				__func__);
+			deinterlace_issue_dma(ctx, YUYV_DMA_ODD, 0);
+			deinterlace_issue_dma(ctx, YUYV_DMA_EVEN_DOUBLING, 1);
+			break;
+		}
+		break;
+	}
+
+	dprintk(ctx->dev, "%s: DMA issue done.\n", __func__);
+}
+
+/*
+ * video ioctls
+ */
+static int vidioc_querycap(struct file *file, void *priv,
+			   struct v4l2_capability *cap)
+{
+	strlcpy(cap->driver, MEM2MEM_NAME, sizeof(cap->driver));
+	strlcpy(cap->card, MEM2MEM_NAME, sizeof(cap->card));
+	strlcpy(cap->bus_info, MEM2MEM_NAME, sizeof(cap->card));
+	/*
+	 * This is only a mem-to-mem video device. The capture and output
+	 * device capability flags are left only for backward compatibility
+	 * and are scheduled for removal.
+	 */
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OUTPUT |
+			   V4L2_CAP_VIDEO_M2M | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+
+	return 0;
+}
+
+static int enum_fmt(struct v4l2_fmtdesc *f, u32 type)
+{
+	int i, num;
+	struct deinterlace_fmt *fmt;
+
+	num = 0;
+
+	for (i = 0; i < NUM_FORMATS; ++i) {
+		if (formats[i].types & type) {
+			/* index-th format of type type found ? */
+			if (num == f->index)
+				break;
+			/* Correct type but haven't reached our index yet,
+			 * just increment per-type index */
+			++num;
+		}
+	}
+
+	if (i < NUM_FORMATS) {
+		/* Format found */
+		fmt = &formats[i];
+		strlcpy(f->description, fmt->name, sizeof(f->description));
+		f->pixelformat = fmt->fourcc;
+		return 0;
+	}
+
+	/* Format not found */
+	return -EINVAL;
+}
+
+static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
+				   struct v4l2_fmtdesc *f)
+{
+	return enum_fmt(f, MEM2MEM_CAPTURE);
+}
+
+static int vidioc_enum_fmt_vid_out(struct file *file, void *priv,
+				   struct v4l2_fmtdesc *f)
+{
+	return enum_fmt(f, MEM2MEM_OUTPUT);
+}
+
+static int vidioc_g_fmt(struct deinterlace_ctx *ctx, struct v4l2_format *f)
+{
+	struct vb2_queue *vq;
+	struct deinterlace_q_data *q_data;
+
+	vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
+	if (!vq)
+		return -EINVAL;
+
+	q_data = get_q_data(f->type);
+
+	f->fmt.pix.width	= q_data->width;
+	f->fmt.pix.height	= q_data->height;
+	f->fmt.pix.field	= q_data->field;
+	f->fmt.pix.pixelformat	= q_data->fmt->fourcc;
+
+	switch (q_data->fmt->fourcc) {
+	case V4L2_PIX_FMT_YUV420:
+		f->fmt.pix.bytesperline = q_data->width * 3 / 2;
+		break;
+	case V4L2_PIX_FMT_YUYV:
+	default:
+		f->fmt.pix.bytesperline = q_data->width * 2;
+	}
+
+	f->fmt.pix.sizeimage	= q_data->sizeimage;
+	f->fmt.pix.colorspace	= ctx->colorspace;
+
+	return 0;
+}
+
+static int vidioc_g_fmt_vid_out(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	return vidioc_g_fmt(priv, f);
+}
+
+static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	return vidioc_g_fmt(priv, f);
+}
+
+static int vidioc_try_fmt(struct v4l2_format *f, struct deinterlace_fmt *fmt)
+{
+	switch (f->fmt.pix.pixelformat) {
+	case V4L2_PIX_FMT_YUV420:
+		f->fmt.pix.bytesperline = f->fmt.pix.width * 3 / 2;
+		break;
+	case V4L2_PIX_FMT_YUYV:
+	default:
+		f->fmt.pix.bytesperline = f->fmt.pix.width * 2;
+	}
+	f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
+
+	return 0;
+}
+
+static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
+				  struct v4l2_format *f)
+{
+	struct deinterlace_fmt *fmt;
+	struct deinterlace_ctx *ctx = priv;
+
+	fmt = find_format(f);
+	if (!fmt || !(fmt->types & MEM2MEM_CAPTURE))
+		f->fmt.pix.pixelformat = V4L2_PIX_FMT_YUV420;
+
+	f->fmt.pix.colorspace = ctx->colorspace;
+
+	if (f->fmt.pix.field != V4L2_FIELD_INTERLACED_TB &&
+	    f->fmt.pix.field != V4L2_FIELD_INTERLACED_BT &&
+	    f->fmt.pix.field != V4L2_FIELD_NONE)
+		f->fmt.pix.field = V4L2_FIELD_INTERLACED_TB;
+
+	return vidioc_try_fmt(f, fmt);
+}
+
+static int vidioc_try_fmt_vid_out(struct file *file, void *priv,
+				  struct v4l2_format *f)
+{
+	struct deinterlace_fmt *fmt;
+
+	fmt = find_format(f);
+	if (!fmt || !(fmt->types & MEM2MEM_OUTPUT))
+		f->fmt.pix.pixelformat = V4L2_PIX_FMT_YUV420;
+
+	if (!f->fmt.pix.colorspace)
+		f->fmt.pix.colorspace = V4L2_COLORSPACE_REC709;
+
+	if (f->fmt.pix.field != V4L2_FIELD_SEQ_TB &&
+	    f->fmt.pix.field != V4L2_FIELD_SEQ_BT)
+		f->fmt.pix.field = V4L2_FIELD_SEQ_TB;
+
+	return vidioc_try_fmt(f, fmt);
+}
+
+static int vidioc_s_fmt(struct deinterlace_ctx *ctx, struct v4l2_format *f)
+{
+	struct deinterlace_q_data *q_data;
+	struct vb2_queue *vq;
+
+	vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
+	if (!vq)
+		return -EINVAL;
+
+	q_data = get_q_data(f->type);
+	if (!q_data)
+		return -EINVAL;
+
+	if (vb2_is_busy(vq)) {
+		v4l2_err(&ctx->dev->v4l2_dev, "%s queue busy\n", __func__);
+		return -EBUSY;
+	}
+
+	q_data->fmt = find_format(f);
+	if (!q_data->fmt) {
+		v4l2_err(&ctx->dev->v4l2_dev,
+			 "Couldn't set format type %d, wxh: %dx%d. fmt: %d, field: %d\n",
+			f->type, f->fmt.pix.width, f->fmt.pix.height,
+			f->fmt.pix.pixelformat, f->fmt.pix.field);
+		return -EINVAL;
+	}
+
+	q_data->width		= f->fmt.pix.width;
+	q_data->height		= f->fmt.pix.height;
+	q_data->field		= f->fmt.pix.field;
+
+	switch (f->fmt.pix.pixelformat) {
+	case V4L2_PIX_FMT_YUV420:
+		f->fmt.pix.bytesperline = f->fmt.pix.width * 3 / 2;
+		q_data->sizeimage = (q_data->width * q_data->height * 3) / 2;
+		break;
+	case V4L2_PIX_FMT_YUYV:
+	default:
+		f->fmt.pix.bytesperline = f->fmt.pix.width * 2;
+		q_data->sizeimage = q_data->width * q_data->height * 2;
+	}
+
+	dprintk(ctx->dev,
+		"Setting format for type %d, wxh: %dx%d, fmt: %d, field: %d\n",
+		f->type, q_data->width, q_data->height, q_data->fmt->fourcc,
+		q_data->field);
+
+	return 0;
+}
+
+static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	int ret;
+
+	ret = vidioc_try_fmt_vid_cap(file, priv, f);
+	if (ret)
+		return ret;
+	return vidioc_s_fmt(priv, f);
+}
+
+static int vidioc_s_fmt_vid_out(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	struct deinterlace_ctx *ctx = priv;
+	int ret;
+
+	ret = vidioc_try_fmt_vid_out(file, priv, f);
+	if (ret)
+		return ret;
+
+	ret = vidioc_s_fmt(priv, f);
+	if (!ret)
+		ctx->colorspace = f->fmt.pix.colorspace;
+
+	return ret;
+}
+
+static int vidioc_reqbufs(struct file *file, void *priv,
+			  struct v4l2_requestbuffers *reqbufs)
+{
+	struct deinterlace_ctx *ctx = priv;
+
+	return v4l2_m2m_reqbufs(file, ctx->m2m_ctx, reqbufs);
+}
+
+static int vidioc_querybuf(struct file *file, void *priv,
+			   struct v4l2_buffer *buf)
+{
+	struct deinterlace_ctx *ctx = priv;
+
+	return v4l2_m2m_querybuf(file, ctx->m2m_ctx, buf);
+}
+
+static int vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
+{
+	struct deinterlace_ctx *ctx = priv;
+
+	return v4l2_m2m_qbuf(file, ctx->m2m_ctx, buf);
+}
+
+static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
+{
+	struct deinterlace_ctx *ctx = priv;
+
+	return v4l2_m2m_dqbuf(file, ctx->m2m_ctx, buf);
+}
+
+static int vidioc_streamon(struct file *file, void *priv,
+			   enum v4l2_buf_type type)
+{
+	struct deinterlace_q_data *s_q_data, *d_q_data;
+	struct deinterlace_ctx *ctx = priv;
+
+	s_q_data = get_q_data(V4L2_BUF_TYPE_VIDEO_OUTPUT);
+	d_q_data = get_q_data(V4L2_BUF_TYPE_VIDEO_CAPTURE);
+
+	/* Check that src and dst queues have the same pix format */
+	if (s_q_data->fmt->fourcc != d_q_data->fmt->fourcc) {
+		v4l2_err(&ctx->dev->v4l2_dev,
+			 "src and dst formats don't match.\n");
+		return -EINVAL;
+	}
+
+	/* Check that input and output deinterlacing types are compatible */
+	switch (s_q_data->field) {
+	case V4L2_FIELD_SEQ_BT:
+		if (d_q_data->field != V4L2_FIELD_NONE &&
+			d_q_data->field != V4L2_FIELD_INTERLACED_BT) {
+			v4l2_err(&ctx->dev->v4l2_dev,
+			 "src and dst field conversion [(%d)->(%d)] not supported.\n",
+				s_q_data->field, d_q_data->field);
+			return -EINVAL;
+		}
+		break;
+	case V4L2_FIELD_SEQ_TB:
+		if (d_q_data->field != V4L2_FIELD_NONE &&
+			d_q_data->field != V4L2_FIELD_INTERLACED_TB) {
+			v4l2_err(&ctx->dev->v4l2_dev,
+			 "src and dst field conversion [(%d)->(%d)] not supported.\n",
+				s_q_data->field, d_q_data->field);
+			return -EINVAL;
+		}
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return v4l2_m2m_streamon(file, ctx->m2m_ctx, type);
+}
+
+static int vidioc_streamoff(struct file *file, void *priv,
+			    enum v4l2_buf_type type)
+{
+	struct deinterlace_ctx *ctx = priv;
+
+	return v4l2_m2m_streamoff(file, ctx->m2m_ctx, type);
+}
+
+static const struct v4l2_ioctl_ops deinterlace_ioctl_ops = {
+	.vidioc_querycap	= vidioc_querycap,
+
+	.vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap	= vidioc_g_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap	= vidioc_try_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap	= vidioc_s_fmt_vid_cap,
+
+	.vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out,
+	.vidioc_g_fmt_vid_out	= vidioc_g_fmt_vid_out,
+	.vidioc_try_fmt_vid_out	= vidioc_try_fmt_vid_out,
+	.vidioc_s_fmt_vid_out	= vidioc_s_fmt_vid_out,
+
+	.vidioc_reqbufs		= vidioc_reqbufs,
+	.vidioc_querybuf	= vidioc_querybuf,
+
+	.vidioc_qbuf		= vidioc_qbuf,
+	.vidioc_dqbuf		= vidioc_dqbuf,
+
+	.vidioc_streamon	= vidioc_streamon,
+	.vidioc_streamoff	= vidioc_streamoff,
+};
+
+
+/*
+ * Queue operations
+ */
+struct vb2_dc_conf {
+	struct device           *dev;
+};
+
+static int deinterlace_queue_setup(struct vb2_queue *vq,
+				const void *parg,
+				unsigned int *nbuffers, unsigned int *nplanes,
+				unsigned int sizes[], void *alloc_ctxs[])
+{
+	struct deinterlace_ctx *ctx = vb2_get_drv_priv(vq);
+	struct deinterlace_q_data *q_data;
+	unsigned int size, count = *nbuffers;
+
+	q_data = get_q_data(vq->type);
+
+	switch (q_data->fmt->fourcc) {
+	case V4L2_PIX_FMT_YUV420:
+		size = q_data->width * q_data->height * 3 / 2;
+		break;
+	case V4L2_PIX_FMT_YUYV:
+	default:
+		size = q_data->width * q_data->height * 2;
+	}
+
+	*nplanes = 1;
+	*nbuffers = count;
+	sizes[0] = size;
+
+	alloc_ctxs[0] = ctx->dev->alloc_ctx;
+
+	dprintk(ctx->dev, "get %d buffer(s) of size %d each.\n", count, size);
+
+	return 0;
+}
+
+static int deinterlace_buf_prepare(struct vb2_buffer *vb)
+{
+	struct deinterlace_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct deinterlace_q_data *q_data;
+
+	dprintk(ctx->dev, "type: %d\n", vb->vb2_queue->type);
+
+	q_data = get_q_data(vb->vb2_queue->type);
+
+	if (vb2_plane_size(vb, 0) < q_data->sizeimage) {
+		dprintk(ctx->dev, "%s data will not fit into plane (%lu < %lu)\n",
+			__func__, vb2_plane_size(vb, 0), (long)q_data->sizeimage);
+		return -EINVAL;
+	}
+
+	vb2_set_plane_payload(vb, 0, q_data->sizeimage);
+
+	return 0;
+}
+
+static void deinterlace_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct deinterlace_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+
+	v4l2_m2m_buf_queue(ctx->m2m_ctx, vbuf);
+}
+
+static struct vb2_ops deinterlace_qops = {
+	.queue_setup	 = deinterlace_queue_setup,
+	.buf_prepare	 = deinterlace_buf_prepare,
+	.buf_queue	 = deinterlace_buf_queue,
+};
+
+static int queue_init(void *priv, struct vb2_queue *src_vq,
+		      struct vb2_queue *dst_vq)
+{
+	struct deinterlace_ctx *ctx = priv;
+	int ret;
+
+	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	src_vq->io_modes = VB2_MMAP | VB2_USERPTR;
+	src_vq->drv_priv = ctx;
+	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	src_vq->ops = &deinterlace_qops;
+	src_vq->mem_ops = &vb2_dma_contig_memops;
+	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	q_data[V4L2_M2M_SRC].fmt = &formats[0];
+	q_data[V4L2_M2M_SRC].width = 640;
+	q_data[V4L2_M2M_SRC].height = 480;
+	q_data[V4L2_M2M_SRC].sizeimage = (640 * 480 * 3) / 2;
+	q_data[V4L2_M2M_SRC].field = V4L2_FIELD_SEQ_TB;
+
+	ret = vb2_queue_init(src_vq);
+	if (ret)
+		return ret;
+
+	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	dst_vq->io_modes = VB2_MMAP | VB2_USERPTR;
+	dst_vq->drv_priv = ctx;
+	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	dst_vq->ops = &deinterlace_qops;
+	dst_vq->mem_ops = &vb2_dma_contig_memops;
+	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	q_data[V4L2_M2M_DST].fmt = &formats[0];
+	q_data[V4L2_M2M_DST].width = 640;
+	q_data[V4L2_M2M_DST].height = 480;
+	q_data[V4L2_M2M_DST].sizeimage = (640 * 480 * 3) / 2;
+	q_data[V4L2_M2M_SRC].field = V4L2_FIELD_INTERLACED_TB;
+
+	return vb2_queue_init(dst_vq);
+}
+
+/*
+ * File operations
+ */
+static int deinterlace_open(struct file *file)
+{
+	struct deinterlace_dev *pcdev = video_drvdata(file);
+	struct deinterlace_ctx *ctx = NULL;
+
+	ctx = kzalloc(sizeof *ctx, GFP_KERNEL);
+	if (!ctx)
+		return -ENOMEM;
+
+	file->private_data = ctx;
+	ctx->dev = pcdev;
+
+	ctx->m2m_ctx = v4l2_m2m_ctx_init(pcdev->m2m_dev, ctx, &queue_init);
+	if (IS_ERR(ctx->m2m_ctx)) {
+		int ret = PTR_ERR(ctx->m2m_ctx);
+
+		kfree(ctx);
+		return ret;
+	}
+
+	ctx->xt = kzalloc(sizeof(struct dma_interleaved_template) +
+				sizeof(struct data_chunk), GFP_KERNEL);
+	if (!ctx->xt) {
+		kfree(ctx);
+		return -ENOMEM;
+	}
+
+	ctx->colorspace = V4L2_COLORSPACE_REC709;
+
+	dprintk(pcdev, "Created instance %p, m2m_ctx: %p\n", ctx, ctx->m2m_ctx);
+
+	return 0;
+}
+
+static int deinterlace_release(struct file *file)
+{
+	struct deinterlace_dev *pcdev = video_drvdata(file);
+	struct deinterlace_ctx *ctx = file->private_data;
+
+	dprintk(pcdev, "Releasing instance %p\n", ctx);
+
+	v4l2_m2m_ctx_release(ctx->m2m_ctx);
+	kfree(ctx->xt);
+	kfree(ctx);
+
+	return 0;
+}
+
+static unsigned int deinterlace_poll(struct file *file,
+				 struct poll_table_struct *wait)
+{
+	struct deinterlace_ctx *ctx = file->private_data;
+	int ret;
+
+	deinterlace_lock(ctx);
+	ret = v4l2_m2m_poll(file, ctx->m2m_ctx, wait);
+	deinterlace_unlock(ctx);
+
+	return ret;
+}
+
+static int deinterlace_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct deinterlace_ctx *ctx = file->private_data;
+
+	return v4l2_m2m_mmap(file, ctx->m2m_ctx, vma);
+}
+
+static const struct v4l2_file_operations deinterlace_fops = {
+	.owner		= THIS_MODULE,
+	.open		= deinterlace_open,
+	.release	= deinterlace_release,
+	.poll		= deinterlace_poll,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap		= deinterlace_mmap,
+};
+
+static struct video_device deinterlace_videodev = {
+	.name		= MEM2MEM_NAME,
+	.fops		= &deinterlace_fops,
+	.ioctl_ops	= &deinterlace_ioctl_ops,
+	.minor		= -1,
+	.release	= video_device_release_empty,
+	.vfl_dir	= VFL_DIR_M2M,
+};
+
+static struct v4l2_m2m_ops m2m_ops = {
+	.device_run	= deinterlace_device_run,
+	.job_ready	= deinterlace_job_ready,
+	.job_abort	= deinterlace_job_abort,
+	.lock		= deinterlace_lock,
+	.unlock		= deinterlace_unlock,
+};
+
+static int deinterlace_probe(struct platform_device *pdev)
+{
+	struct deinterlace_dev *pcdev;
+	struct video_device *vfd;
+	dma_cap_mask_t mask;
+	int ret = 0;
+
+	pcdev = devm_kzalloc(&pdev->dev, sizeof(*pcdev), GFP_KERNEL);
+	if (!pcdev)
+		return -ENOMEM;
+
+	spin_lock_init(&pcdev->irqlock);
+
+	dma_cap_zero(mask);
+	dma_cap_set(DMA_INTERLEAVE, mask);
+	pcdev->dma_chan = dma_request_channel(mask, NULL, pcdev);
+	if (!pcdev->dma_chan)
+		return -ENODEV;
+
+	if (!dma_has_cap(DMA_INTERLEAVE, pcdev->dma_chan->device->cap_mask)) {
+		v4l2_err(&pcdev->v4l2_dev, "DMA does not support INTERLEAVE\n");
+		goto rel_dma;
+	}
+
+	ret = v4l2_device_register(&pdev->dev, &pcdev->v4l2_dev);
+	if (ret)
+		goto rel_dma;
+
+	atomic_set(&pcdev->busy, 0);
+	mutex_init(&pcdev->dev_mutex);
+
+	vfd = &pcdev->vfd;
+	*vfd = deinterlace_videodev;
+	vfd->lock = &pcdev->dev_mutex;
+	vfd->v4l2_dev = &pcdev->v4l2_dev;
+
+	ret = video_register_device(vfd, VFL_TYPE_GRABBER, 0);
+	if (ret) {
+		v4l2_err(&pcdev->v4l2_dev, "Failed to register video device\n");
+		goto unreg_dev;
+	}
+
+	video_set_drvdata(vfd, pcdev);
+	snprintf(vfd->name, sizeof(vfd->name), "%s", deinterlace_videodev.name);
+	v4l2_info(&pcdev->v4l2_dev, MEM2MEM_TEST_MODULE_NAME
+			" Device registered as /dev/video%d\n", vfd->num);
+
+	platform_set_drvdata(pdev, pcdev);
+
+	pcdev->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
+	if (IS_ERR(pcdev->alloc_ctx)) {
+		v4l2_err(&pcdev->v4l2_dev, "Failed to alloc vb2 context\n");
+		ret = PTR_ERR(pcdev->alloc_ctx);
+		goto err_ctx;
+	}
+
+	pcdev->m2m_dev = v4l2_m2m_init(&m2m_ops);
+	if (IS_ERR(pcdev->m2m_dev)) {
+		v4l2_err(&pcdev->v4l2_dev, "Failed to init mem2mem device\n");
+		ret = PTR_ERR(pcdev->m2m_dev);
+		goto err_m2m;
+	}
+
+	return 0;
+
+err_m2m:
+	video_unregister_device(&pcdev->vfd);
+err_ctx:
+	vb2_dma_contig_cleanup_ctx(pcdev->alloc_ctx);
+unreg_dev:
+	v4l2_device_unregister(&pcdev->v4l2_dev);
+rel_dma:
+	dma_release_channel(pcdev->dma_chan);
+
+	return ret;
+}
+
+static int deinterlace_remove(struct platform_device *pdev)
+{
+	struct deinterlace_dev *pcdev = platform_get_drvdata(pdev);
+
+	v4l2_info(&pcdev->v4l2_dev, "Removing " MEM2MEM_TEST_MODULE_NAME);
+	v4l2_m2m_release(pcdev->m2m_dev);
+	video_unregister_device(&pcdev->vfd);
+	v4l2_device_unregister(&pcdev->v4l2_dev);
+	vb2_dma_contig_cleanup_ctx(pcdev->alloc_ctx);
+	dma_release_channel(pcdev->dma_chan);
+
+	return 0;
+}
+
+static struct platform_driver deinterlace_pdrv = {
+	.probe		= deinterlace_probe,
+	.remove		= deinterlace_remove,
+	.driver		= {
+		.name	= MEM2MEM_NAME,
+	},
+};
+module_platform_driver(deinterlace_pdrv);
+
diff --git a/drivers/media/platform/marvell-ccic/Kconfig b/drivers/media/platform/marvell-ccic/Kconfig
new file mode 100644
index 0000000..4bf5bd1
--- /dev/null
+++ b/drivers/media/platform/marvell-ccic/Kconfig
@@ -0,0 +1,26 @@
+config VIDEO_CAFE_CCIC
+	tristate "Marvell 88ALP01 (Cafe) CMOS Camera Controller support"
+	depends on PCI && I2C && VIDEO_V4L2
+	depends on HAS_DMA
+	select VIDEO_OV7670
+	select VIDEOBUF2_VMALLOC
+	select VIDEOBUF2_DMA_CONTIG
+	select VIDEOBUF2_DMA_SG
+	---help---
+	  This is a video4linux2 driver for the Marvell 88ALP01 integrated
+	  CMOS camera controller.  This is the controller found on first-
+	  generation OLPC systems.
+
+config VIDEO_MMP_CAMERA
+	tristate "Marvell Armada 610 integrated camera controller support"
+	depends on ARCH_MMP && I2C && VIDEO_V4L2
+	depends on HAS_DMA && BROKEN
+	select VIDEO_OV7670
+	select I2C_GPIO
+	select VIDEOBUF2_DMA_SG
+	---help---
+	  This is a Video4Linux2 driver for the integrated camera
+	  controller found on Marvell Armada 610 application
+	  processors (and likely beyond).  This is the controller found
+	  in OLPC XO 1.75 systems.
+
diff --git a/drivers/media/platform/marvell-ccic/Makefile b/drivers/media/platform/marvell-ccic/Makefile
new file mode 100644
index 0000000..05a792c
--- /dev/null
+++ b/drivers/media/platform/marvell-ccic/Makefile
@@ -0,0 +1,6 @@
+obj-$(CONFIG_VIDEO_CAFE_CCIC) += cafe_ccic.o
+cafe_ccic-y := cafe-driver.o mcam-core.o
+
+obj-$(CONFIG_VIDEO_MMP_CAMERA) += mmp_camera.o
+mmp_camera-y := mmp-driver.o mcam-core.o
+
diff --git a/drivers/media/platform/marvell-ccic/cafe-driver.c b/drivers/media/platform/marvell-ccic/cafe-driver.c
new file mode 100644
index 0000000..77890bd
--- /dev/null
+++ b/drivers/media/platform/marvell-ccic/cafe-driver.c
@@ -0,0 +1,661 @@
+/*
+ * A driver for the CMOS camera controller in the Marvell 88ALP01 "cafe"
+ * multifunction chip.  Currently works with the Omnivision OV7670
+ * sensor.
+ *
+ * The data sheet for this device can be found at:
+ *    http://www.marvell.com/products/pc_connectivity/88alp01/
+ *
+ * Copyright 2006-11 One Laptop Per Child Association, Inc.
+ * Copyright 2006-11 Jonathan Corbet <corbet@lwn.net>
+ *
+ * Written by Jonathan Corbet, corbet@lwn.net.
+ *
+ * v4l2_device/v4l2_subdev conversion by:
+ * Copyright (C) 2009 Hans Verkuil <hverkuil@xs4all.nl>
+ *
+ * This file may be distributed under the terms of the GNU General
+ * Public License, version 2.
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/pci.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <linux/device.h>
+#include <linux/wait.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+
+#include "mcam-core.h"
+
+#define CAFE_VERSION 0x000002
+
+
+/*
+ * Parameters.
+ */
+MODULE_AUTHOR("Jonathan Corbet <corbet@lwn.net>");
+MODULE_DESCRIPTION("Marvell 88ALP01 CMOS Camera Controller driver");
+MODULE_LICENSE("GPL");
+MODULE_SUPPORTED_DEVICE("Video");
+
+
+
+
+struct cafe_camera {
+	int registered;			/* Fully initialized? */
+	struct mcam_camera mcam;
+	struct pci_dev *pdev;
+	wait_queue_head_t smbus_wait;	/* Waiting on i2c events */
+};
+
+/*
+ * Most of the camera controller registers are defined in mcam-core.h,
+ * but the Cafe platform has some additional registers of its own;
+ * they are described here.
+ */
+
+/*
+ * "General purpose register" has a couple of GPIOs used for sensor
+ * power and reset on OLPC XO 1.0 systems.
+ */
+#define REG_GPR		0xb4
+#define	  GPR_C1EN	  0x00000020	/* Pad 1 (power down) enable */
+#define	  GPR_C0EN	  0x00000010	/* Pad 0 (reset) enable */
+#define	  GPR_C1	  0x00000002	/* Control 1 value */
+/*
+ * Control 0 is wired to reset on OLPC machines.  For ov7x sensors,
+ * it is active low.
+ */
+#define	  GPR_C0	  0x00000001	/* Control 0 value */
+
+/*
+ * These registers control the SMBUS module for communicating
+ * with the sensor.
+ */
+#define REG_TWSIC0	0xb8	/* TWSI (smbus) control 0 */
+#define	  TWSIC0_EN	  0x00000001	/* TWSI enable */
+#define	  TWSIC0_MODE	  0x00000002	/* 1 = 16-bit, 0 = 8-bit */
+#define	  TWSIC0_SID	  0x000003fc	/* Slave ID */
+/*
+ * Subtle trickery: the slave ID field starts with bit 2.  But the
+ * Linux i2c stack wants to treat the bottommost bit as a separate
+ * read/write bit, which is why slave ID's are usually presented
+ * >>1.  For consistency with that behavior, we shift over three
+ * bits instead of two.
+ */
+#define	  TWSIC0_SID_SHIFT 3
+#define	  TWSIC0_CLKDIV	  0x0007fc00	/* Clock divider */
+#define	  TWSIC0_MASKACK  0x00400000	/* Mask ack from sensor */
+#define	  TWSIC0_OVMAGIC  0x00800000	/* Make it work on OV sensors */
+
+#define REG_TWSIC1	0xbc	/* TWSI control 1 */
+#define	  TWSIC1_DATA	  0x0000ffff	/* Data to/from camchip */
+#define	  TWSIC1_ADDR	  0x00ff0000	/* Address (register) */
+#define	  TWSIC1_ADDR_SHIFT 16
+#define	  TWSIC1_READ	  0x01000000	/* Set for read op */
+#define	  TWSIC1_WSTAT	  0x02000000	/* Write status */
+#define	  TWSIC1_RVALID	  0x04000000	/* Read data valid */
+#define	  TWSIC1_ERROR	  0x08000000	/* Something screwed up */
+
+/*
+ * Here's the weird global control registers
+ */
+#define REG_GL_CSR     0x3004  /* Control/status register */
+#define	  GCSR_SRS	 0x00000001	/* SW Reset set */
+#define	  GCSR_SRC	 0x00000002	/* SW Reset clear */
+#define	  GCSR_MRS	 0x00000004	/* Master reset set */
+#define	  GCSR_MRC	 0x00000008	/* HW Reset clear */
+#define	  GCSR_CCIC_EN	 0x00004000    /* CCIC Clock enable */
+#define REG_GL_IMASK   0x300c  /* Interrupt mask register */
+#define	  GIMSK_CCIC_EN		 0x00000004    /* CCIC Interrupt enable */
+
+#define REG_GL_FCR	0x3038	/* GPIO functional control register */
+#define	  GFCR_GPIO_ON	  0x08		/* Camera GPIO enabled */
+#define REG_GL_GPIOR	0x315c	/* GPIO register */
+#define	  GGPIO_OUT		0x80000	/* GPIO output */
+#define	  GGPIO_VAL		0x00008	/* Output pin value */
+
+#define REG_LEN		       (REG_GL_IMASK + 4)
+
+
+/*
+ * Debugging and related.
+ */
+#define cam_err(cam, fmt, arg...) \
+	dev_err(&(cam)->pdev->dev, fmt, ##arg);
+#define cam_warn(cam, fmt, arg...) \
+	dev_warn(&(cam)->pdev->dev, fmt, ##arg);
+
+/* -------------------------------------------------------------------- */
+/*
+ * The I2C/SMBUS interface to the camera itself starts here.  The
+ * controller handles SMBUS itself, presenting a relatively simple register
+ * interface; all we have to do is to tell it where to route the data.
+ */
+#define CAFE_SMBUS_TIMEOUT (HZ)  /* generous */
+
+static inline struct cafe_camera *to_cam(struct v4l2_device *dev)
+{
+	struct mcam_camera *m = container_of(dev, struct mcam_camera, v4l2_dev);
+	return container_of(m, struct cafe_camera, mcam);
+}
+
+
+static int cafe_smbus_write_done(struct mcam_camera *mcam)
+{
+	unsigned long flags;
+	int c1;
+
+	/*
+	 * We must delay after the interrupt, or the controller gets confused
+	 * and never does give us good status.  Fortunately, we don't do this
+	 * often.
+	 */
+	udelay(20);
+	spin_lock_irqsave(&mcam->dev_lock, flags);
+	c1 = mcam_reg_read(mcam, REG_TWSIC1);
+	spin_unlock_irqrestore(&mcam->dev_lock, flags);
+	return (c1 & (TWSIC1_WSTAT|TWSIC1_ERROR)) != TWSIC1_WSTAT;
+}
+
+static int cafe_smbus_write_data(struct cafe_camera *cam,
+		u16 addr, u8 command, u8 value)
+{
+	unsigned int rval;
+	unsigned long flags;
+	struct mcam_camera *mcam = &cam->mcam;
+
+	spin_lock_irqsave(&mcam->dev_lock, flags);
+	rval = TWSIC0_EN | ((addr << TWSIC0_SID_SHIFT) & TWSIC0_SID);
+	rval |= TWSIC0_OVMAGIC;  /* Make OV sensors work */
+	/*
+	 * Marvell sez set clkdiv to all 1's for now.
+	 */
+	rval |= TWSIC0_CLKDIV;
+	mcam_reg_write(mcam, REG_TWSIC0, rval);
+	(void) mcam_reg_read(mcam, REG_TWSIC1); /* force write */
+	rval = value | ((command << TWSIC1_ADDR_SHIFT) & TWSIC1_ADDR);
+	mcam_reg_write(mcam, REG_TWSIC1, rval);
+	spin_unlock_irqrestore(&mcam->dev_lock, flags);
+
+	/* Unfortunately, reading TWSIC1 too soon after sending a command
+	 * causes the device to die.
+	 * Use a busy-wait because we often send a large quantity of small
+	 * commands at-once; using msleep() would cause a lot of context
+	 * switches which take longer than 2ms, resulting in a noticeable
+	 * boot-time and capture-start delays.
+	 */
+	mdelay(2);
+
+	/*
+	 * Another sad fact is that sometimes, commands silently complete but
+	 * cafe_smbus_write_done() never becomes aware of this.
+	 * This happens at random and appears to possible occur with any
+	 * command.
+	 * We don't understand why this is. We work around this issue
+	 * with the timeout in the wait below, assuming that all commands
+	 * complete within the timeout.
+	 */
+	wait_event_timeout(cam->smbus_wait, cafe_smbus_write_done(mcam),
+			CAFE_SMBUS_TIMEOUT);
+
+	spin_lock_irqsave(&mcam->dev_lock, flags);
+	rval = mcam_reg_read(mcam, REG_TWSIC1);
+	spin_unlock_irqrestore(&mcam->dev_lock, flags);
+
+	if (rval & TWSIC1_WSTAT) {
+		cam_err(cam, "SMBUS write (%02x/%02x/%02x) timed out\n", addr,
+				command, value);
+		return -EIO;
+	}
+	if (rval & TWSIC1_ERROR) {
+		cam_err(cam, "SMBUS write (%02x/%02x/%02x) error\n", addr,
+				command, value);
+		return -EIO;
+	}
+	return 0;
+}
+
+
+
+static int cafe_smbus_read_done(struct mcam_camera *mcam)
+{
+	unsigned long flags;
+	int c1;
+
+	/*
+	 * We must delay after the interrupt, or the controller gets confused
+	 * and never does give us good status.  Fortunately, we don't do this
+	 * often.
+	 */
+	udelay(20);
+	spin_lock_irqsave(&mcam->dev_lock, flags);
+	c1 = mcam_reg_read(mcam, REG_TWSIC1);
+	spin_unlock_irqrestore(&mcam->dev_lock, flags);
+	return c1 & (TWSIC1_RVALID|TWSIC1_ERROR);
+}
+
+
+
+static int cafe_smbus_read_data(struct cafe_camera *cam,
+		u16 addr, u8 command, u8 *value)
+{
+	unsigned int rval;
+	unsigned long flags;
+	struct mcam_camera *mcam = &cam->mcam;
+
+	spin_lock_irqsave(&mcam->dev_lock, flags);
+	rval = TWSIC0_EN | ((addr << TWSIC0_SID_SHIFT) & TWSIC0_SID);
+	rval |= TWSIC0_OVMAGIC; /* Make OV sensors work */
+	/*
+	 * Marvel sez set clkdiv to all 1's for now.
+	 */
+	rval |= TWSIC0_CLKDIV;
+	mcam_reg_write(mcam, REG_TWSIC0, rval);
+	(void) mcam_reg_read(mcam, REG_TWSIC1); /* force write */
+	rval = TWSIC1_READ | ((command << TWSIC1_ADDR_SHIFT) & TWSIC1_ADDR);
+	mcam_reg_write(mcam, REG_TWSIC1, rval);
+	spin_unlock_irqrestore(&mcam->dev_lock, flags);
+
+	wait_event_timeout(cam->smbus_wait,
+			cafe_smbus_read_done(mcam), CAFE_SMBUS_TIMEOUT);
+	spin_lock_irqsave(&mcam->dev_lock, flags);
+	rval = mcam_reg_read(mcam, REG_TWSIC1);
+	spin_unlock_irqrestore(&mcam->dev_lock, flags);
+
+	if (rval & TWSIC1_ERROR) {
+		cam_err(cam, "SMBUS read (%02x/%02x) error\n", addr, command);
+		return -EIO;
+	}
+	if (!(rval & TWSIC1_RVALID)) {
+		cam_err(cam, "SMBUS read (%02x/%02x) timed out\n", addr,
+				command);
+		return -EIO;
+	}
+	*value = rval & 0xff;
+	return 0;
+}
+
+/*
+ * Perform a transfer over SMBUS.  This thing is called under
+ * the i2c bus lock, so we shouldn't race with ourselves...
+ */
+static int cafe_smbus_xfer(struct i2c_adapter *adapter, u16 addr,
+		unsigned short flags, char rw, u8 command,
+		int size, union i2c_smbus_data *data)
+{
+	struct cafe_camera *cam = i2c_get_adapdata(adapter);
+	int ret = -EINVAL;
+
+	/*
+	 * This interface would appear to only do byte data ops.  OK
+	 * it can do word too, but the cam chip has no use for that.
+	 */
+	if (size != I2C_SMBUS_BYTE_DATA) {
+		cam_err(cam, "funky xfer size %d\n", size);
+		return -EINVAL;
+	}
+
+	if (rw == I2C_SMBUS_WRITE)
+		ret = cafe_smbus_write_data(cam, addr, command, data->byte);
+	else if (rw == I2C_SMBUS_READ)
+		ret = cafe_smbus_read_data(cam, addr, command, &data->byte);
+	return ret;
+}
+
+
+static void cafe_smbus_enable_irq(struct cafe_camera *cam)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&cam->mcam.dev_lock, flags);
+	mcam_reg_set_bit(&cam->mcam, REG_IRQMASK, TWSIIRQS);
+	spin_unlock_irqrestore(&cam->mcam.dev_lock, flags);
+}
+
+static u32 cafe_smbus_func(struct i2c_adapter *adapter)
+{
+	return I2C_FUNC_SMBUS_READ_BYTE_DATA  |
+	       I2C_FUNC_SMBUS_WRITE_BYTE_DATA;
+}
+
+static struct i2c_algorithm cafe_smbus_algo = {
+	.smbus_xfer = cafe_smbus_xfer,
+	.functionality = cafe_smbus_func
+};
+
+static int cafe_smbus_setup(struct cafe_camera *cam)
+{
+	struct i2c_adapter *adap;
+	int ret;
+
+	adap = kzalloc(sizeof(*adap), GFP_KERNEL);
+	if (adap == NULL)
+		return -ENOMEM;
+	adap->owner = THIS_MODULE;
+	adap->algo = &cafe_smbus_algo;
+	strcpy(adap->name, "cafe_ccic");
+	adap->dev.parent = &cam->pdev->dev;
+	i2c_set_adapdata(adap, cam);
+	ret = i2c_add_adapter(adap);
+	if (ret) {
+		printk(KERN_ERR "Unable to register cafe i2c adapter\n");
+		kfree(adap);
+		return ret;
+	}
+
+	cam->mcam.i2c_adapter = adap;
+	cafe_smbus_enable_irq(cam);
+	return 0;
+}
+
+static void cafe_smbus_shutdown(struct cafe_camera *cam)
+{
+	i2c_del_adapter(cam->mcam.i2c_adapter);
+	kfree(cam->mcam.i2c_adapter);
+}
+
+
+/*
+ * Controller-level stuff
+ */
+
+static void cafe_ctlr_init(struct mcam_camera *mcam)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&mcam->dev_lock, flags);
+	/*
+	 * Added magic to bring up the hardware on the B-Test board
+	 */
+	mcam_reg_write(mcam, 0x3038, 0x8);
+	mcam_reg_write(mcam, 0x315c, 0x80008);
+	/*
+	 * Go through the dance needed to wake the device up.
+	 * Note that these registers are global and shared
+	 * with the NAND and SD devices.  Interaction between the
+	 * three still needs to be examined.
+	 */
+	mcam_reg_write(mcam, REG_GL_CSR, GCSR_SRS|GCSR_MRS); /* Needed? */
+	mcam_reg_write(mcam, REG_GL_CSR, GCSR_SRC|GCSR_MRC);
+	mcam_reg_write(mcam, REG_GL_CSR, GCSR_SRC|GCSR_MRS);
+	/*
+	 * Here we must wait a bit for the controller to come around.
+	 */
+	spin_unlock_irqrestore(&mcam->dev_lock, flags);
+	msleep(5);
+	spin_lock_irqsave(&mcam->dev_lock, flags);
+
+	mcam_reg_write(mcam, REG_GL_CSR, GCSR_CCIC_EN|GCSR_SRC|GCSR_MRC);
+	mcam_reg_set_bit(mcam, REG_GL_IMASK, GIMSK_CCIC_EN);
+	/*
+	 * Mask all interrupts.
+	 */
+	mcam_reg_write(mcam, REG_IRQMASK, 0);
+	spin_unlock_irqrestore(&mcam->dev_lock, flags);
+}
+
+
+static int cafe_ctlr_power_up(struct mcam_camera *mcam)
+{
+	/*
+	 * Part one of the sensor dance: turn the global
+	 * GPIO signal on.
+	 */
+	mcam_reg_write(mcam, REG_GL_FCR, GFCR_GPIO_ON);
+	mcam_reg_write(mcam, REG_GL_GPIOR, GGPIO_OUT|GGPIO_VAL);
+	/*
+	 * Put the sensor into operational mode (assumes OLPC-style
+	 * wiring).  Control 0 is reset - set to 1 to operate.
+	 * Control 1 is power down, set to 0 to operate.
+	 */
+	mcam_reg_write(mcam, REG_GPR, GPR_C1EN|GPR_C0EN); /* pwr up, reset */
+	mcam_reg_write(mcam, REG_GPR, GPR_C1EN|GPR_C0EN|GPR_C0);
+
+	return 0;
+}
+
+static void cafe_ctlr_power_down(struct mcam_camera *mcam)
+{
+	mcam_reg_write(mcam, REG_GPR, GPR_C1EN|GPR_C0EN|GPR_C1);
+	mcam_reg_write(mcam, REG_GL_FCR, GFCR_GPIO_ON);
+	mcam_reg_write(mcam, REG_GL_GPIOR, GGPIO_OUT);
+}
+
+
+
+/*
+ * The platform interrupt handler.
+ */
+static irqreturn_t cafe_irq(int irq, void *data)
+{
+	struct cafe_camera *cam = data;
+	struct mcam_camera *mcam = &cam->mcam;
+	unsigned int irqs, handled;
+
+	spin_lock(&mcam->dev_lock);
+	irqs = mcam_reg_read(mcam, REG_IRQSTAT);
+	handled = cam->registered && mccic_irq(mcam, irqs);
+	if (irqs & TWSIIRQS) {
+		mcam_reg_write(mcam, REG_IRQSTAT, TWSIIRQS);
+		wake_up(&cam->smbus_wait);
+		handled = 1;
+	}
+	spin_unlock(&mcam->dev_lock);
+	return IRQ_RETVAL(handled);
+}
+
+
+/* -------------------------------------------------------------------------- */
+/*
+ * PCI interface stuff.
+ */
+
+static int cafe_pci_probe(struct pci_dev *pdev,
+		const struct pci_device_id *id)
+{
+	int ret;
+	struct cafe_camera *cam;
+	struct mcam_camera *mcam;
+
+	/*
+	 * Start putting together one of our big camera structures.
+	 */
+	ret = -ENOMEM;
+	cam = kzalloc(sizeof(struct cafe_camera), GFP_KERNEL);
+	if (cam == NULL)
+		goto out;
+	cam->pdev = pdev;
+	mcam = &cam->mcam;
+	mcam->chip_id = MCAM_CAFE;
+	spin_lock_init(&mcam->dev_lock);
+	init_waitqueue_head(&cam->smbus_wait);
+	mcam->plat_power_up = cafe_ctlr_power_up;
+	mcam->plat_power_down = cafe_ctlr_power_down;
+	mcam->dev = &pdev->dev;
+	snprintf(mcam->bus_info, sizeof(mcam->bus_info), "PCI:%s", pci_name(pdev));
+	/*
+	 * Set the clock speed for the XO 1; I don't believe this
+	 * driver has ever run anywhere else.
+	 */
+	mcam->clock_speed = 45;
+	mcam->use_smbus = 1;
+	/*
+	 * Vmalloc mode for buffers is traditional with this driver.
+	 * We *might* be able to run DMA_contig, especially on a system
+	 * with CMA in it.
+	 */
+	mcam->buffer_mode = B_vmalloc;
+	/*
+	 * Get set up on the PCI bus.
+	 */
+	ret = pci_enable_device(pdev);
+	if (ret)
+		goto out_free;
+	pci_set_master(pdev);
+
+	ret = -EIO;
+	mcam->regs = pci_iomap(pdev, 0, 0);
+	if (!mcam->regs) {
+		printk(KERN_ERR "Unable to ioremap cafe-ccic regs\n");
+		goto out_disable;
+	}
+	mcam->regs_size = pci_resource_len(pdev, 0);
+	ret = request_irq(pdev->irq, cafe_irq, IRQF_SHARED, "cafe-ccic", cam);
+	if (ret)
+		goto out_iounmap;
+
+	/*
+	 * Initialize the controller and leave it powered up.  It will
+	 * stay that way until the sensor driver shows up.
+	 */
+	cafe_ctlr_init(mcam);
+	cafe_ctlr_power_up(mcam);
+	/*
+	 * Set up I2C/SMBUS communications.  We have to drop the mutex here
+	 * because the sensor could attach in this call chain, leading to
+	 * unsightly deadlocks.
+	 */
+	ret = cafe_smbus_setup(cam);
+	if (ret)
+		goto out_pdown;
+
+	ret = mccic_register(mcam);
+	if (ret == 0) {
+		cam->registered = 1;
+		return 0;
+	}
+
+	cafe_smbus_shutdown(cam);
+out_pdown:
+	cafe_ctlr_power_down(mcam);
+	free_irq(pdev->irq, cam);
+out_iounmap:
+	pci_iounmap(pdev, mcam->regs);
+out_disable:
+	pci_disable_device(pdev);
+out_free:
+	kfree(cam);
+out:
+	return ret;
+}
+
+
+/*
+ * Shut down an initialized device
+ */
+static void cafe_shutdown(struct cafe_camera *cam)
+{
+	mccic_shutdown(&cam->mcam);
+	cafe_smbus_shutdown(cam);
+	free_irq(cam->pdev->irq, cam);
+	pci_iounmap(cam->pdev, cam->mcam.regs);
+}
+
+
+static void cafe_pci_remove(struct pci_dev *pdev)
+{
+	struct v4l2_device *v4l2_dev = dev_get_drvdata(&pdev->dev);
+	struct cafe_camera *cam = to_cam(v4l2_dev);
+
+	if (cam == NULL) {
+		printk(KERN_WARNING "pci_remove on unknown pdev %p\n", pdev);
+		return;
+	}
+	cafe_shutdown(cam);
+	kfree(cam);
+}
+
+
+#ifdef CONFIG_PM
+/*
+ * Basic power management.
+ */
+static int cafe_pci_suspend(struct pci_dev *pdev, pm_message_t state)
+{
+	struct v4l2_device *v4l2_dev = dev_get_drvdata(&pdev->dev);
+	struct cafe_camera *cam = to_cam(v4l2_dev);
+	int ret;
+
+	ret = pci_save_state(pdev);
+	if (ret)
+		return ret;
+	mccic_suspend(&cam->mcam);
+	pci_disable_device(pdev);
+	return 0;
+}
+
+
+static int cafe_pci_resume(struct pci_dev *pdev)
+{
+	struct v4l2_device *v4l2_dev = dev_get_drvdata(&pdev->dev);
+	struct cafe_camera *cam = to_cam(v4l2_dev);
+	int ret = 0;
+
+	pci_restore_state(pdev);
+	ret = pci_enable_device(pdev);
+
+	if (ret) {
+		cam_warn(cam, "Unable to re-enable device on resume!\n");
+		return ret;
+	}
+	cafe_ctlr_init(&cam->mcam);
+	return mccic_resume(&cam->mcam);
+}
+
+#endif  /* CONFIG_PM */
+
+static struct pci_device_id cafe_ids[] = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_MARVELL,
+		     PCI_DEVICE_ID_MARVELL_88ALP01_CCIC) },
+	{ 0, }
+};
+
+MODULE_DEVICE_TABLE(pci, cafe_ids);
+
+static struct pci_driver cafe_pci_driver = {
+	.name = "cafe1000-ccic",
+	.id_table = cafe_ids,
+	.probe = cafe_pci_probe,
+	.remove = cafe_pci_remove,
+#ifdef CONFIG_PM
+	.suspend = cafe_pci_suspend,
+	.resume = cafe_pci_resume,
+#endif
+};
+
+
+
+
+static int __init cafe_init(void)
+{
+	int ret;
+
+	printk(KERN_NOTICE "Marvell M88ALP01 'CAFE' Camera Controller version %d\n",
+			CAFE_VERSION);
+	ret = pci_register_driver(&cafe_pci_driver);
+	if (ret) {
+		printk(KERN_ERR "Unable to register cafe_ccic driver\n");
+		goto out;
+	}
+	ret = 0;
+
+out:
+	return ret;
+}
+
+
+static void __exit cafe_exit(void)
+{
+	pci_unregister_driver(&cafe_pci_driver);
+}
+
+module_init(cafe_init);
+module_exit(cafe_exit);
diff --git a/drivers/media/platform/marvell-ccic/mcam-core.c b/drivers/media/platform/marvell-ccic/mcam-core.c
new file mode 100644
index 0000000..aa2b440
--- /dev/null
+++ b/drivers/media/platform/marvell-ccic/mcam-core.c
@@ -0,0 +1,1932 @@
+/*
+ * The Marvell camera core.  This device appears in a number of settings,
+ * so it needs platform-specific support outside of the core.
+ *
+ * Copyright 2011 Jonathan Corbet corbet@lwn.net
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/fs.h>
+#include <linux/mm.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/slab.h>
+#include <linux/device.h>
+#include <linux/wait.h>
+#include <linux/list.h>
+#include <linux/dma-mapping.h>
+#include <linux/delay.h>
+#include <linux/vmalloc.h>
+#include <linux/io.h>
+#include <linux/clk.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
+#include <media/ov7670.h>
+#include <media/videobuf2-vmalloc.h>
+#include <media/videobuf2-dma-contig.h>
+#include <media/videobuf2-dma-sg.h>
+
+#include "mcam-core.h"
+
+#ifdef MCAM_MODE_VMALLOC
+/*
+ * Internal DMA buffer management.  Since the controller cannot do S/G I/O,
+ * we must have physically contiguous buffers to bring frames into.
+ * These parameters control how many buffers we use, whether we
+ * allocate them at load time (better chance of success, but nails down
+ * memory) or when somebody tries to use the camera (riskier), and,
+ * for load-time allocation, how big they should be.
+ *
+ * The controller can cycle through three buffers.  We could use
+ * more by flipping pointers around, but it probably makes little
+ * sense.
+ */
+
+static bool alloc_bufs_at_read;
+module_param(alloc_bufs_at_read, bool, 0444);
+MODULE_PARM_DESC(alloc_bufs_at_read,
+		"Non-zero value causes DMA buffers to be allocated when the "
+		"video capture device is read, rather than at module load "
+		"time.  This saves memory, but decreases the chances of "
+		"successfully getting those buffers.  This parameter is "
+		"only used in the vmalloc buffer mode");
+
+static int n_dma_bufs = 3;
+module_param(n_dma_bufs, uint, 0644);
+MODULE_PARM_DESC(n_dma_bufs,
+		"The number of DMA buffers to allocate.  Can be either two "
+		"(saves memory, makes timing tighter) or three.");
+
+static int dma_buf_size = VGA_WIDTH * VGA_HEIGHT * 2;  /* Worst case */
+module_param(dma_buf_size, uint, 0444);
+MODULE_PARM_DESC(dma_buf_size,
+		"The size of the allocated DMA buffers.  If actual operating "
+		"parameters require larger buffers, an attempt to reallocate "
+		"will be made.");
+#else /* MCAM_MODE_VMALLOC */
+static const bool alloc_bufs_at_read;
+static const int n_dma_bufs = 3;  /* Used by S/G_PARM */
+#endif /* MCAM_MODE_VMALLOC */
+
+static bool flip;
+module_param(flip, bool, 0444);
+MODULE_PARM_DESC(flip,
+		"If set, the sensor will be instructed to flip the image "
+		"vertically.");
+
+static int buffer_mode = -1;
+module_param(buffer_mode, int, 0444);
+MODULE_PARM_DESC(buffer_mode,
+		"Set the buffer mode to be used; default is to go with what "
+		"the platform driver asks for.  Set to 0 for vmalloc, 1 for "
+		"DMA contiguous.");
+
+/*
+ * Status flags.  Always manipulated with bit operations.
+ */
+#define CF_BUF0_VALID	 0	/* Buffers valid - first three */
+#define CF_BUF1_VALID	 1
+#define CF_BUF2_VALID	 2
+#define CF_DMA_ACTIVE	 3	/* A frame is incoming */
+#define CF_CONFIG_NEEDED 4	/* Must configure hardware */
+#define CF_SINGLE_BUFFER 5	/* Running with a single buffer */
+#define CF_SG_RESTART	 6	/* SG restart needed */
+#define CF_FRAME_SOF0	 7	/* Frame 0 started */
+#define CF_FRAME_SOF1	 8
+#define CF_FRAME_SOF2	 9
+
+#define sensor_call(cam, o, f, args...) \
+	v4l2_subdev_call(cam->sensor, o, f, ##args)
+
+static struct mcam_format_struct {
+	__u8 *desc;
+	__u32 pixelformat;
+	int bpp;   /* Bytes per pixel */
+	bool planar;
+	u32 mbus_code;
+} mcam_formats[] = {
+	{
+		.desc		= "YUYV 4:2:2",
+		.pixelformat	= V4L2_PIX_FMT_YUYV,
+		.mbus_code	= MEDIA_BUS_FMT_YUYV8_2X8,
+		.bpp		= 2,
+		.planar		= false,
+	},
+	{
+		.desc		= "YVYU 4:2:2",
+		.pixelformat	= V4L2_PIX_FMT_YVYU,
+		.mbus_code	= MEDIA_BUS_FMT_YUYV8_2X8,
+		.bpp		= 2,
+		.planar		= false,
+	},
+	{
+		.desc		= "YUV 4:2:0 PLANAR",
+		.pixelformat	= V4L2_PIX_FMT_YUV420,
+		.mbus_code	= MEDIA_BUS_FMT_YUYV8_2X8,
+		.bpp		= 1,
+		.planar		= true,
+	},
+	{
+		.desc		= "YVU 4:2:0 PLANAR",
+		.pixelformat	= V4L2_PIX_FMT_YVU420,
+		.mbus_code	= MEDIA_BUS_FMT_YUYV8_2X8,
+		.bpp		= 1,
+		.planar		= true,
+	},
+	{
+		.desc		= "XRGB 444",
+		.pixelformat	= V4L2_PIX_FMT_XRGB444,
+		.mbus_code	= MEDIA_BUS_FMT_RGB444_2X8_PADHI_LE,
+		.bpp		= 2,
+		.planar		= false,
+	},
+	{
+		.desc		= "RGB 565",
+		.pixelformat	= V4L2_PIX_FMT_RGB565,
+		.mbus_code	= MEDIA_BUS_FMT_RGB565_2X8_LE,
+		.bpp		= 2,
+		.planar		= false,
+	},
+	{
+		.desc		= "Raw RGB Bayer",
+		.pixelformat	= V4L2_PIX_FMT_SBGGR8,
+		.mbus_code	= MEDIA_BUS_FMT_SBGGR8_1X8,
+		.bpp		= 1,
+		.planar		= false,
+	},
+};
+#define N_MCAM_FMTS ARRAY_SIZE(mcam_formats)
+
+static struct mcam_format_struct *mcam_find_format(u32 pixelformat)
+{
+	unsigned i;
+
+	for (i = 0; i < N_MCAM_FMTS; i++)
+		if (mcam_formats[i].pixelformat == pixelformat)
+			return mcam_formats + i;
+	/* Not found? Then return the first format. */
+	return mcam_formats;
+}
+
+/*
+ * The default format we use until somebody says otherwise.
+ */
+static const struct v4l2_pix_format mcam_def_pix_format = {
+	.width		= VGA_WIDTH,
+	.height		= VGA_HEIGHT,
+	.pixelformat	= V4L2_PIX_FMT_YUYV,
+	.field		= V4L2_FIELD_NONE,
+	.bytesperline	= VGA_WIDTH*2,
+	.sizeimage	= VGA_WIDTH*VGA_HEIGHT*2,
+	.colorspace	= V4L2_COLORSPACE_SRGB,
+};
+
+static const u32 mcam_def_mbus_code = MEDIA_BUS_FMT_YUYV8_2X8;
+
+
+/*
+ * The two-word DMA descriptor format used by the Armada 610 and like.  There
+ * Is a three-word format as well (set C1_DESC_3WORD) where the third
+ * word is a pointer to the next descriptor, but we don't use it.  Two-word
+ * descriptors have to be contiguous in memory.
+ */
+struct mcam_dma_desc {
+	u32 dma_addr;
+	u32 segment_len;
+};
+
+/*
+ * Our buffer type for working with videobuf2.  Note that the vb2
+ * developers have decreed that struct vb2_v4l2_buffer must be at the
+ * beginning of this structure.
+ */
+struct mcam_vb_buffer {
+	struct vb2_v4l2_buffer vb_buf;
+	struct list_head queue;
+	struct mcam_dma_desc *dma_desc;	/* Descriptor virtual address */
+	dma_addr_t dma_desc_pa;		/* Descriptor physical address */
+	int dma_desc_nent;		/* Number of mapped descriptors */
+};
+
+static inline struct mcam_vb_buffer *vb_to_mvb(struct vb2_v4l2_buffer *vb)
+{
+	return container_of(vb, struct mcam_vb_buffer, vb_buf);
+}
+
+/*
+ * Hand a completed buffer back to user space.
+ */
+static void mcam_buffer_done(struct mcam_camera *cam, int frame,
+		struct vb2_v4l2_buffer *vbuf)
+{
+	vbuf->vb2_buf.planes[0].bytesused = cam->pix_format.sizeimage;
+	vbuf->sequence = cam->buf_seq[frame];
+	vbuf->field = V4L2_FIELD_NONE;
+	v4l2_get_timestamp(&vbuf->timestamp);
+	vb2_set_plane_payload(&vbuf->vb2_buf, 0, cam->pix_format.sizeimage);
+	vb2_buffer_done(&vbuf->vb2_buf, VB2_BUF_STATE_DONE);
+}
+
+
+
+/*
+ * Debugging and related.
+ */
+#define cam_err(cam, fmt, arg...) \
+	dev_err((cam)->dev, fmt, ##arg);
+#define cam_warn(cam, fmt, arg...) \
+	dev_warn((cam)->dev, fmt, ##arg);
+#define cam_dbg(cam, fmt, arg...) \
+	dev_dbg((cam)->dev, fmt, ##arg);
+
+
+/*
+ * Flag manipulation helpers
+ */
+static void mcam_reset_buffers(struct mcam_camera *cam)
+{
+	int i;
+
+	cam->next_buf = -1;
+	for (i = 0; i < cam->nbufs; i++) {
+		clear_bit(i, &cam->flags);
+		clear_bit(CF_FRAME_SOF0 + i, &cam->flags);
+	}
+}
+
+static inline int mcam_needs_config(struct mcam_camera *cam)
+{
+	return test_bit(CF_CONFIG_NEEDED, &cam->flags);
+}
+
+static void mcam_set_config_needed(struct mcam_camera *cam, int needed)
+{
+	if (needed)
+		set_bit(CF_CONFIG_NEEDED, &cam->flags);
+	else
+		clear_bit(CF_CONFIG_NEEDED, &cam->flags);
+}
+
+/* ------------------------------------------------------------------- */
+/*
+ * Make the controller start grabbing images.  Everything must
+ * be set up before doing this.
+ */
+static void mcam_ctlr_start(struct mcam_camera *cam)
+{
+	/* set_bit performs a read, so no other barrier should be
+	   needed here */
+	mcam_reg_set_bit(cam, REG_CTRL0, C0_ENABLE);
+}
+
+static void mcam_ctlr_stop(struct mcam_camera *cam)
+{
+	mcam_reg_clear_bit(cam, REG_CTRL0, C0_ENABLE);
+}
+
+static void mcam_enable_mipi(struct mcam_camera *mcam)
+{
+	/* Using MIPI mode and enable MIPI */
+	cam_dbg(mcam, "camera: DPHY3=0x%x, DPHY5=0x%x, DPHY6=0x%x\n",
+			mcam->dphy[0], mcam->dphy[1], mcam->dphy[2]);
+	mcam_reg_write(mcam, REG_CSI2_DPHY3, mcam->dphy[0]);
+	mcam_reg_write(mcam, REG_CSI2_DPHY5, mcam->dphy[1]);
+	mcam_reg_write(mcam, REG_CSI2_DPHY6, mcam->dphy[2]);
+
+	if (!mcam->mipi_enabled) {
+		if (mcam->lane > 4 || mcam->lane <= 0) {
+			cam_warn(mcam, "lane number error\n");
+			mcam->lane = 1;	/* set the default value */
+		}
+		/*
+		 * 0x41 actives 1 lane
+		 * 0x43 actives 2 lanes
+		 * 0x45 actives 3 lanes (never happen)
+		 * 0x47 actives 4 lanes
+		 */
+		mcam_reg_write(mcam, REG_CSI2_CTRL0,
+			CSI2_C0_MIPI_EN | CSI2_C0_ACT_LANE(mcam->lane));
+		mcam_reg_write(mcam, REG_CLKCTRL,
+			(mcam->mclk_src << 29) | mcam->mclk_div);
+
+		mcam->mipi_enabled = true;
+	}
+}
+
+static void mcam_disable_mipi(struct mcam_camera *mcam)
+{
+	/* Using Parallel mode or disable MIPI */
+	mcam_reg_write(mcam, REG_CSI2_CTRL0, 0x0);
+	mcam_reg_write(mcam, REG_CSI2_DPHY3, 0x0);
+	mcam_reg_write(mcam, REG_CSI2_DPHY5, 0x0);
+	mcam_reg_write(mcam, REG_CSI2_DPHY6, 0x0);
+	mcam->mipi_enabled = false;
+}
+
+static bool mcam_fmt_is_planar(__u32 pfmt)
+{
+	struct mcam_format_struct *f;
+
+	f = mcam_find_format(pfmt);
+	return f->planar;
+}
+
+static void mcam_write_yuv_bases(struct mcam_camera *cam,
+				 unsigned frame, dma_addr_t base)
+{
+	struct v4l2_pix_format *fmt = &cam->pix_format;
+	u32 pixel_count = fmt->width * fmt->height;
+	dma_addr_t y, u = 0, v = 0;
+
+	y = base;
+
+	switch (fmt->pixelformat) {
+	case V4L2_PIX_FMT_YUV420:
+		u = y + pixel_count;
+		v = u + pixel_count / 4;
+		break;
+	case V4L2_PIX_FMT_YVU420:
+		v = y + pixel_count;
+		u = v + pixel_count / 4;
+		break;
+	default:
+		break;
+	}
+
+	mcam_reg_write(cam, REG_Y0BAR + frame * 4, y);
+	if (mcam_fmt_is_planar(fmt->pixelformat)) {
+		mcam_reg_write(cam, REG_U0BAR + frame * 4, u);
+		mcam_reg_write(cam, REG_V0BAR + frame * 4, v);
+	}
+}
+
+/* ------------------------------------------------------------------- */
+
+#ifdef MCAM_MODE_VMALLOC
+/*
+ * Code specific to the vmalloc buffer mode.
+ */
+
+/*
+ * Allocate in-kernel DMA buffers for vmalloc mode.
+ */
+static int mcam_alloc_dma_bufs(struct mcam_camera *cam, int loadtime)
+{
+	int i;
+
+	mcam_set_config_needed(cam, 1);
+	if (loadtime)
+		cam->dma_buf_size = dma_buf_size;
+	else
+		cam->dma_buf_size = cam->pix_format.sizeimage;
+	if (n_dma_bufs > 3)
+		n_dma_bufs = 3;
+
+	cam->nbufs = 0;
+	for (i = 0; i < n_dma_bufs; i++) {
+		cam->dma_bufs[i] = dma_alloc_coherent(cam->dev,
+				cam->dma_buf_size, cam->dma_handles + i,
+				GFP_KERNEL);
+		if (cam->dma_bufs[i] == NULL) {
+			cam_warn(cam, "Failed to allocate DMA buffer\n");
+			break;
+		}
+		(cam->nbufs)++;
+	}
+
+	switch (cam->nbufs) {
+	case 1:
+		dma_free_coherent(cam->dev, cam->dma_buf_size,
+				cam->dma_bufs[0], cam->dma_handles[0]);
+		cam->nbufs = 0;
+	case 0:
+		cam_err(cam, "Insufficient DMA buffers, cannot operate\n");
+		return -ENOMEM;
+
+	case 2:
+		if (n_dma_bufs > 2)
+			cam_warn(cam, "Will limp along with only 2 buffers\n");
+		break;
+	}
+	return 0;
+}
+
+static void mcam_free_dma_bufs(struct mcam_camera *cam)
+{
+	int i;
+
+	for (i = 0; i < cam->nbufs; i++) {
+		dma_free_coherent(cam->dev, cam->dma_buf_size,
+				cam->dma_bufs[i], cam->dma_handles[i]);
+		cam->dma_bufs[i] = NULL;
+	}
+	cam->nbufs = 0;
+}
+
+
+/*
+ * Set up DMA buffers when operating in vmalloc mode
+ */
+static void mcam_ctlr_dma_vmalloc(struct mcam_camera *cam)
+{
+	/*
+	 * Store the first two YUV buffers. Then either
+	 * set the third if it exists, or tell the controller
+	 * to just use two.
+	 */
+	mcam_write_yuv_bases(cam, 0, cam->dma_handles[0]);
+	mcam_write_yuv_bases(cam, 1, cam->dma_handles[1]);
+	if (cam->nbufs > 2) {
+		mcam_write_yuv_bases(cam, 2, cam->dma_handles[2]);
+		mcam_reg_clear_bit(cam, REG_CTRL1, C1_TWOBUFS);
+	} else
+		mcam_reg_set_bit(cam, REG_CTRL1, C1_TWOBUFS);
+	if (cam->chip_id == MCAM_CAFE)
+		mcam_reg_write(cam, REG_UBAR, 0); /* 32 bits only */
+}
+
+/*
+ * Copy data out to user space in the vmalloc case
+ */
+static void mcam_frame_tasklet(unsigned long data)
+{
+	struct mcam_camera *cam = (struct mcam_camera *) data;
+	int i;
+	unsigned long flags;
+	struct mcam_vb_buffer *buf;
+
+	spin_lock_irqsave(&cam->dev_lock, flags);
+	for (i = 0; i < cam->nbufs; i++) {
+		int bufno = cam->next_buf;
+
+		if (cam->state != S_STREAMING || bufno < 0)
+			break;  /* I/O got stopped */
+		if (++(cam->next_buf) >= cam->nbufs)
+			cam->next_buf = 0;
+		if (!test_bit(bufno, &cam->flags))
+			continue;
+		if (list_empty(&cam->buffers)) {
+			cam->frame_state.singles++;
+			break;  /* Leave it valid, hope for better later */
+		}
+		cam->frame_state.delivered++;
+		clear_bit(bufno, &cam->flags);
+		buf = list_first_entry(&cam->buffers, struct mcam_vb_buffer,
+				queue);
+		list_del_init(&buf->queue);
+		/*
+		 * Drop the lock during the big copy.  This *should* be safe...
+		 */
+		spin_unlock_irqrestore(&cam->dev_lock, flags);
+		memcpy(vb2_plane_vaddr(&buf->vb_buf.vb2_buf, 0),
+				cam->dma_bufs[bufno],
+				cam->pix_format.sizeimage);
+		mcam_buffer_done(cam, bufno, &buf->vb_buf);
+		spin_lock_irqsave(&cam->dev_lock, flags);
+	}
+	spin_unlock_irqrestore(&cam->dev_lock, flags);
+}
+
+
+/*
+ * Make sure our allocated buffers are up to the task.
+ */
+static int mcam_check_dma_buffers(struct mcam_camera *cam)
+{
+	if (cam->nbufs > 0 && cam->dma_buf_size < cam->pix_format.sizeimage)
+			mcam_free_dma_bufs(cam);
+	if (cam->nbufs == 0)
+		return mcam_alloc_dma_bufs(cam, 0);
+	return 0;
+}
+
+static void mcam_vmalloc_done(struct mcam_camera *cam, int frame)
+{
+	tasklet_schedule(&cam->s_tasklet);
+}
+
+#else /* MCAM_MODE_VMALLOC */
+
+static inline int mcam_alloc_dma_bufs(struct mcam_camera *cam, int loadtime)
+{
+	return 0;
+}
+
+static inline void mcam_free_dma_bufs(struct mcam_camera *cam)
+{
+	return;
+}
+
+static inline int mcam_check_dma_buffers(struct mcam_camera *cam)
+{
+	return 0;
+}
+
+
+
+#endif /* MCAM_MODE_VMALLOC */
+
+
+#ifdef MCAM_MODE_DMA_CONTIG
+/* ---------------------------------------------------------------------- */
+/*
+ * DMA-contiguous code.
+ */
+
+/*
+ * Set up a contiguous buffer for the given frame.  Here also is where
+ * the underrun strategy is set: if there is no buffer available, reuse
+ * the buffer from the other BAR and set the CF_SINGLE_BUFFER flag to
+ * keep the interrupt handler from giving that buffer back to user
+ * space.  In this way, we always have a buffer to DMA to and don't
+ * have to try to play games stopping and restarting the controller.
+ */
+static void mcam_set_contig_buffer(struct mcam_camera *cam, int frame)
+{
+	struct mcam_vb_buffer *buf;
+	dma_addr_t dma_handle;
+	struct vb2_v4l2_buffer *vb;
+
+	/*
+	 * If there are no available buffers, go into single mode
+	 */
+	if (list_empty(&cam->buffers)) {
+		buf = cam->vb_bufs[frame ^ 0x1];
+		set_bit(CF_SINGLE_BUFFER, &cam->flags);
+		cam->frame_state.singles++;
+	} else {
+		/*
+		 * OK, we have a buffer we can use.
+		 */
+		buf = list_first_entry(&cam->buffers, struct mcam_vb_buffer,
+					queue);
+		list_del_init(&buf->queue);
+		clear_bit(CF_SINGLE_BUFFER, &cam->flags);
+	}
+
+	cam->vb_bufs[frame] = buf;
+	vb = &buf->vb_buf;
+
+	dma_handle = vb2_dma_contig_plane_dma_addr(&vb->vb2_buf, 0);
+	mcam_write_yuv_bases(cam, frame, dma_handle);
+}
+
+/*
+ * Initial B_DMA_contig setup.
+ */
+static void mcam_ctlr_dma_contig(struct mcam_camera *cam)
+{
+	mcam_reg_set_bit(cam, REG_CTRL1, C1_TWOBUFS);
+	cam->nbufs = 2;
+	mcam_set_contig_buffer(cam, 0);
+	mcam_set_contig_buffer(cam, 1);
+}
+
+/*
+ * Frame completion handling.
+ */
+static void mcam_dma_contig_done(struct mcam_camera *cam, int frame)
+{
+	struct mcam_vb_buffer *buf = cam->vb_bufs[frame];
+
+	if (!test_bit(CF_SINGLE_BUFFER, &cam->flags)) {
+		cam->frame_state.delivered++;
+		cam->vb_bufs[frame] = NULL;
+		mcam_buffer_done(cam, frame, &buf->vb_buf);
+	}
+	mcam_set_contig_buffer(cam, frame);
+}
+
+#endif /* MCAM_MODE_DMA_CONTIG */
+
+#ifdef MCAM_MODE_DMA_SG
+/* ---------------------------------------------------------------------- */
+/*
+ * Scatter/gather-specific code.
+ */
+
+/*
+ * Set up the next buffer for S/G I/O; caller should be sure that
+ * the controller is stopped and a buffer is available.
+ */
+static void mcam_sg_next_buffer(struct mcam_camera *cam)
+{
+	struct mcam_vb_buffer *buf;
+
+	buf = list_first_entry(&cam->buffers, struct mcam_vb_buffer, queue);
+	list_del_init(&buf->queue);
+	/*
+	 * Very Bad Not Good Things happen if you don't clear
+	 * C1_DESC_ENA before making any descriptor changes.
+	 */
+	mcam_reg_clear_bit(cam, REG_CTRL1, C1_DESC_ENA);
+	mcam_reg_write(cam, REG_DMA_DESC_Y, buf->dma_desc_pa);
+	mcam_reg_write(cam, REG_DESC_LEN_Y,
+			buf->dma_desc_nent*sizeof(struct mcam_dma_desc));
+	mcam_reg_write(cam, REG_DESC_LEN_U, 0);
+	mcam_reg_write(cam, REG_DESC_LEN_V, 0);
+	mcam_reg_set_bit(cam, REG_CTRL1, C1_DESC_ENA);
+	cam->vb_bufs[0] = buf;
+}
+
+/*
+ * Initial B_DMA_sg setup
+ */
+static void mcam_ctlr_dma_sg(struct mcam_camera *cam)
+{
+	/*
+	 * The list-empty condition can hit us at resume time
+	 * if the buffer list was empty when the system was suspended.
+	 */
+	if (list_empty(&cam->buffers)) {
+		set_bit(CF_SG_RESTART, &cam->flags);
+		return;
+	}
+
+	mcam_reg_clear_bit(cam, REG_CTRL1, C1_DESC_3WORD);
+	mcam_sg_next_buffer(cam);
+	cam->nbufs = 3;
+}
+
+
+/*
+ * Frame completion with S/G is trickier.  We can't muck with
+ * a descriptor chain on the fly, since the controller buffers it
+ * internally.  So we have to actually stop and restart; Marvell
+ * says this is the way to do it.
+ *
+ * Of course, stopping is easier said than done; experience shows
+ * that the controller can start a frame *after* C0_ENABLE has been
+ * cleared.  So when running in S/G mode, the controller is "stopped"
+ * on receipt of the start-of-frame interrupt.  That means we can
+ * safely change the DMA descriptor array here and restart things
+ * (assuming there's another buffer waiting to go).
+ */
+static void mcam_dma_sg_done(struct mcam_camera *cam, int frame)
+{
+	struct mcam_vb_buffer *buf = cam->vb_bufs[0];
+
+	/*
+	 * If we're no longer supposed to be streaming, don't do anything.
+	 */
+	if (cam->state != S_STREAMING)
+		return;
+	/*
+	 * If we have another buffer available, put it in and
+	 * restart the engine.
+	 */
+	if (!list_empty(&cam->buffers)) {
+		mcam_sg_next_buffer(cam);
+		mcam_ctlr_start(cam);
+	/*
+	 * Otherwise set CF_SG_RESTART and the controller will
+	 * be restarted once another buffer shows up.
+	 */
+	} else {
+		set_bit(CF_SG_RESTART, &cam->flags);
+		cam->frame_state.singles++;
+		cam->vb_bufs[0] = NULL;
+	}
+	/*
+	 * Now we can give the completed frame back to user space.
+	 */
+	cam->frame_state.delivered++;
+	mcam_buffer_done(cam, frame, &buf->vb_buf);
+}
+
+
+/*
+ * Scatter/gather mode requires stopping the controller between
+ * frames so we can put in a new DMA descriptor array.  If no new
+ * buffer exists at frame completion, the controller is left stopped;
+ * this function is charged with gettig things going again.
+ */
+static void mcam_sg_restart(struct mcam_camera *cam)
+{
+	mcam_ctlr_dma_sg(cam);
+	mcam_ctlr_start(cam);
+	clear_bit(CF_SG_RESTART, &cam->flags);
+}
+
+#else /* MCAM_MODE_DMA_SG */
+
+static inline void mcam_sg_restart(struct mcam_camera *cam)
+{
+	return;
+}
+
+#endif /* MCAM_MODE_DMA_SG */
+
+/* ---------------------------------------------------------------------- */
+/*
+ * Buffer-mode-independent controller code.
+ */
+
+/*
+ * Image format setup
+ */
+static void mcam_ctlr_image(struct mcam_camera *cam)
+{
+	struct v4l2_pix_format *fmt = &cam->pix_format;
+	u32 widthy = 0, widthuv = 0, imgsz_h, imgsz_w;
+
+	cam_dbg(cam, "camera: bytesperline = %d; height = %d\n",
+		fmt->bytesperline, fmt->sizeimage / fmt->bytesperline);
+	imgsz_h = (fmt->height << IMGSZ_V_SHIFT) & IMGSZ_V_MASK;
+	imgsz_w = (fmt->width * 2) & IMGSZ_H_MASK;
+
+	switch (fmt->pixelformat) {
+	case V4L2_PIX_FMT_YUYV:
+	case V4L2_PIX_FMT_YVYU:
+		widthy = fmt->width * 2;
+		widthuv = 0;
+		break;
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_YVU420:
+		widthy = fmt->width;
+		widthuv = fmt->width / 2;
+		break;
+	default:
+		widthy = fmt->bytesperline;
+		widthuv = 0;
+		break;
+	}
+
+	mcam_reg_write_mask(cam, REG_IMGPITCH, widthuv << 16 | widthy,
+			IMGP_YP_MASK | IMGP_UVP_MASK);
+	mcam_reg_write(cam, REG_IMGSIZE, imgsz_h | imgsz_w);
+	mcam_reg_write(cam, REG_IMGOFFSET, 0x0);
+
+	/*
+	 * Tell the controller about the image format we are using.
+	 */
+	switch (fmt->pixelformat) {
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_YVU420:
+		mcam_reg_write_mask(cam, REG_CTRL0,
+			C0_DF_YUV | C0_YUV_420PL | C0_YUVE_VYUY, C0_DF_MASK);
+		break;
+	case V4L2_PIX_FMT_YUYV:
+		mcam_reg_write_mask(cam, REG_CTRL0,
+			C0_DF_YUV | C0_YUV_PACKED | C0_YUVE_NOSWAP, C0_DF_MASK);
+		break;
+	case V4L2_PIX_FMT_YVYU:
+		mcam_reg_write_mask(cam, REG_CTRL0,
+			C0_DF_YUV | C0_YUV_PACKED | C0_YUVE_SWAP24, C0_DF_MASK);
+		break;
+	case V4L2_PIX_FMT_XRGB444:
+		mcam_reg_write_mask(cam, REG_CTRL0,
+			C0_DF_RGB | C0_RGBF_444 | C0_RGB4_XBGR, C0_DF_MASK);
+		break;
+	case V4L2_PIX_FMT_RGB565:
+		mcam_reg_write_mask(cam, REG_CTRL0,
+			C0_DF_RGB | C0_RGBF_565 | C0_RGB5_BGGR, C0_DF_MASK);
+		break;
+	case V4L2_PIX_FMT_SBGGR8:
+		mcam_reg_write_mask(cam, REG_CTRL0,
+			C0_DF_RGB | C0_RGB5_GRBG, C0_DF_MASK);
+		break;
+	default:
+		cam_err(cam, "camera: unknown format: %#x\n", fmt->pixelformat);
+		break;
+	}
+
+	/*
+	 * Make sure it knows we want to use hsync/vsync.
+	 */
+	mcam_reg_write_mask(cam, REG_CTRL0, C0_SIF_HVSYNC, C0_SIFM_MASK);
+	/*
+	 * This field controls the generation of EOF(DVP only)
+	 */
+	if (cam->bus_type != V4L2_MBUS_CSI2)
+		mcam_reg_set_bit(cam, REG_CTRL0,
+				C0_EOF_VSYNC | C0_VEDGE_CTRL);
+}
+
+
+/*
+ * Configure the controller for operation; caller holds the
+ * device mutex.
+ */
+static int mcam_ctlr_configure(struct mcam_camera *cam)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&cam->dev_lock, flags);
+	clear_bit(CF_SG_RESTART, &cam->flags);
+	cam->dma_setup(cam);
+	mcam_ctlr_image(cam);
+	mcam_set_config_needed(cam, 0);
+	spin_unlock_irqrestore(&cam->dev_lock, flags);
+	return 0;
+}
+
+static void mcam_ctlr_irq_enable(struct mcam_camera *cam)
+{
+	/*
+	 * Clear any pending interrupts, since we do not
+	 * expect to have I/O active prior to enabling.
+	 */
+	mcam_reg_write(cam, REG_IRQSTAT, FRAMEIRQS);
+	mcam_reg_set_bit(cam, REG_IRQMASK, FRAMEIRQS);
+}
+
+static void mcam_ctlr_irq_disable(struct mcam_camera *cam)
+{
+	mcam_reg_clear_bit(cam, REG_IRQMASK, FRAMEIRQS);
+}
+
+
+
+static void mcam_ctlr_init(struct mcam_camera *cam)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&cam->dev_lock, flags);
+	/*
+	 * Make sure it's not powered down.
+	 */
+	mcam_reg_clear_bit(cam, REG_CTRL1, C1_PWRDWN);
+	/*
+	 * Turn off the enable bit.  It sure should be off anyway,
+	 * but it's good to be sure.
+	 */
+	mcam_reg_clear_bit(cam, REG_CTRL0, C0_ENABLE);
+	/*
+	 * Clock the sensor appropriately.  Controller clock should
+	 * be 48MHz, sensor "typical" value is half that.
+	 */
+	mcam_reg_write_mask(cam, REG_CLKCTRL, 2, CLK_DIV_MASK);
+	spin_unlock_irqrestore(&cam->dev_lock, flags);
+}
+
+
+/*
+ * Stop the controller, and don't return until we're really sure that no
+ * further DMA is going on.
+ */
+static void mcam_ctlr_stop_dma(struct mcam_camera *cam)
+{
+	unsigned long flags;
+
+	/*
+	 * Theory: stop the camera controller (whether it is operating
+	 * or not).  Delay briefly just in case we race with the SOF
+	 * interrupt, then wait until no DMA is active.
+	 */
+	spin_lock_irqsave(&cam->dev_lock, flags);
+	clear_bit(CF_SG_RESTART, &cam->flags);
+	mcam_ctlr_stop(cam);
+	cam->state = S_IDLE;
+	spin_unlock_irqrestore(&cam->dev_lock, flags);
+	/*
+	 * This is a brutally long sleep, but experience shows that
+	 * it can take the controller a while to get the message that
+	 * it needs to stop grabbing frames.  In particular, we can
+	 * sometimes (on mmp) get a frame at the end WITHOUT the
+	 * start-of-frame indication.
+	 */
+	msleep(150);
+	if (test_bit(CF_DMA_ACTIVE, &cam->flags))
+		cam_err(cam, "Timeout waiting for DMA to end\n");
+		/* This would be bad news - what now? */
+	spin_lock_irqsave(&cam->dev_lock, flags);
+	mcam_ctlr_irq_disable(cam);
+	spin_unlock_irqrestore(&cam->dev_lock, flags);
+}
+
+/*
+ * Power up and down.
+ */
+static int mcam_ctlr_power_up(struct mcam_camera *cam)
+{
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&cam->dev_lock, flags);
+	ret = cam->plat_power_up(cam);
+	if (ret) {
+		spin_unlock_irqrestore(&cam->dev_lock, flags);
+		return ret;
+	}
+	mcam_reg_clear_bit(cam, REG_CTRL1, C1_PWRDWN);
+	spin_unlock_irqrestore(&cam->dev_lock, flags);
+	msleep(5); /* Just to be sure */
+	return 0;
+}
+
+static void mcam_ctlr_power_down(struct mcam_camera *cam)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&cam->dev_lock, flags);
+	/*
+	 * School of hard knocks department: be sure we do any register
+	 * twiddling on the controller *before* calling the platform
+	 * power down routine.
+	 */
+	mcam_reg_set_bit(cam, REG_CTRL1, C1_PWRDWN);
+	cam->plat_power_down(cam);
+	spin_unlock_irqrestore(&cam->dev_lock, flags);
+}
+
+/* -------------------------------------------------------------------- */
+/*
+ * Communications with the sensor.
+ */
+
+static int __mcam_cam_reset(struct mcam_camera *cam)
+{
+	return sensor_call(cam, core, reset, 0);
+}
+
+/*
+ * We have found the sensor on the i2c.  Let's try to have a
+ * conversation.
+ */
+static int mcam_cam_init(struct mcam_camera *cam)
+{
+	int ret;
+
+	if (cam->state != S_NOTREADY)
+		cam_warn(cam, "Cam init with device in funky state %d",
+				cam->state);
+	ret = __mcam_cam_reset(cam);
+	/* Get/set parameters? */
+	cam->state = S_IDLE;
+	mcam_ctlr_power_down(cam);
+	return ret;
+}
+
+/*
+ * Configure the sensor to match the parameters we have.  Caller should
+ * hold s_mutex
+ */
+static int mcam_cam_set_flip(struct mcam_camera *cam)
+{
+	struct v4l2_control ctrl;
+
+	memset(&ctrl, 0, sizeof(ctrl));
+	ctrl.id = V4L2_CID_VFLIP;
+	ctrl.value = flip;
+	return sensor_call(cam, core, s_ctrl, &ctrl);
+}
+
+
+static int mcam_cam_configure(struct mcam_camera *cam)
+{
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	int ret;
+
+	v4l2_fill_mbus_format(&format.format, &cam->pix_format, cam->mbus_code);
+	ret = sensor_call(cam, core, init, 0);
+	if (ret == 0)
+		ret = sensor_call(cam, pad, set_fmt, NULL, &format);
+	/*
+	 * OV7670 does weird things if flip is set *before* format...
+	 */
+	ret += mcam_cam_set_flip(cam);
+	return ret;
+}
+
+/*
+ * Get everything ready, and start grabbing frames.
+ */
+static int mcam_read_setup(struct mcam_camera *cam)
+{
+	int ret;
+	unsigned long flags;
+
+	/*
+	 * Configuration.  If we still don't have DMA buffers,
+	 * make one last, desperate attempt.
+	 */
+	if (cam->buffer_mode == B_vmalloc && cam->nbufs == 0 &&
+			mcam_alloc_dma_bufs(cam, 0))
+		return -ENOMEM;
+
+	if (mcam_needs_config(cam)) {
+		mcam_cam_configure(cam);
+		ret = mcam_ctlr_configure(cam);
+		if (ret)
+			return ret;
+	}
+
+	/*
+	 * Turn it loose.
+	 */
+	spin_lock_irqsave(&cam->dev_lock, flags);
+	clear_bit(CF_DMA_ACTIVE, &cam->flags);
+	mcam_reset_buffers(cam);
+	/*
+	 * Update CSI2_DPHY value
+	 */
+	if (cam->calc_dphy)
+		cam->calc_dphy(cam);
+	cam_dbg(cam, "camera: DPHY sets: dphy3=0x%x, dphy5=0x%x, dphy6=0x%x\n",
+			cam->dphy[0], cam->dphy[1], cam->dphy[2]);
+	if (cam->bus_type == V4L2_MBUS_CSI2)
+		mcam_enable_mipi(cam);
+	else
+		mcam_disable_mipi(cam);
+	mcam_ctlr_irq_enable(cam);
+	cam->state = S_STREAMING;
+	if (!test_bit(CF_SG_RESTART, &cam->flags))
+		mcam_ctlr_start(cam);
+	spin_unlock_irqrestore(&cam->dev_lock, flags);
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+/*
+ * Videobuf2 interface code.
+ */
+
+static int mcam_vb_queue_setup(struct vb2_queue *vq,
+		const void *parg, unsigned int *nbufs,
+		unsigned int *num_planes, unsigned int sizes[],
+		void *alloc_ctxs[])
+{
+	const struct v4l2_format *fmt = parg;
+	struct mcam_camera *cam = vb2_get_drv_priv(vq);
+	int minbufs = (cam->buffer_mode == B_DMA_contig) ? 3 : 2;
+
+	if (fmt && fmt->fmt.pix.sizeimage < cam->pix_format.sizeimage)
+		return -EINVAL;
+	sizes[0] = fmt ? fmt->fmt.pix.sizeimage : cam->pix_format.sizeimage;
+	*num_planes = 1; /* Someday we have to support planar formats... */
+	if (*nbufs < minbufs)
+		*nbufs = minbufs;
+	if (cam->buffer_mode == B_DMA_contig)
+		alloc_ctxs[0] = cam->vb_alloc_ctx;
+	else if (cam->buffer_mode == B_DMA_sg)
+		alloc_ctxs[0] = cam->vb_alloc_ctx_sg;
+	return 0;
+}
+
+
+static void mcam_vb_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct mcam_vb_buffer *mvb = vb_to_mvb(vbuf);
+	struct mcam_camera *cam = vb2_get_drv_priv(vb->vb2_queue);
+	unsigned long flags;
+	int start;
+
+	spin_lock_irqsave(&cam->dev_lock, flags);
+	start = (cam->state == S_BUFWAIT) && !list_empty(&cam->buffers);
+	list_add(&mvb->queue, &cam->buffers);
+	if (cam->state == S_STREAMING && test_bit(CF_SG_RESTART, &cam->flags))
+		mcam_sg_restart(cam);
+	spin_unlock_irqrestore(&cam->dev_lock, flags);
+	if (start)
+		mcam_read_setup(cam);
+}
+
+static void mcam_vb_requeue_bufs(struct vb2_queue *vq,
+				 enum vb2_buffer_state state)
+{
+	struct mcam_camera *cam = vb2_get_drv_priv(vq);
+	struct mcam_vb_buffer *buf, *node;
+	unsigned long flags;
+	unsigned i;
+
+	spin_lock_irqsave(&cam->dev_lock, flags);
+	list_for_each_entry_safe(buf, node, &cam->buffers, queue) {
+		vb2_buffer_done(&buf->vb_buf.vb2_buf, state);
+		list_del(&buf->queue);
+	}
+	for (i = 0; i < MAX_DMA_BUFS; i++) {
+		buf = cam->vb_bufs[i];
+
+		if (buf) {
+			vb2_buffer_done(&buf->vb_buf.vb2_buf, state);
+			cam->vb_bufs[i] = NULL;
+		}
+	}
+	spin_unlock_irqrestore(&cam->dev_lock, flags);
+}
+
+/*
+ * These need to be called with the mutex held from vb2
+ */
+static int mcam_vb_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct mcam_camera *cam = vb2_get_drv_priv(vq);
+	unsigned int frame;
+	int ret;
+
+	if (cam->state != S_IDLE) {
+		mcam_vb_requeue_bufs(vq, VB2_BUF_STATE_QUEUED);
+		return -EINVAL;
+	}
+	cam->frame_state.frames = 0;
+	cam->frame_state.singles = 0;
+	cam->frame_state.delivered = 0;
+	cam->sequence = 0;
+	/*
+	 * Videobuf2 sneakily hoards all the buffers and won't
+	 * give them to us until *after* streaming starts.  But
+	 * we can't actually start streaming until we have a
+	 * destination.  So go into a wait state and hope they
+	 * give us buffers soon.
+	 */
+	if (cam->buffer_mode != B_vmalloc && list_empty(&cam->buffers)) {
+		cam->state = S_BUFWAIT;
+		return 0;
+	}
+
+	/*
+	 * Ensure clear the left over frame flags
+	 * before every really start streaming
+	 */
+	for (frame = 0; frame < cam->nbufs; frame++)
+		clear_bit(CF_FRAME_SOF0 + frame, &cam->flags);
+
+	ret = mcam_read_setup(cam);
+	if (ret)
+		mcam_vb_requeue_bufs(vq, VB2_BUF_STATE_QUEUED);
+	return ret;
+}
+
+static void mcam_vb_stop_streaming(struct vb2_queue *vq)
+{
+	struct mcam_camera *cam = vb2_get_drv_priv(vq);
+
+	cam_dbg(cam, "stop_streaming: %d frames, %d singles, %d delivered\n",
+			cam->frame_state.frames, cam->frame_state.singles,
+			cam->frame_state.delivered);
+	if (cam->state == S_BUFWAIT) {
+		/* They never gave us buffers */
+		cam->state = S_IDLE;
+		return;
+	}
+	if (cam->state != S_STREAMING)
+		return;
+	mcam_ctlr_stop_dma(cam);
+	/*
+	 * Reset the CCIC PHY after stopping streaming,
+	 * otherwise, the CCIC may be unstable.
+	 */
+	if (cam->ctlr_reset)
+		cam->ctlr_reset(cam);
+	/*
+	 * VB2 reclaims the buffers, so we need to forget
+	 * about them.
+	 */
+	mcam_vb_requeue_bufs(vq, VB2_BUF_STATE_ERROR);
+}
+
+
+static const struct vb2_ops mcam_vb2_ops = {
+	.queue_setup		= mcam_vb_queue_setup,
+	.buf_queue		= mcam_vb_buf_queue,
+	.start_streaming	= mcam_vb_start_streaming,
+	.stop_streaming		= mcam_vb_stop_streaming,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
+};
+
+
+#ifdef MCAM_MODE_DMA_SG
+/*
+ * Scatter/gather mode uses all of the above functions plus a
+ * few extras to deal with DMA mapping.
+ */
+static int mcam_vb_sg_buf_init(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct mcam_vb_buffer *mvb = vb_to_mvb(vbuf);
+	struct mcam_camera *cam = vb2_get_drv_priv(vb->vb2_queue);
+	int ndesc = cam->pix_format.sizeimage/PAGE_SIZE + 1;
+
+	mvb->dma_desc = dma_alloc_coherent(cam->dev,
+			ndesc * sizeof(struct mcam_dma_desc),
+			&mvb->dma_desc_pa, GFP_KERNEL);
+	if (mvb->dma_desc == NULL) {
+		cam_err(cam, "Unable to get DMA descriptor array\n");
+		return -ENOMEM;
+	}
+	return 0;
+}
+
+static int mcam_vb_sg_buf_prepare(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct mcam_vb_buffer *mvb = vb_to_mvb(vbuf);
+	struct sg_table *sg_table = vb2_dma_sg_plane_desc(vb, 0);
+	struct mcam_dma_desc *desc = mvb->dma_desc;
+	struct scatterlist *sg;
+	int i;
+
+	for_each_sg(sg_table->sgl, sg, sg_table->nents, i) {
+		desc->dma_addr = sg_dma_address(sg);
+		desc->segment_len = sg_dma_len(sg);
+		desc++;
+	}
+	return 0;
+}
+
+static void mcam_vb_sg_buf_cleanup(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct mcam_camera *cam = vb2_get_drv_priv(vb->vb2_queue);
+	struct mcam_vb_buffer *mvb = vb_to_mvb(vbuf);
+	int ndesc = cam->pix_format.sizeimage/PAGE_SIZE + 1;
+
+	dma_free_coherent(cam->dev, ndesc * sizeof(struct mcam_dma_desc),
+			mvb->dma_desc, mvb->dma_desc_pa);
+}
+
+
+static const struct vb2_ops mcam_vb2_sg_ops = {
+	.queue_setup		= mcam_vb_queue_setup,
+	.buf_init		= mcam_vb_sg_buf_init,
+	.buf_prepare		= mcam_vb_sg_buf_prepare,
+	.buf_queue		= mcam_vb_buf_queue,
+	.buf_cleanup		= mcam_vb_sg_buf_cleanup,
+	.start_streaming	= mcam_vb_start_streaming,
+	.stop_streaming		= mcam_vb_stop_streaming,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
+};
+
+#endif /* MCAM_MODE_DMA_SG */
+
+static int mcam_setup_vb2(struct mcam_camera *cam)
+{
+	struct vb2_queue *vq = &cam->vb_queue;
+
+	memset(vq, 0, sizeof(*vq));
+	vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	vq->drv_priv = cam;
+	vq->lock = &cam->s_mutex;
+	vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF | VB2_READ;
+	vq->buf_struct_size = sizeof(struct mcam_vb_buffer);
+	INIT_LIST_HEAD(&cam->buffers);
+	switch (cam->buffer_mode) {
+	case B_DMA_contig:
+#ifdef MCAM_MODE_DMA_CONTIG
+		vq->ops = &mcam_vb2_ops;
+		vq->mem_ops = &vb2_dma_contig_memops;
+		cam->dma_setup = mcam_ctlr_dma_contig;
+		cam->frame_complete = mcam_dma_contig_done;
+		cam->vb_alloc_ctx = vb2_dma_contig_init_ctx(cam->dev);
+		if (IS_ERR(cam->vb_alloc_ctx))
+			return PTR_ERR(cam->vb_alloc_ctx);
+#endif
+		break;
+	case B_DMA_sg:
+#ifdef MCAM_MODE_DMA_SG
+		vq->ops = &mcam_vb2_sg_ops;
+		vq->mem_ops = &vb2_dma_sg_memops;
+		cam->dma_setup = mcam_ctlr_dma_sg;
+		cam->frame_complete = mcam_dma_sg_done;
+		cam->vb_alloc_ctx_sg = vb2_dma_sg_init_ctx(cam->dev);
+		if (IS_ERR(cam->vb_alloc_ctx_sg))
+			return PTR_ERR(cam->vb_alloc_ctx_sg);
+#endif
+		break;
+	case B_vmalloc:
+#ifdef MCAM_MODE_VMALLOC
+		tasklet_init(&cam->s_tasklet, mcam_frame_tasklet,
+				(unsigned long) cam);
+		vq->ops = &mcam_vb2_ops;
+		vq->mem_ops = &vb2_vmalloc_memops;
+		cam->dma_setup = mcam_ctlr_dma_vmalloc;
+		cam->frame_complete = mcam_vmalloc_done;
+#endif
+		break;
+	}
+	return vb2_queue_init(vq);
+}
+
+static void mcam_cleanup_vb2(struct mcam_camera *cam)
+{
+#ifdef MCAM_MODE_DMA_CONTIG
+	if (cam->buffer_mode == B_DMA_contig)
+		vb2_dma_contig_cleanup_ctx(cam->vb_alloc_ctx);
+#endif
+#ifdef MCAM_MODE_DMA_SG
+	if (cam->buffer_mode == B_DMA_sg)
+		vb2_dma_sg_cleanup_ctx(cam->vb_alloc_ctx_sg);
+#endif
+}
+
+
+/* ---------------------------------------------------------------------- */
+/*
+ * The long list of V4L2 ioctl() operations.
+ */
+
+static int mcam_vidioc_querycap(struct file *file, void *priv,
+		struct v4l2_capability *cap)
+{
+	struct mcam_camera *cam = video_drvdata(file);
+
+	strcpy(cap->driver, "marvell_ccic");
+	strcpy(cap->card, "marvell_ccic");
+	strlcpy(cap->bus_info, cam->bus_info, sizeof(cap->bus_info));
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE |
+		V4L2_CAP_READWRITE | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	return 0;
+}
+
+
+static int mcam_vidioc_enum_fmt_vid_cap(struct file *filp,
+		void *priv, struct v4l2_fmtdesc *fmt)
+{
+	if (fmt->index >= N_MCAM_FMTS)
+		return -EINVAL;
+	strlcpy(fmt->description, mcam_formats[fmt->index].desc,
+			sizeof(fmt->description));
+	fmt->pixelformat = mcam_formats[fmt->index].pixelformat;
+	return 0;
+}
+
+static int mcam_vidioc_try_fmt_vid_cap(struct file *filp, void *priv,
+		struct v4l2_format *fmt)
+{
+	struct mcam_camera *cam = video_drvdata(filp);
+	struct mcam_format_struct *f;
+	struct v4l2_pix_format *pix = &fmt->fmt.pix;
+	struct v4l2_subdev_pad_config pad_cfg;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+	};
+	int ret;
+
+	f = mcam_find_format(pix->pixelformat);
+	pix->pixelformat = f->pixelformat;
+	v4l2_fill_mbus_format(&format.format, pix, f->mbus_code);
+	ret = sensor_call(cam, pad, set_fmt, &pad_cfg, &format);
+	v4l2_fill_pix_format(pix, &format.format);
+	pix->bytesperline = pix->width * f->bpp;
+	switch (f->pixelformat) {
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_YVU420:
+		pix->sizeimage = pix->height * pix->bytesperline * 3 / 2;
+		break;
+	default:
+		pix->sizeimage = pix->height * pix->bytesperline;
+		break;
+	}
+	pix->colorspace = V4L2_COLORSPACE_SRGB;
+	return ret;
+}
+
+static int mcam_vidioc_s_fmt_vid_cap(struct file *filp, void *priv,
+		struct v4l2_format *fmt)
+{
+	struct mcam_camera *cam = video_drvdata(filp);
+	struct mcam_format_struct *f;
+	int ret;
+
+	/*
+	 * Can't do anything if the device is not idle
+	 * Also can't if there are streaming buffers in place.
+	 */
+	if (cam->state != S_IDLE || vb2_is_busy(&cam->vb_queue))
+		return -EBUSY;
+
+	f = mcam_find_format(fmt->fmt.pix.pixelformat);
+
+	/*
+	 * See if the formatting works in principle.
+	 */
+	ret = mcam_vidioc_try_fmt_vid_cap(filp, priv, fmt);
+	if (ret)
+		return ret;
+	/*
+	 * Now we start to change things for real, so let's do it
+	 * under lock.
+	 */
+	cam->pix_format = fmt->fmt.pix;
+	cam->mbus_code = f->mbus_code;
+
+	/*
+	 * Make sure we have appropriate DMA buffers.
+	 */
+	if (cam->buffer_mode == B_vmalloc) {
+		ret = mcam_check_dma_buffers(cam);
+		if (ret)
+			goto out;
+	}
+	mcam_set_config_needed(cam, 1);
+out:
+	return ret;
+}
+
+/*
+ * Return our stored notion of how the camera is/should be configured.
+ * The V4l2 spec wants us to be smarter, and actually get this from
+ * the camera (and not mess with it at open time).  Someday.
+ */
+static int mcam_vidioc_g_fmt_vid_cap(struct file *filp, void *priv,
+		struct v4l2_format *f)
+{
+	struct mcam_camera *cam = video_drvdata(filp);
+
+	f->fmt.pix = cam->pix_format;
+	return 0;
+}
+
+/*
+ * We only have one input - the sensor - so minimize the nonsense here.
+ */
+static int mcam_vidioc_enum_input(struct file *filp, void *priv,
+		struct v4l2_input *input)
+{
+	if (input->index != 0)
+		return -EINVAL;
+
+	input->type = V4L2_INPUT_TYPE_CAMERA;
+	strcpy(input->name, "Camera");
+	return 0;
+}
+
+static int mcam_vidioc_g_input(struct file *filp, void *priv, unsigned int *i)
+{
+	*i = 0;
+	return 0;
+}
+
+static int mcam_vidioc_s_input(struct file *filp, void *priv, unsigned int i)
+{
+	if (i != 0)
+		return -EINVAL;
+	return 0;
+}
+
+/*
+ * G/S_PARM.  Most of this is done by the sensor, but we are
+ * the level which controls the number of read buffers.
+ */
+static int mcam_vidioc_g_parm(struct file *filp, void *priv,
+		struct v4l2_streamparm *parms)
+{
+	struct mcam_camera *cam = video_drvdata(filp);
+	int ret;
+
+	ret = sensor_call(cam, video, g_parm, parms);
+	parms->parm.capture.readbuffers = n_dma_bufs;
+	return ret;
+}
+
+static int mcam_vidioc_s_parm(struct file *filp, void *priv,
+		struct v4l2_streamparm *parms)
+{
+	struct mcam_camera *cam = video_drvdata(filp);
+	int ret;
+
+	ret = sensor_call(cam, video, s_parm, parms);
+	parms->parm.capture.readbuffers = n_dma_bufs;
+	return ret;
+}
+
+static int mcam_vidioc_enum_framesizes(struct file *filp, void *priv,
+		struct v4l2_frmsizeenum *sizes)
+{
+	struct mcam_camera *cam = video_drvdata(filp);
+	struct mcam_format_struct *f;
+	struct v4l2_subdev_frame_size_enum fse = {
+		.index = sizes->index,
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	int ret;
+
+	f = mcam_find_format(sizes->pixel_format);
+	if (f->pixelformat != sizes->pixel_format)
+		return -EINVAL;
+	fse.code = f->mbus_code;
+	ret = sensor_call(cam, pad, enum_frame_size, NULL, &fse);
+	if (ret)
+		return ret;
+	if (fse.min_width == fse.max_width &&
+	    fse.min_height == fse.max_height) {
+		sizes->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+		sizes->discrete.width = fse.min_width;
+		sizes->discrete.height = fse.min_height;
+		return 0;
+	}
+	sizes->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
+	sizes->stepwise.min_width = fse.min_width;
+	sizes->stepwise.max_width = fse.max_width;
+	sizes->stepwise.min_height = fse.min_height;
+	sizes->stepwise.max_height = fse.max_height;
+	sizes->stepwise.step_width = 1;
+	sizes->stepwise.step_height = 1;
+	return 0;
+}
+
+static int mcam_vidioc_enum_frameintervals(struct file *filp, void *priv,
+		struct v4l2_frmivalenum *interval)
+{
+	struct mcam_camera *cam = video_drvdata(filp);
+	struct mcam_format_struct *f;
+	struct v4l2_subdev_frame_interval_enum fie = {
+		.index = interval->index,
+		.width = interval->width,
+		.height = interval->height,
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	int ret;
+
+	f = mcam_find_format(interval->pixel_format);
+	if (f->pixelformat != interval->pixel_format)
+		return -EINVAL;
+	fie.code = f->mbus_code;
+	ret = sensor_call(cam, pad, enum_frame_interval, NULL, &fie);
+	if (ret)
+		return ret;
+	interval->type = V4L2_FRMIVAL_TYPE_DISCRETE;
+	interval->discrete = fie.interval;
+	return 0;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int mcam_vidioc_g_register(struct file *file, void *priv,
+		struct v4l2_dbg_register *reg)
+{
+	struct mcam_camera *cam = video_drvdata(file);
+
+	if (reg->reg > cam->regs_size - 4)
+		return -EINVAL;
+	reg->val = mcam_reg_read(cam, reg->reg);
+	reg->size = 4;
+	return 0;
+}
+
+static int mcam_vidioc_s_register(struct file *file, void *priv,
+		const struct v4l2_dbg_register *reg)
+{
+	struct mcam_camera *cam = video_drvdata(file);
+
+	if (reg->reg > cam->regs_size - 4)
+		return -EINVAL;
+	mcam_reg_write(cam, reg->reg, reg->val);
+	return 0;
+}
+#endif
+
+static const struct v4l2_ioctl_ops mcam_v4l_ioctl_ops = {
+	.vidioc_querycap	= mcam_vidioc_querycap,
+	.vidioc_enum_fmt_vid_cap = mcam_vidioc_enum_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap	= mcam_vidioc_try_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap	= mcam_vidioc_s_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap	= mcam_vidioc_g_fmt_vid_cap,
+	.vidioc_enum_input	= mcam_vidioc_enum_input,
+	.vidioc_g_input		= mcam_vidioc_g_input,
+	.vidioc_s_input		= mcam_vidioc_s_input,
+	.vidioc_reqbufs		= vb2_ioctl_reqbufs,
+	.vidioc_create_bufs	= vb2_ioctl_create_bufs,
+	.vidioc_querybuf	= vb2_ioctl_querybuf,
+	.vidioc_qbuf		= vb2_ioctl_qbuf,
+	.vidioc_dqbuf		= vb2_ioctl_dqbuf,
+	.vidioc_expbuf		= vb2_ioctl_expbuf,
+	.vidioc_streamon	= vb2_ioctl_streamon,
+	.vidioc_streamoff	= vb2_ioctl_streamoff,
+	.vidioc_g_parm		= mcam_vidioc_g_parm,
+	.vidioc_s_parm		= mcam_vidioc_s_parm,
+	.vidioc_enum_framesizes = mcam_vidioc_enum_framesizes,
+	.vidioc_enum_frameintervals = mcam_vidioc_enum_frameintervals,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.vidioc_g_register	= mcam_vidioc_g_register,
+	.vidioc_s_register	= mcam_vidioc_s_register,
+#endif
+};
+
+/* ---------------------------------------------------------------------- */
+/*
+ * Our various file operations.
+ */
+static int mcam_v4l_open(struct file *filp)
+{
+	struct mcam_camera *cam = video_drvdata(filp);
+	int ret;
+
+	mutex_lock(&cam->s_mutex);
+	ret = v4l2_fh_open(filp);
+	if (ret)
+		goto out;
+	if (v4l2_fh_is_singular_file(filp)) {
+		ret = mcam_ctlr_power_up(cam);
+		if (ret)
+			goto out;
+		__mcam_cam_reset(cam);
+		mcam_set_config_needed(cam, 1);
+	}
+out:
+	mutex_unlock(&cam->s_mutex);
+	if (ret)
+		v4l2_fh_release(filp);
+	return ret;
+}
+
+
+static int mcam_v4l_release(struct file *filp)
+{
+	struct mcam_camera *cam = video_drvdata(filp);
+	bool last_open;
+
+	mutex_lock(&cam->s_mutex);
+	last_open = v4l2_fh_is_singular_file(filp);
+	_vb2_fop_release(filp, NULL);
+	if (last_open) {
+		mcam_disable_mipi(cam);
+		mcam_ctlr_power_down(cam);
+		if (cam->buffer_mode == B_vmalloc && alloc_bufs_at_read)
+			mcam_free_dma_bufs(cam);
+	}
+
+	mutex_unlock(&cam->s_mutex);
+	return 0;
+}
+
+static const struct v4l2_file_operations mcam_v4l_fops = {
+	.owner = THIS_MODULE,
+	.open = mcam_v4l_open,
+	.release = mcam_v4l_release,
+	.read = vb2_fop_read,
+	.poll = vb2_fop_poll,
+	.mmap = vb2_fop_mmap,
+	.unlocked_ioctl = video_ioctl2,
+};
+
+
+/*
+ * This template device holds all of those v4l2 methods; we
+ * clone it for specific real devices.
+ */
+static struct video_device mcam_v4l_template = {
+	.name = "mcam",
+	.fops = &mcam_v4l_fops,
+	.ioctl_ops = &mcam_v4l_ioctl_ops,
+	.release = video_device_release_empty,
+};
+
+/* ---------------------------------------------------------------------- */
+/*
+ * Interrupt handler stuff
+ */
+static void mcam_frame_complete(struct mcam_camera *cam, int frame)
+{
+	/*
+	 * Basic frame housekeeping.
+	 */
+	set_bit(frame, &cam->flags);
+	clear_bit(CF_DMA_ACTIVE, &cam->flags);
+	cam->next_buf = frame;
+	cam->buf_seq[frame] = cam->sequence++;
+	cam->frame_state.frames++;
+	/*
+	 * "This should never happen"
+	 */
+	if (cam->state != S_STREAMING)
+		return;
+	/*
+	 * Process the frame and set up the next one.
+	 */
+	cam->frame_complete(cam, frame);
+}
+
+
+/*
+ * The interrupt handler; this needs to be called from the
+ * platform irq handler with the lock held.
+ */
+int mccic_irq(struct mcam_camera *cam, unsigned int irqs)
+{
+	unsigned int frame, handled = 0;
+
+	mcam_reg_write(cam, REG_IRQSTAT, FRAMEIRQS); /* Clear'em all */
+	/*
+	 * Handle any frame completions.  There really should
+	 * not be more than one of these, or we have fallen
+	 * far behind.
+	 *
+	 * When running in S/G mode, the frame number lacks any
+	 * real meaning - there's only one descriptor array - but
+	 * the controller still picks a different one to signal
+	 * each time.
+	 */
+	for (frame = 0; frame < cam->nbufs; frame++)
+		if (irqs & (IRQ_EOF0 << frame) &&
+			test_bit(CF_FRAME_SOF0 + frame, &cam->flags)) {
+			mcam_frame_complete(cam, frame);
+			handled = 1;
+			clear_bit(CF_FRAME_SOF0 + frame, &cam->flags);
+			if (cam->buffer_mode == B_DMA_sg)
+				break;
+		}
+	/*
+	 * If a frame starts, note that we have DMA active.  This
+	 * code assumes that we won't get multiple frame interrupts
+	 * at once; may want to rethink that.
+	 */
+	for (frame = 0; frame < cam->nbufs; frame++) {
+		if (irqs & (IRQ_SOF0 << frame)) {
+			set_bit(CF_FRAME_SOF0 + frame, &cam->flags);
+			handled = IRQ_HANDLED;
+		}
+	}
+
+	if (handled == IRQ_HANDLED) {
+		set_bit(CF_DMA_ACTIVE, &cam->flags);
+		if (cam->buffer_mode == B_DMA_sg)
+			mcam_ctlr_stop(cam);
+	}
+	return handled;
+}
+
+/* ---------------------------------------------------------------------- */
+/*
+ * Registration and such.
+ */
+static struct ov7670_config sensor_cfg = {
+	/*
+	 * Exclude QCIF mode, because it only captures a tiny portion
+	 * of the sensor FOV
+	 */
+	.min_width = 320,
+	.min_height = 240,
+};
+
+
+int mccic_register(struct mcam_camera *cam)
+{
+	struct i2c_board_info ov7670_info = {
+		.type = "ov7670",
+		.addr = 0x42 >> 1,
+		.platform_data = &sensor_cfg,
+	};
+	int ret;
+
+	/*
+	 * Validate the requested buffer mode.
+	 */
+	if (buffer_mode >= 0)
+		cam->buffer_mode = buffer_mode;
+	if (cam->buffer_mode == B_DMA_sg &&
+			cam->chip_id == MCAM_CAFE) {
+		printk(KERN_ERR "marvell-cam: Cafe can't do S/G I/O, "
+			"attempting vmalloc mode instead\n");
+		cam->buffer_mode = B_vmalloc;
+	}
+	if (!mcam_buffer_mode_supported(cam->buffer_mode)) {
+		printk(KERN_ERR "marvell-cam: buffer mode %d unsupported\n",
+				cam->buffer_mode);
+		return -EINVAL;
+	}
+	/*
+	 * Register with V4L
+	 */
+	ret = v4l2_device_register(cam->dev, &cam->v4l2_dev);
+	if (ret)
+		return ret;
+
+	mutex_init(&cam->s_mutex);
+	cam->state = S_NOTREADY;
+	mcam_set_config_needed(cam, 1);
+	cam->pix_format = mcam_def_pix_format;
+	cam->mbus_code = mcam_def_mbus_code;
+	mcam_ctlr_init(cam);
+
+	/*
+	 * Get the v4l2 setup done.
+	 */
+	ret = v4l2_ctrl_handler_init(&cam->ctrl_handler, 10);
+	if (ret)
+		goto out_unregister;
+	cam->v4l2_dev.ctrl_handler = &cam->ctrl_handler;
+
+	/*
+	 * Try to find the sensor.
+	 */
+	sensor_cfg.clock_speed = cam->clock_speed;
+	sensor_cfg.use_smbus = cam->use_smbus;
+	cam->sensor_addr = ov7670_info.addr;
+	cam->sensor = v4l2_i2c_new_subdev_board(&cam->v4l2_dev,
+			cam->i2c_adapter, &ov7670_info, NULL);
+	if (cam->sensor == NULL) {
+		ret = -ENODEV;
+		goto out_unregister;
+	}
+
+	ret = mcam_cam_init(cam);
+	if (ret)
+		goto out_unregister;
+
+	ret = mcam_setup_vb2(cam);
+	if (ret)
+		goto out_unregister;
+
+	mutex_lock(&cam->s_mutex);
+	cam->vdev = mcam_v4l_template;
+	cam->vdev.v4l2_dev = &cam->v4l2_dev;
+	cam->vdev.lock = &cam->s_mutex;
+	cam->vdev.queue = &cam->vb_queue;
+	video_set_drvdata(&cam->vdev, cam);
+	ret = video_register_device(&cam->vdev, VFL_TYPE_GRABBER, -1);
+	if (ret) {
+		mutex_unlock(&cam->s_mutex);
+		goto out_unregister;
+	}
+
+	/*
+	 * If so requested, try to get our DMA buffers now.
+	 */
+	if (cam->buffer_mode == B_vmalloc && !alloc_bufs_at_read) {
+		if (mcam_alloc_dma_bufs(cam, 1))
+			cam_warn(cam, "Unable to alloc DMA buffers at load"
+					" will try again later.");
+	}
+
+	mutex_unlock(&cam->s_mutex);
+	return 0;
+
+out_unregister:
+	v4l2_ctrl_handler_free(&cam->ctrl_handler);
+	v4l2_device_unregister(&cam->v4l2_dev);
+	return ret;
+}
+
+
+void mccic_shutdown(struct mcam_camera *cam)
+{
+	/*
+	 * If we have no users (and we really, really should have no
+	 * users) the device will already be powered down.  Trying to
+	 * take it down again will wedge the machine, which is frowned
+	 * upon.
+	 */
+	if (!list_empty(&cam->vdev.fh_list)) {
+		cam_warn(cam, "Removing a device with users!\n");
+		mcam_ctlr_power_down(cam);
+	}
+	mcam_cleanup_vb2(cam);
+	if (cam->buffer_mode == B_vmalloc)
+		mcam_free_dma_bufs(cam);
+	video_unregister_device(&cam->vdev);
+	v4l2_ctrl_handler_free(&cam->ctrl_handler);
+	v4l2_device_unregister(&cam->v4l2_dev);
+}
+
+/*
+ * Power management
+ */
+#ifdef CONFIG_PM
+
+void mccic_suspend(struct mcam_camera *cam)
+{
+	mutex_lock(&cam->s_mutex);
+	if (!list_empty(&cam->vdev.fh_list)) {
+		enum mcam_state cstate = cam->state;
+
+		mcam_ctlr_stop_dma(cam);
+		mcam_ctlr_power_down(cam);
+		cam->state = cstate;
+	}
+	mutex_unlock(&cam->s_mutex);
+}
+
+int mccic_resume(struct mcam_camera *cam)
+{
+	int ret = 0;
+
+	mutex_lock(&cam->s_mutex);
+	if (!list_empty(&cam->vdev.fh_list)) {
+		ret = mcam_ctlr_power_up(cam);
+		if (ret) {
+			mutex_unlock(&cam->s_mutex);
+			return ret;
+		}
+		__mcam_cam_reset(cam);
+	} else {
+		mcam_ctlr_power_down(cam);
+	}
+	mutex_unlock(&cam->s_mutex);
+
+	set_bit(CF_CONFIG_NEEDED, &cam->flags);
+	if (cam->state == S_STREAMING) {
+		/*
+		 * If there was a buffer in the DMA engine at suspend
+		 * time, put it back on the queue or we'll forget about it.
+		 */
+		if (cam->buffer_mode == B_DMA_sg && cam->vb_bufs[0])
+			list_add(&cam->vb_bufs[0]->queue, &cam->buffers);
+		ret = mcam_read_setup(cam);
+	}
+	return ret;
+}
+#endif /* CONFIG_PM */
diff --git a/drivers/media/platform/marvell-ccic/mcam-core.h b/drivers/media/platform/marvell-ccic/mcam-core.h
new file mode 100644
index 0000000..35cd9e5
--- /dev/null
+++ b/drivers/media/platform/marvell-ccic/mcam-core.h
@@ -0,0 +1,384 @@
+/*
+ * Marvell camera core structures.
+ *
+ * Copyright 2011 Jonathan Corbet corbet@lwn.net
+ */
+#ifndef _MCAM_CORE_H
+#define _MCAM_CORE_H
+
+#include <linux/list.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-dev.h>
+#include <media/videobuf2-v4l2.h>
+
+/*
+ * Create our own symbols for the supported buffer modes, but, for now,
+ * base them entirely on which videobuf2 options have been selected.
+ */
+#if IS_ENABLED(CONFIG_VIDEOBUF2_VMALLOC)
+#define MCAM_MODE_VMALLOC 1
+#endif
+
+#if IS_ENABLED(CONFIG_VIDEOBUF2_DMA_CONTIG)
+#define MCAM_MODE_DMA_CONTIG 1
+#endif
+
+#if IS_ENABLED(CONFIG_VIDEOBUF2_DMA_SG)
+#define MCAM_MODE_DMA_SG 1
+#endif
+
+#if !defined(MCAM_MODE_VMALLOC) && !defined(MCAM_MODE_DMA_CONTIG) && \
+	!defined(MCAM_MODE_DMA_SG)
+#error One of the videobuf buffer modes must be selected in the config
+#endif
+
+
+enum mcam_state {
+	S_NOTREADY,	/* Not yet initialized */
+	S_IDLE,		/* Just hanging around */
+	S_FLAKED,	/* Some sort of problem */
+	S_STREAMING,	/* Streaming data */
+	S_BUFWAIT	/* streaming requested but no buffers yet */
+};
+#define MAX_DMA_BUFS 3
+
+/*
+ * Different platforms work best with different buffer modes, so we
+ * let the platform pick.
+ */
+enum mcam_buffer_mode {
+	B_vmalloc = 0,
+	B_DMA_contig = 1,
+	B_DMA_sg = 2
+};
+
+enum mcam_chip_id {
+	MCAM_CAFE,
+	MCAM_ARMADA610,
+};
+
+/*
+ * Is a given buffer mode supported by the current kernel configuration?
+ */
+static inline int mcam_buffer_mode_supported(enum mcam_buffer_mode mode)
+{
+	switch (mode) {
+#ifdef MCAM_MODE_VMALLOC
+	case B_vmalloc:
+#endif
+#ifdef MCAM_MODE_DMA_CONTIG
+	case B_DMA_contig:
+#endif
+#ifdef MCAM_MODE_DMA_SG
+	case B_DMA_sg:
+#endif
+		return 1;
+	default:
+		return 0;
+	}
+}
+
+/*
+ * Basic frame states
+ */
+struct mcam_frame_state {
+	unsigned int frames;
+	unsigned int singles;
+	unsigned int delivered;
+};
+
+#define NR_MCAM_CLK 3
+
+/*
+ * A description of one of our devices.
+ * Locking: controlled by s_mutex.  Certain fields, however, require
+ *          the dev_lock spinlock; they are marked as such by comments.
+ *          dev_lock is also required for access to device registers.
+ */
+struct mcam_camera {
+	/*
+	 * These fields should be set by the platform code prior to
+	 * calling mcam_register().
+	 */
+	struct i2c_adapter *i2c_adapter;
+	unsigned char __iomem *regs;
+	unsigned regs_size; /* size in bytes of the register space */
+	spinlock_t dev_lock;
+	struct device *dev; /* For messages, dma alloc */
+	enum mcam_chip_id chip_id;
+	short int clock_speed;	/* Sensor clock speed, default 30 */
+	short int use_smbus;	/* SMBUS or straight I2c? */
+	enum mcam_buffer_mode buffer_mode;
+
+	int mclk_min;	/* The minimal value of mclk */
+	int mclk_src;	/* which clock source the mclk derives from */
+	int mclk_div;	/* Clock Divider Value for MCLK */
+
+	int ccic_id;
+	enum v4l2_mbus_type bus_type;
+	/* MIPI support */
+	/* The dphy config value, allocated in board file
+	 * dphy[0]: DPHY3
+	 * dphy[1]: DPHY5
+	 * dphy[2]: DPHY6
+	 */
+	int *dphy;
+	bool mipi_enabled;	/* flag whether mipi is enabled already */
+	int lane;			/* lane number */
+
+	/* clock tree support */
+	struct clk *clk[NR_MCAM_CLK];
+
+	/*
+	 * Callbacks from the core to the platform code.
+	 */
+	int (*plat_power_up) (struct mcam_camera *cam);
+	void (*plat_power_down) (struct mcam_camera *cam);
+	void (*calc_dphy) (struct mcam_camera *cam);
+	void (*ctlr_reset) (struct mcam_camera *cam);
+
+	/*
+	 * Everything below here is private to the mcam core and
+	 * should not be touched by the platform code.
+	 */
+	struct v4l2_device v4l2_dev;
+	struct v4l2_ctrl_handler ctrl_handler;
+	enum mcam_state state;
+	unsigned long flags;		/* Buffer status, mainly (dev_lock) */
+
+	struct mcam_frame_state frame_state;	/* Frame state counter */
+	/*
+	 * Subsystem structures.
+	 */
+	struct video_device vdev;
+	struct v4l2_subdev *sensor;
+	unsigned short sensor_addr;
+
+	/* Videobuf2 stuff */
+	struct vb2_queue vb_queue;
+	struct list_head buffers;	/* Available frames */
+
+	unsigned int nbufs;		/* How many are alloc'd */
+	int next_buf;			/* Next to consume (dev_lock) */
+
+	char bus_info[32];		/* querycap bus_info */
+
+	/* DMA buffers - vmalloc mode */
+#ifdef MCAM_MODE_VMALLOC
+	unsigned int dma_buf_size;	/* allocated size */
+	void *dma_bufs[MAX_DMA_BUFS];	/* Internal buffer addresses */
+	dma_addr_t dma_handles[MAX_DMA_BUFS]; /* Buffer bus addresses */
+	struct tasklet_struct s_tasklet;
+#endif
+	unsigned int sequence;		/* Frame sequence number */
+	unsigned int buf_seq[MAX_DMA_BUFS]; /* Sequence for individual bufs */
+
+	/* DMA buffers - DMA modes */
+	struct mcam_vb_buffer *vb_bufs[MAX_DMA_BUFS];
+	struct vb2_alloc_ctx *vb_alloc_ctx;
+	struct vb2_alloc_ctx *vb_alloc_ctx_sg;
+
+	/* Mode-specific ops, set at open time */
+	void (*dma_setup)(struct mcam_camera *cam);
+	void (*frame_complete)(struct mcam_camera *cam, int frame);
+
+	/* Current operating parameters */
+	struct v4l2_pix_format pix_format;
+	u32 mbus_code;
+
+	/* Locks */
+	struct mutex s_mutex; /* Access to this structure */
+};
+
+
+/*
+ * Register I/O functions.  These are here because the platform code
+ * may legitimately need to mess with the register space.
+ */
+/*
+ * Device register I/O
+ */
+static inline void mcam_reg_write(struct mcam_camera *cam, unsigned int reg,
+		unsigned int val)
+{
+	iowrite32(val, cam->regs + reg);
+}
+
+static inline unsigned int mcam_reg_read(struct mcam_camera *cam,
+		unsigned int reg)
+{
+	return ioread32(cam->regs + reg);
+}
+
+
+static inline void mcam_reg_write_mask(struct mcam_camera *cam, unsigned int reg,
+		unsigned int val, unsigned int mask)
+{
+	unsigned int v = mcam_reg_read(cam, reg);
+
+	v = (v & ~mask) | (val & mask);
+	mcam_reg_write(cam, reg, v);
+}
+
+static inline void mcam_reg_clear_bit(struct mcam_camera *cam,
+		unsigned int reg, unsigned int val)
+{
+	mcam_reg_write_mask(cam, reg, 0, val);
+}
+
+static inline void mcam_reg_set_bit(struct mcam_camera *cam,
+		unsigned int reg, unsigned int val)
+{
+	mcam_reg_write_mask(cam, reg, val, val);
+}
+
+/*
+ * Functions for use by platform code.
+ */
+int mccic_register(struct mcam_camera *cam);
+int mccic_irq(struct mcam_camera *cam, unsigned int irqs);
+void mccic_shutdown(struct mcam_camera *cam);
+#ifdef CONFIG_PM
+void mccic_suspend(struct mcam_camera *cam);
+int mccic_resume(struct mcam_camera *cam);
+#endif
+
+/*
+ * Register definitions for the m88alp01 camera interface.  Offsets in bytes
+ * as given in the spec.
+ */
+#define REG_Y0BAR	0x00
+#define REG_Y1BAR	0x04
+#define REG_Y2BAR	0x08
+#define REG_U0BAR	0x0c
+#define REG_U1BAR	0x10
+#define REG_U2BAR	0x14
+#define REG_V0BAR	0x18
+#define REG_V1BAR	0x1C
+#define REG_V2BAR	0x20
+
+/*
+ * register definitions for MIPI support
+ */
+#define REG_CSI2_CTRL0	0x100
+#define   CSI2_C0_MIPI_EN (0x1 << 0)
+#define   CSI2_C0_ACT_LANE(n) ((n-1) << 1)
+#define REG_CSI2_DPHY3	0x12c
+#define REG_CSI2_DPHY5	0x134
+#define REG_CSI2_DPHY6	0x138
+
+/* ... */
+
+#define REG_IMGPITCH	0x24	/* Image pitch register */
+#define   IMGP_YP_SHFT	  2		/* Y pitch params */
+#define   IMGP_YP_MASK	  0x00003ffc	/* Y pitch field */
+#define	  IMGP_UVP_SHFT	  18		/* UV pitch (planar) */
+#define   IMGP_UVP_MASK   0x3ffc0000
+#define REG_IRQSTATRAW	0x28	/* RAW IRQ Status */
+#define   IRQ_EOF0	  0x00000001	/* End of frame 0 */
+#define   IRQ_EOF1	  0x00000002	/* End of frame 1 */
+#define   IRQ_EOF2	  0x00000004	/* End of frame 2 */
+#define   IRQ_SOF0	  0x00000008	/* Start of frame 0 */
+#define   IRQ_SOF1	  0x00000010	/* Start of frame 1 */
+#define   IRQ_SOF2	  0x00000020	/* Start of frame 2 */
+#define   IRQ_OVERFLOW	  0x00000040	/* FIFO overflow */
+#define   IRQ_TWSIW	  0x00010000	/* TWSI (smbus) write */
+#define   IRQ_TWSIR	  0x00020000	/* TWSI read */
+#define   IRQ_TWSIE	  0x00040000	/* TWSI error */
+#define   TWSIIRQS (IRQ_TWSIW|IRQ_TWSIR|IRQ_TWSIE)
+#define   FRAMEIRQS (IRQ_EOF0|IRQ_EOF1|IRQ_EOF2|IRQ_SOF0|IRQ_SOF1|IRQ_SOF2)
+#define   ALLIRQS (TWSIIRQS|FRAMEIRQS|IRQ_OVERFLOW)
+#define REG_IRQMASK	0x2c	/* IRQ mask - same bits as IRQSTAT */
+#define REG_IRQSTAT	0x30	/* IRQ status / clear */
+
+#define REG_IMGSIZE	0x34	/* Image size */
+#define  IMGSZ_V_MASK	  0x1fff0000
+#define  IMGSZ_V_SHIFT	  16
+#define	 IMGSZ_H_MASK	  0x00003fff
+#define REG_IMGOFFSET	0x38	/* IMage offset */
+
+#define REG_CTRL0	0x3c	/* Control 0 */
+#define   C0_ENABLE	  0x00000001	/* Makes the whole thing go */
+
+/* Mask for all the format bits */
+#define   C0_DF_MASK	  0x00fffffc    /* Bits 2-23 */
+
+/* RGB ordering */
+#define	  C0_RGB4_RGBX	  0x00000000
+#define	  C0_RGB4_XRGB	  0x00000004
+#define	  C0_RGB4_BGRX	  0x00000008
+#define	  C0_RGB4_XBGR	  0x0000000c
+#define	  C0_RGB5_RGGB	  0x00000000
+#define	  C0_RGB5_GRBG	  0x00000004
+#define	  C0_RGB5_GBRG	  0x00000008
+#define	  C0_RGB5_BGGR	  0x0000000c
+
+/* Spec has two fields for DIN and DOUT, but they must match, so
+   combine them here. */
+#define	  C0_DF_YUV	  0x00000000	/* Data is YUV	    */
+#define	  C0_DF_RGB	  0x000000a0	/* ... RGB		    */
+#define	  C0_DF_BAYER	  0x00000140	/* ... Bayer		    */
+/* 8-8-8 must be missing from the below - ask */
+#define	  C0_RGBF_565	  0x00000000
+#define	  C0_RGBF_444	  0x00000800
+#define	  C0_RGB_BGR	  0x00001000	/* Blue comes first */
+#define	  C0_YUV_PLANAR	  0x00000000	/* YUV 422 planar format */
+#define	  C0_YUV_PACKED	  0x00008000	/* YUV 422 packed	*/
+#define	  C0_YUV_420PL	  0x0000a000	/* YUV 420 planar	*/
+/* Think that 420 packed must be 111 - ask */
+#define	  C0_YUVE_YUYV	  0x00000000	/* Y1CbY0Cr		*/
+#define	  C0_YUVE_YVYU	  0x00010000	/* Y1CrY0Cb		*/
+#define	  C0_YUVE_VYUY	  0x00020000	/* CrY1CbY0		*/
+#define	  C0_YUVE_UYVY	  0x00030000	/* CbY1CrY0		*/
+#define	  C0_YUVE_NOSWAP  0x00000000	/* no bytes swapping	*/
+#define	  C0_YUVE_SWAP13  0x00010000	/* swap byte 1 and 3	*/
+#define	  C0_YUVE_SWAP24  0x00020000	/* swap byte 2 and 4	*/
+#define	  C0_YUVE_SWAP1324 0x00030000	/* swap bytes 1&3 and 2&4 */
+/* Bayer bits 18,19 if needed */
+#define	  C0_EOF_VSYNC	  0x00400000	/* Generate EOF by VSYNC */
+#define	  C0_VEDGE_CTRL   0x00800000	/* Detect falling edge of VSYNC */
+#define	  C0_HPOL_LOW	  0x01000000	/* HSYNC polarity active low */
+#define	  C0_VPOL_LOW	  0x02000000	/* VSYNC polarity active low */
+#define	  C0_VCLK_LOW	  0x04000000	/* VCLK on falling edge */
+#define	  C0_DOWNSCALE	  0x08000000	/* Enable downscaler */
+/* SIFMODE */
+#define	  C0_SIF_HVSYNC	  0x00000000	/* Use H/VSYNC */
+#define	  C0_SOF_NOSYNC	  0x40000000	/* Use inband active signaling */
+#define	  C0_SIFM_MASK	  0xc0000000	/* SIF mode bits */
+
+/* Bits below C1_444ALPHA are not present in Cafe */
+#define REG_CTRL1	0x40	/* Control 1 */
+#define	  C1_CLKGATE	  0x00000001	/* Sensor clock gate */
+#define   C1_DESC_ENA	  0x00000100	/* DMA descriptor enable */
+#define   C1_DESC_3WORD   0x00000200	/* Three-word descriptors used */
+#define	  C1_444ALPHA	  0x00f00000	/* Alpha field in RGB444 */
+#define	  C1_ALPHA_SHFT	  20
+#define	  C1_DMAB32	  0x00000000	/* 32-byte DMA burst */
+#define	  C1_DMAB16	  0x02000000	/* 16-byte DMA burst */
+#define	  C1_DMAB64	  0x04000000	/* 64-byte DMA burst */
+#define	  C1_DMAB_MASK	  0x06000000
+#define	  C1_TWOBUFS	  0x08000000	/* Use only two DMA buffers */
+#define	  C1_PWRDWN	  0x10000000	/* Power down */
+
+#define REG_CLKCTRL	0x88	/* Clock control */
+#define	  CLK_DIV_MASK	  0x0000ffff	/* Upper bits RW "reserved" */
+
+/* This appears to be a Cafe-only register */
+#define REG_UBAR	0xc4	/* Upper base address register */
+
+/* Armada 610 DMA descriptor registers */
+#define	REG_DMA_DESC_Y	0x200
+#define	REG_DMA_DESC_U	0x204
+#define	REG_DMA_DESC_V	0x208
+#define REG_DESC_LEN_Y	0x20c	/* Lengths are in bytes */
+#define	REG_DESC_LEN_U	0x210
+#define REG_DESC_LEN_V	0x214
+
+/*
+ * Useful stuff that probably belongs somewhere global.
+ */
+#define VGA_WIDTH	640
+#define VGA_HEIGHT	480
+
+#endif /* _MCAM_CORE_H */
diff --git a/drivers/media/platform/marvell-ccic/mmp-driver.c b/drivers/media/platform/marvell-ccic/mmp-driver.c
new file mode 100644
index 0000000..b5f165a
--- /dev/null
+++ b/drivers/media/platform/marvell-ccic/mmp-driver.c
@@ -0,0 +1,537 @@
+/*
+ * Support for the camera device found on Marvell MMP processors; known
+ * to work with the Armada 610 as used in the OLPC 1.75 system.
+ *
+ * Copyright 2011 Jonathan Corbet <corbet@lwn.net>
+ *
+ * This file may be distributed under the terms of the GNU General
+ * Public License, version 2.
+ */
+
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/i2c-gpio.h>
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/mmp-camera.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/gpio.h>
+#include <linux/io.h>
+#include <linux/delay.h>
+#include <linux/list.h>
+#include <linux/pm.h>
+#include <linux/clk.h>
+
+#include "mcam-core.h"
+
+MODULE_ALIAS("platform:mmp-camera");
+MODULE_AUTHOR("Jonathan Corbet <corbet@lwn.net>");
+MODULE_LICENSE("GPL");
+
+static char *mcam_clks[] = {"CCICAXICLK", "CCICFUNCLK", "CCICPHYCLK"};
+
+struct mmp_camera {
+	void *power_regs;
+	struct platform_device *pdev;
+	struct mcam_camera mcam;
+	struct list_head devlist;
+	struct clk *mipi_clk;
+	int irq;
+};
+
+static inline struct mmp_camera *mcam_to_cam(struct mcam_camera *mcam)
+{
+	return container_of(mcam, struct mmp_camera, mcam);
+}
+
+/*
+ * A silly little infrastructure so we can keep track of our devices.
+ * Chances are that we will never have more than one of them, but
+ * the Armada 610 *does* have two controllers...
+ */
+
+static LIST_HEAD(mmpcam_devices);
+static struct mutex mmpcam_devices_lock;
+
+static void mmpcam_add_device(struct mmp_camera *cam)
+{
+	mutex_lock(&mmpcam_devices_lock);
+	list_add(&cam->devlist, &mmpcam_devices);
+	mutex_unlock(&mmpcam_devices_lock);
+}
+
+static void mmpcam_remove_device(struct mmp_camera *cam)
+{
+	mutex_lock(&mmpcam_devices_lock);
+	list_del(&cam->devlist);
+	mutex_unlock(&mmpcam_devices_lock);
+}
+
+/*
+ * Platform dev remove passes us a platform_device, and there's
+ * no handy unused drvdata to stash a backpointer in.  So just
+ * dig it out of our list.
+ */
+static struct mmp_camera *mmpcam_find_device(struct platform_device *pdev)
+{
+	struct mmp_camera *cam;
+
+	mutex_lock(&mmpcam_devices_lock);
+	list_for_each_entry(cam, &mmpcam_devices, devlist) {
+		if (cam->pdev == pdev) {
+			mutex_unlock(&mmpcam_devices_lock);
+			return cam;
+		}
+	}
+	mutex_unlock(&mmpcam_devices_lock);
+	return NULL;
+}
+
+
+
+
+/*
+ * Power-related registers; this almost certainly belongs
+ * somewhere else.
+ *
+ * ARMADA 610 register manual, sec 7.2.1, p1842.
+ */
+#define CPU_SUBSYS_PMU_BASE	0xd4282800
+#define REG_CCIC_DCGCR		0x28	/* CCIC dyn clock gate ctrl reg */
+#define REG_CCIC_CRCR		0x50	/* CCIC clk reset ctrl reg	*/
+#define REG_CCIC2_CRCR		0xf4	/* CCIC2 clk reset ctrl reg	*/
+
+static void mcam_clk_enable(struct mcam_camera *mcam)
+{
+	unsigned int i;
+
+	for (i = 0; i < NR_MCAM_CLK; i++) {
+		if (!IS_ERR(mcam->clk[i]))
+			clk_prepare_enable(mcam->clk[i]);
+	}
+}
+
+static void mcam_clk_disable(struct mcam_camera *mcam)
+{
+	int i;
+
+	for (i = NR_MCAM_CLK - 1; i >= 0; i--) {
+		if (!IS_ERR(mcam->clk[i]))
+			clk_disable_unprepare(mcam->clk[i]);
+	}
+}
+
+/*
+ * Power control.
+ */
+static void mmpcam_power_up_ctlr(struct mmp_camera *cam)
+{
+	iowrite32(0x3f, cam->power_regs + REG_CCIC_DCGCR);
+	iowrite32(0x3805b, cam->power_regs + REG_CCIC_CRCR);
+	mdelay(1);
+}
+
+static int mmpcam_power_up(struct mcam_camera *mcam)
+{
+	struct mmp_camera *cam = mcam_to_cam(mcam);
+	struct mmp_camera_platform_data *pdata;
+
+/*
+ * Turn on power and clocks to the controller.
+ */
+	mmpcam_power_up_ctlr(cam);
+/*
+ * Provide power to the sensor.
+ */
+	mcam_reg_write(mcam, REG_CLKCTRL, 0x60000002);
+	pdata = cam->pdev->dev.platform_data;
+	gpio_set_value(pdata->sensor_power_gpio, 1);
+	mdelay(5);
+	mcam_reg_clear_bit(mcam, REG_CTRL1, 0x10000000);
+	gpio_set_value(pdata->sensor_reset_gpio, 0); /* reset is active low */
+	mdelay(5);
+	gpio_set_value(pdata->sensor_reset_gpio, 1); /* reset is active low */
+	mdelay(5);
+
+	mcam_clk_enable(mcam);
+
+	return 0;
+}
+
+static void mmpcam_power_down(struct mcam_camera *mcam)
+{
+	struct mmp_camera *cam = mcam_to_cam(mcam);
+	struct mmp_camera_platform_data *pdata;
+/*
+ * Turn off clocks and set reset lines
+ */
+	iowrite32(0, cam->power_regs + REG_CCIC_DCGCR);
+	iowrite32(0, cam->power_regs + REG_CCIC_CRCR);
+/*
+ * Shut down the sensor.
+ */
+	pdata = cam->pdev->dev.platform_data;
+	gpio_set_value(pdata->sensor_power_gpio, 0);
+	gpio_set_value(pdata->sensor_reset_gpio, 0);
+
+	mcam_clk_disable(mcam);
+}
+
+void mcam_ctlr_reset(struct mcam_camera *mcam)
+{
+	unsigned long val;
+	struct mmp_camera *cam = mcam_to_cam(mcam);
+
+	if (mcam->ccic_id) {
+		/*
+		 * Using CCIC2
+		 */
+		val = ioread32(cam->power_regs + REG_CCIC2_CRCR);
+		iowrite32(val & ~0x2, cam->power_regs + REG_CCIC2_CRCR);
+		iowrite32(val | 0x2, cam->power_regs + REG_CCIC2_CRCR);
+	} else {
+		/*
+		 * Using CCIC1
+		 */
+		val = ioread32(cam->power_regs + REG_CCIC_CRCR);
+		iowrite32(val & ~0x2, cam->power_regs + REG_CCIC_CRCR);
+		iowrite32(val | 0x2, cam->power_regs + REG_CCIC_CRCR);
+	}
+}
+
+/*
+ * calc the dphy register values
+ * There are three dphy registers being used.
+ * dphy[0] - CSI2_DPHY3
+ * dphy[1] - CSI2_DPHY5
+ * dphy[2] - CSI2_DPHY6
+ * CSI2_DPHY3 and CSI2_DPHY6 can be set with a default value
+ * or be calculated dynamically
+ */
+void mmpcam_calc_dphy(struct mcam_camera *mcam)
+{
+	struct mmp_camera *cam = mcam_to_cam(mcam);
+	struct mmp_camera_platform_data *pdata = cam->pdev->dev.platform_data;
+	struct device *dev = &cam->pdev->dev;
+	unsigned long tx_clk_esc;
+
+	/*
+	 * If CSI2_DPHY3 is calculated dynamically,
+	 * pdata->lane_clk should be already set
+	 * either in the board driver statically
+	 * or in the sensor driver dynamically.
+	 */
+	/*
+	 * dphy[0] - CSI2_DPHY3:
+	 *  bit 0 ~ bit 7: HS Term Enable.
+	 *   defines the time that the DPHY
+	 *   wait before enabling the data
+	 *   lane termination after detecting
+	 *   that the sensor has driven the data
+	 *   lanes to the LP00 bridge state.
+	 *   The value is calculated by:
+	 *   (Max T(D_TERM_EN)/Period(DDR)) - 1
+	 *  bit 8 ~ bit 15: HS_SETTLE
+	 *   Time interval during which the HS
+	 *   receiver shall ignore any Data Lane
+	 *   HS transistions.
+	 *   The vaule has been calibrated on
+	 *   different boards. It seems to work well.
+	 *
+	 *  More detail please refer
+	 *  MIPI Alliance Spectification for D-PHY
+	 *  document for explanation of HS-SETTLE
+	 *  and D-TERM-EN.
+	 */
+	switch (pdata->dphy3_algo) {
+	case DPHY3_ALGO_PXA910:
+		/*
+		 * Calculate CSI2_DPHY3 algo for PXA910
+		 */
+		pdata->dphy[0] =
+			(((1 + (pdata->lane_clk * 80) / 1000) & 0xff) << 8)
+			| (1 + pdata->lane_clk * 35 / 1000);
+		break;
+	case DPHY3_ALGO_PXA2128:
+		/*
+		 * Calculate CSI2_DPHY3 algo for PXA2128
+		 */
+		pdata->dphy[0] =
+			(((2 + (pdata->lane_clk * 110) / 1000) & 0xff) << 8)
+			| (1 + pdata->lane_clk * 35 / 1000);
+		break;
+	default:
+		/*
+		 * Use default CSI2_DPHY3 value for PXA688/PXA988
+		 */
+		dev_dbg(dev, "camera: use the default CSI2_DPHY3 value\n");
+	}
+
+	/*
+	 * mipi_clk will never be changed, it is a fixed value on MMP
+	 */
+	if (IS_ERR(cam->mipi_clk))
+		return;
+
+	/* get the escape clk, this is hard coded */
+	clk_prepare_enable(cam->mipi_clk);
+	tx_clk_esc = (clk_get_rate(cam->mipi_clk) / 1000000) / 12;
+	clk_disable_unprepare(cam->mipi_clk);
+	/*
+	 * dphy[2] - CSI2_DPHY6:
+	 * bit 0 ~ bit 7: CK Term Enable
+	 *  Time for the Clock Lane receiver to enable the HS line
+	 *  termination. The value is calculated similarly with
+	 *  HS Term Enable
+	 * bit 8 ~ bit 15: CK Settle
+	 *  Time interval during which the HS receiver shall ignore
+	 *  any Clock Lane HS transitions.
+	 *  The value is calibrated on the boards.
+	 */
+	pdata->dphy[2] =
+		((((534 * tx_clk_esc) / 2000 - 1) & 0xff) << 8)
+		| (((38 * tx_clk_esc) / 1000 - 1) & 0xff);
+
+	dev_dbg(dev, "camera: DPHY sets: dphy3=0x%x, dphy5=0x%x, dphy6=0x%x\n",
+		pdata->dphy[0], pdata->dphy[1], pdata->dphy[2]);
+}
+
+static irqreturn_t mmpcam_irq(int irq, void *data)
+{
+	struct mcam_camera *mcam = data;
+	unsigned int irqs, handled;
+
+	spin_lock(&mcam->dev_lock);
+	irqs = mcam_reg_read(mcam, REG_IRQSTAT);
+	handled = mccic_irq(mcam, irqs);
+	spin_unlock(&mcam->dev_lock);
+	return IRQ_RETVAL(handled);
+}
+
+static void mcam_init_clk(struct mcam_camera *mcam)
+{
+	unsigned int i;
+
+	for (i = 0; i < NR_MCAM_CLK; i++) {
+		if (mcam_clks[i] != NULL) {
+			/* Some clks are not necessary on some boards
+			 * We still try to run even it fails getting clk
+			 */
+			mcam->clk[i] = devm_clk_get(mcam->dev, mcam_clks[i]);
+			if (IS_ERR(mcam->clk[i]))
+				dev_warn(mcam->dev, "Could not get clk: %s\n",
+						mcam_clks[i]);
+		}
+	}
+}
+
+static int mmpcam_probe(struct platform_device *pdev)
+{
+	struct mmp_camera *cam;
+	struct mcam_camera *mcam;
+	struct resource *res;
+	struct mmp_camera_platform_data *pdata;
+	int ret;
+
+	pdata = pdev->dev.platform_data;
+	if (!pdata)
+		return -ENODEV;
+
+	cam = devm_kzalloc(&pdev->dev, sizeof(*cam), GFP_KERNEL);
+	if (cam == NULL)
+		return -ENOMEM;
+	cam->pdev = pdev;
+	INIT_LIST_HEAD(&cam->devlist);
+
+	mcam = &cam->mcam;
+	mcam->plat_power_up = mmpcam_power_up;
+	mcam->plat_power_down = mmpcam_power_down;
+	mcam->ctlr_reset = mcam_ctlr_reset;
+	mcam->calc_dphy = mmpcam_calc_dphy;
+	mcam->dev = &pdev->dev;
+	mcam->use_smbus = 0;
+	mcam->ccic_id = pdev->id;
+	mcam->mclk_min = pdata->mclk_min;
+	mcam->mclk_src = pdata->mclk_src;
+	mcam->mclk_div = pdata->mclk_div;
+	mcam->bus_type = pdata->bus_type;
+	mcam->dphy = pdata->dphy;
+	if (mcam->bus_type == V4L2_MBUS_CSI2) {
+		cam->mipi_clk = devm_clk_get(mcam->dev, "mipi");
+		if ((IS_ERR(cam->mipi_clk) && mcam->dphy[2] == 0))
+			return PTR_ERR(cam->mipi_clk);
+	}
+	mcam->mipi_enabled = false;
+	mcam->lane = pdata->lane;
+	mcam->chip_id = MCAM_ARMADA610;
+	mcam->buffer_mode = B_DMA_sg;
+	strlcpy(mcam->bus_info, "platform:mmp-camera", sizeof(mcam->bus_info));
+	spin_lock_init(&mcam->dev_lock);
+	/*
+	 * Get our I/O memory.
+	 */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	mcam->regs = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(mcam->regs))
+		return PTR_ERR(mcam->regs);
+	mcam->regs_size = resource_size(res);
+	/*
+	 * Power/clock memory is elsewhere; get it too.  Perhaps this
+	 * should really be managed outside of this driver?
+	 */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	cam->power_regs = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(cam->power_regs))
+		return PTR_ERR(cam->power_regs);
+	/*
+	 * Find the i2c adapter.  This assumes, of course, that the
+	 * i2c bus is already up and functioning.
+	 */
+	mcam->i2c_adapter = platform_get_drvdata(pdata->i2c_device);
+	if (mcam->i2c_adapter == NULL) {
+		dev_err(&pdev->dev, "No i2c adapter\n");
+		return -ENODEV;
+	}
+	/*
+	 * Sensor GPIO pins.
+	 */
+	ret = devm_gpio_request(&pdev->dev, pdata->sensor_power_gpio,
+							"cam-power");
+	if (ret) {
+		dev_err(&pdev->dev, "Can't get sensor power gpio %d",
+				pdata->sensor_power_gpio);
+		return ret;
+	}
+	gpio_direction_output(pdata->sensor_power_gpio, 0);
+	ret = devm_gpio_request(&pdev->dev, pdata->sensor_reset_gpio,
+							"cam-reset");
+	if (ret) {
+		dev_err(&pdev->dev, "Can't get sensor reset gpio %d",
+				pdata->sensor_reset_gpio);
+		return ret;
+	}
+	gpio_direction_output(pdata->sensor_reset_gpio, 0);
+
+	mcam_init_clk(mcam);
+
+	/*
+	 * Power the device up and hand it off to the core.
+	 */
+	ret = mmpcam_power_up(mcam);
+	if (ret)
+		return ret;
+	ret = mccic_register(mcam);
+	if (ret)
+		goto out_power_down;
+	/*
+	 * Finally, set up our IRQ now that the core is ready to
+	 * deal with it.
+	 */
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (res == NULL) {
+		ret = -ENODEV;
+		goto out_unregister;
+	}
+	cam->irq = res->start;
+	ret = devm_request_irq(&pdev->dev, cam->irq, mmpcam_irq, IRQF_SHARED,
+					"mmp-camera", mcam);
+	if (ret == 0) {
+		mmpcam_add_device(cam);
+		return 0;
+	}
+
+out_unregister:
+	mccic_shutdown(mcam);
+out_power_down:
+	mmpcam_power_down(mcam);
+	return ret;
+}
+
+
+static int mmpcam_remove(struct mmp_camera *cam)
+{
+	struct mcam_camera *mcam = &cam->mcam;
+
+	mmpcam_remove_device(cam);
+	mccic_shutdown(mcam);
+	mmpcam_power_down(mcam);
+	return 0;
+}
+
+static int mmpcam_platform_remove(struct platform_device *pdev)
+{
+	struct mmp_camera *cam = mmpcam_find_device(pdev);
+
+	if (cam == NULL)
+		return -ENODEV;
+	return mmpcam_remove(cam);
+}
+
+/*
+ * Suspend/resume support.
+ */
+#ifdef CONFIG_PM
+
+static int mmpcam_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct mmp_camera *cam = mmpcam_find_device(pdev);
+
+	if (state.event != PM_EVENT_SUSPEND)
+		return 0;
+	mccic_suspend(&cam->mcam);
+	return 0;
+}
+
+static int mmpcam_resume(struct platform_device *pdev)
+{
+	struct mmp_camera *cam = mmpcam_find_device(pdev);
+
+	/*
+	 * Power up unconditionally just in case the core tries to
+	 * touch a register even if nothing was active before; trust
+	 * me, it's better this way.
+	 */
+	mmpcam_power_up_ctlr(cam);
+	return mccic_resume(&cam->mcam);
+}
+
+#endif
+
+
+static struct platform_driver mmpcam_driver = {
+	.probe		= mmpcam_probe,
+	.remove		= mmpcam_platform_remove,
+#ifdef CONFIG_PM
+	.suspend	= mmpcam_suspend,
+	.resume		= mmpcam_resume,
+#endif
+	.driver = {
+		.name	= "mmp-camera",
+	}
+};
+
+
+static int __init mmpcam_init_module(void)
+{
+	mutex_init(&mmpcam_devices_lock);
+	return platform_driver_register(&mmpcam_driver);
+}
+
+static void __exit mmpcam_exit_module(void)
+{
+	platform_driver_unregister(&mmpcam_driver);
+	/*
+	 * platform_driver_unregister() should have emptied the list
+	 */
+	if (!list_empty(&mmpcam_devices))
+		printk(KERN_ERR "mmp_camera leaving devices behind\n");
+}
+
+module_init(mmpcam_init_module);
+module_exit(mmpcam_exit_module);
diff --git a/drivers/media/platform/mx2_emmaprp.c b/drivers/media/platform/mx2_emmaprp.c
new file mode 100644
index 0000000..03a1b60
--- /dev/null
+++ b/drivers/media/platform/mx2_emmaprp.c
@@ -0,0 +1,1011 @@
+/*
+ * Support eMMa-PrP through mem2mem framework.
+ *
+ * eMMa-PrP is a piece of HW that allows fetching buffers
+ * from one memory location and do several operations on
+ * them such as scaling or format conversion giving, as a result
+ * a new processed buffer in another memory location.
+ *
+ * Based on mem2mem_testdev.c by Pawel Osciak.
+ *
+ * Copyright (c) 2011 Vista Silicon S.L.
+ * Javier Martin <javier.martin@vista-silicon.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation; either version 2 of the
+ * License, or (at your option) any later version
+ */
+#include <linux/module.h>
+#include <linux/clk.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+
+#include <linux/platform_device.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-dma-contig.h>
+#include <linux/sizes.h>
+
+#define EMMAPRP_MODULE_NAME "mem2mem-emmaprp"
+
+MODULE_DESCRIPTION("Mem-to-mem device which supports eMMa-PrP present in mx2 SoCs");
+MODULE_AUTHOR("Javier Martin <javier.martin@vista-silicon.com");
+MODULE_LICENSE("GPL");
+MODULE_VERSION("0.0.1");
+
+static bool debug;
+module_param(debug, bool, 0644);
+
+#define MIN_W 32
+#define MIN_H 32
+#define MAX_W 2040
+#define MAX_H 2046
+
+#define S_ALIGN		1 /* multiple of 2 */
+#define W_ALIGN_YUV420	3 /* multiple of 8 */
+#define W_ALIGN_OTHERS	2 /* multiple of 4 */
+#define H_ALIGN		1 /* multiple of 2 */
+
+/* Flags that indicate a format can be used for capture/output */
+#define MEM2MEM_CAPTURE	(1 << 0)
+#define MEM2MEM_OUTPUT	(1 << 1)
+
+#define MEM2MEM_NAME		"m2m-emmaprp"
+
+/* In bytes, per queue */
+#define MEM2MEM_VID_MEM_LIMIT	SZ_16M
+
+#define dprintk(dev, fmt, arg...) \
+	v4l2_dbg(1, debug, &dev->v4l2_dev, "%s: " fmt, __func__, ## arg)
+
+/* EMMA PrP */
+#define PRP_CNTL                        0x00
+#define PRP_INTR_CNTL                   0x04
+#define PRP_INTRSTATUS                  0x08
+#define PRP_SOURCE_Y_PTR                0x0c
+#define PRP_SOURCE_CB_PTR               0x10
+#define PRP_SOURCE_CR_PTR               0x14
+#define PRP_DEST_RGB1_PTR               0x18
+#define PRP_DEST_RGB2_PTR               0x1c
+#define PRP_DEST_Y_PTR                  0x20
+#define PRP_DEST_CB_PTR                 0x24
+#define PRP_DEST_CR_PTR                 0x28
+#define PRP_SRC_FRAME_SIZE              0x2c
+#define PRP_DEST_CH1_LINE_STRIDE        0x30
+#define PRP_SRC_PIXEL_FORMAT_CNTL       0x34
+#define PRP_CH1_PIXEL_FORMAT_CNTL       0x38
+#define PRP_CH1_OUT_IMAGE_SIZE          0x3c
+#define PRP_CH2_OUT_IMAGE_SIZE          0x40
+#define PRP_SRC_LINE_STRIDE             0x44
+#define PRP_CSC_COEF_012                0x48
+#define PRP_CSC_COEF_345                0x4c
+#define PRP_CSC_COEF_678                0x50
+#define PRP_CH1_RZ_HORI_COEF1           0x54
+#define PRP_CH1_RZ_HORI_COEF2           0x58
+#define PRP_CH1_RZ_HORI_VALID           0x5c
+#define PRP_CH1_RZ_VERT_COEF1           0x60
+#define PRP_CH1_RZ_VERT_COEF2           0x64
+#define PRP_CH1_RZ_VERT_VALID           0x68
+#define PRP_CH2_RZ_HORI_COEF1           0x6c
+#define PRP_CH2_RZ_HORI_COEF2           0x70
+#define PRP_CH2_RZ_HORI_VALID           0x74
+#define PRP_CH2_RZ_VERT_COEF1           0x78
+#define PRP_CH2_RZ_VERT_COEF2           0x7c
+#define PRP_CH2_RZ_VERT_VALID           0x80
+
+#define PRP_CNTL_CH1EN          (1 << 0)
+#define PRP_CNTL_CH2EN          (1 << 1)
+#define PRP_CNTL_CSIEN          (1 << 2)
+#define PRP_CNTL_DATA_IN_YUV420 (0 << 3)
+#define PRP_CNTL_DATA_IN_YUV422 (1 << 3)
+#define PRP_CNTL_DATA_IN_RGB16  (2 << 3)
+#define PRP_CNTL_DATA_IN_RGB32  (3 << 3)
+#define PRP_CNTL_CH1_OUT_RGB8   (0 << 5)
+#define PRP_CNTL_CH1_OUT_RGB16  (1 << 5)
+#define PRP_CNTL_CH1_OUT_RGB32  (2 << 5)
+#define PRP_CNTL_CH1_OUT_YUV422 (3 << 5)
+#define PRP_CNTL_CH2_OUT_YUV420 (0 << 7)
+#define PRP_CNTL_CH2_OUT_YUV422 (1 << 7)
+#define PRP_CNTL_CH2_OUT_YUV444 (2 << 7)
+#define PRP_CNTL_CH1_LEN        (1 << 9)
+#define PRP_CNTL_CH2_LEN        (1 << 10)
+#define PRP_CNTL_SKIP_FRAME     (1 << 11)
+#define PRP_CNTL_SWRST          (1 << 12)
+#define PRP_CNTL_CLKEN          (1 << 13)
+#define PRP_CNTL_WEN            (1 << 14)
+#define PRP_CNTL_CH1BYP         (1 << 15)
+#define PRP_CNTL_IN_TSKIP(x)    ((x) << 16)
+#define PRP_CNTL_CH1_TSKIP(x)   ((x) << 19)
+#define PRP_CNTL_CH2_TSKIP(x)   ((x) << 22)
+#define PRP_CNTL_INPUT_FIFO_LEVEL(x)    ((x) << 25)
+#define PRP_CNTL_RZ_FIFO_LEVEL(x)       ((x) << 27)
+#define PRP_CNTL_CH2B1EN        (1 << 29)
+#define PRP_CNTL_CH2B2EN        (1 << 30)
+#define PRP_CNTL_CH2FEN         (1 << 31)
+
+#define PRP_SIZE_HEIGHT(x)	(x)
+#define PRP_SIZE_WIDTH(x)	((x) << 16)
+
+/* IRQ Enable and status register */
+#define PRP_INTR_RDERR          (1 << 0)
+#define PRP_INTR_CH1WERR        (1 << 1)
+#define PRP_INTR_CH2WERR        (1 << 2)
+#define PRP_INTR_CH1FC          (1 << 3)
+#define PRP_INTR_CH2FC          (1 << 5)
+#define PRP_INTR_LBOVF          (1 << 7)
+#define PRP_INTR_CH2OVF         (1 << 8)
+
+#define PRP_INTR_ST_RDERR	(1 << 0)
+#define PRP_INTR_ST_CH1WERR	(1 << 1)
+#define PRP_INTR_ST_CH2WERR	(1 << 2)
+#define PRP_INTR_ST_CH2B2CI	(1 << 3)
+#define PRP_INTR_ST_CH2B1CI	(1 << 4)
+#define PRP_INTR_ST_CH1B2CI	(1 << 5)
+#define PRP_INTR_ST_CH1B1CI	(1 << 6)
+#define PRP_INTR_ST_LBOVF	(1 << 7)
+#define PRP_INTR_ST_CH2OVF	(1 << 8)
+
+struct emmaprp_fmt {
+	char	*name;
+	u32	fourcc;
+	/* Types the format can be used for */
+	u32	types;
+};
+
+static struct emmaprp_fmt formats[] = {
+	{
+		.name	= "YUV 4:2:0 Planar",
+		.fourcc	= V4L2_PIX_FMT_YUV420,
+		.types	= MEM2MEM_CAPTURE,
+	},
+	{
+		.name	= "4:2:2, packed, YUYV",
+		.fourcc	= V4L2_PIX_FMT_YUYV,
+		.types	= MEM2MEM_OUTPUT,
+	},
+};
+
+/* Per-queue, driver-specific private data */
+struct emmaprp_q_data {
+	unsigned int		width;
+	unsigned int		height;
+	unsigned int		sizeimage;
+	struct emmaprp_fmt	*fmt;
+};
+
+enum {
+	V4L2_M2M_SRC = 0,
+	V4L2_M2M_DST = 1,
+};
+
+#define NUM_FORMATS ARRAY_SIZE(formats)
+
+static struct emmaprp_fmt *find_format(struct v4l2_format *f)
+{
+	struct emmaprp_fmt *fmt;
+	unsigned int k;
+
+	for (k = 0; k < NUM_FORMATS; k++) {
+		fmt = &formats[k];
+		if (fmt->fourcc == f->fmt.pix.pixelformat)
+			break;
+	}
+
+	if (k == NUM_FORMATS)
+		return NULL;
+
+	return &formats[k];
+}
+
+struct emmaprp_dev {
+	struct v4l2_device	v4l2_dev;
+	struct video_device	*vfd;
+
+	struct mutex		dev_mutex;
+	spinlock_t		irqlock;
+
+	void __iomem		*base_emma;
+	struct clk		*clk_emma_ahb, *clk_emma_ipg;
+
+	struct v4l2_m2m_dev	*m2m_dev;
+	struct vb2_alloc_ctx	*alloc_ctx;
+};
+
+struct emmaprp_ctx {
+	struct emmaprp_dev	*dev;
+	/* Abort requested by m2m */
+	int			aborting;
+	struct emmaprp_q_data	q_data[2];
+	struct v4l2_m2m_ctx	*m2m_ctx;
+};
+
+static struct emmaprp_q_data *get_q_data(struct emmaprp_ctx *ctx,
+					 enum v4l2_buf_type type)
+{
+	switch (type) {
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+		return &(ctx->q_data[V4L2_M2M_SRC]);
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		return &(ctx->q_data[V4L2_M2M_DST]);
+	default:
+		BUG();
+	}
+	return NULL;
+}
+
+/*
+ * mem2mem callbacks
+ */
+static void emmaprp_job_abort(void *priv)
+{
+	struct emmaprp_ctx *ctx = priv;
+	struct emmaprp_dev *pcdev = ctx->dev;
+
+	ctx->aborting = 1;
+
+	dprintk(pcdev, "Aborting task\n");
+
+	v4l2_m2m_job_finish(pcdev->m2m_dev, ctx->m2m_ctx);
+}
+
+static void emmaprp_lock(void *priv)
+{
+	struct emmaprp_ctx *ctx = priv;
+	struct emmaprp_dev *pcdev = ctx->dev;
+	mutex_lock(&pcdev->dev_mutex);
+}
+
+static void emmaprp_unlock(void *priv)
+{
+	struct emmaprp_ctx *ctx = priv;
+	struct emmaprp_dev *pcdev = ctx->dev;
+	mutex_unlock(&pcdev->dev_mutex);
+}
+
+static inline void emmaprp_dump_regs(struct emmaprp_dev *pcdev)
+{
+	dprintk(pcdev,
+		"eMMa-PrP Registers:\n"
+		"  SOURCE_Y_PTR = 0x%08X\n"
+		"  SRC_FRAME_SIZE = 0x%08X\n"
+		"  DEST_Y_PTR = 0x%08X\n"
+		"  DEST_CR_PTR = 0x%08X\n"
+		"  DEST_CB_PTR = 0x%08X\n"
+		"  CH2_OUT_IMAGE_SIZE = 0x%08X\n"
+		"  CNTL = 0x%08X\n",
+		readl(pcdev->base_emma + PRP_SOURCE_Y_PTR),
+		readl(pcdev->base_emma + PRP_SRC_FRAME_SIZE),
+		readl(pcdev->base_emma + PRP_DEST_Y_PTR),
+		readl(pcdev->base_emma + PRP_DEST_CR_PTR),
+		readl(pcdev->base_emma + PRP_DEST_CB_PTR),
+		readl(pcdev->base_emma + PRP_CH2_OUT_IMAGE_SIZE),
+		readl(pcdev->base_emma + PRP_CNTL));
+}
+
+static void emmaprp_device_run(void *priv)
+{
+	struct emmaprp_ctx *ctx = priv;
+	struct emmaprp_q_data *s_q_data, *d_q_data;
+	struct vb2_buffer *src_buf, *dst_buf;
+	struct emmaprp_dev *pcdev = ctx->dev;
+	unsigned int s_width, s_height;
+	unsigned int d_width, d_height;
+	unsigned int d_size;
+	dma_addr_t p_in, p_out;
+	u32 tmp;
+
+	src_buf = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
+	dst_buf = v4l2_m2m_next_dst_buf(ctx->m2m_ctx);
+
+	s_q_data = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
+	s_width	= s_q_data->width;
+	s_height = s_q_data->height;
+
+	d_q_data = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
+	d_width = d_q_data->width;
+	d_height = d_q_data->height;
+	d_size = d_width * d_height;
+
+	p_in = vb2_dma_contig_plane_dma_addr(src_buf, 0);
+	p_out = vb2_dma_contig_plane_dma_addr(dst_buf, 0);
+	if (!p_in || !p_out) {
+		v4l2_err(&pcdev->v4l2_dev,
+			 "Acquiring kernel pointers to buffers failed\n");
+		return;
+	}
+
+	/* Input frame parameters */
+	writel(p_in, pcdev->base_emma + PRP_SOURCE_Y_PTR);
+	writel(PRP_SIZE_WIDTH(s_width) | PRP_SIZE_HEIGHT(s_height),
+	       pcdev->base_emma + PRP_SRC_FRAME_SIZE);
+
+	/* Output frame parameters */
+	writel(p_out, pcdev->base_emma + PRP_DEST_Y_PTR);
+	writel(p_out + d_size, pcdev->base_emma + PRP_DEST_CB_PTR);
+	writel(p_out + d_size + (d_size >> 2),
+	       pcdev->base_emma + PRP_DEST_CR_PTR);
+	writel(PRP_SIZE_WIDTH(d_width) | PRP_SIZE_HEIGHT(d_height),
+	       pcdev->base_emma + PRP_CH2_OUT_IMAGE_SIZE);
+
+	/* IRQ configuration */
+	tmp = readl(pcdev->base_emma + PRP_INTR_CNTL);
+	writel(tmp | PRP_INTR_RDERR |
+		PRP_INTR_CH2WERR |
+		PRP_INTR_CH2FC,
+		pcdev->base_emma + PRP_INTR_CNTL);
+
+	emmaprp_dump_regs(pcdev);
+
+	/* Enable transfer */
+	tmp = readl(pcdev->base_emma + PRP_CNTL);
+	writel(tmp | PRP_CNTL_CH2_OUT_YUV420 |
+		PRP_CNTL_DATA_IN_YUV422 |
+		PRP_CNTL_CH2EN,
+		pcdev->base_emma + PRP_CNTL);
+}
+
+static irqreturn_t emmaprp_irq(int irq_emma, void *data)
+{
+	struct emmaprp_dev *pcdev = data;
+	struct emmaprp_ctx *curr_ctx;
+	struct vb2_v4l2_buffer *src_vb, *dst_vb;
+	unsigned long flags;
+	u32 irqst;
+
+	/* Check irq flags and clear irq */
+	irqst = readl(pcdev->base_emma + PRP_INTRSTATUS);
+	writel(irqst, pcdev->base_emma + PRP_INTRSTATUS);
+	dprintk(pcdev, "irqst = 0x%08x\n", irqst);
+
+	curr_ctx = v4l2_m2m_get_curr_priv(pcdev->m2m_dev);
+	if (curr_ctx == NULL) {
+		pr_err("Instance released before the end of transaction\n");
+		return IRQ_HANDLED;
+	}
+
+	if (!curr_ctx->aborting) {
+		if ((irqst & PRP_INTR_ST_RDERR) ||
+		(irqst & PRP_INTR_ST_CH2WERR)) {
+			pr_err("PrP bus error occurred, this transfer is probably corrupted\n");
+			writel(PRP_CNTL_SWRST, pcdev->base_emma + PRP_CNTL);
+		} else if (irqst & PRP_INTR_ST_CH2B1CI) { /* buffer ready */
+			src_vb = v4l2_m2m_src_buf_remove(curr_ctx->m2m_ctx);
+			dst_vb = v4l2_m2m_dst_buf_remove(curr_ctx->m2m_ctx);
+
+			dst_vb->timestamp = src_vb->timestamp;
+			dst_vb->flags &=
+				~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+			dst_vb->flags |=
+				src_vb->flags
+				& V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+			dst_vb->timecode = src_vb->timecode;
+
+			spin_lock_irqsave(&pcdev->irqlock, flags);
+			v4l2_m2m_buf_done(src_vb, VB2_BUF_STATE_DONE);
+			v4l2_m2m_buf_done(dst_vb, VB2_BUF_STATE_DONE);
+			spin_unlock_irqrestore(&pcdev->irqlock, flags);
+		}
+	}
+
+	v4l2_m2m_job_finish(pcdev->m2m_dev, curr_ctx->m2m_ctx);
+	return IRQ_HANDLED;
+}
+
+/*
+ * video ioctls
+ */
+static int vidioc_querycap(struct file *file, void *priv,
+			   struct v4l2_capability *cap)
+{
+	strncpy(cap->driver, MEM2MEM_NAME, sizeof(cap->driver) - 1);
+	strncpy(cap->card, MEM2MEM_NAME, sizeof(cap->card) - 1);
+	cap->device_caps = V4L2_CAP_VIDEO_M2M | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	return 0;
+}
+
+static int enum_fmt(struct v4l2_fmtdesc *f, u32 type)
+{
+	int i, num;
+	struct emmaprp_fmt *fmt;
+
+	num = 0;
+
+	for (i = 0; i < NUM_FORMATS; ++i) {
+		if (formats[i].types & type) {
+			/* index-th format of type type found ? */
+			if (num == f->index)
+				break;
+			/* Correct type but haven't reached our index yet,
+			 * just increment per-type index */
+			++num;
+		}
+	}
+
+	if (i < NUM_FORMATS) {
+		/* Format found */
+		fmt = &formats[i];
+		strlcpy(f->description, fmt->name, sizeof(f->description) - 1);
+		f->pixelformat = fmt->fourcc;
+		return 0;
+	}
+
+	/* Format not found */
+	return -EINVAL;
+}
+
+static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
+				   struct v4l2_fmtdesc *f)
+{
+	return enum_fmt(f, MEM2MEM_CAPTURE);
+}
+
+static int vidioc_enum_fmt_vid_out(struct file *file, void *priv,
+				   struct v4l2_fmtdesc *f)
+{
+	return enum_fmt(f, MEM2MEM_OUTPUT);
+}
+
+static int vidioc_g_fmt(struct emmaprp_ctx *ctx, struct v4l2_format *f)
+{
+	struct vb2_queue *vq;
+	struct emmaprp_q_data *q_data;
+
+	vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
+	if (!vq)
+		return -EINVAL;
+
+	q_data = get_q_data(ctx, f->type);
+
+	f->fmt.pix.width	= q_data->width;
+	f->fmt.pix.height	= q_data->height;
+	f->fmt.pix.field	= V4L2_FIELD_NONE;
+	f->fmt.pix.pixelformat	= q_data->fmt->fourcc;
+	if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_YUV420)
+		f->fmt.pix.bytesperline = q_data->width * 3 / 2;
+	else /* YUYV */
+		f->fmt.pix.bytesperline = q_data->width * 2;
+	f->fmt.pix.sizeimage	= q_data->sizeimage;
+
+	return 0;
+}
+
+static int vidioc_g_fmt_vid_out(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	return vidioc_g_fmt(priv, f);
+}
+
+static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	return vidioc_g_fmt(priv, f);
+}
+
+static int vidioc_try_fmt(struct v4l2_format *f)
+{
+	enum v4l2_field field;
+
+
+	if (!find_format(f))
+		return -EINVAL;
+
+	field = f->fmt.pix.field;
+	if (field == V4L2_FIELD_ANY)
+		field = V4L2_FIELD_NONE;
+	else if (V4L2_FIELD_NONE != field)
+		return -EINVAL;
+
+	/* V4L2 specification suggests the driver corrects the format struct
+	 * if any of the dimensions is unsupported */
+	f->fmt.pix.field = field;
+
+	if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_YUV420) {
+		v4l_bound_align_image(&f->fmt.pix.width, MIN_W, MAX_W,
+				      W_ALIGN_YUV420, &f->fmt.pix.height,
+				      MIN_H, MAX_H, H_ALIGN, S_ALIGN);
+		f->fmt.pix.bytesperline = f->fmt.pix.width * 3 / 2;
+	} else {
+		v4l_bound_align_image(&f->fmt.pix.width, MIN_W, MAX_W,
+				      W_ALIGN_OTHERS, &f->fmt.pix.height,
+				      MIN_H, MAX_H, H_ALIGN, S_ALIGN);
+		f->fmt.pix.bytesperline = f->fmt.pix.width * 2;
+	}
+	f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
+
+	return 0;
+}
+
+static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
+				  struct v4l2_format *f)
+{
+	struct emmaprp_fmt *fmt;
+	struct emmaprp_ctx *ctx = priv;
+
+	fmt = find_format(f);
+	if (!fmt || !(fmt->types & MEM2MEM_CAPTURE)) {
+		v4l2_err(&ctx->dev->v4l2_dev,
+			 "Fourcc format (0x%08x) invalid.\n",
+			 f->fmt.pix.pixelformat);
+		return -EINVAL;
+	}
+
+	return vidioc_try_fmt(f);
+}
+
+static int vidioc_try_fmt_vid_out(struct file *file, void *priv,
+				  struct v4l2_format *f)
+{
+	struct emmaprp_fmt *fmt;
+	struct emmaprp_ctx *ctx = priv;
+
+	fmt = find_format(f);
+	if (!fmt || !(fmt->types & MEM2MEM_OUTPUT)) {
+		v4l2_err(&ctx->dev->v4l2_dev,
+			 "Fourcc format (0x%08x) invalid.\n",
+			 f->fmt.pix.pixelformat);
+		return -EINVAL;
+	}
+
+	return vidioc_try_fmt(f);
+}
+
+static int vidioc_s_fmt(struct emmaprp_ctx *ctx, struct v4l2_format *f)
+{
+	struct emmaprp_q_data *q_data;
+	struct vb2_queue *vq;
+	int ret;
+
+	vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
+	if (!vq)
+		return -EINVAL;
+
+	q_data = get_q_data(ctx, f->type);
+	if (!q_data)
+		return -EINVAL;
+
+	if (vb2_is_busy(vq)) {
+		v4l2_err(&ctx->dev->v4l2_dev, "%s queue busy\n", __func__);
+		return -EBUSY;
+	}
+
+	ret = vidioc_try_fmt(f);
+	if (ret)
+		return ret;
+
+	q_data->fmt		= find_format(f);
+	q_data->width		= f->fmt.pix.width;
+	q_data->height		= f->fmt.pix.height;
+	if (q_data->fmt->fourcc == V4L2_PIX_FMT_YUV420)
+		q_data->sizeimage = q_data->width * q_data->height * 3 / 2;
+	else /* YUYV */
+		q_data->sizeimage = q_data->width * q_data->height * 2;
+
+	dprintk(ctx->dev,
+		"Setting format for type %d, wxh: %dx%d, fmt: %d\n",
+		f->type, q_data->width, q_data->height, q_data->fmt->fourcc);
+
+	return 0;
+}
+
+static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	int ret;
+
+	ret = vidioc_try_fmt_vid_cap(file, priv, f);
+	if (ret)
+		return ret;
+
+	return vidioc_s_fmt(priv, f);
+}
+
+static int vidioc_s_fmt_vid_out(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	int ret;
+
+	ret = vidioc_try_fmt_vid_out(file, priv, f);
+	if (ret)
+		return ret;
+
+	return vidioc_s_fmt(priv, f);
+}
+
+static int vidioc_reqbufs(struct file *file, void *priv,
+			  struct v4l2_requestbuffers *reqbufs)
+{
+	struct emmaprp_ctx *ctx = priv;
+
+	return v4l2_m2m_reqbufs(file, ctx->m2m_ctx, reqbufs);
+}
+
+static int vidioc_querybuf(struct file *file, void *priv,
+			   struct v4l2_buffer *buf)
+{
+	struct emmaprp_ctx *ctx = priv;
+
+	return v4l2_m2m_querybuf(file, ctx->m2m_ctx, buf);
+}
+
+static int vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
+{
+	struct emmaprp_ctx *ctx = priv;
+
+	return v4l2_m2m_qbuf(file, ctx->m2m_ctx, buf);
+}
+
+static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
+{
+	struct emmaprp_ctx *ctx = priv;
+
+	return v4l2_m2m_dqbuf(file, ctx->m2m_ctx, buf);
+}
+
+static int vidioc_streamon(struct file *file, void *priv,
+			   enum v4l2_buf_type type)
+{
+	struct emmaprp_ctx *ctx = priv;
+
+	return v4l2_m2m_streamon(file, ctx->m2m_ctx, type);
+}
+
+static int vidioc_streamoff(struct file *file, void *priv,
+			    enum v4l2_buf_type type)
+{
+	struct emmaprp_ctx *ctx = priv;
+
+	return v4l2_m2m_streamoff(file, ctx->m2m_ctx, type);
+}
+
+static const struct v4l2_ioctl_ops emmaprp_ioctl_ops = {
+	.vidioc_querycap	= vidioc_querycap,
+
+	.vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap	= vidioc_g_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap	= vidioc_try_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap	= vidioc_s_fmt_vid_cap,
+
+	.vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out,
+	.vidioc_g_fmt_vid_out	= vidioc_g_fmt_vid_out,
+	.vidioc_try_fmt_vid_out	= vidioc_try_fmt_vid_out,
+	.vidioc_s_fmt_vid_out	= vidioc_s_fmt_vid_out,
+
+	.vidioc_reqbufs		= vidioc_reqbufs,
+	.vidioc_querybuf	= vidioc_querybuf,
+
+	.vidioc_qbuf		= vidioc_qbuf,
+	.vidioc_dqbuf		= vidioc_dqbuf,
+
+	.vidioc_streamon	= vidioc_streamon,
+	.vidioc_streamoff	= vidioc_streamoff,
+};
+
+
+/*
+ * Queue operations
+ */
+static int emmaprp_queue_setup(struct vb2_queue *vq,
+				const void *parg,
+				unsigned int *nbuffers, unsigned int *nplanes,
+				unsigned int sizes[], void *alloc_ctxs[])
+{
+	struct emmaprp_ctx *ctx = vb2_get_drv_priv(vq);
+	struct emmaprp_q_data *q_data;
+	unsigned int size, count = *nbuffers;
+
+	q_data = get_q_data(ctx, vq->type);
+
+	if (q_data->fmt->fourcc == V4L2_PIX_FMT_YUV420)
+		size = q_data->width * q_data->height * 3 / 2;
+	else
+		size = q_data->width * q_data->height * 2;
+
+	while (size * count > MEM2MEM_VID_MEM_LIMIT)
+		(count)--;
+
+	*nplanes = 1;
+	*nbuffers = count;
+	sizes[0] = size;
+
+	alloc_ctxs[0] = ctx->dev->alloc_ctx;
+
+	dprintk(ctx->dev, "get %d buffer(s) of size %d each.\n", count, size);
+
+	return 0;
+}
+
+static int emmaprp_buf_prepare(struct vb2_buffer *vb)
+{
+	struct emmaprp_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct emmaprp_q_data *q_data;
+
+	dprintk(ctx->dev, "type: %d\n", vb->vb2_queue->type);
+
+	q_data = get_q_data(ctx, vb->vb2_queue->type);
+
+	if (vb2_plane_size(vb, 0) < q_data->sizeimage) {
+		dprintk(ctx->dev, "%s data will not fit into plane"
+				  "(%lu < %lu)\n", __func__,
+				  vb2_plane_size(vb, 0),
+				  (long)q_data->sizeimage);
+		return -EINVAL;
+	}
+
+	vb2_set_plane_payload(vb, 0, q_data->sizeimage);
+
+	return 0;
+}
+
+static void emmaprp_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct emmaprp_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	v4l2_m2m_buf_queue(ctx->m2m_ctx, vbuf);
+}
+
+static struct vb2_ops emmaprp_qops = {
+	.queue_setup	 = emmaprp_queue_setup,
+	.buf_prepare	 = emmaprp_buf_prepare,
+	.buf_queue	 = emmaprp_buf_queue,
+};
+
+static int queue_init(void *priv, struct vb2_queue *src_vq,
+		      struct vb2_queue *dst_vq)
+{
+	struct emmaprp_ctx *ctx = priv;
+	int ret;
+
+	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	src_vq->io_modes = VB2_MMAP | VB2_USERPTR;
+	src_vq->drv_priv = ctx;
+	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	src_vq->ops = &emmaprp_qops;
+	src_vq->mem_ops = &vb2_dma_contig_memops;
+	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+
+	ret = vb2_queue_init(src_vq);
+	if (ret)
+		return ret;
+
+	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	dst_vq->io_modes = VB2_MMAP | VB2_USERPTR;
+	dst_vq->drv_priv = ctx;
+	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	dst_vq->ops = &emmaprp_qops;
+	dst_vq->mem_ops = &vb2_dma_contig_memops;
+	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+
+	return vb2_queue_init(dst_vq);
+}
+
+/*
+ * File operations
+ */
+static int emmaprp_open(struct file *file)
+{
+	struct emmaprp_dev *pcdev = video_drvdata(file);
+	struct emmaprp_ctx *ctx;
+
+	ctx = kzalloc(sizeof *ctx, GFP_KERNEL);
+	if (!ctx)
+		return -ENOMEM;
+
+	file->private_data = ctx;
+	ctx->dev = pcdev;
+
+	if (mutex_lock_interruptible(&pcdev->dev_mutex)) {
+		kfree(ctx);
+		return -ERESTARTSYS;
+	}
+
+	ctx->m2m_ctx = v4l2_m2m_ctx_init(pcdev->m2m_dev, ctx, &queue_init);
+
+	if (IS_ERR(ctx->m2m_ctx)) {
+		int ret = PTR_ERR(ctx->m2m_ctx);
+
+		mutex_unlock(&pcdev->dev_mutex);
+		kfree(ctx);
+		return ret;
+	}
+
+	clk_prepare_enable(pcdev->clk_emma_ipg);
+	clk_prepare_enable(pcdev->clk_emma_ahb);
+	ctx->q_data[V4L2_M2M_SRC].fmt = &formats[1];
+	ctx->q_data[V4L2_M2M_DST].fmt = &formats[0];
+	mutex_unlock(&pcdev->dev_mutex);
+
+	dprintk(pcdev, "Created instance %p, m2m_ctx: %p\n", ctx, ctx->m2m_ctx);
+
+	return 0;
+}
+
+static int emmaprp_release(struct file *file)
+{
+	struct emmaprp_dev *pcdev = video_drvdata(file);
+	struct emmaprp_ctx *ctx = file->private_data;
+
+	dprintk(pcdev, "Releasing instance %p\n", ctx);
+
+	mutex_lock(&pcdev->dev_mutex);
+	clk_disable_unprepare(pcdev->clk_emma_ahb);
+	clk_disable_unprepare(pcdev->clk_emma_ipg);
+	v4l2_m2m_ctx_release(ctx->m2m_ctx);
+	mutex_unlock(&pcdev->dev_mutex);
+	kfree(ctx);
+
+	return 0;
+}
+
+static unsigned int emmaprp_poll(struct file *file,
+				 struct poll_table_struct *wait)
+{
+	struct emmaprp_dev *pcdev = video_drvdata(file);
+	struct emmaprp_ctx *ctx = file->private_data;
+	unsigned int res;
+
+	mutex_lock(&pcdev->dev_mutex);
+	res = v4l2_m2m_poll(file, ctx->m2m_ctx, wait);
+	mutex_unlock(&pcdev->dev_mutex);
+	return res;
+}
+
+static int emmaprp_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct emmaprp_dev *pcdev = video_drvdata(file);
+	struct emmaprp_ctx *ctx = file->private_data;
+	int ret;
+
+	if (mutex_lock_interruptible(&pcdev->dev_mutex))
+		return -ERESTARTSYS;
+	ret = v4l2_m2m_mmap(file, ctx->m2m_ctx, vma);
+	mutex_unlock(&pcdev->dev_mutex);
+	return ret;
+}
+
+static const struct v4l2_file_operations emmaprp_fops = {
+	.owner		= THIS_MODULE,
+	.open		= emmaprp_open,
+	.release	= emmaprp_release,
+	.poll		= emmaprp_poll,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap		= emmaprp_mmap,
+};
+
+static struct video_device emmaprp_videodev = {
+	.name		= MEM2MEM_NAME,
+	.fops		= &emmaprp_fops,
+	.ioctl_ops	= &emmaprp_ioctl_ops,
+	.minor		= -1,
+	.release	= video_device_release,
+	.vfl_dir	= VFL_DIR_M2M,
+};
+
+static struct v4l2_m2m_ops m2m_ops = {
+	.device_run	= emmaprp_device_run,
+	.job_abort	= emmaprp_job_abort,
+	.lock		= emmaprp_lock,
+	.unlock		= emmaprp_unlock,
+};
+
+static int emmaprp_probe(struct platform_device *pdev)
+{
+	struct emmaprp_dev *pcdev;
+	struct video_device *vfd;
+	struct resource *res;
+	int irq, ret;
+
+	pcdev = devm_kzalloc(&pdev->dev, sizeof(*pcdev), GFP_KERNEL);
+	if (!pcdev)
+		return -ENOMEM;
+
+	spin_lock_init(&pcdev->irqlock);
+
+	pcdev->clk_emma_ipg = devm_clk_get(&pdev->dev, "ipg");
+	if (IS_ERR(pcdev->clk_emma_ipg)) {
+		return PTR_ERR(pcdev->clk_emma_ipg);
+	}
+
+	pcdev->clk_emma_ahb = devm_clk_get(&pdev->dev, "ahb");
+	if (IS_ERR(pcdev->clk_emma_ahb))
+		return PTR_ERR(pcdev->clk_emma_ahb);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	pcdev->base_emma = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(pcdev->base_emma))
+		return PTR_ERR(pcdev->base_emma);
+
+	ret = v4l2_device_register(&pdev->dev, &pcdev->v4l2_dev);
+	if (ret)
+		return ret;
+
+	mutex_init(&pcdev->dev_mutex);
+
+	vfd = video_device_alloc();
+	if (!vfd) {
+		v4l2_err(&pcdev->v4l2_dev, "Failed to allocate video device\n");
+		ret = -ENOMEM;
+		goto unreg_dev;
+	}
+
+	*vfd = emmaprp_videodev;
+	vfd->lock = &pcdev->dev_mutex;
+	vfd->v4l2_dev = &pcdev->v4l2_dev;
+
+	video_set_drvdata(vfd, pcdev);
+	snprintf(vfd->name, sizeof(vfd->name), "%s", emmaprp_videodev.name);
+	pcdev->vfd = vfd;
+	v4l2_info(&pcdev->v4l2_dev, EMMAPRP_MODULE_NAME
+			" Device registered as /dev/video%d\n", vfd->num);
+
+	platform_set_drvdata(pdev, pcdev);
+
+	irq = platform_get_irq(pdev, 0);
+	ret = devm_request_irq(&pdev->dev, irq, emmaprp_irq, 0,
+			       dev_name(&pdev->dev), pcdev);
+	if (ret)
+		goto rel_vdev;
+
+	pcdev->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
+	if (IS_ERR(pcdev->alloc_ctx)) {
+		v4l2_err(&pcdev->v4l2_dev, "Failed to alloc vb2 context\n");
+		ret = PTR_ERR(pcdev->alloc_ctx);
+		goto rel_vdev;
+	}
+
+	pcdev->m2m_dev = v4l2_m2m_init(&m2m_ops);
+	if (IS_ERR(pcdev->m2m_dev)) {
+		v4l2_err(&pcdev->v4l2_dev, "Failed to init mem2mem device\n");
+		ret = PTR_ERR(pcdev->m2m_dev);
+		goto rel_ctx;
+	}
+
+	ret = video_register_device(vfd, VFL_TYPE_GRABBER, 0);
+	if (ret) {
+		v4l2_err(&pcdev->v4l2_dev, "Failed to register video device\n");
+		goto rel_m2m;
+	}
+
+	return 0;
+
+
+rel_m2m:
+	v4l2_m2m_release(pcdev->m2m_dev);
+rel_ctx:
+	vb2_dma_contig_cleanup_ctx(pcdev->alloc_ctx);
+rel_vdev:
+	video_device_release(vfd);
+unreg_dev:
+	v4l2_device_unregister(&pcdev->v4l2_dev);
+
+	mutex_destroy(&pcdev->dev_mutex);
+
+	return ret;
+}
+
+static int emmaprp_remove(struct platform_device *pdev)
+{
+	struct emmaprp_dev *pcdev = platform_get_drvdata(pdev);
+
+	v4l2_info(&pcdev->v4l2_dev, "Removing " EMMAPRP_MODULE_NAME);
+
+	video_unregister_device(pcdev->vfd);
+	v4l2_m2m_release(pcdev->m2m_dev);
+	vb2_dma_contig_cleanup_ctx(pcdev->alloc_ctx);
+	v4l2_device_unregister(&pcdev->v4l2_dev);
+	mutex_destroy(&pcdev->dev_mutex);
+
+	return 0;
+}
+
+static struct platform_driver emmaprp_pdrv = {
+	.probe		= emmaprp_probe,
+	.remove		= emmaprp_remove,
+	.driver		= {
+		.name	= MEM2MEM_NAME,
+	},
+};
+module_platform_driver(emmaprp_pdrv);
diff --git a/drivers/media/platform/omap/Kconfig b/drivers/media/platform/omap/Kconfig
new file mode 100644
index 0000000..217d613
--- /dev/null
+++ b/drivers/media/platform/omap/Kconfig
@@ -0,0 +1,16 @@
+config VIDEO_OMAP2_VOUT_VRFB
+	bool
+
+config VIDEO_OMAP2_VOUT
+	tristate "OMAP2/OMAP3 V4L2-Display driver"
+	depends on MMU
+	depends on ARCH_OMAP2 || ARCH_OMAP3
+	select VIDEOBUF_GEN
+	select VIDEOBUF_DMA_CONTIG
+	select OMAP2_DSS if HAS_IOMEM && ARCH_OMAP2PLUS
+	select OMAP2_VRFB if ARCH_OMAP2 || ARCH_OMAP3
+	select VIDEO_OMAP2_VOUT_VRFB if VIDEO_OMAP2_VOUT && OMAP2_VRFB
+	select FRAME_VECTOR
+	default n
+	---help---
+	  V4L2 Display driver support for OMAP2/3 based boards.
diff --git a/drivers/media/platform/omap/Makefile b/drivers/media/platform/omap/Makefile
new file mode 100644
index 0000000..d80df41
--- /dev/null
+++ b/drivers/media/platform/omap/Makefile
@@ -0,0 +1,8 @@
+#
+# Makefile for the omap video device drivers.
+#
+
+# OMAP2/3 Display driver
+omap-vout-y += omap_vout.o omap_voutlib.o
+omap-vout-$(CONFIG_VIDEO_OMAP2_VOUT_VRFB) += omap_vout_vrfb.o
+obj-$(CONFIG_VIDEO_OMAP2_VOUT) += omap-vout.o
diff --git a/drivers/media/platform/omap/omap_vout.c b/drivers/media/platform/omap/omap_vout.c
new file mode 100644
index 0000000..70c28d1
--- /dev/null
+++ b/drivers/media/platform/omap/omap_vout.c
@@ -0,0 +1,2278 @@
+/*
+ * omap_vout.c
+ *
+ * Copyright (C) 2005-2010 Texas Instruments.
+ *
+ * This file is licensed under the terms of the GNU General Public License
+ * version 2. This program is licensed "as is" without any warranty of any
+ * kind, whether express or implied.
+ *
+ * Leveraged code from the OMAP2 camera driver
+ * Video-for-Linux (Version 2) camera capture driver for
+ * the OMAP24xx camera controller.
+ *
+ * Author: Andy Lowe (source@mvista.com)
+ *
+ * Copyright (C) 2004 MontaVista Software, Inc.
+ * Copyright (C) 2010 Texas Instruments.
+ *
+ * History:
+ * 20-APR-2006 Khasim		Modified VRFB based Rotation,
+ *				The image data is always read from 0 degree
+ *				view and written
+ *				to the virtual space of desired rotation angle
+ * 4-DEC-2006  Jian		Changed to support better memory management
+ *
+ * 17-Nov-2008 Hardik		Changed driver to use video_ioctl2
+ *
+ * 23-Feb-2010 Vaibhav H	Modified to use new DSS2 interface
+ *
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/vmalloc.h>
+#include <linux/sched.h>
+#include <linux/types.h>
+#include <linux/platform_device.h>
+#include <linux/irq.h>
+#include <linux/videodev2.h>
+#include <linux/dma-mapping.h>
+#include <linux/slab.h>
+
+#include <media/videobuf-dma-contig.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+
+#include <video/omapvrfb.h>
+#include <video/omapdss.h>
+
+#include "omap_voutlib.h"
+#include "omap_voutdef.h"
+#include "omap_vout_vrfb.h"
+
+MODULE_AUTHOR("Texas Instruments");
+MODULE_DESCRIPTION("OMAP Video for Linux Video out driver");
+MODULE_LICENSE("GPL");
+
+/* Driver Configuration macros */
+#define VOUT_NAME		"omap_vout"
+
+enum omap_vout_channels {
+	OMAP_VIDEO1,
+	OMAP_VIDEO2,
+};
+
+static struct videobuf_queue_ops video_vbq_ops;
+/* Variables configurable through module params*/
+static u32 video1_numbuffers = 3;
+static u32 video2_numbuffers = 3;
+static u32 video1_bufsize = OMAP_VOUT_MAX_BUF_SIZE;
+static u32 video2_bufsize = OMAP_VOUT_MAX_BUF_SIZE;
+static bool vid1_static_vrfb_alloc;
+static bool vid2_static_vrfb_alloc;
+static bool debug;
+
+/* Module parameters */
+module_param(video1_numbuffers, uint, S_IRUGO);
+MODULE_PARM_DESC(video1_numbuffers,
+	"Number of buffers to be allocated at init time for Video1 device.");
+
+module_param(video2_numbuffers, uint, S_IRUGO);
+MODULE_PARM_DESC(video2_numbuffers,
+	"Number of buffers to be allocated at init time for Video2 device.");
+
+module_param(video1_bufsize, uint, S_IRUGO);
+MODULE_PARM_DESC(video1_bufsize,
+	"Size of the buffer to be allocated for video1 device");
+
+module_param(video2_bufsize, uint, S_IRUGO);
+MODULE_PARM_DESC(video2_bufsize,
+	"Size of the buffer to be allocated for video2 device");
+
+module_param(vid1_static_vrfb_alloc, bool, S_IRUGO);
+MODULE_PARM_DESC(vid1_static_vrfb_alloc,
+	"Static allocation of the VRFB buffer for video1 device");
+
+module_param(vid2_static_vrfb_alloc, bool, S_IRUGO);
+MODULE_PARM_DESC(vid2_static_vrfb_alloc,
+	"Static allocation of the VRFB buffer for video2 device");
+
+module_param(debug, bool, S_IRUGO);
+MODULE_PARM_DESC(debug, "Debug level (0-1)");
+
+/* list of image formats supported by OMAP2 video pipelines */
+static const struct v4l2_fmtdesc omap_formats[] = {
+	{
+		/* Note:  V4L2 defines RGB565 as:
+		 *
+		 *      Byte 0                    Byte 1
+		 *      g2 g1 g0 r4 r3 r2 r1 r0   b4 b3 b2 b1 b0 g5 g4 g3
+		 *
+		 * We interpret RGB565 as:
+		 *
+		 *      Byte 0                    Byte 1
+		 *      g2 g1 g0 b4 b3 b2 b1 b0   r4 r3 r2 r1 r0 g5 g4 g3
+		 */
+		.description = "RGB565, le",
+		.pixelformat = V4L2_PIX_FMT_RGB565,
+	},
+	{
+		/* Note:  V4L2 defines RGB32 as: RGB-8-8-8-8  we use
+		 *  this for RGB24 unpack mode, the last 8 bits are ignored
+		 * */
+		.description = "RGB32, le",
+		.pixelformat = V4L2_PIX_FMT_RGB32,
+	},
+	{
+		/* Note:  V4L2 defines RGB24 as: RGB-8-8-8  we use
+		 *        this for RGB24 packed mode
+		 *
+		 */
+		.description = "RGB24, le",
+		.pixelformat = V4L2_PIX_FMT_RGB24,
+	},
+	{
+		.description = "YUYV (YUV 4:2:2), packed",
+		.pixelformat = V4L2_PIX_FMT_YUYV,
+	},
+	{
+		.description = "UYVY, packed",
+		.pixelformat = V4L2_PIX_FMT_UYVY,
+	},
+};
+
+#define NUM_OUTPUT_FORMATS (ARRAY_SIZE(omap_formats))
+
+/*
+ * Try format
+ */
+static int omap_vout_try_format(struct v4l2_pix_format *pix)
+{
+	int ifmt, bpp = 0;
+
+	pix->height = clamp(pix->height, (u32)VID_MIN_HEIGHT,
+						(u32)VID_MAX_HEIGHT);
+	pix->width = clamp(pix->width, (u32)VID_MIN_WIDTH, (u32)VID_MAX_WIDTH);
+
+	for (ifmt = 0; ifmt < NUM_OUTPUT_FORMATS; ifmt++) {
+		if (pix->pixelformat == omap_formats[ifmt].pixelformat)
+			break;
+	}
+
+	if (ifmt == NUM_OUTPUT_FORMATS)
+		ifmt = 0;
+
+	pix->pixelformat = omap_formats[ifmt].pixelformat;
+	pix->field = V4L2_FIELD_ANY;
+
+	switch (pix->pixelformat) {
+	case V4L2_PIX_FMT_YUYV:
+	case V4L2_PIX_FMT_UYVY:
+	default:
+		pix->colorspace = V4L2_COLORSPACE_JPEG;
+		bpp = YUYV_BPP;
+		break;
+	case V4L2_PIX_FMT_RGB565:
+	case V4L2_PIX_FMT_RGB565X:
+		pix->colorspace = V4L2_COLORSPACE_SRGB;
+		bpp = RGB565_BPP;
+		break;
+	case V4L2_PIX_FMT_RGB24:
+		pix->colorspace = V4L2_COLORSPACE_SRGB;
+		bpp = RGB24_BPP;
+		break;
+	case V4L2_PIX_FMT_RGB32:
+	case V4L2_PIX_FMT_BGR32:
+		pix->colorspace = V4L2_COLORSPACE_SRGB;
+		bpp = RGB32_BPP;
+		break;
+	}
+	pix->bytesperline = pix->width * bpp;
+	pix->sizeimage = pix->bytesperline * pix->height;
+
+	return bpp;
+}
+
+/*
+ * omap_vout_get_userptr: Convert user space virtual address to physical
+ * address.
+ */
+static int omap_vout_get_userptr(struct videobuf_buffer *vb, u32 virtp,
+				 u32 *physp)
+{
+	struct frame_vector *vec;
+	int ret;
+
+	/* For kernel direct-mapped memory, take the easy way */
+	if (virtp >= PAGE_OFFSET) {
+		*physp = virt_to_phys((void *)virtp);
+		return 0;
+	}
+
+	vec = frame_vector_create(1);
+	if (!vec)
+		return -ENOMEM;
+
+	ret = get_vaddr_frames(virtp, 1, true, false, vec);
+	if (ret != 1) {
+		frame_vector_destroy(vec);
+		return -EINVAL;
+	}
+	*physp = __pfn_to_phys(frame_vector_pfns(vec)[0]);
+	vb->priv = vec;
+
+	return 0;
+}
+
+/*
+ * Free the V4L2 buffers
+ */
+void omap_vout_free_buffers(struct omap_vout_device *vout)
+{
+	int i, numbuffers;
+
+	/* Allocate memory for the buffers */
+	numbuffers = (vout->vid) ?  video2_numbuffers : video1_numbuffers;
+	vout->buffer_size = (vout->vid) ? video2_bufsize : video1_bufsize;
+
+	for (i = 0; i < numbuffers; i++) {
+		omap_vout_free_buffer(vout->buf_virt_addr[i],
+				vout->buffer_size);
+		vout->buf_phy_addr[i] = 0;
+		vout->buf_virt_addr[i] = 0;
+	}
+}
+
+/*
+ * Convert V4L2 rotation to DSS rotation
+ *	V4L2 understand 0, 90, 180, 270.
+ *	Convert to 0, 1, 2 and 3 respectively for DSS
+ */
+static int v4l2_rot_to_dss_rot(int v4l2_rotation,
+			enum dss_rotation *rotation, bool mirror)
+{
+	int ret = 0;
+
+	switch (v4l2_rotation) {
+	case 90:
+		*rotation = dss_rotation_90_degree;
+		break;
+	case 180:
+		*rotation = dss_rotation_180_degree;
+		break;
+	case 270:
+		*rotation = dss_rotation_270_degree;
+		break;
+	case 0:
+		*rotation = dss_rotation_0_degree;
+		break;
+	default:
+		ret = -EINVAL;
+	}
+	return ret;
+}
+
+static int omap_vout_calculate_offset(struct omap_vout_device *vout)
+{
+	struct omapvideo_info *ovid;
+	struct v4l2_rect *crop = &vout->crop;
+	struct v4l2_pix_format *pix = &vout->pix;
+	int *cropped_offset = &vout->cropped_offset;
+	int ps = 2, line_length = 0;
+
+	ovid = &vout->vid_info;
+
+	if (ovid->rotation_type == VOUT_ROT_VRFB) {
+		omap_vout_calculate_vrfb_offset(vout);
+	} else {
+		vout->line_length = line_length = pix->width;
+
+		if (V4L2_PIX_FMT_YUYV == pix->pixelformat ||
+			V4L2_PIX_FMT_UYVY == pix->pixelformat)
+			ps = 2;
+		else if (V4L2_PIX_FMT_RGB32 == pix->pixelformat)
+			ps = 4;
+		else if (V4L2_PIX_FMT_RGB24 == pix->pixelformat)
+			ps = 3;
+
+		vout->ps = ps;
+
+		*cropped_offset = (line_length * ps) *
+			crop->top + crop->left * ps;
+	}
+
+	v4l2_dbg(1, debug, &vout->vid_dev->v4l2_dev, "%s Offset:%x\n",
+			__func__, vout->cropped_offset);
+
+	return 0;
+}
+
+/*
+ * Convert V4L2 pixel format to DSS pixel format
+ */
+static int video_mode_to_dss_mode(struct omap_vout_device *vout)
+{
+	struct omap_overlay *ovl;
+	struct omapvideo_info *ovid;
+	struct v4l2_pix_format *pix = &vout->pix;
+	enum omap_color_mode mode;
+
+	ovid = &vout->vid_info;
+	ovl = ovid->overlays[0];
+
+	switch (pix->pixelformat) {
+	case V4L2_PIX_FMT_YUYV:
+		mode = OMAP_DSS_COLOR_YUV2;
+		break;
+	case V4L2_PIX_FMT_UYVY:
+		mode = OMAP_DSS_COLOR_UYVY;
+		break;
+	case V4L2_PIX_FMT_RGB565:
+		mode = OMAP_DSS_COLOR_RGB16;
+		break;
+	case V4L2_PIX_FMT_RGB24:
+		mode = OMAP_DSS_COLOR_RGB24P;
+		break;
+	case V4L2_PIX_FMT_RGB32:
+		mode = (ovl->id == OMAP_DSS_VIDEO1) ?
+			OMAP_DSS_COLOR_RGB24U : OMAP_DSS_COLOR_ARGB32;
+		break;
+	case V4L2_PIX_FMT_BGR32:
+		mode = OMAP_DSS_COLOR_RGBX32;
+		break;
+	default:
+		mode = -EINVAL;
+		break;
+	}
+	return mode;
+}
+
+/*
+ * Setup the overlay
+ */
+static int omapvid_setup_overlay(struct omap_vout_device *vout,
+		struct omap_overlay *ovl, int posx, int posy, int outw,
+		int outh, u32 addr)
+{
+	int ret = 0;
+	struct omap_overlay_info info;
+	int cropheight, cropwidth, pixwidth;
+
+	if ((ovl->caps & OMAP_DSS_OVL_CAP_SCALE) == 0 &&
+			(outw != vout->pix.width || outh != vout->pix.height)) {
+		ret = -EINVAL;
+		goto setup_ovl_err;
+	}
+
+	vout->dss_mode = video_mode_to_dss_mode(vout);
+	if (vout->dss_mode == -EINVAL) {
+		ret = -EINVAL;
+		goto setup_ovl_err;
+	}
+
+	/* Setup the input plane parameters according to
+	 * rotation value selected.
+	 */
+	if (is_rotation_90_or_270(vout)) {
+		cropheight = vout->crop.width;
+		cropwidth = vout->crop.height;
+		pixwidth = vout->pix.height;
+	} else {
+		cropheight = vout->crop.height;
+		cropwidth = vout->crop.width;
+		pixwidth = vout->pix.width;
+	}
+
+	ovl->get_overlay_info(ovl, &info);
+	info.paddr = addr;
+	info.width = cropwidth;
+	info.height = cropheight;
+	info.color_mode = vout->dss_mode;
+	info.mirror = vout->mirror;
+	info.pos_x = posx;
+	info.pos_y = posy;
+	info.out_width = outw;
+	info.out_height = outh;
+	info.global_alpha = vout->win.global_alpha;
+	if (!is_rotation_enabled(vout)) {
+		info.rotation = 0;
+		info.rotation_type = OMAP_DSS_ROT_DMA;
+		info.screen_width = pixwidth;
+	} else {
+		info.rotation = vout->rotation;
+		info.rotation_type = OMAP_DSS_ROT_VRFB;
+		info.screen_width = 2048;
+	}
+
+	v4l2_dbg(1, debug, &vout->vid_dev->v4l2_dev,
+		"%s enable=%d addr=%pad width=%d\n height=%d color_mode=%d\n"
+		"rotation=%d mirror=%d posx=%d posy=%d out_width = %d \n"
+		"out_height=%d rotation_type=%d screen_width=%d\n",
+		__func__, ovl->is_enabled(ovl), &info.paddr, info.width, info.height,
+		info.color_mode, info.rotation, info.mirror, info.pos_x,
+		info.pos_y, info.out_width, info.out_height, info.rotation_type,
+		info.screen_width);
+
+	ret = ovl->set_overlay_info(ovl, &info);
+	if (ret)
+		goto setup_ovl_err;
+
+	return 0;
+
+setup_ovl_err:
+	v4l2_warn(&vout->vid_dev->v4l2_dev, "setup_overlay failed\n");
+	return ret;
+}
+
+/*
+ * Initialize the overlay structure
+ */
+static int omapvid_init(struct omap_vout_device *vout, u32 addr)
+{
+	int ret = 0, i;
+	struct v4l2_window *win;
+	struct omap_overlay *ovl;
+	int posx, posy, outw, outh;
+	struct omap_video_timings *timing;
+	struct omapvideo_info *ovid = &vout->vid_info;
+
+	win = &vout->win;
+	for (i = 0; i < ovid->num_overlays; i++) {
+		struct omap_dss_device *dssdev;
+
+		ovl = ovid->overlays[i];
+		dssdev = ovl->get_device(ovl);
+
+		if (!dssdev)
+			return -EINVAL;
+
+		timing = &dssdev->panel.timings;
+
+		outw = win->w.width;
+		outh = win->w.height;
+		switch (vout->rotation) {
+		case dss_rotation_90_degree:
+			/* Invert the height and width for 90
+			 * and 270 degree rotation
+			 */
+			swap(outw, outh);
+			posy = (timing->y_res - win->w.width) - win->w.left;
+			posx = win->w.top;
+			break;
+
+		case dss_rotation_180_degree:
+			posx = (timing->x_res - win->w.width) - win->w.left;
+			posy = (timing->y_res - win->w.height) - win->w.top;
+			break;
+
+		case dss_rotation_270_degree:
+			swap(outw, outh);
+			posy = win->w.left;
+			posx = (timing->x_res - win->w.height) - win->w.top;
+			break;
+
+		default:
+			posx = win->w.left;
+			posy = win->w.top;
+			break;
+		}
+
+		ret = omapvid_setup_overlay(vout, ovl, posx, posy,
+				outw, outh, addr);
+		if (ret)
+			goto omapvid_init_err;
+	}
+	return 0;
+
+omapvid_init_err:
+	v4l2_warn(&vout->vid_dev->v4l2_dev, "apply_changes failed\n");
+	return ret;
+}
+
+/*
+ * Apply the changes set the go bit of DSS
+ */
+static int omapvid_apply_changes(struct omap_vout_device *vout)
+{
+	int i;
+	struct omap_overlay *ovl;
+	struct omapvideo_info *ovid = &vout->vid_info;
+
+	for (i = 0; i < ovid->num_overlays; i++) {
+		struct omap_dss_device *dssdev;
+
+		ovl = ovid->overlays[i];
+		dssdev = ovl->get_device(ovl);
+		if (!dssdev)
+			return -EINVAL;
+		ovl->manager->apply(ovl->manager);
+	}
+
+	return 0;
+}
+
+static int omapvid_handle_interlace_display(struct omap_vout_device *vout,
+		unsigned int irqstatus, struct timeval timevalue)
+{
+	u32 fid;
+
+	if (vout->first_int) {
+		vout->first_int = 0;
+		goto err;
+	}
+
+	if (irqstatus & DISPC_IRQ_EVSYNC_ODD)
+		fid = 1;
+	else if (irqstatus & DISPC_IRQ_EVSYNC_EVEN)
+		fid = 0;
+	else
+		goto err;
+
+	vout->field_id ^= 1;
+	if (fid != vout->field_id) {
+		if (fid == 0)
+			vout->field_id = fid;
+	} else if (0 == fid) {
+		if (vout->cur_frm == vout->next_frm)
+			goto err;
+
+		vout->cur_frm->ts = timevalue;
+		vout->cur_frm->state = VIDEOBUF_DONE;
+		wake_up_interruptible(&vout->cur_frm->done);
+		vout->cur_frm = vout->next_frm;
+	} else {
+		if (list_empty(&vout->dma_queue) ||
+				(vout->cur_frm != vout->next_frm))
+			goto err;
+	}
+
+	return vout->field_id;
+err:
+	return 0;
+}
+
+static void omap_vout_isr(void *arg, unsigned int irqstatus)
+{
+	int ret, fid, mgr_id;
+	u32 addr, irq;
+	struct omap_overlay *ovl;
+	struct timeval timevalue;
+	struct omapvideo_info *ovid;
+	struct omap_dss_device *cur_display;
+	struct omap_vout_device *vout = (struct omap_vout_device *)arg;
+
+	if (!vout->streaming)
+		return;
+
+	ovid = &vout->vid_info;
+	ovl = ovid->overlays[0];
+
+	mgr_id = ovl->manager->id;
+
+	/* get the display device attached to the overlay */
+	cur_display = ovl->get_device(ovl);
+
+	if (!cur_display)
+		return;
+
+	spin_lock(&vout->vbq_lock);
+	v4l2_get_timestamp(&timevalue);
+
+	switch (cur_display->type) {
+	case OMAP_DISPLAY_TYPE_DSI:
+	case OMAP_DISPLAY_TYPE_DPI:
+	case OMAP_DISPLAY_TYPE_DVI:
+		if (mgr_id == OMAP_DSS_CHANNEL_LCD)
+			irq = DISPC_IRQ_VSYNC;
+		else if (mgr_id == OMAP_DSS_CHANNEL_LCD2)
+			irq = DISPC_IRQ_VSYNC2;
+		else
+			goto vout_isr_err;
+
+		if (!(irqstatus & irq))
+			goto vout_isr_err;
+		break;
+	case OMAP_DISPLAY_TYPE_VENC:
+		fid = omapvid_handle_interlace_display(vout, irqstatus,
+				timevalue);
+		if (!fid)
+			goto vout_isr_err;
+		break;
+	case OMAP_DISPLAY_TYPE_HDMI:
+		if (!(irqstatus & DISPC_IRQ_EVSYNC_EVEN))
+			goto vout_isr_err;
+		break;
+	default:
+		goto vout_isr_err;
+	}
+
+	if (!vout->first_int && (vout->cur_frm != vout->next_frm)) {
+		vout->cur_frm->ts = timevalue;
+		vout->cur_frm->state = VIDEOBUF_DONE;
+		wake_up_interruptible(&vout->cur_frm->done);
+		vout->cur_frm = vout->next_frm;
+	}
+
+	vout->first_int = 0;
+	if (list_empty(&vout->dma_queue))
+		goto vout_isr_err;
+
+	vout->next_frm = list_entry(vout->dma_queue.next,
+			struct videobuf_buffer, queue);
+	list_del(&vout->next_frm->queue);
+
+	vout->next_frm->state = VIDEOBUF_ACTIVE;
+
+	addr = (unsigned long) vout->queued_buf_addr[vout->next_frm->i]
+		+ vout->cropped_offset;
+
+	/* First save the configuration in ovelray structure */
+	ret = omapvid_init(vout, addr);
+	if (ret) {
+		printk(KERN_ERR VOUT_NAME
+			"failed to set overlay info\n");
+		goto vout_isr_err;
+	}
+
+	/* Enable the pipeline and set the Go bit */
+	ret = omapvid_apply_changes(vout);
+	if (ret)
+		printk(KERN_ERR VOUT_NAME "failed to change mode\n");
+
+vout_isr_err:
+	spin_unlock(&vout->vbq_lock);
+}
+
+/* Video buffer call backs */
+
+/*
+ * Buffer setup function is called by videobuf layer when REQBUF ioctl is
+ * called. This is used to setup buffers and return size and count of
+ * buffers allocated. After the call to this buffer, videobuf layer will
+ * setup buffer queue depending on the size and count of buffers
+ */
+static int omap_vout_buffer_setup(struct videobuf_queue *q, unsigned int *count,
+			  unsigned int *size)
+{
+	int startindex = 0, i, j;
+	u32 phy_addr = 0, virt_addr = 0;
+	struct omap_vout_device *vout = q->priv_data;
+	struct omapvideo_info *ovid = &vout->vid_info;
+	int vid_max_buf_size;
+
+	if (!vout)
+		return -EINVAL;
+
+	vid_max_buf_size = vout->vid == OMAP_VIDEO1 ? video1_bufsize :
+		video2_bufsize;
+
+	if (V4L2_BUF_TYPE_VIDEO_OUTPUT != q->type)
+		return -EINVAL;
+
+	startindex = (vout->vid == OMAP_VIDEO1) ?
+		video1_numbuffers : video2_numbuffers;
+	if (V4L2_MEMORY_MMAP == vout->memory && *count < startindex)
+		*count = startindex;
+
+	if (ovid->rotation_type == VOUT_ROT_VRFB) {
+		if (omap_vout_vrfb_buffer_setup(vout, count, startindex))
+			return -ENOMEM;
+	}
+
+	if (V4L2_MEMORY_MMAP != vout->memory)
+		return 0;
+
+	/* Now allocated the V4L2 buffers */
+	*size = PAGE_ALIGN(vout->pix.width * vout->pix.height * vout->bpp);
+	startindex = (vout->vid == OMAP_VIDEO1) ?
+		video1_numbuffers : video2_numbuffers;
+
+	/* Check the size of the buffer */
+	if (*size > vid_max_buf_size) {
+		v4l2_err(&vout->vid_dev->v4l2_dev,
+				"buffer allocation mismatch [%u] [%u]\n",
+				*size, vout->buffer_size);
+		return -ENOMEM;
+	}
+
+	for (i = startindex; i < *count; i++) {
+		vout->buffer_size = *size;
+
+		virt_addr = omap_vout_alloc_buffer(vout->buffer_size,
+				&phy_addr);
+		if (!virt_addr) {
+			if (ovid->rotation_type == VOUT_ROT_NONE) {
+				break;
+			} else {
+				if (!is_rotation_enabled(vout))
+					break;
+			/* Free the VRFB buffers if no space for V4L2 buffers */
+			for (j = i; j < *count; j++) {
+				omap_vout_free_buffer(
+						vout->smsshado_virt_addr[j],
+						vout->smsshado_size);
+				vout->smsshado_virt_addr[j] = 0;
+				vout->smsshado_phy_addr[j] = 0;
+				}
+			}
+		}
+		vout->buf_virt_addr[i] = virt_addr;
+		vout->buf_phy_addr[i] = phy_addr;
+	}
+	*count = vout->buffer_allocated = i;
+
+	return 0;
+}
+
+/*
+ * Free the V4L2 buffers additionally allocated than default
+ * number of buffers
+ */
+static void omap_vout_free_extra_buffers(struct omap_vout_device *vout)
+{
+	int num_buffers = 0, i;
+
+	num_buffers = (vout->vid == OMAP_VIDEO1) ?
+		video1_numbuffers : video2_numbuffers;
+
+	for (i = num_buffers; i < vout->buffer_allocated; i++) {
+		if (vout->buf_virt_addr[i])
+			omap_vout_free_buffer(vout->buf_virt_addr[i],
+					vout->buffer_size);
+
+		vout->buf_virt_addr[i] = 0;
+		vout->buf_phy_addr[i] = 0;
+	}
+	vout->buffer_allocated = num_buffers;
+}
+
+/*
+ * This function will be called when VIDIOC_QBUF ioctl is called.
+ * It prepare buffers before give out for the display. This function
+ * converts user space virtual address into physical address if userptr memory
+ * exchange mechanism is used. If rotation is enabled, it copies entire
+ * buffer into VRFB memory space before giving it to the DSS.
+ */
+static int omap_vout_buffer_prepare(struct videobuf_queue *q,
+			struct videobuf_buffer *vb,
+			enum v4l2_field field)
+{
+	struct omap_vout_device *vout = q->priv_data;
+	struct omapvideo_info *ovid = &vout->vid_info;
+
+	if (VIDEOBUF_NEEDS_INIT == vb->state) {
+		vb->width = vout->pix.width;
+		vb->height = vout->pix.height;
+		vb->size = vb->width * vb->height * vout->bpp;
+		vb->field = field;
+	}
+	vb->state = VIDEOBUF_PREPARED;
+	/* if user pointer memory mechanism is used, get the physical
+	 * address of the buffer
+	 */
+	if (V4L2_MEMORY_USERPTR == vb->memory) {
+		int ret;
+
+		if (0 == vb->baddr)
+			return -EINVAL;
+		/* Physical address */
+		ret = omap_vout_get_userptr(vb, vb->baddr,
+				(u32 *)&vout->queued_buf_addr[vb->i]);
+		if (ret < 0)
+			return ret;
+	} else {
+		unsigned long addr, dma_addr;
+		unsigned long size;
+
+		addr = (unsigned long) vout->buf_virt_addr[vb->i];
+		size = (unsigned long) vb->size;
+
+		dma_addr = dma_map_single(vout->vid_dev->v4l2_dev.dev, (void *) addr,
+				size, DMA_TO_DEVICE);
+		if (dma_mapping_error(vout->vid_dev->v4l2_dev.dev, dma_addr))
+			v4l2_err(&vout->vid_dev->v4l2_dev, "dma_map_single failed\n");
+
+		vout->queued_buf_addr[vb->i] = (u8 *)vout->buf_phy_addr[vb->i];
+	}
+
+	if (ovid->rotation_type == VOUT_ROT_VRFB)
+		return omap_vout_prepare_vrfb(vout, vb);
+	else
+		return 0;
+}
+
+/*
+ * Buffer queue function will be called from the videobuf layer when _QBUF
+ * ioctl is called. It is used to enqueue buffer, which is ready to be
+ * displayed.
+ */
+static void omap_vout_buffer_queue(struct videobuf_queue *q,
+			  struct videobuf_buffer *vb)
+{
+	struct omap_vout_device *vout = q->priv_data;
+
+	/* Driver is also maintainig a queue. So enqueue buffer in the driver
+	 * queue */
+	list_add_tail(&vb->queue, &vout->dma_queue);
+
+	vb->state = VIDEOBUF_QUEUED;
+}
+
+/*
+ * Buffer release function is called from videobuf layer to release buffer
+ * which are already allocated
+ */
+static void omap_vout_buffer_release(struct videobuf_queue *q,
+			    struct videobuf_buffer *vb)
+{
+	vb->state = VIDEOBUF_NEEDS_INIT;
+	if (vb->memory == V4L2_MEMORY_USERPTR && vb->priv) {
+		struct frame_vector *vec = vb->priv;
+
+		put_vaddr_frames(vec);
+		frame_vector_destroy(vec);
+	}
+}
+
+/*
+ *  File operations
+ */
+static unsigned int omap_vout_poll(struct file *file,
+				   struct poll_table_struct *wait)
+{
+	struct omap_vout_device *vout = file->private_data;
+	struct videobuf_queue *q = &vout->vbq;
+
+	return videobuf_poll_stream(file, q, wait);
+}
+
+static void omap_vout_vm_open(struct vm_area_struct *vma)
+{
+	struct omap_vout_device *vout = vma->vm_private_data;
+
+	v4l2_dbg(1, debug, &vout->vid_dev->v4l2_dev,
+		"vm_open [vma=%08lx-%08lx]\n", vma->vm_start, vma->vm_end);
+	vout->mmap_count++;
+}
+
+static void omap_vout_vm_close(struct vm_area_struct *vma)
+{
+	struct omap_vout_device *vout = vma->vm_private_data;
+
+	v4l2_dbg(1, debug, &vout->vid_dev->v4l2_dev,
+		"vm_close [vma=%08lx-%08lx]\n", vma->vm_start, vma->vm_end);
+	vout->mmap_count--;
+}
+
+static const struct vm_operations_struct omap_vout_vm_ops = {
+	.open	= omap_vout_vm_open,
+	.close	= omap_vout_vm_close,
+};
+
+static int omap_vout_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	int i;
+	void *pos;
+	unsigned long start = vma->vm_start;
+	unsigned long size = (vma->vm_end - vma->vm_start);
+	struct omap_vout_device *vout = file->private_data;
+	struct videobuf_queue *q = &vout->vbq;
+
+	v4l2_dbg(1, debug, &vout->vid_dev->v4l2_dev,
+			" %s pgoff=0x%lx, start=0x%lx, end=0x%lx\n", __func__,
+			vma->vm_pgoff, vma->vm_start, vma->vm_end);
+
+	/* look for the buffer to map */
+	for (i = 0; i < VIDEO_MAX_FRAME; i++) {
+		if (NULL == q->bufs[i])
+			continue;
+		if (V4L2_MEMORY_MMAP != q->bufs[i]->memory)
+			continue;
+		if (q->bufs[i]->boff == (vma->vm_pgoff << PAGE_SHIFT))
+			break;
+	}
+
+	if (VIDEO_MAX_FRAME == i) {
+		v4l2_dbg(1, debug, &vout->vid_dev->v4l2_dev,
+				"offset invalid [offset=0x%lx]\n",
+				(vma->vm_pgoff << PAGE_SHIFT));
+		return -EINVAL;
+	}
+	/* Check the size of the buffer */
+	if (size > vout->buffer_size) {
+		v4l2_err(&vout->vid_dev->v4l2_dev,
+				"insufficient memory [%lu] [%u]\n",
+				size, vout->buffer_size);
+		return -ENOMEM;
+	}
+
+	q->bufs[i]->baddr = vma->vm_start;
+
+	vma->vm_flags |= VM_DONTEXPAND | VM_DONTDUMP;
+	vma->vm_page_prot = pgprot_writecombine(vma->vm_page_prot);
+	vma->vm_ops = &omap_vout_vm_ops;
+	vma->vm_private_data = (void *) vout;
+	pos = (void *)vout->buf_virt_addr[i];
+	vma->vm_pgoff = virt_to_phys((void *)pos) >> PAGE_SHIFT;
+	while (size > 0) {
+		unsigned long pfn;
+		pfn = virt_to_phys((void *) pos) >> PAGE_SHIFT;
+		if (remap_pfn_range(vma, start, pfn, PAGE_SIZE, PAGE_SHARED))
+			return -EAGAIN;
+		start += PAGE_SIZE;
+		pos += PAGE_SIZE;
+		size -= PAGE_SIZE;
+	}
+	vout->mmap_count++;
+	v4l2_dbg(1, debug, &vout->vid_dev->v4l2_dev, "Exiting %s\n", __func__);
+
+	return 0;
+}
+
+static int omap_vout_release(struct file *file)
+{
+	unsigned int ret, i;
+	struct videobuf_queue *q;
+	struct omapvideo_info *ovid;
+	struct omap_vout_device *vout = file->private_data;
+
+	v4l2_dbg(1, debug, &vout->vid_dev->v4l2_dev, "Entering %s\n", __func__);
+	ovid = &vout->vid_info;
+
+	if (!vout)
+		return 0;
+
+	q = &vout->vbq;
+	/* Disable all the overlay managers connected with this interface */
+	for (i = 0; i < ovid->num_overlays; i++) {
+		struct omap_overlay *ovl = ovid->overlays[i];
+		struct omap_dss_device *dssdev = ovl->get_device(ovl);
+
+		if (dssdev)
+			ovl->disable(ovl);
+	}
+	/* Turn off the pipeline */
+	ret = omapvid_apply_changes(vout);
+	if (ret)
+		v4l2_warn(&vout->vid_dev->v4l2_dev,
+				"Unable to apply changes\n");
+
+	/* Free all buffers */
+	omap_vout_free_extra_buffers(vout);
+
+	/* Free the VRFB buffers only if they are allocated
+	 * during reqbufs.  Don't free if init time allocated
+	 */
+	if (ovid->rotation_type == VOUT_ROT_VRFB) {
+		if (!vout->vrfb_static_allocation)
+			omap_vout_free_vrfb_buffers(vout);
+	}
+	videobuf_mmap_free(q);
+
+	/* Even if apply changes fails we should continue
+	   freeing allocated memory */
+	if (vout->streaming) {
+		u32 mask = 0;
+
+		mask = DISPC_IRQ_VSYNC | DISPC_IRQ_EVSYNC_EVEN |
+			DISPC_IRQ_EVSYNC_ODD | DISPC_IRQ_VSYNC2;
+		omap_dispc_unregister_isr(omap_vout_isr, vout, mask);
+		vout->streaming = false;
+
+		videobuf_streamoff(q);
+		videobuf_queue_cancel(q);
+	}
+
+	if (vout->mmap_count != 0)
+		vout->mmap_count = 0;
+
+	vout->opened -= 1;
+	file->private_data = NULL;
+
+	if (vout->buffer_allocated)
+		videobuf_mmap_free(q);
+
+	v4l2_dbg(1, debug, &vout->vid_dev->v4l2_dev, "Exiting %s\n", __func__);
+	return ret;
+}
+
+static int omap_vout_open(struct file *file)
+{
+	struct videobuf_queue *q;
+	struct omap_vout_device *vout = NULL;
+
+	vout = video_drvdata(file);
+	v4l2_dbg(1, debug, &vout->vid_dev->v4l2_dev, "Entering %s\n", __func__);
+
+	if (vout == NULL)
+		return -ENODEV;
+
+	/* for now, we only support single open */
+	if (vout->opened)
+		return -EBUSY;
+
+	vout->opened += 1;
+
+	file->private_data = vout;
+	vout->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+
+	q = &vout->vbq;
+	video_vbq_ops.buf_setup = omap_vout_buffer_setup;
+	video_vbq_ops.buf_prepare = omap_vout_buffer_prepare;
+	video_vbq_ops.buf_release = omap_vout_buffer_release;
+	video_vbq_ops.buf_queue = omap_vout_buffer_queue;
+	spin_lock_init(&vout->vbq_lock);
+
+	videobuf_queue_dma_contig_init(q, &video_vbq_ops, q->dev,
+			&vout->vbq_lock, vout->type, V4L2_FIELD_NONE,
+			sizeof(struct videobuf_buffer), vout, NULL);
+
+	v4l2_dbg(1, debug, &vout->vid_dev->v4l2_dev, "Exiting %s\n", __func__);
+	return 0;
+}
+
+/*
+ * V4L2 ioctls
+ */
+static int vidioc_querycap(struct file *file, void *fh,
+		struct v4l2_capability *cap)
+{
+	struct omap_vout_device *vout = fh;
+
+	strlcpy(cap->driver, VOUT_NAME, sizeof(cap->driver));
+	strlcpy(cap->card, vout->vfd->name, sizeof(cap->card));
+	cap->bus_info[0] = '\0';
+	cap->device_caps = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_OUTPUT |
+		V4L2_CAP_VIDEO_OUTPUT_OVERLAY;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+
+	return 0;
+}
+
+static int vidioc_enum_fmt_vid_out(struct file *file, void *fh,
+			struct v4l2_fmtdesc *fmt)
+{
+	int index = fmt->index;
+
+	if (index >= NUM_OUTPUT_FORMATS)
+		return -EINVAL;
+
+	fmt->flags = omap_formats[index].flags;
+	strlcpy(fmt->description, omap_formats[index].description,
+			sizeof(fmt->description));
+	fmt->pixelformat = omap_formats[index].pixelformat;
+
+	return 0;
+}
+
+static int vidioc_g_fmt_vid_out(struct file *file, void *fh,
+			struct v4l2_format *f)
+{
+	struct omap_vout_device *vout = fh;
+
+	f->fmt.pix = vout->pix;
+	return 0;
+
+}
+
+static int vidioc_try_fmt_vid_out(struct file *file, void *fh,
+			struct v4l2_format *f)
+{
+	struct omap_overlay *ovl;
+	struct omapvideo_info *ovid;
+	struct omap_video_timings *timing;
+	struct omap_vout_device *vout = fh;
+	struct omap_dss_device *dssdev;
+
+	ovid = &vout->vid_info;
+	ovl = ovid->overlays[0];
+	/* get the display device attached to the overlay */
+	dssdev = ovl->get_device(ovl);
+
+	if (!dssdev)
+		return -EINVAL;
+
+	timing = &dssdev->panel.timings;
+
+	vout->fbuf.fmt.height = timing->y_res;
+	vout->fbuf.fmt.width = timing->x_res;
+
+	omap_vout_try_format(&f->fmt.pix);
+	return 0;
+}
+
+static int vidioc_s_fmt_vid_out(struct file *file, void *fh,
+			struct v4l2_format *f)
+{
+	int ret, bpp;
+	struct omap_overlay *ovl;
+	struct omapvideo_info *ovid;
+	struct omap_video_timings *timing;
+	struct omap_vout_device *vout = fh;
+	struct omap_dss_device *dssdev;
+
+	if (vout->streaming)
+		return -EBUSY;
+
+	mutex_lock(&vout->lock);
+
+	ovid = &vout->vid_info;
+	ovl = ovid->overlays[0];
+	dssdev = ovl->get_device(ovl);
+
+	/* get the display device attached to the overlay */
+	if (!dssdev) {
+		ret = -EINVAL;
+		goto s_fmt_vid_out_exit;
+	}
+	timing = &dssdev->panel.timings;
+
+	/* We dont support RGB24-packed mode if vrfb rotation
+	 * is enabled*/
+	if ((is_rotation_enabled(vout)) &&
+			f->fmt.pix.pixelformat == V4L2_PIX_FMT_RGB24) {
+		ret = -EINVAL;
+		goto s_fmt_vid_out_exit;
+	}
+
+	/* get the framebuffer parameters */
+
+	if (is_rotation_90_or_270(vout)) {
+		vout->fbuf.fmt.height = timing->x_res;
+		vout->fbuf.fmt.width = timing->y_res;
+	} else {
+		vout->fbuf.fmt.height = timing->y_res;
+		vout->fbuf.fmt.width = timing->x_res;
+	}
+
+	/* change to samller size is OK */
+
+	bpp = omap_vout_try_format(&f->fmt.pix);
+	f->fmt.pix.sizeimage = f->fmt.pix.width * f->fmt.pix.height * bpp;
+
+	/* try & set the new output format */
+	vout->bpp = bpp;
+	vout->pix = f->fmt.pix;
+	vout->vrfb_bpp = 1;
+
+	/* If YUYV then vrfb bpp is 2, for  others its 1 */
+	if (V4L2_PIX_FMT_YUYV == vout->pix.pixelformat ||
+			V4L2_PIX_FMT_UYVY == vout->pix.pixelformat)
+		vout->vrfb_bpp = 2;
+
+	/* set default crop and win */
+	omap_vout_new_format(&vout->pix, &vout->fbuf, &vout->crop, &vout->win);
+
+	ret = 0;
+
+s_fmt_vid_out_exit:
+	mutex_unlock(&vout->lock);
+	return ret;
+}
+
+static int vidioc_try_fmt_vid_overlay(struct file *file, void *fh,
+			struct v4l2_format *f)
+{
+	int ret = 0;
+	struct omap_vout_device *vout = fh;
+	struct omap_overlay *ovl;
+	struct omapvideo_info *ovid;
+	struct v4l2_window *win = &f->fmt.win;
+
+	ovid = &vout->vid_info;
+	ovl = ovid->overlays[0];
+
+	ret = omap_vout_try_window(&vout->fbuf, win);
+
+	if (!ret) {
+		if ((ovl->caps & OMAP_DSS_OVL_CAP_GLOBAL_ALPHA) == 0)
+			win->global_alpha = 255;
+		else
+			win->global_alpha = f->fmt.win.global_alpha;
+	}
+
+	return ret;
+}
+
+static int vidioc_s_fmt_vid_overlay(struct file *file, void *fh,
+			struct v4l2_format *f)
+{
+	int ret = 0;
+	struct omap_overlay *ovl;
+	struct omapvideo_info *ovid;
+	struct omap_vout_device *vout = fh;
+	struct v4l2_window *win = &f->fmt.win;
+
+	mutex_lock(&vout->lock);
+	ovid = &vout->vid_info;
+	ovl = ovid->overlays[0];
+
+	ret = omap_vout_new_window(&vout->crop, &vout->win, &vout->fbuf, win);
+	if (!ret) {
+		/* Video1 plane does not support global alpha on OMAP3 */
+		if ((ovl->caps & OMAP_DSS_OVL_CAP_GLOBAL_ALPHA) == 0)
+			vout->win.global_alpha = 255;
+		else
+			vout->win.global_alpha = f->fmt.win.global_alpha;
+
+		vout->win.chromakey = f->fmt.win.chromakey;
+	}
+	mutex_unlock(&vout->lock);
+	return ret;
+}
+
+static int vidioc_g_fmt_vid_overlay(struct file *file, void *fh,
+			struct v4l2_format *f)
+{
+	u32 key_value =  0;
+	struct omap_overlay *ovl;
+	struct omapvideo_info *ovid;
+	struct omap_vout_device *vout = fh;
+	struct omap_overlay_manager_info info;
+	struct v4l2_window *win = &f->fmt.win;
+
+	ovid = &vout->vid_info;
+	ovl = ovid->overlays[0];
+
+	win->w = vout->win.w;
+	win->field = vout->win.field;
+	win->global_alpha = vout->win.global_alpha;
+
+	if (ovl->manager && ovl->manager->get_manager_info) {
+		ovl->manager->get_manager_info(ovl->manager, &info);
+		key_value = info.trans_key;
+	}
+	win->chromakey = key_value;
+	return 0;
+}
+
+static int vidioc_cropcap(struct file *file, void *fh,
+		struct v4l2_cropcap *cropcap)
+{
+	struct omap_vout_device *vout = fh;
+	struct v4l2_pix_format *pix = &vout->pix;
+
+	if (cropcap->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		return -EINVAL;
+
+	/* Width and height are always even */
+	cropcap->bounds.width = pix->width & ~1;
+	cropcap->bounds.height = pix->height & ~1;
+
+	omap_vout_default_crop(&vout->pix, &vout->fbuf, &cropcap->defrect);
+	cropcap->pixelaspect.numerator = 1;
+	cropcap->pixelaspect.denominator = 1;
+	return 0;
+}
+
+static int vidioc_g_crop(struct file *file, void *fh, struct v4l2_crop *crop)
+{
+	struct omap_vout_device *vout = fh;
+
+	if (crop->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		return -EINVAL;
+	crop->c = vout->crop;
+	return 0;
+}
+
+static int vidioc_s_crop(struct file *file, void *fh, const struct v4l2_crop *crop)
+{
+	int ret = -EINVAL;
+	struct omap_vout_device *vout = fh;
+	struct omapvideo_info *ovid;
+	struct omap_overlay *ovl;
+	struct omap_video_timings *timing;
+	struct omap_dss_device *dssdev;
+
+	if (vout->streaming)
+		return -EBUSY;
+
+	mutex_lock(&vout->lock);
+	ovid = &vout->vid_info;
+	ovl = ovid->overlays[0];
+	/* get the display device attached to the overlay */
+	dssdev = ovl->get_device(ovl);
+
+	if (!dssdev) {
+		ret = -EINVAL;
+		goto s_crop_err;
+	}
+
+	timing = &dssdev->panel.timings;
+
+	if (is_rotation_90_or_270(vout)) {
+		vout->fbuf.fmt.height = timing->x_res;
+		vout->fbuf.fmt.width = timing->y_res;
+	} else {
+		vout->fbuf.fmt.height = timing->y_res;
+		vout->fbuf.fmt.width = timing->x_res;
+	}
+
+	if (crop->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		ret = omap_vout_new_crop(&vout->pix, &vout->crop, &vout->win,
+				&vout->fbuf, &crop->c);
+
+s_crop_err:
+	mutex_unlock(&vout->lock);
+	return ret;
+}
+
+static int vidioc_queryctrl(struct file *file, void *fh,
+		struct v4l2_queryctrl *ctrl)
+{
+	int ret = 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_ROTATE:
+		ret = v4l2_ctrl_query_fill(ctrl, 0, 270, 90, 0);
+		break;
+	case V4L2_CID_BG_COLOR:
+		ret = v4l2_ctrl_query_fill(ctrl, 0, 0xFFFFFF, 1, 0);
+		break;
+	case V4L2_CID_VFLIP:
+		ret = v4l2_ctrl_query_fill(ctrl, 0, 1, 1, 0);
+		break;
+	default:
+		ctrl->name[0] = '\0';
+		ret = -EINVAL;
+	}
+	return ret;
+}
+
+static int vidioc_g_ctrl(struct file *file, void *fh, struct v4l2_control *ctrl)
+{
+	int ret = 0;
+	struct omap_vout_device *vout = fh;
+
+	switch (ctrl->id) {
+	case V4L2_CID_ROTATE:
+		ctrl->value = vout->control[0].value;
+		break;
+	case V4L2_CID_BG_COLOR:
+	{
+		struct omap_overlay_manager_info info;
+		struct omap_overlay *ovl;
+
+		ovl = vout->vid_info.overlays[0];
+		if (!ovl->manager || !ovl->manager->get_manager_info) {
+			ret = -EINVAL;
+			break;
+		}
+
+		ovl->manager->get_manager_info(ovl->manager, &info);
+		ctrl->value = info.default_color;
+		break;
+	}
+	case V4L2_CID_VFLIP:
+		ctrl->value = vout->control[2].value;
+		break;
+	default:
+		ret = -EINVAL;
+	}
+	return ret;
+}
+
+static int vidioc_s_ctrl(struct file *file, void *fh, struct v4l2_control *a)
+{
+	int ret = 0;
+	struct omap_vout_device *vout = fh;
+
+	switch (a->id) {
+	case V4L2_CID_ROTATE:
+	{
+		struct omapvideo_info *ovid;
+		int rotation = a->value;
+
+		ovid = &vout->vid_info;
+
+		mutex_lock(&vout->lock);
+		if (rotation && ovid->rotation_type == VOUT_ROT_NONE) {
+			mutex_unlock(&vout->lock);
+			ret = -ERANGE;
+			break;
+		}
+
+		if (rotation && vout->pix.pixelformat == V4L2_PIX_FMT_RGB24) {
+			mutex_unlock(&vout->lock);
+			ret = -EINVAL;
+			break;
+		}
+
+		if (v4l2_rot_to_dss_rot(rotation, &vout->rotation,
+							vout->mirror)) {
+			mutex_unlock(&vout->lock);
+			ret = -EINVAL;
+			break;
+		}
+
+		vout->control[0].value = rotation;
+		mutex_unlock(&vout->lock);
+		break;
+	}
+	case V4L2_CID_BG_COLOR:
+	{
+		struct omap_overlay *ovl;
+		unsigned int  color = a->value;
+		struct omap_overlay_manager_info info;
+
+		ovl = vout->vid_info.overlays[0];
+
+		mutex_lock(&vout->lock);
+		if (!ovl->manager || !ovl->manager->get_manager_info) {
+			mutex_unlock(&vout->lock);
+			ret = -EINVAL;
+			break;
+		}
+
+		ovl->manager->get_manager_info(ovl->manager, &info);
+		info.default_color = color;
+		if (ovl->manager->set_manager_info(ovl->manager, &info)) {
+			mutex_unlock(&vout->lock);
+			ret = -EINVAL;
+			break;
+		}
+
+		vout->control[1].value = color;
+		mutex_unlock(&vout->lock);
+		break;
+	}
+	case V4L2_CID_VFLIP:
+	{
+		struct omapvideo_info *ovid;
+		unsigned int  mirror = a->value;
+
+		ovid = &vout->vid_info;
+
+		mutex_lock(&vout->lock);
+		if (mirror && ovid->rotation_type == VOUT_ROT_NONE) {
+			mutex_unlock(&vout->lock);
+			ret = -ERANGE;
+			break;
+		}
+
+		if (mirror  && vout->pix.pixelformat == V4L2_PIX_FMT_RGB24) {
+			mutex_unlock(&vout->lock);
+			ret = -EINVAL;
+			break;
+		}
+		vout->mirror = mirror;
+		vout->control[2].value = mirror;
+		mutex_unlock(&vout->lock);
+		break;
+	}
+	default:
+		ret = -EINVAL;
+	}
+	return ret;
+}
+
+static int vidioc_reqbufs(struct file *file, void *fh,
+			struct v4l2_requestbuffers *req)
+{
+	int ret = 0;
+	unsigned int i, num_buffers = 0;
+	struct omap_vout_device *vout = fh;
+	struct videobuf_queue *q = &vout->vbq;
+
+	if (req->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		return -EINVAL;
+	/* if memory is not mmp or userptr
+	   return error */
+	if ((V4L2_MEMORY_MMAP != req->memory) &&
+			(V4L2_MEMORY_USERPTR != req->memory))
+		return -EINVAL;
+
+	mutex_lock(&vout->lock);
+	/* Cannot be requested when streaming is on */
+	if (vout->streaming) {
+		ret = -EBUSY;
+		goto reqbuf_err;
+	}
+
+	/* If buffers are already allocated free them */
+	if (q->bufs[0] && (V4L2_MEMORY_MMAP == q->bufs[0]->memory)) {
+		if (vout->mmap_count) {
+			ret = -EBUSY;
+			goto reqbuf_err;
+		}
+		num_buffers = (vout->vid == OMAP_VIDEO1) ?
+			video1_numbuffers : video2_numbuffers;
+		for (i = num_buffers; i < vout->buffer_allocated; i++) {
+			omap_vout_free_buffer(vout->buf_virt_addr[i],
+					vout->buffer_size);
+			vout->buf_virt_addr[i] = 0;
+			vout->buf_phy_addr[i] = 0;
+		}
+		vout->buffer_allocated = num_buffers;
+		videobuf_mmap_free(q);
+	} else if (q->bufs[0] && (V4L2_MEMORY_USERPTR == q->bufs[0]->memory)) {
+		if (vout->buffer_allocated) {
+			videobuf_mmap_free(q);
+			for (i = 0; i < vout->buffer_allocated; i++) {
+				kfree(q->bufs[i]);
+				q->bufs[i] = NULL;
+			}
+			vout->buffer_allocated = 0;
+		}
+	}
+
+	/*store the memory type in data structure */
+	vout->memory = req->memory;
+
+	INIT_LIST_HEAD(&vout->dma_queue);
+
+	/* call videobuf_reqbufs api */
+	ret = videobuf_reqbufs(q, req);
+	if (ret < 0)
+		goto reqbuf_err;
+
+	vout->buffer_allocated = req->count;
+
+reqbuf_err:
+	mutex_unlock(&vout->lock);
+	return ret;
+}
+
+static int vidioc_querybuf(struct file *file, void *fh,
+			struct v4l2_buffer *b)
+{
+	struct omap_vout_device *vout = fh;
+
+	return videobuf_querybuf(&vout->vbq, b);
+}
+
+static int vidioc_qbuf(struct file *file, void *fh,
+			struct v4l2_buffer *buffer)
+{
+	struct omap_vout_device *vout = fh;
+	struct videobuf_queue *q = &vout->vbq;
+
+	if ((V4L2_BUF_TYPE_VIDEO_OUTPUT != buffer->type) ||
+			(buffer->index >= vout->buffer_allocated) ||
+			(q->bufs[buffer->index]->memory != buffer->memory)) {
+		return -EINVAL;
+	}
+	if (V4L2_MEMORY_USERPTR == buffer->memory) {
+		if ((buffer->length < vout->pix.sizeimage) ||
+				(0 == buffer->m.userptr)) {
+			return -EINVAL;
+		}
+	}
+
+	if ((is_rotation_enabled(vout)) &&
+			vout->vrfb_dma_tx.req_status == DMA_CHAN_NOT_ALLOTED) {
+		v4l2_warn(&vout->vid_dev->v4l2_dev,
+				"DMA Channel not allocated for Rotation\n");
+		return -EINVAL;
+	}
+
+	return videobuf_qbuf(q, buffer);
+}
+
+static int vidioc_dqbuf(struct file *file, void *fh, struct v4l2_buffer *b)
+{
+	struct omap_vout_device *vout = fh;
+	struct videobuf_queue *q = &vout->vbq;
+
+	int ret;
+	u32 addr;
+	unsigned long size;
+	struct videobuf_buffer *vb;
+
+	vb = q->bufs[b->index];
+
+	if (!vout->streaming)
+		return -EINVAL;
+
+	if (file->f_flags & O_NONBLOCK)
+		/* Call videobuf_dqbuf for non blocking mode */
+		ret = videobuf_dqbuf(q, (struct v4l2_buffer *)b, 1);
+	else
+		/* Call videobuf_dqbuf for  blocking mode */
+		ret = videobuf_dqbuf(q, (struct v4l2_buffer *)b, 0);
+
+	addr = (unsigned long) vout->buf_phy_addr[vb->i];
+	size = (unsigned long) vb->size;
+	dma_unmap_single(vout->vid_dev->v4l2_dev.dev,  addr,
+				size, DMA_TO_DEVICE);
+	return ret;
+}
+
+static int vidioc_streamon(struct file *file, void *fh, enum v4l2_buf_type i)
+{
+	int ret = 0, j;
+	u32 addr = 0, mask = 0;
+	struct omap_vout_device *vout = fh;
+	struct videobuf_queue *q = &vout->vbq;
+	struct omapvideo_info *ovid = &vout->vid_info;
+
+	mutex_lock(&vout->lock);
+
+	if (vout->streaming) {
+		ret = -EBUSY;
+		goto streamon_err;
+	}
+
+	ret = videobuf_streamon(q);
+	if (ret)
+		goto streamon_err;
+
+	if (list_empty(&vout->dma_queue)) {
+		ret = -EIO;
+		goto streamon_err1;
+	}
+
+	/* Get the next frame from the buffer queue */
+	vout->next_frm = vout->cur_frm = list_entry(vout->dma_queue.next,
+			struct videobuf_buffer, queue);
+	/* Remove buffer from the buffer queue */
+	list_del(&vout->cur_frm->queue);
+	/* Mark state of the current frame to active */
+	vout->cur_frm->state = VIDEOBUF_ACTIVE;
+	/* Initialize field_id and started member */
+	vout->field_id = 0;
+
+	/* set flag here. Next QBUF will start DMA */
+	vout->streaming = true;
+
+	vout->first_int = 1;
+
+	if (omap_vout_calculate_offset(vout)) {
+		ret = -EINVAL;
+		goto streamon_err1;
+	}
+	addr = (unsigned long) vout->queued_buf_addr[vout->cur_frm->i]
+		+ vout->cropped_offset;
+
+	mask = DISPC_IRQ_VSYNC | DISPC_IRQ_EVSYNC_EVEN | DISPC_IRQ_EVSYNC_ODD
+		| DISPC_IRQ_VSYNC2;
+
+	/* First save the configuration in ovelray structure */
+	ret = omapvid_init(vout, addr);
+	if (ret) {
+		v4l2_err(&vout->vid_dev->v4l2_dev,
+				"failed to set overlay info\n");
+		goto streamon_err1;
+	}
+
+	omap_dispc_register_isr(omap_vout_isr, vout, mask);
+
+	/* Enable the pipeline and set the Go bit */
+	ret = omapvid_apply_changes(vout);
+	if (ret)
+		v4l2_err(&vout->vid_dev->v4l2_dev, "failed to change mode\n");
+
+	for (j = 0; j < ovid->num_overlays; j++) {
+		struct omap_overlay *ovl = ovid->overlays[j];
+		struct omap_dss_device *dssdev = ovl->get_device(ovl);
+
+		if (dssdev) {
+			ret = ovl->enable(ovl);
+			if (ret)
+				goto streamon_err1;
+		}
+	}
+
+	ret = 0;
+
+streamon_err1:
+	if (ret)
+		ret = videobuf_streamoff(q);
+streamon_err:
+	mutex_unlock(&vout->lock);
+	return ret;
+}
+
+static int vidioc_streamoff(struct file *file, void *fh, enum v4l2_buf_type i)
+{
+	u32 mask = 0;
+	int ret = 0, j;
+	struct omap_vout_device *vout = fh;
+	struct omapvideo_info *ovid = &vout->vid_info;
+
+	if (!vout->streaming)
+		return -EINVAL;
+
+	vout->streaming = false;
+	mask = DISPC_IRQ_VSYNC | DISPC_IRQ_EVSYNC_EVEN | DISPC_IRQ_EVSYNC_ODD
+		| DISPC_IRQ_VSYNC2;
+
+	omap_dispc_unregister_isr(omap_vout_isr, vout, mask);
+
+	for (j = 0; j < ovid->num_overlays; j++) {
+		struct omap_overlay *ovl = ovid->overlays[j];
+		struct omap_dss_device *dssdev = ovl->get_device(ovl);
+
+		if (dssdev)
+			ovl->disable(ovl);
+	}
+
+	/* Turn of the pipeline */
+	ret = omapvid_apply_changes(vout);
+	if (ret)
+		v4l2_err(&vout->vid_dev->v4l2_dev, "failed to change mode in"
+				" streamoff\n");
+
+	INIT_LIST_HEAD(&vout->dma_queue);
+	ret = videobuf_streamoff(&vout->vbq);
+
+	return ret;
+}
+
+static int vidioc_s_fbuf(struct file *file, void *fh,
+				const struct v4l2_framebuffer *a)
+{
+	int enable = 0;
+	struct omap_overlay *ovl;
+	struct omapvideo_info *ovid;
+	struct omap_vout_device *vout = fh;
+	struct omap_overlay_manager_info info;
+	enum omap_dss_trans_key_type key_type = OMAP_DSS_COLOR_KEY_GFX_DST;
+
+	ovid = &vout->vid_info;
+	ovl = ovid->overlays[0];
+
+	/* OMAP DSS doesn't support Source and Destination color
+	   key together */
+	if ((a->flags & V4L2_FBUF_FLAG_SRC_CHROMAKEY) &&
+			(a->flags & V4L2_FBUF_FLAG_CHROMAKEY))
+		return -EINVAL;
+	/* OMAP DSS Doesn't support the Destination color key
+	   and alpha blending together */
+	if ((a->flags & V4L2_FBUF_FLAG_CHROMAKEY) &&
+			(a->flags & V4L2_FBUF_FLAG_LOCAL_ALPHA))
+		return -EINVAL;
+
+	if ((a->flags & V4L2_FBUF_FLAG_SRC_CHROMAKEY)) {
+		vout->fbuf.flags |= V4L2_FBUF_FLAG_SRC_CHROMAKEY;
+		key_type =  OMAP_DSS_COLOR_KEY_VID_SRC;
+	} else
+		vout->fbuf.flags &= ~V4L2_FBUF_FLAG_SRC_CHROMAKEY;
+
+	if ((a->flags & V4L2_FBUF_FLAG_CHROMAKEY)) {
+		vout->fbuf.flags |= V4L2_FBUF_FLAG_CHROMAKEY;
+		key_type =  OMAP_DSS_COLOR_KEY_GFX_DST;
+	} else
+		vout->fbuf.flags &=  ~V4L2_FBUF_FLAG_CHROMAKEY;
+
+	if (a->flags & (V4L2_FBUF_FLAG_CHROMAKEY |
+				V4L2_FBUF_FLAG_SRC_CHROMAKEY))
+		enable = 1;
+	else
+		enable = 0;
+	if (ovl->manager && ovl->manager->get_manager_info &&
+			ovl->manager->set_manager_info) {
+
+		ovl->manager->get_manager_info(ovl->manager, &info);
+		info.trans_enabled = enable;
+		info.trans_key_type = key_type;
+		info.trans_key = vout->win.chromakey;
+
+		if (ovl->manager->set_manager_info(ovl->manager, &info))
+			return -EINVAL;
+	}
+	if (a->flags & V4L2_FBUF_FLAG_LOCAL_ALPHA) {
+		vout->fbuf.flags |= V4L2_FBUF_FLAG_LOCAL_ALPHA;
+		enable = 1;
+	} else {
+		vout->fbuf.flags &= ~V4L2_FBUF_FLAG_LOCAL_ALPHA;
+		enable = 0;
+	}
+	if (ovl->manager && ovl->manager->get_manager_info &&
+			ovl->manager->set_manager_info) {
+		ovl->manager->get_manager_info(ovl->manager, &info);
+		/* enable this only if there is no zorder cap */
+		if ((ovl->caps & OMAP_DSS_OVL_CAP_ZORDER) == 0)
+			info.partial_alpha_enabled = enable;
+		if (ovl->manager->set_manager_info(ovl->manager, &info))
+			return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int vidioc_g_fbuf(struct file *file, void *fh,
+		struct v4l2_framebuffer *a)
+{
+	struct omap_overlay *ovl;
+	struct omapvideo_info *ovid;
+	struct omap_vout_device *vout = fh;
+	struct omap_overlay_manager_info info;
+
+	ovid = &vout->vid_info;
+	ovl = ovid->overlays[0];
+
+	/* The video overlay must stay within the framebuffer and can't be
+	   positioned independently. */
+	a->flags = V4L2_FBUF_FLAG_OVERLAY;
+	a->capability = V4L2_FBUF_CAP_LOCAL_ALPHA | V4L2_FBUF_CAP_CHROMAKEY
+		| V4L2_FBUF_CAP_SRC_CHROMAKEY;
+
+	if (ovl->manager && ovl->manager->get_manager_info) {
+		ovl->manager->get_manager_info(ovl->manager, &info);
+		if (info.trans_key_type == OMAP_DSS_COLOR_KEY_VID_SRC)
+			a->flags |= V4L2_FBUF_FLAG_SRC_CHROMAKEY;
+		if (info.trans_key_type == OMAP_DSS_COLOR_KEY_GFX_DST)
+			a->flags |= V4L2_FBUF_FLAG_CHROMAKEY;
+	}
+	if (ovl->manager && ovl->manager->get_manager_info) {
+		ovl->manager->get_manager_info(ovl->manager, &info);
+		if (info.partial_alpha_enabled)
+			a->flags |= V4L2_FBUF_FLAG_LOCAL_ALPHA;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops vout_ioctl_ops = {
+	.vidioc_querycap      			= vidioc_querycap,
+	.vidioc_enum_fmt_vid_out 		= vidioc_enum_fmt_vid_out,
+	.vidioc_g_fmt_vid_out			= vidioc_g_fmt_vid_out,
+	.vidioc_try_fmt_vid_out			= vidioc_try_fmt_vid_out,
+	.vidioc_s_fmt_vid_out			= vidioc_s_fmt_vid_out,
+	.vidioc_queryctrl    			= vidioc_queryctrl,
+	.vidioc_g_ctrl       			= vidioc_g_ctrl,
+	.vidioc_s_fbuf				= vidioc_s_fbuf,
+	.vidioc_g_fbuf				= vidioc_g_fbuf,
+	.vidioc_s_ctrl       			= vidioc_s_ctrl,
+	.vidioc_try_fmt_vid_out_overlay		= vidioc_try_fmt_vid_overlay,
+	.vidioc_s_fmt_vid_out_overlay		= vidioc_s_fmt_vid_overlay,
+	.vidioc_g_fmt_vid_out_overlay		= vidioc_g_fmt_vid_overlay,
+	.vidioc_cropcap				= vidioc_cropcap,
+	.vidioc_g_crop				= vidioc_g_crop,
+	.vidioc_s_crop				= vidioc_s_crop,
+	.vidioc_reqbufs				= vidioc_reqbufs,
+	.vidioc_querybuf			= vidioc_querybuf,
+	.vidioc_qbuf				= vidioc_qbuf,
+	.vidioc_dqbuf				= vidioc_dqbuf,
+	.vidioc_streamon			= vidioc_streamon,
+	.vidioc_streamoff			= vidioc_streamoff,
+};
+
+static const struct v4l2_file_operations omap_vout_fops = {
+	.owner 		= THIS_MODULE,
+	.poll		= omap_vout_poll,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap 		= omap_vout_mmap,
+	.open 		= omap_vout_open,
+	.release 	= omap_vout_release,
+};
+
+/* Init functions used during driver initialization */
+/* Initial setup of video_data */
+static int __init omap_vout_setup_video_data(struct omap_vout_device *vout)
+{
+	struct video_device *vfd;
+	struct v4l2_pix_format *pix;
+	struct v4l2_control *control;
+	struct omap_overlay *ovl = vout->vid_info.overlays[0];
+	struct omap_dss_device *display = ovl->get_device(ovl);
+
+	/* set the default pix */
+	pix = &vout->pix;
+
+	/* Set the default picture of QVGA  */
+	pix->width = QQVGA_WIDTH;
+	pix->height = QQVGA_HEIGHT;
+
+	/* Default pixel format is RGB 5-6-5 */
+	pix->pixelformat = V4L2_PIX_FMT_RGB565;
+	pix->field = V4L2_FIELD_ANY;
+	pix->bytesperline = pix->width * 2;
+	pix->sizeimage = pix->bytesperline * pix->height;
+	pix->colorspace = V4L2_COLORSPACE_JPEG;
+
+	vout->bpp = RGB565_BPP;
+	vout->fbuf.fmt.width  =  display->panel.timings.x_res;
+	vout->fbuf.fmt.height =  display->panel.timings.y_res;
+
+	/* Set the data structures for the overlay parameters*/
+	vout->win.global_alpha = 255;
+	vout->fbuf.flags = 0;
+	vout->fbuf.capability = V4L2_FBUF_CAP_LOCAL_ALPHA |
+		V4L2_FBUF_CAP_SRC_CHROMAKEY | V4L2_FBUF_CAP_CHROMAKEY;
+	vout->win.chromakey = 0;
+
+	omap_vout_new_format(pix, &vout->fbuf, &vout->crop, &vout->win);
+
+	/*Initialize the control variables for
+	  rotation, flipping and background color. */
+	control = vout->control;
+	control[0].id = V4L2_CID_ROTATE;
+	control[0].value = 0;
+	vout->rotation = 0;
+	vout->mirror = false;
+	vout->control[2].id = V4L2_CID_HFLIP;
+	vout->control[2].value = 0;
+	if (vout->vid_info.rotation_type == VOUT_ROT_VRFB)
+		vout->vrfb_bpp = 2;
+
+	control[1].id = V4L2_CID_BG_COLOR;
+	control[1].value = 0;
+
+	/* initialize the video_device struct */
+	vfd = vout->vfd = video_device_alloc();
+
+	if (!vfd) {
+		printk(KERN_ERR VOUT_NAME ": could not allocate"
+				" video device struct\n");
+		return -ENOMEM;
+	}
+	vfd->release = video_device_release;
+	vfd->ioctl_ops = &vout_ioctl_ops;
+
+	strlcpy(vfd->name, VOUT_NAME, sizeof(vfd->name));
+
+	vfd->fops = &omap_vout_fops;
+	vfd->v4l2_dev = &vout->vid_dev->v4l2_dev;
+	vfd->vfl_dir = VFL_DIR_TX;
+	mutex_init(&vout->lock);
+
+	vfd->minor = -1;
+	return 0;
+
+}
+
+/* Setup video buffers */
+static int __init omap_vout_setup_video_bufs(struct platform_device *pdev,
+		int vid_num)
+{
+	u32 numbuffers;
+	int ret = 0, i;
+	struct omapvideo_info *ovid;
+	struct omap_vout_device *vout;
+	struct v4l2_device *v4l2_dev = platform_get_drvdata(pdev);
+	struct omap2video_device *vid_dev =
+		container_of(v4l2_dev, struct omap2video_device, v4l2_dev);
+
+	vout = vid_dev->vouts[vid_num];
+	ovid = &vout->vid_info;
+
+	numbuffers = (vid_num == 0) ? video1_numbuffers : video2_numbuffers;
+	vout->buffer_size = (vid_num == 0) ? video1_bufsize : video2_bufsize;
+	dev_info(&pdev->dev, "Buffer Size = %d\n", vout->buffer_size);
+
+	for (i = 0; i < numbuffers; i++) {
+		vout->buf_virt_addr[i] =
+			omap_vout_alloc_buffer(vout->buffer_size,
+					(u32 *) &vout->buf_phy_addr[i]);
+		if (!vout->buf_virt_addr[i]) {
+			numbuffers = i;
+			ret = -ENOMEM;
+			goto free_buffers;
+		}
+	}
+
+	vout->cropped_offset = 0;
+
+	if (ovid->rotation_type == VOUT_ROT_VRFB) {
+		bool static_vrfb_allocation = (vid_num == 0) ?
+			vid1_static_vrfb_alloc : vid2_static_vrfb_alloc;
+		ret = omap_vout_setup_vrfb_bufs(pdev, vid_num,
+				static_vrfb_allocation);
+	}
+
+	return ret;
+
+free_buffers:
+	for (i = 0; i < numbuffers; i++) {
+		omap_vout_free_buffer(vout->buf_virt_addr[i],
+						vout->buffer_size);
+		vout->buf_virt_addr[i] = 0;
+		vout->buf_phy_addr[i] = 0;
+	}
+	return ret;
+
+}
+
+/* Create video out devices */
+static int __init omap_vout_create_video_devices(struct platform_device *pdev)
+{
+	int ret = 0, k;
+	struct omap_vout_device *vout;
+	struct video_device *vfd = NULL;
+	struct v4l2_device *v4l2_dev = platform_get_drvdata(pdev);
+	struct omap2video_device *vid_dev = container_of(v4l2_dev,
+			struct omap2video_device, v4l2_dev);
+
+	for (k = 0; k < pdev->num_resources; k++) {
+
+		vout = kzalloc(sizeof(struct omap_vout_device), GFP_KERNEL);
+		if (!vout) {
+			dev_err(&pdev->dev, ": could not allocate memory\n");
+			return -ENOMEM;
+		}
+
+		vout->vid = k;
+		vid_dev->vouts[k] = vout;
+		vout->vid_dev = vid_dev;
+		/* Select video2 if only 1 overlay is controlled by V4L2 */
+		if (pdev->num_resources == 1)
+			vout->vid_info.overlays[0] = vid_dev->overlays[k + 2];
+		else
+			/* Else select video1 and video2 one by one. */
+			vout->vid_info.overlays[0] = vid_dev->overlays[k + 1];
+		vout->vid_info.num_overlays = 1;
+		vout->vid_info.id = k + 1;
+
+		/* Set VRFB as rotation_type for omap2 and omap3 */
+		if (omap_vout_dss_omap24xx() || omap_vout_dss_omap34xx())
+			vout->vid_info.rotation_type = VOUT_ROT_VRFB;
+
+		/* Setup the default configuration for the video devices
+		 */
+		if (omap_vout_setup_video_data(vout) != 0) {
+			ret = -ENOMEM;
+			goto error;
+		}
+
+		/* Allocate default number of buffers for the video streaming
+		 * and reserve the VRFB space for rotation
+		 */
+		if (omap_vout_setup_video_bufs(pdev, k) != 0) {
+			ret = -ENOMEM;
+			goto error1;
+		}
+
+		/* Register the Video device with V4L2
+		 */
+		vfd = vout->vfd;
+		if (video_register_device(vfd, VFL_TYPE_GRABBER, -1) < 0) {
+			dev_err(&pdev->dev, ": Could not register "
+					"Video for Linux device\n");
+			vfd->minor = -1;
+			ret = -ENODEV;
+			goto error2;
+		}
+		video_set_drvdata(vfd, vout);
+
+		dev_info(&pdev->dev, ": registered and initialized"
+				" video device %d\n", vfd->minor);
+		if (k == (pdev->num_resources - 1))
+			return 0;
+
+		continue;
+error2:
+		if (vout->vid_info.rotation_type == VOUT_ROT_VRFB)
+			omap_vout_release_vrfb(vout);
+		omap_vout_free_buffers(vout);
+error1:
+		video_device_release(vfd);
+error:
+		kfree(vout);
+		return ret;
+	}
+
+	return -ENODEV;
+}
+/* Driver functions */
+static void omap_vout_cleanup_device(struct omap_vout_device *vout)
+{
+	struct video_device *vfd;
+	struct omapvideo_info *ovid;
+
+	if (!vout)
+		return;
+
+	vfd = vout->vfd;
+	ovid = &vout->vid_info;
+	if (vfd) {
+		if (!video_is_registered(vfd)) {
+			/*
+			 * The device was never registered, so release the
+			 * video_device struct directly.
+			 */
+			video_device_release(vfd);
+		} else {
+			/*
+			 * The unregister function will release the video_device
+			 * struct as well as unregistering it.
+			 */
+			video_unregister_device(vfd);
+		}
+	}
+	if (ovid->rotation_type == VOUT_ROT_VRFB) {
+		omap_vout_release_vrfb(vout);
+		/* Free the VRFB buffer if allocated
+		 * init time
+		 */
+		if (vout->vrfb_static_allocation)
+			omap_vout_free_vrfb_buffers(vout);
+	}
+	omap_vout_free_buffers(vout);
+
+	kfree(vout);
+}
+
+static int omap_vout_remove(struct platform_device *pdev)
+{
+	int k;
+	struct v4l2_device *v4l2_dev = platform_get_drvdata(pdev);
+	struct omap2video_device *vid_dev = container_of(v4l2_dev, struct
+			omap2video_device, v4l2_dev);
+
+	v4l2_device_unregister(v4l2_dev);
+	for (k = 0; k < pdev->num_resources; k++)
+		omap_vout_cleanup_device(vid_dev->vouts[k]);
+
+	for (k = 0; k < vid_dev->num_displays; k++) {
+		if (vid_dev->displays[k]->state != OMAP_DSS_DISPLAY_DISABLED)
+			vid_dev->displays[k]->driver->disable(vid_dev->displays[k]);
+
+		omap_dss_put_device(vid_dev->displays[k]);
+	}
+	kfree(vid_dev);
+	return 0;
+}
+
+static int __init omap_vout_probe(struct platform_device *pdev)
+{
+	int ret = 0, i;
+	struct omap_overlay *ovl;
+	struct omap_dss_device *dssdev = NULL;
+	struct omap_dss_device *def_display;
+	struct omap2video_device *vid_dev = NULL;
+
+	if (omapdss_is_initialized() == false)
+		return -EPROBE_DEFER;
+
+	ret = omapdss_compat_init();
+	if (ret) {
+		dev_err(&pdev->dev, "failed to init dss\n");
+		return ret;
+	}
+
+	if (pdev->num_resources == 0) {
+		dev_err(&pdev->dev, "probed for an unknown device\n");
+		ret = -ENODEV;
+		goto err_dss_init;
+	}
+
+	vid_dev = kzalloc(sizeof(struct omap2video_device), GFP_KERNEL);
+	if (vid_dev == NULL) {
+		ret = -ENOMEM;
+		goto err_dss_init;
+	}
+
+	vid_dev->num_displays = 0;
+	for_each_dss_dev(dssdev) {
+		omap_dss_get_device(dssdev);
+
+		if (!dssdev->driver) {
+			dev_warn(&pdev->dev, "no driver for display: %s\n",
+					dssdev->name);
+			omap_dss_put_device(dssdev);
+			continue;
+		}
+
+		vid_dev->displays[vid_dev->num_displays++] = dssdev;
+	}
+
+	if (vid_dev->num_displays == 0) {
+		dev_err(&pdev->dev, "no displays\n");
+		ret = -EINVAL;
+		goto probe_err0;
+	}
+
+	vid_dev->num_overlays = omap_dss_get_num_overlays();
+	for (i = 0; i < vid_dev->num_overlays; i++)
+		vid_dev->overlays[i] = omap_dss_get_overlay(i);
+
+	vid_dev->num_managers = omap_dss_get_num_overlay_managers();
+	for (i = 0; i < vid_dev->num_managers; i++)
+		vid_dev->managers[i] = omap_dss_get_overlay_manager(i);
+
+	/* Get the Video1 overlay and video2 overlay.
+	 * Setup the Display attached to that overlays
+	 */
+	for (i = 1; i < vid_dev->num_overlays; i++) {
+		ovl = omap_dss_get_overlay(i);
+		dssdev = ovl->get_device(ovl);
+
+		if (dssdev) {
+			def_display = dssdev;
+		} else {
+			dev_warn(&pdev->dev, "cannot find display\n");
+			def_display = NULL;
+		}
+		if (def_display) {
+			struct omap_dss_driver *dssdrv = def_display->driver;
+
+			ret = dssdrv->enable(def_display);
+			if (ret) {
+				/* Here we are not considering a error
+				 *  as display may be enabled by frame
+				 *  buffer driver
+				 */
+				dev_warn(&pdev->dev,
+					"'%s' Display already enabled\n",
+					def_display->name);
+			}
+		}
+	}
+
+	if (v4l2_device_register(&pdev->dev, &vid_dev->v4l2_dev) < 0) {
+		dev_err(&pdev->dev, "v4l2_device_register failed\n");
+		ret = -ENODEV;
+		goto probe_err1;
+	}
+
+	ret = omap_vout_create_video_devices(pdev);
+	if (ret)
+		goto probe_err2;
+
+	for (i = 0; i < vid_dev->num_displays; i++) {
+		struct omap_dss_device *display = vid_dev->displays[i];
+
+		if (display->driver->update)
+			display->driver->update(display, 0, 0,
+					display->panel.timings.x_res,
+					display->panel.timings.y_res);
+	}
+	return 0;
+
+probe_err2:
+	v4l2_device_unregister(&vid_dev->v4l2_dev);
+probe_err1:
+	for (i = 1; i < vid_dev->num_overlays; i++) {
+		def_display = NULL;
+		ovl = omap_dss_get_overlay(i);
+		dssdev = ovl->get_device(ovl);
+
+		if (dssdev)
+			def_display = dssdev;
+
+		if (def_display && def_display->driver)
+			def_display->driver->disable(def_display);
+	}
+probe_err0:
+	kfree(vid_dev);
+err_dss_init:
+	omapdss_compat_uninit();
+	return ret;
+}
+
+static struct platform_driver omap_vout_driver = {
+	.driver = {
+		.name = VOUT_NAME,
+	},
+	.remove = omap_vout_remove,
+};
+
+static int __init omap_vout_init(void)
+{
+	if (platform_driver_probe(&omap_vout_driver, omap_vout_probe) != 0) {
+		printk(KERN_ERR VOUT_NAME ":Could not register Video driver\n");
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static void omap_vout_cleanup(void)
+{
+	platform_driver_unregister(&omap_vout_driver);
+}
+
+late_initcall(omap_vout_init);
+module_exit(omap_vout_cleanup);
diff --git a/drivers/media/platform/omap/omap_vout_vrfb.c b/drivers/media/platform/omap/omap_vout_vrfb.c
new file mode 100644
index 0000000..c6e2527
--- /dev/null
+++ b/drivers/media/platform/omap/omap_vout_vrfb.c
@@ -0,0 +1,394 @@
+/*
+ * omap_vout_vrfb.c
+ *
+ * Copyright (C) 2010 Texas Instruments.
+ *
+ * This file is licensed under the terms of the GNU General Public License
+ * version 2. This program is licensed "as is" without any warranty of any
+ * kind, whether express or implied.
+ *
+ */
+
+#include <linux/sched.h>
+#include <linux/platform_device.h>
+#include <linux/videodev2.h>
+
+#include <media/videobuf-dma-contig.h>
+#include <media/v4l2-device.h>
+
+#include <linux/omap-dma.h>
+#include <video/omapvrfb.h>
+
+#include "omap_voutdef.h"
+#include "omap_voutlib.h"
+#include "omap_vout_vrfb.h"
+
+#define OMAP_DMA_NO_DEVICE	0
+
+/*
+ * Function for allocating video buffers
+ */
+static int omap_vout_allocate_vrfb_buffers(struct omap_vout_device *vout,
+		unsigned int *count, int startindex)
+{
+	int i, j;
+
+	for (i = 0; i < *count; i++) {
+		if (!vout->smsshado_virt_addr[i]) {
+			vout->smsshado_virt_addr[i] =
+				omap_vout_alloc_buffer(vout->smsshado_size,
+						&vout->smsshado_phy_addr[i]);
+		}
+		if (!vout->smsshado_virt_addr[i] && startindex != -1) {
+			if (V4L2_MEMORY_MMAP == vout->memory && i >= startindex)
+				break;
+		}
+		if (!vout->smsshado_virt_addr[i]) {
+			for (j = 0; j < i; j++) {
+				omap_vout_free_buffer(
+						vout->smsshado_virt_addr[j],
+						vout->smsshado_size);
+				vout->smsshado_virt_addr[j] = 0;
+				vout->smsshado_phy_addr[j] = 0;
+			}
+			*count = 0;
+			return -ENOMEM;
+		}
+		memset((void *) vout->smsshado_virt_addr[i], 0,
+				vout->smsshado_size);
+	}
+	return 0;
+}
+
+/*
+ * Wakes up the application once the DMA transfer to VRFB space is completed.
+ */
+static void omap_vout_vrfb_dma_tx_callback(int lch, u16 ch_status, void *data)
+{
+	struct vid_vrfb_dma *t = (struct vid_vrfb_dma *) data;
+
+	t->tx_status = 1;
+	wake_up_interruptible(&t->wait);
+}
+
+/*
+ * Free VRFB buffers
+ */
+void omap_vout_free_vrfb_buffers(struct omap_vout_device *vout)
+{
+	int j;
+
+	for (j = 0; j < VRFB_NUM_BUFS; j++) {
+		omap_vout_free_buffer(vout->smsshado_virt_addr[j],
+				vout->smsshado_size);
+		vout->smsshado_virt_addr[j] = 0;
+		vout->smsshado_phy_addr[j] = 0;
+	}
+}
+
+int omap_vout_setup_vrfb_bufs(struct platform_device *pdev, int vid_num,
+			      bool static_vrfb_allocation)
+{
+	int ret = 0, i, j;
+	struct omap_vout_device *vout;
+	struct video_device *vfd;
+	int image_width, image_height;
+	int vrfb_num_bufs = VRFB_NUM_BUFS;
+	struct v4l2_device *v4l2_dev = platform_get_drvdata(pdev);
+	struct omap2video_device *vid_dev =
+		container_of(v4l2_dev, struct omap2video_device, v4l2_dev);
+
+	vout = vid_dev->vouts[vid_num];
+	vfd = vout->vfd;
+
+	for (i = 0; i < VRFB_NUM_BUFS; i++) {
+		if (omap_vrfb_request_ctx(&vout->vrfb_context[i])) {
+			dev_info(&pdev->dev, ": VRFB allocation failed\n");
+			for (j = 0; j < i; j++)
+				omap_vrfb_release_ctx(&vout->vrfb_context[j]);
+			ret = -ENOMEM;
+			goto free_buffers;
+		}
+	}
+
+	/* Calculate VRFB memory size */
+	/* allocate for worst case size */
+	image_width = VID_MAX_WIDTH / TILE_SIZE;
+	if (VID_MAX_WIDTH % TILE_SIZE)
+		image_width++;
+
+	image_width = image_width * TILE_SIZE;
+	image_height = VID_MAX_HEIGHT / TILE_SIZE;
+
+	if (VID_MAX_HEIGHT % TILE_SIZE)
+		image_height++;
+
+	image_height = image_height * TILE_SIZE;
+	vout->smsshado_size = PAGE_ALIGN(image_width * image_height * 2 * 2);
+
+	/*
+	 * Request and Initialize DMA, for DMA based VRFB transfer
+	 */
+	vout->vrfb_dma_tx.dev_id = OMAP_DMA_NO_DEVICE;
+	vout->vrfb_dma_tx.dma_ch = -1;
+	vout->vrfb_dma_tx.req_status = DMA_CHAN_ALLOTED;
+	ret = omap_request_dma(vout->vrfb_dma_tx.dev_id, "VRFB DMA TX",
+			omap_vout_vrfb_dma_tx_callback,
+			(void *) &vout->vrfb_dma_tx, &vout->vrfb_dma_tx.dma_ch);
+	if (ret < 0) {
+		vout->vrfb_dma_tx.req_status = DMA_CHAN_NOT_ALLOTED;
+		dev_info(&pdev->dev, ": failed to allocate DMA Channel for"
+				" video%d\n", vfd->minor);
+	}
+	init_waitqueue_head(&vout->vrfb_dma_tx.wait);
+
+	/* statically allocated the VRFB buffer is done through
+	   commands line aruments */
+	if (static_vrfb_allocation) {
+		if (omap_vout_allocate_vrfb_buffers(vout, &vrfb_num_bufs, -1)) {
+			ret =  -ENOMEM;
+			goto release_vrfb_ctx;
+		}
+		vout->vrfb_static_allocation = true;
+	}
+	return 0;
+
+release_vrfb_ctx:
+	for (j = 0; j < VRFB_NUM_BUFS; j++)
+		omap_vrfb_release_ctx(&vout->vrfb_context[j]);
+free_buffers:
+	omap_vout_free_buffers(vout);
+
+	return ret;
+}
+
+/*
+ * Release the VRFB context once the module exits
+ */
+void omap_vout_release_vrfb(struct omap_vout_device *vout)
+{
+	int i;
+
+	for (i = 0; i < VRFB_NUM_BUFS; i++)
+		omap_vrfb_release_ctx(&vout->vrfb_context[i]);
+
+	if (vout->vrfb_dma_tx.req_status == DMA_CHAN_ALLOTED) {
+		vout->vrfb_dma_tx.req_status = DMA_CHAN_NOT_ALLOTED;
+		omap_free_dma(vout->vrfb_dma_tx.dma_ch);
+	}
+}
+
+/*
+ * Allocate the buffers for the VRFB space.  Data is copied from V4L2
+ * buffers to the VRFB buffers using the DMA engine.
+ */
+int omap_vout_vrfb_buffer_setup(struct omap_vout_device *vout,
+			  unsigned int *count, unsigned int startindex)
+{
+	int i;
+	bool yuv_mode;
+
+	if (!is_rotation_enabled(vout))
+		return 0;
+
+	/* If rotation is enabled, allocate memory for VRFB space also */
+	*count = *count > VRFB_NUM_BUFS ? VRFB_NUM_BUFS : *count;
+
+	/* Allocate the VRFB buffers only if the buffers are not
+	 * allocated during init time.
+	 */
+	if (!vout->vrfb_static_allocation)
+		if (omap_vout_allocate_vrfb_buffers(vout, count, startindex))
+			return -ENOMEM;
+
+	if (vout->dss_mode == OMAP_DSS_COLOR_YUV2 ||
+			vout->dss_mode == OMAP_DSS_COLOR_UYVY)
+		yuv_mode = true;
+	else
+		yuv_mode = false;
+
+	for (i = 0; i < *count; i++)
+		omap_vrfb_setup(&vout->vrfb_context[i],
+				vout->smsshado_phy_addr[i], vout->pix.width,
+				vout->pix.height, vout->bpp, yuv_mode);
+
+	return 0;
+}
+
+int omap_vout_prepare_vrfb(struct omap_vout_device *vout,
+				struct videobuf_buffer *vb)
+{
+	dma_addr_t dmabuf;
+	struct vid_vrfb_dma *tx;
+	enum dss_rotation rotation;
+	u32 dest_frame_index = 0, src_element_index = 0;
+	u32 dest_element_index = 0, src_frame_index = 0;
+	u32 elem_count = 0, frame_count = 0, pixsize = 2;
+
+	if (!is_rotation_enabled(vout))
+		return 0;
+
+	dmabuf = vout->buf_phy_addr[vb->i];
+	/* If rotation is enabled, copy input buffer into VRFB
+	 * memory space using DMA. We are copying input buffer
+	 * into VRFB memory space of desired angle and DSS will
+	 * read image VRFB memory for 0 degree angle
+	 */
+	pixsize = vout->bpp * vout->vrfb_bpp;
+	/*
+	 * DMA transfer in double index mode
+	 */
+
+	/* Frame index */
+	dest_frame_index = ((MAX_PIXELS_PER_LINE * pixsize) -
+			(vout->pix.width * vout->bpp)) + 1;
+
+	/* Source and destination parameters */
+	src_element_index = 0;
+	src_frame_index = 0;
+	dest_element_index = 1;
+	/* Number of elements per frame */
+	elem_count = vout->pix.width * vout->bpp;
+	frame_count = vout->pix.height;
+	tx = &vout->vrfb_dma_tx;
+	tx->tx_status = 0;
+	omap_set_dma_transfer_params(tx->dma_ch, OMAP_DMA_DATA_TYPE_S32,
+			(elem_count / 4), frame_count, OMAP_DMA_SYNC_ELEMENT,
+			tx->dev_id, 0x0);
+	/* src_port required only for OMAP1 */
+	omap_set_dma_src_params(tx->dma_ch, 0, OMAP_DMA_AMODE_POST_INC,
+			dmabuf, src_element_index, src_frame_index);
+	/*set dma source burst mode for VRFB */
+	omap_set_dma_src_burst_mode(tx->dma_ch, OMAP_DMA_DATA_BURST_16);
+	rotation = calc_rotation(vout);
+
+	/* dest_port required only for OMAP1 */
+	omap_set_dma_dest_params(tx->dma_ch, 0, OMAP_DMA_AMODE_DOUBLE_IDX,
+			vout->vrfb_context[vb->i].paddr[0], dest_element_index,
+			dest_frame_index);
+	/*set dma dest burst mode for VRFB */
+	omap_set_dma_dest_burst_mode(tx->dma_ch, OMAP_DMA_DATA_BURST_16);
+	omap_dma_set_global_params(DMA_DEFAULT_ARB_RATE, 0x20, 0);
+
+	omap_start_dma(tx->dma_ch);
+	wait_event_interruptible_timeout(tx->wait, tx->tx_status == 1,
+					 VRFB_TX_TIMEOUT);
+
+	if (tx->tx_status == 0) {
+		omap_stop_dma(tx->dma_ch);
+		return -EINVAL;
+	}
+	/* Store buffers physical address into an array. Addresses
+	 * from this array will be used to configure DSS */
+	vout->queued_buf_addr[vb->i] = (u8 *)
+		vout->vrfb_context[vb->i].paddr[rotation];
+	return 0;
+}
+
+/*
+ * Calculate the buffer offsets from which the streaming should
+ * start. This offset calculation is mainly required because of
+ * the VRFB 32 pixels alignment with rotation.
+ */
+void omap_vout_calculate_vrfb_offset(struct omap_vout_device *vout)
+{
+	enum dss_rotation rotation;
+	bool mirroring = vout->mirror;
+	struct v4l2_rect *crop = &vout->crop;
+	struct v4l2_pix_format *pix = &vout->pix;
+	int *cropped_offset = &vout->cropped_offset;
+	int vr_ps = 1, ps = 2, temp_ps = 2;
+	int offset = 0, ctop = 0, cleft = 0, line_length = 0;
+
+	rotation = calc_rotation(vout);
+
+	if (V4L2_PIX_FMT_YUYV == pix->pixelformat ||
+			V4L2_PIX_FMT_UYVY == pix->pixelformat) {
+		if (is_rotation_enabled(vout)) {
+			/*
+			 * ps    - Actual pixel size for YUYV/UYVY for
+			 *         VRFB/Mirroring is 4 bytes
+			 * vr_ps - Virtually pixel size for YUYV/UYVY is
+			 *         2 bytes
+			 */
+			ps = 4;
+			vr_ps = 2;
+		} else {
+			ps = 2;	/* otherwise the pixel size is 2 byte */
+		}
+	} else if (V4L2_PIX_FMT_RGB32 == pix->pixelformat) {
+		ps = 4;
+	} else if (V4L2_PIX_FMT_RGB24 == pix->pixelformat) {
+		ps = 3;
+	}
+	vout->ps = ps;
+	vout->vr_ps = vr_ps;
+
+	if (is_rotation_enabled(vout)) {
+		line_length = MAX_PIXELS_PER_LINE;
+		ctop = (pix->height - crop->height) - crop->top;
+		cleft = (pix->width - crop->width) - crop->left;
+	} else {
+		line_length = pix->width;
+	}
+	vout->line_length = line_length;
+	switch (rotation) {
+	case dss_rotation_90_degree:
+		offset = vout->vrfb_context[0].yoffset *
+			vout->vrfb_context[0].bytespp;
+		temp_ps = ps / vr_ps;
+		if (!mirroring) {
+			*cropped_offset = offset + line_length *
+				temp_ps * cleft + crop->top * temp_ps;
+		} else {
+			*cropped_offset = offset + line_length * temp_ps *
+				cleft + crop->top * temp_ps + (line_length *
+				((crop->width / (vr_ps)) - 1) * ps);
+		}
+		break;
+	case dss_rotation_180_degree:
+		offset = ((MAX_PIXELS_PER_LINE * vout->vrfb_context[0].yoffset *
+			vout->vrfb_context[0].bytespp) +
+			(vout->vrfb_context[0].xoffset *
+			vout->vrfb_context[0].bytespp));
+		if (!mirroring) {
+			*cropped_offset = offset + (line_length * ps * ctop) +
+				(cleft / vr_ps) * ps;
+
+		} else {
+			*cropped_offset = offset + (line_length * ps * ctop) +
+				(cleft / vr_ps) * ps + (line_length *
+				(crop->height - 1) * ps);
+		}
+		break;
+	case dss_rotation_270_degree:
+		offset = MAX_PIXELS_PER_LINE * vout->vrfb_context[0].xoffset *
+			vout->vrfb_context[0].bytespp;
+		temp_ps = ps / vr_ps;
+		if (!mirroring) {
+			*cropped_offset = offset + line_length *
+			    temp_ps * crop->left + ctop * ps;
+		} else {
+			*cropped_offset = offset + line_length *
+				temp_ps * crop->left + ctop * ps +
+				(line_length * ((crop->width / vr_ps) - 1) *
+				 ps);
+		}
+		break;
+	case dss_rotation_0_degree:
+		if (!mirroring) {
+			*cropped_offset = (line_length * ps) *
+				crop->top + (crop->left / vr_ps) * ps;
+		} else {
+			*cropped_offset = (line_length * ps) *
+				crop->top + (crop->left / vr_ps) * ps +
+				(line_length * (crop->height - 1) * ps);
+		}
+		break;
+	default:
+		*cropped_offset = (line_length * ps * crop->top) /
+			vr_ps + (crop->left * ps) / vr_ps +
+			((crop->width / vr_ps) - 1) * ps;
+		break;
+	}
+}
diff --git a/drivers/media/platform/omap/omap_vout_vrfb.h b/drivers/media/platform/omap/omap_vout_vrfb.h
new file mode 100644
index 0000000..c976975
--- /dev/null
+++ b/drivers/media/platform/omap/omap_vout_vrfb.h
@@ -0,0 +1,40 @@
+/*
+ * omap_vout_vrfb.h
+ *
+ * Copyright (C) 2010 Texas Instruments.
+ *
+ * This file is licensed under the terms of the GNU General Public License
+ * version 2. This program is licensed "as is" without any warranty of any
+ * kind, whether express or implied.
+ *
+ */
+
+#ifndef OMAP_VOUT_VRFB_H
+#define OMAP_VOUT_VRFB_H
+
+#ifdef CONFIG_VIDEO_OMAP2_VOUT_VRFB
+void omap_vout_free_vrfb_buffers(struct omap_vout_device *vout);
+int omap_vout_setup_vrfb_bufs(struct platform_device *pdev, int vid_num,
+			bool static_vrfb_allocation);
+void omap_vout_release_vrfb(struct omap_vout_device *vout);
+int omap_vout_vrfb_buffer_setup(struct omap_vout_device *vout,
+			unsigned int *count, unsigned int startindex);
+int omap_vout_prepare_vrfb(struct omap_vout_device *vout,
+			struct videobuf_buffer *vb);
+void omap_vout_calculate_vrfb_offset(struct omap_vout_device *vout);
+#else
+static inline void omap_vout_free_vrfb_buffers(struct omap_vout_device *vout) { };
+static inline int omap_vout_setup_vrfb_bufs(struct platform_device *pdev, int vid_num,
+			bool static_vrfb_allocation)
+		{ return 0; };
+static inline void omap_vout_release_vrfb(struct omap_vout_device *vout) { };
+static inline int omap_vout_vrfb_buffer_setup(struct omap_vout_device *vout,
+			unsigned int *count, unsigned int startindex)
+		{ return 0; };
+static inline int omap_vout_prepare_vrfb(struct omap_vout_device *vout,
+			struct videobuf_buffer *vb)
+		{ return 0; };
+static inline void omap_vout_calculate_vrfb_offset(struct omap_vout_device *vout) { };
+#endif
+
+#endif
diff --git a/drivers/media/platform/omap/omap_voutdef.h b/drivers/media/platform/omap/omap_voutdef.h
new file mode 100644
index 0000000..9ccfe1f
--- /dev/null
+++ b/drivers/media/platform/omap/omap_voutdef.h
@@ -0,0 +1,225 @@
+/*
+ * omap_voutdef.h
+ *
+ * Copyright (C) 2010 Texas Instruments.
+ *
+ * This file is licensed under the terms of the GNU General Public License
+ * version 2. This program is licensed "as is" without any warranty of any
+ * kind, whether express or implied.
+ */
+
+#ifndef OMAP_VOUTDEF_H
+#define OMAP_VOUTDEF_H
+
+#include <video/omapdss.h>
+#include <video/omapvrfb.h>
+
+#define YUYV_BPP        2
+#define RGB565_BPP      2
+#define RGB24_BPP       3
+#define RGB32_BPP       4
+#define TILE_SIZE       32
+#define YUYV_VRFB_BPP   2
+#define RGB_VRFB_BPP    1
+#define MAX_CID		3
+#define MAC_VRFB_CTXS	4
+#define MAX_VOUT_DEV	2
+#define MAX_OVLS	3
+#define MAX_DISPLAYS	10
+#define MAX_MANAGERS	3
+
+#define QQVGA_WIDTH		160
+#define QQVGA_HEIGHT		120
+
+/* Max Resolution supported by the driver */
+#define VID_MAX_WIDTH		1280	/* Largest width */
+#define VID_MAX_HEIGHT		720	/* Largest height */
+
+/* Mimimum requirement is 2x2 for DSS */
+#define VID_MIN_WIDTH		2
+#define VID_MIN_HEIGHT		2
+
+/* 2048 x 2048 is max res supported by OMAP display controller */
+#define MAX_PIXELS_PER_LINE     2048
+
+#define VRFB_TX_TIMEOUT         1000
+#define VRFB_NUM_BUFS		4
+
+/* Max buffer size tobe allocated during init */
+#define OMAP_VOUT_MAX_BUF_SIZE (VID_MAX_WIDTH*VID_MAX_HEIGHT*4)
+
+enum dma_channel_state {
+	DMA_CHAN_NOT_ALLOTED,
+	DMA_CHAN_ALLOTED,
+};
+
+/* Enum for Rotation
+ * DSS understands rotation in 0, 1, 2, 3 context
+ * while V4L2 driver understands it as 0, 90, 180, 270
+ */
+enum dss_rotation {
+	dss_rotation_0_degree	= 0,
+	dss_rotation_90_degree	= 1,
+	dss_rotation_180_degree	= 2,
+	dss_rotation_270_degree = 3,
+};
+
+/* Enum for choosing rotation type for vout
+ * DSS2 doesn't understand no rotation as an
+ * option while V4L2 driver doesn't support
+ * rotation in the case where VRFB is not built in
+ * the kernel
+ */
+enum vout_rotaion_type {
+	VOUT_ROT_NONE	= 0,
+	VOUT_ROT_VRFB	= 1,
+};
+
+/*
+ * This structure is used to store the DMA transfer parameters
+ * for VRFB hidden buffer
+ */
+struct vid_vrfb_dma {
+	int dev_id;
+	int dma_ch;
+	int req_status;
+	int tx_status;
+	wait_queue_head_t wait;
+};
+
+struct omapvideo_info {
+	int id;
+	int num_overlays;
+	struct omap_overlay *overlays[MAX_OVLS];
+	enum vout_rotaion_type rotation_type;
+};
+
+struct omap2video_device {
+	struct mutex  mtx;
+
+	int state;
+
+	struct v4l2_device v4l2_dev;
+	struct omap_vout_device *vouts[MAX_VOUT_DEV];
+
+	int num_displays;
+	struct omap_dss_device *displays[MAX_DISPLAYS];
+	int num_overlays;
+	struct omap_overlay *overlays[MAX_OVLS];
+	int num_managers;
+	struct omap_overlay_manager *managers[MAX_MANAGERS];
+};
+
+/* per-device data structure */
+struct omap_vout_device {
+
+	struct omapvideo_info vid_info;
+	struct video_device *vfd;
+	struct omap2video_device *vid_dev;
+	int vid;
+	int opened;
+
+	/* we don't allow to change image fmt/size once buffer has
+	 * been allocated
+	 */
+	int buffer_allocated;
+	/* allow to reuse previously allocated buffer which is big enough */
+	int buffer_size;
+	/* keep buffer info across opens */
+	unsigned long buf_virt_addr[VIDEO_MAX_FRAME];
+	unsigned long buf_phy_addr[VIDEO_MAX_FRAME];
+	enum omap_color_mode dss_mode;
+
+	/* we don't allow to request new buffer when old buffers are
+	 * still mmaped
+	 */
+	int mmap_count;
+
+	spinlock_t vbq_lock;		/* spinlock for videobuf queues */
+	unsigned long field_count;	/* field counter for videobuf_buffer */
+
+	/* non-NULL means streaming is in progress. */
+	bool streaming;
+
+	struct v4l2_pix_format pix;
+	struct v4l2_rect crop;
+	struct v4l2_window win;
+	struct v4l2_framebuffer fbuf;
+
+	/* Lock to protect the shared data structures in ioctl */
+	struct mutex lock;
+
+	/* V4L2 control structure for different control id */
+	struct v4l2_control control[MAX_CID];
+	enum dss_rotation rotation;
+	bool mirror;
+	int flicker_filter;
+	/* V4L2 control structure for different control id */
+
+	int bpp; /* bytes per pixel */
+	int vrfb_bpp; /* bytes per pixel with respect to VRFB */
+
+	struct vid_vrfb_dma vrfb_dma_tx;
+	unsigned int smsshado_phy_addr[MAC_VRFB_CTXS];
+	unsigned int smsshado_virt_addr[MAC_VRFB_CTXS];
+	struct vrfb vrfb_context[MAC_VRFB_CTXS];
+	bool vrfb_static_allocation;
+	unsigned int smsshado_size;
+	unsigned char pos;
+
+	int ps, vr_ps, line_length, first_int, field_id;
+	enum v4l2_memory memory;
+	struct videobuf_buffer *cur_frm, *next_frm;
+	struct list_head dma_queue;
+	u8 *queued_buf_addr[VIDEO_MAX_FRAME];
+	u32 cropped_offset;
+	s32 tv_field1_offset;
+	void *isr_handle;
+
+	/* Buffer queue variables */
+	struct omap_vout_device *vout;
+	enum v4l2_buf_type type;
+	struct videobuf_queue vbq;
+	int io_allowed;
+
+};
+
+/*
+ * Return true if rotation is 90 or 270
+ */
+static inline int is_rotation_90_or_270(const struct omap_vout_device *vout)
+{
+	return (vout->rotation == dss_rotation_90_degree ||
+			vout->rotation == dss_rotation_270_degree);
+}
+
+/*
+ * Return true if rotation is enabled
+ */
+static inline int is_rotation_enabled(const struct omap_vout_device *vout)
+{
+	return vout->rotation || vout->mirror;
+}
+
+/*
+ * Reverse the rotation degree if mirroring is enabled
+ */
+static inline int calc_rotation(const struct omap_vout_device *vout)
+{
+	if (!vout->mirror)
+		return vout->rotation;
+
+	switch (vout->rotation) {
+	case dss_rotation_90_degree:
+		return dss_rotation_270_degree;
+	case dss_rotation_270_degree:
+		return dss_rotation_90_degree;
+	case dss_rotation_180_degree:
+		return dss_rotation_0_degree;
+	default:
+		return dss_rotation_180_degree;
+	}
+}
+
+void omap_vout_free_buffers(struct omap_vout_device *vout);
+#endif	/* ifndef OMAP_VOUTDEF_H */
diff --git a/drivers/media/platform/omap/omap_voutlib.c b/drivers/media/platform/omap/omap_voutlib.c
new file mode 100644
index 0000000..80b0d88
--- /dev/null
+++ b/drivers/media/platform/omap/omap_voutlib.c
@@ -0,0 +1,357 @@
+/*
+ * omap_voutlib.c
+ *
+ * Copyright (C) 2005-2010 Texas Instruments.
+ *
+ * This file is licensed under the terms of the GNU General Public License
+ * version 2. This program is licensed "as is" without any warranty of any
+ * kind, whether express or implied.
+ *
+ * Based on the OMAP2 camera driver
+ * Video-for-Linux (Version 2) camera capture driver for
+ * the OMAP24xx camera controller.
+ *
+ * Author: Andy Lowe (source@mvista.com)
+ *
+ * Copyright (C) 2004 MontaVista Software, Inc.
+ * Copyright (C) 2010 Texas Instruments.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/videodev2.h>
+
+#include <linux/dma-mapping.h>
+
+#include <video/omapdss.h>
+
+#include "omap_voutlib.h"
+
+MODULE_AUTHOR("Texas Instruments");
+MODULE_DESCRIPTION("OMAP Video library");
+MODULE_LICENSE("GPL");
+
+/* Return the default overlay cropping rectangle in crop given the image
+ * size in pix and the video display size in fbuf.  The default
+ * cropping rectangle is the largest rectangle no larger than the capture size
+ * that will fit on the display.  The default cropping rectangle is centered in
+ * the image.  All dimensions and offsets are rounded down to even numbers.
+ */
+void omap_vout_default_crop(struct v4l2_pix_format *pix,
+		  struct v4l2_framebuffer *fbuf, struct v4l2_rect *crop)
+{
+	crop->width = (pix->width < fbuf->fmt.width) ?
+		pix->width : fbuf->fmt.width;
+	crop->height = (pix->height < fbuf->fmt.height) ?
+		pix->height : fbuf->fmt.height;
+	crop->width &= ~1;
+	crop->height &= ~1;
+	crop->left = ((pix->width - crop->width) >> 1) & ~1;
+	crop->top = ((pix->height - crop->height) >> 1) & ~1;
+}
+EXPORT_SYMBOL_GPL(omap_vout_default_crop);
+
+/* Given a new render window in new_win, adjust the window to the
+ * nearest supported configuration.  The adjusted window parameters are
+ * returned in new_win.
+ * Returns zero if successful, or -EINVAL if the requested window is
+ * impossible and cannot reasonably be adjusted.
+ */
+int omap_vout_try_window(struct v4l2_framebuffer *fbuf,
+			struct v4l2_window *new_win)
+{
+	struct v4l2_rect try_win;
+
+	/* make a working copy of the new_win rectangle */
+	try_win = new_win->w;
+
+	/* adjust the preview window so it fits on the display by clipping any
+	 * offscreen areas
+	 */
+	if (try_win.left < 0) {
+		try_win.width += try_win.left;
+		try_win.left = 0;
+	}
+	if (try_win.top < 0) {
+		try_win.height += try_win.top;
+		try_win.top = 0;
+	}
+	try_win.width = (try_win.width < fbuf->fmt.width) ?
+		try_win.width : fbuf->fmt.width;
+	try_win.height = (try_win.height < fbuf->fmt.height) ?
+		try_win.height : fbuf->fmt.height;
+	if (try_win.left + try_win.width > fbuf->fmt.width)
+		try_win.width = fbuf->fmt.width - try_win.left;
+	if (try_win.top + try_win.height > fbuf->fmt.height)
+		try_win.height = fbuf->fmt.height - try_win.top;
+	try_win.width &= ~1;
+	try_win.height &= ~1;
+
+	if (try_win.width <= 0 || try_win.height <= 0)
+		return -EINVAL;
+
+	/* We now have a valid preview window, so go with it */
+	new_win->w = try_win;
+	new_win->field = V4L2_FIELD_ANY;
+	return 0;
+}
+EXPORT_SYMBOL_GPL(omap_vout_try_window);
+
+/* Given a new render window in new_win, adjust the window to the
+ * nearest supported configuration.  The image cropping window in crop
+ * will also be adjusted if necessary.  Preference is given to keeping the
+ * the window as close to the requested configuration as possible.  If
+ * successful, new_win, vout->win, and crop are updated.
+ * Returns zero if successful, or -EINVAL if the requested preview window is
+ * impossible and cannot reasonably be adjusted.
+ */
+int omap_vout_new_window(struct v4l2_rect *crop,
+		struct v4l2_window *win, struct v4l2_framebuffer *fbuf,
+		struct v4l2_window *new_win)
+{
+	int err;
+
+	err = omap_vout_try_window(fbuf, new_win);
+	if (err)
+		return err;
+
+	/* update our preview window */
+	win->w = new_win->w;
+	win->field = new_win->field;
+	win->chromakey = new_win->chromakey;
+
+	/* Adjust the cropping window to allow for resizing limitation */
+	if (omap_vout_dss_omap24xx()) {
+		/* For 24xx limit is 8x to 1/2x scaling. */
+		if ((crop->height/win->w.height) >= 2)
+			crop->height = win->w.height * 2;
+
+		if ((crop->width/win->w.width) >= 2)
+			crop->width = win->w.width * 2;
+
+		if (crop->width > 768) {
+			/* The OMAP2420 vertical resizing line buffer is 768
+			 * pixels wide. If the cropped image is wider than
+			 * 768 pixels then it cannot be vertically resized.
+			 */
+			if (crop->height != win->w.height)
+				crop->width = 768;
+		}
+	} else if (omap_vout_dss_omap34xx()) {
+		/* For 34xx limit is 8x to 1/4x scaling. */
+		if ((crop->height/win->w.height) >= 4)
+			crop->height = win->w.height * 4;
+
+		if ((crop->width/win->w.width) >= 4)
+			crop->width = win->w.width * 4;
+	}
+	return 0;
+}
+EXPORT_SYMBOL_GPL(omap_vout_new_window);
+
+/* Given a new cropping rectangle in new_crop, adjust the cropping rectangle to
+ * the nearest supported configuration.  The image render window in win will
+ * also be adjusted if necessary.  The preview window is adjusted such that the
+ * horizontal and vertical rescaling ratios stay constant.  If the render
+ * window would fall outside the display boundaries, the cropping rectangle
+ * will also be adjusted to maintain the rescaling ratios.  If successful, crop
+ * and win are updated.
+ * Returns zero if successful, or -EINVAL if the requested cropping rectangle is
+ * impossible and cannot reasonably be adjusted.
+ */
+int omap_vout_new_crop(struct v4l2_pix_format *pix,
+	      struct v4l2_rect *crop, struct v4l2_window *win,
+	      struct v4l2_framebuffer *fbuf, const struct v4l2_rect *new_crop)
+{
+	struct v4l2_rect try_crop;
+	unsigned long vresize, hresize;
+
+	/* make a working copy of the new_crop rectangle */
+	try_crop = *new_crop;
+
+	/* adjust the cropping rectangle so it fits in the image */
+	if (try_crop.left < 0) {
+		try_crop.width += try_crop.left;
+		try_crop.left = 0;
+	}
+	if (try_crop.top < 0) {
+		try_crop.height += try_crop.top;
+		try_crop.top = 0;
+	}
+	try_crop.width = (try_crop.width < pix->width) ?
+		try_crop.width : pix->width;
+	try_crop.height = (try_crop.height < pix->height) ?
+		try_crop.height : pix->height;
+	if (try_crop.left + try_crop.width > pix->width)
+		try_crop.width = pix->width - try_crop.left;
+	if (try_crop.top + try_crop.height > pix->height)
+		try_crop.height = pix->height - try_crop.top;
+
+	try_crop.width &= ~1;
+	try_crop.height &= ~1;
+
+	if (try_crop.width <= 0 || try_crop.height <= 0)
+		return -EINVAL;
+
+	if (omap_vout_dss_omap24xx()) {
+		if (try_crop.height != win->w.height) {
+			/* If we're resizing vertically, we can't support a
+			 * crop width wider than 768 pixels.
+			 */
+			if (try_crop.width > 768)
+				try_crop.width = 768;
+		}
+	}
+	/* vertical resizing */
+	vresize = (1024 * try_crop.height) / win->w.height;
+	if (omap_vout_dss_omap24xx() && (vresize > 2048))
+		vresize = 2048;
+	else if (omap_vout_dss_omap34xx() && (vresize > 4096))
+		vresize = 4096;
+
+	win->w.height = ((1024 * try_crop.height) / vresize) & ~1;
+	if (win->w.height == 0)
+		win->w.height = 2;
+	if (win->w.height + win->w.top > fbuf->fmt.height) {
+		/* We made the preview window extend below the bottom of the
+		 * display, so clip it to the display boundary and resize the
+		 * cropping height to maintain the vertical resizing ratio.
+		 */
+		win->w.height = (fbuf->fmt.height - win->w.top) & ~1;
+		if (try_crop.height == 0)
+			try_crop.height = 2;
+	}
+	/* horizontal resizing */
+	hresize = (1024 * try_crop.width) / win->w.width;
+	if (omap_vout_dss_omap24xx() && (hresize > 2048))
+		hresize = 2048;
+	else if (omap_vout_dss_omap34xx() && (hresize > 4096))
+		hresize = 4096;
+
+	win->w.width = ((1024 * try_crop.width) / hresize) & ~1;
+	if (win->w.width == 0)
+		win->w.width = 2;
+	if (win->w.width + win->w.left > fbuf->fmt.width) {
+		/* We made the preview window extend past the right side of the
+		 * display, so clip it to the display boundary and resize the
+		 * cropping width to maintain the horizontal resizing ratio.
+		 */
+		win->w.width = (fbuf->fmt.width - win->w.left) & ~1;
+		if (try_crop.width == 0)
+			try_crop.width = 2;
+	}
+	if (omap_vout_dss_omap24xx()) {
+		if ((try_crop.height/win->w.height) >= 2)
+			try_crop.height = win->w.height * 2;
+
+		if ((try_crop.width/win->w.width) >= 2)
+			try_crop.width = win->w.width * 2;
+
+		if (try_crop.width > 768) {
+			/* The OMAP2420 vertical resizing line buffer is
+			 * 768 pixels wide.  If the cropped image is wider
+			 * than 768 pixels then it cannot be vertically resized.
+			 */
+			if (try_crop.height != win->w.height)
+				try_crop.width = 768;
+		}
+	} else if (omap_vout_dss_omap34xx()) {
+		if ((try_crop.height/win->w.height) >= 4)
+			try_crop.height = win->w.height * 4;
+
+		if ((try_crop.width/win->w.width) >= 4)
+			try_crop.width = win->w.width * 4;
+	}
+	/* update our cropping rectangle and we're done */
+	*crop = try_crop;
+	return 0;
+}
+EXPORT_SYMBOL_GPL(omap_vout_new_crop);
+
+/* Given a new format in pix and fbuf,  crop and win
+ * structures are initialized to default values. crop
+ * is initialized to the largest window size that will fit on the display.  The
+ * crop window is centered in the image. win is initialized to
+ * the same size as crop and is centered on the display.
+ * All sizes and offsets are constrained to be even numbers.
+ */
+void omap_vout_new_format(struct v4l2_pix_format *pix,
+		struct v4l2_framebuffer *fbuf, struct v4l2_rect *crop,
+		struct v4l2_window *win)
+{
+	/* crop defines the preview source window in the image capture
+	 * buffer
+	 */
+	omap_vout_default_crop(pix, fbuf, crop);
+
+	/* win defines the preview target window on the display */
+	win->w.width = crop->width;
+	win->w.height = crop->height;
+	win->w.left = ((fbuf->fmt.width - win->w.width) >> 1) & ~1;
+	win->w.top = ((fbuf->fmt.height - win->w.height) >> 1) & ~1;
+}
+EXPORT_SYMBOL_GPL(omap_vout_new_format);
+
+/*
+ * Allocate buffers
+ */
+unsigned long omap_vout_alloc_buffer(u32 buf_size, u32 *phys_addr)
+{
+	u32 order, size;
+	unsigned long virt_addr, addr;
+
+	size = PAGE_ALIGN(buf_size);
+	order = get_order(size);
+	virt_addr = __get_free_pages(GFP_KERNEL, order);
+	addr = virt_addr;
+
+	if (virt_addr) {
+		while (size > 0) {
+			SetPageReserved(virt_to_page(addr));
+			addr += PAGE_SIZE;
+			size -= PAGE_SIZE;
+		}
+	}
+	*phys_addr = (u32) virt_to_phys((void *) virt_addr);
+	return virt_addr;
+}
+
+/*
+ * Free buffers
+ */
+void omap_vout_free_buffer(unsigned long virtaddr, u32 buf_size)
+{
+	u32 order, size;
+	unsigned long addr = virtaddr;
+
+	size = PAGE_ALIGN(buf_size);
+	order = get_order(size);
+
+	while (size > 0) {
+		ClearPageReserved(virt_to_page(addr));
+		addr += PAGE_SIZE;
+		size -= PAGE_SIZE;
+	}
+	free_pages((unsigned long) virtaddr, order);
+}
+
+bool omap_vout_dss_omap24xx(void)
+{
+	return omapdss_get_version() == OMAPDSS_VER_OMAP24xx;
+}
+
+bool omap_vout_dss_omap34xx(void)
+{
+	switch (omapdss_get_version()) {
+	case OMAPDSS_VER_OMAP34xx_ES1:
+	case OMAPDSS_VER_OMAP34xx_ES3:
+	case OMAPDSS_VER_OMAP3630:
+	case OMAPDSS_VER_AM35xx:
+		return true;
+	default:
+		return false;
+	}
+}
diff --git a/drivers/media/platform/omap/omap_voutlib.h b/drivers/media/platform/omap/omap_voutlib.h
new file mode 100644
index 0000000..f9d1c07
--- /dev/null
+++ b/drivers/media/platform/omap/omap_voutlib.h
@@ -0,0 +1,39 @@
+/*
+ * omap_voutlib.h
+ *
+ * Copyright (C) 2010 Texas Instruments.
+ *
+ * This file is licensed under the terms of the GNU General Public License
+ * version 2. This program is licensed "as is" without any warranty of any
+ * kind, whether express or implied.
+ *
+ */
+
+#ifndef OMAP_VOUTLIB_H
+#define OMAP_VOUTLIB_H
+
+void omap_vout_default_crop(struct v4l2_pix_format *pix,
+		struct v4l2_framebuffer *fbuf, struct v4l2_rect *crop);
+
+int omap_vout_new_crop(struct v4l2_pix_format *pix,
+		struct v4l2_rect *crop, struct v4l2_window *win,
+		struct v4l2_framebuffer *fbuf,
+		const struct v4l2_rect *new_crop);
+
+int omap_vout_try_window(struct v4l2_framebuffer *fbuf,
+		struct v4l2_window *new_win);
+
+int omap_vout_new_window(struct v4l2_rect *crop,
+		struct v4l2_window *win, struct v4l2_framebuffer *fbuf,
+		struct v4l2_window *new_win);
+
+void omap_vout_new_format(struct v4l2_pix_format *pix,
+		struct v4l2_framebuffer *fbuf, struct v4l2_rect *crop,
+		struct v4l2_window *win);
+unsigned long omap_vout_alloc_buffer(u32 buf_size, u32 *phys_addr);
+void omap_vout_free_buffer(unsigned long virtaddr, u32 buf_size);
+
+bool omap_vout_dss_omap24xx(void);
+bool omap_vout_dss_omap34xx(void);
+#endif	/* #ifndef OMAP_VOUTLIB_H */
+
diff --git a/drivers/media/platform/omap3isp/Makefile b/drivers/media/platform/omap3isp/Makefile
new file mode 100644
index 0000000..254975a
--- /dev/null
+++ b/drivers/media/platform/omap3isp/Makefile
@@ -0,0 +1,11 @@
+# Makefile for OMAP3 ISP driver
+
+ccflags-$(CONFIG_VIDEO_OMAP3_DEBUG) += -DDEBUG
+
+omap3-isp-objs += \
+	isp.o ispvideo.o \
+	ispcsiphy.o ispccp2.o ispcsi2.o \
+	ispccdc.o isppreview.o ispresizer.o \
+	ispstat.o isph3a_aewb.o isph3a_af.o isphist.o
+
+obj-$(CONFIG_VIDEO_OMAP3) += omap3-isp.o
diff --git a/drivers/media/platform/omap3isp/cfa_coef_table.h b/drivers/media/platform/omap3isp/cfa_coef_table.h
new file mode 100644
index 0000000..e75b0eb
--- /dev/null
+++ b/drivers/media/platform/omap3isp/cfa_coef_table.h
@@ -0,0 +1,51 @@
+/*
+ * cfa_coef_table.h
+ *
+ * TI OMAP3 ISP - CFA coefficients table
+ *
+ * Copyright (C) 2009-2010 Nokia Corporation
+ *
+ * Contacts: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *	     Sakari Ailus <sakari.ailus@iki.fi>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ */
+
+{ 244, 0, 247,   0,  12,  27,  36, 247, 250,   0,  27,   0,   4, 250,  12, 244,
+248,   0,   0,   0,   0,  40,   0,   0, 244,  12, 250,   4,   0,  27,   0, 250,
+247,  36,  27,  12,   0, 247,   0, 244,   0,   0,  40,   0,   0,   0,   0, 248,
+244,   0, 247,   0,  12,  27,  36, 247, 250,   0,  27,   0,   4, 250,  12, 244,
+248,   0,   0,   0,   0,  40,   0,   0, 244,  12, 250,   4,   0,  27,   0, 250,
+247,  36,  27,  12,   0, 247,   0, 244,   0,   0,  40,   0,   0,   0,   0, 248,
+244,   0, 247,   0,  12,  27,  36, 247, 250,   0,  27,   0,   4, 250,  12, 244,
+248,   0,   0,   0,   0,  40,   0,   0, 244,  12, 250,   4,   0,  27,   0, 250,
+247,  36,  27,  12,   0, 247,   0, 244,   0,   0,  40,   0,   0,   0,   0, 248 },
+{ 0, 247,   0, 244, 247,  36,  27,  12,   0,  27,   0, 250, 244,  12, 250,   4,
+  0,   0,   0, 248,   0,   0,  40,   0,   4, 250,  12, 244, 250,   0,  27,   0,
+ 12,  27,  36, 247, 244,   0, 247,   0,   0,  40,   0,   0, 248,   0,   0,   0,
+  0, 247,   0, 244, 247,  36,  27,  12,   0,  27,   0, 250, 244,  12, 250,   4,
+  0,   0,   0, 248,   0,   0,  40,   0,   4, 250,  12, 244, 250,   0,  27,   0,
+ 12,  27,  36, 247, 244,   0, 247,   0,   0,  40,   0,   0, 248,   0,   0,   0,
+  0, 247,   0, 244, 247,  36,  27,  12,   0,  27,   0, 250, 244,  12, 250,   4,
+  0,   0,   0, 248,   0,   0,  40,   0,   4, 250,  12, 244, 250,   0,  27,   0,
+ 12,  27,  36, 247, 244,   0, 247,   0,   0,  40,   0,   0, 248,   0,   0,   0 },
+{ 4, 250,  12, 244, 250,   0,  27,   0,  12,  27,  36, 247, 244,   0, 247,   0,
+  0,   0,   0, 248,   0,   0,  40,   0,   0, 247,   0, 244, 247,  36,  27,  12,
+  0,  27,   0, 250, 244,  12, 250,   4,   0,  40,   0,   0, 248,   0,   0,   0,
+  4, 250,  12, 244, 250,   0,  27,   0,  12,  27,  36, 247, 244,   0, 247,   0,
+  0,   0,   0, 248,   0,   0,  40,   0,   0, 247,   0, 244, 247,  36,  27,  12,
+  0,  27,   0, 250, 244,  12, 250,   4,   0,  40,   0,   0, 248,   0,   0,   0,
+  4, 250,  12, 244, 250,   0,  27,   0,  12,  27,  36, 247, 244,   0, 247,   0,
+  0,   0,   0, 248,   0,   0,  40,   0,   0, 247,   0, 244, 247,  36,  27,  12,
+  0,  27,   0, 250, 244,  12, 250,   4,   0,  40,   0,   0, 248,   0,   0,   0 },
+{ 244,12, 250,   4,   0,  27,   0, 250, 247,  36,  27,  12,   0, 247,   0, 244,
+248,   0,   0,   0,   0,  40,   0,   0, 244,   0, 247,   0,  12,  27,  36, 247,
+250,   0,  27,   0,   4, 250,  12, 244,   0,   0,  40,   0,   0,   0,   0, 248,
+244,  12, 250,   4,   0,  27,   0, 250, 247,  36,  27,  12,   0, 247,   0, 244,
+248,   0,   0,   0,   0,  40,   0,   0, 244,   0, 247,   0,  12,  27,  36, 247,
+250,   0,  27,   0,   4, 250,  12, 244,   0,   0,  40,   0,   0,   0,   0, 248,
+244,  12, 250,   4,   0,  27,   0, 250, 247,  36,  27,  12,   0, 247,   0, 244,
+248,   0,   0,   0,   0,  40,   0,   0, 244,   0, 247,   0,  12,  27,  36, 247,
+250,   0,  27,   0,   4, 250,  12, 244,   0,   0,  40,   0,   0,   0,   0, 248 },
diff --git a/drivers/media/platform/omap3isp/gamma_table.h b/drivers/media/platform/omap3isp/gamma_table.h
new file mode 100644
index 0000000..3b50707
--- /dev/null
+++ b/drivers/media/platform/omap3isp/gamma_table.h
@@ -0,0 +1,80 @@
+/*
+ * gamma_table.h
+ *
+ * TI OMAP3 ISP - Default gamma table for all components
+ *
+ * Copyright (C) 2010 Nokia Corporation
+ * Copyright (C) 2009 Texas Instruments, Inc.
+ *
+ * Contacts: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *	     Sakari Ailus <sakari.ailus@iki.fi>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+  0,   0,   1,   2,   3,   3,   4,   5,   6,   8,  10,  12,  14,  16,  18,  20,
+ 22,  23,  25,  26,  28,  29,  31,  32,  34,  35,  36,  37,  39,  40,  41,  42,
+ 43,  44,  45,  46,  47,  48,  49,  50,  51,  52,  52,  53,  54,  55,  56,  57,
+ 58,  59,  60,  61,  62,  63,  63,  64,  65,  66,  66,  67,  68,  69,  69,  70,
+ 71,  72,  72,  73,  74,  75,  75,  76,  77,  78,  78,  79,  80,  81,  81,  82,
+ 83,  84,  84,  85,  86,  87,  88,  88,  89,  90,  91,  91,  92,  93,  94,  94,
+ 95,  96,  97,  97,  98,  98,  99,  99, 100, 100, 101, 101, 102, 103, 104, 104,
+105, 106, 107, 108, 108, 109, 110, 111, 111, 112, 113, 114, 114, 115, 116, 117,
+117, 118, 119, 119, 120, 120, 121, 121, 122, 122, 123, 123, 124, 124, 125, 125,
+126, 126, 127, 127, 128, 128, 129, 129, 130, 130, 131, 131, 132, 132, 133, 133,
+134, 134, 135, 135, 136, 136, 137, 137, 138, 138, 139, 139, 140, 140, 141, 141,
+142, 142, 143, 143, 144, 144, 145, 145, 146, 146, 147, 147, 148, 148, 149, 149,
+150, 150, 151, 151, 152, 152, 153, 153, 153, 153, 154, 154, 154, 154, 155, 155,
+156, 156, 157, 157, 158, 158, 158, 159, 159, 159, 160, 160, 160, 161, 161, 162,
+162, 163, 163, 164, 164, 164, 164, 165, 165, 165, 165, 166, 166, 167, 167, 168,
+168, 169, 169, 170, 170, 170, 170, 171, 171, 171, 171, 172, 172, 173, 173, 174,
+174, 175, 175, 176, 176, 176, 176, 177, 177, 177, 177, 178, 178, 178, 178, 179,
+179, 179, 179, 180, 180, 180, 180, 181, 181, 181, 181, 182, 182, 182, 182, 183,
+183, 183, 183, 184, 184, 184, 184, 185, 185, 185, 185, 186, 186, 186, 186, 187,
+187, 187, 187, 188, 188, 188, 188, 189, 189, 189, 189, 190, 190, 190, 190, 191,
+191, 191, 191, 192, 192, 192, 192, 193, 193, 193, 193, 194, 194, 194, 194, 195,
+195, 195, 195, 196, 196, 196, 196, 197, 197, 197, 197, 198, 198, 198, 198, 199,
+199, 199, 199, 200, 200, 200, 200, 201, 201, 201, 201, 202, 202, 202, 203, 203,
+203, 203, 204, 204, 204, 204, 205, 205, 205, 205, 206, 206, 206, 206, 207, 207,
+207, 207, 208, 208, 208, 208, 209, 209, 209, 209, 210, 210, 210, 210, 210, 210,
+210, 210, 210, 210, 210, 210, 211, 211, 211, 211, 211, 211, 211, 211, 211, 211,
+211, 212, 212, 212, 212, 213, 213, 213, 213, 213, 213, 213, 213, 213, 213, 213,
+213, 214, 214, 214, 214, 215, 215, 215, 215, 215, 215, 215, 215, 215, 215, 215,
+216, 216, 216, 216, 217, 217, 217, 217, 218, 218, 218, 218, 219, 219, 219, 219,
+219, 219, 219, 219, 219, 219, 219, 219, 220, 220, 220, 220, 221, 221, 221, 221,
+221, 221, 221, 221, 221, 221, 221, 222, 222, 222, 222, 223, 223, 223, 223, 223,
+223, 223, 223, 223, 223, 223, 223, 224, 224, 224, 224, 225, 225, 225, 225, 225,
+225, 225, 225, 225, 225, 225, 225, 225, 225, 225, 225, 225, 225, 225, 226, 226,
+226, 226, 227, 227, 227, 227, 227, 227, 227, 227, 227, 227, 227, 227, 228, 228,
+228, 229, 229, 229, 229, 229, 229, 229, 229, 229, 229, 229, 229, 230, 230, 230,
+230, 231, 231, 231, 231, 231, 231, 231, 231, 231, 231, 231, 231, 232, 232, 232,
+232, 232, 232, 232, 232, 232, 232, 232, 232, 232, 232, 232, 232, 232, 232, 232,
+233, 233, 233, 233, 234, 234, 234, 234, 234, 234, 234, 234, 234, 234, 234, 235,
+235, 235, 235, 236, 236, 236, 236, 236, 236, 236, 236, 236, 236, 236, 236, 236,
+236, 236, 236, 236, 236, 236, 237, 237, 237, 237, 238, 238, 238, 238, 238, 238,
+238, 238, 238, 238, 238, 238, 238, 238, 238, 238, 238, 238, 238, 238, 238, 238,
+238, 238, 238, 238, 238, 239, 239, 239, 239, 240, 240, 240, 240, 240, 240, 240,
+240, 240, 240, 240, 240, 240, 240, 240, 240, 240, 240, 240, 240, 240, 240, 240,
+240, 240, 240, 240, 241, 241, 241, 241, 242, 242, 242, 242, 242, 242, 242, 242,
+242, 242, 242, 242, 242, 242, 242, 242, 242, 242, 242, 242, 242, 242, 242, 242,
+242, 242, 243, 243, 243, 243, 244, 244, 244, 244, 244, 244, 244, 244, 244, 244,
+244, 244, 244, 244, 244, 244, 244, 244, 244, 244, 244, 244, 244, 244, 244, 244,
+244, 245, 245, 245, 245, 246, 246, 246, 246, 246, 246, 246, 246, 246, 246, 246,
+246, 246, 246, 246, 246, 246, 246, 246, 246, 246, 246, 246, 246, 246, 246, 246,
+246, 246, 246, 246, 246, 246, 246, 247, 247, 247, 247, 248, 248, 248, 248, 248,
+248, 248, 248, 248, 248, 248, 248, 248, 248, 248, 248, 248, 248, 248, 248, 248,
+248, 248, 248, 248, 248, 248, 249, 249, 249, 249, 250, 250, 250, 250, 250, 250,
+250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250,
+250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250, 250,
+250, 250, 250, 250, 251, 251, 251, 251, 252, 252, 252, 252, 252, 252, 252, 252,
+252, 252, 252, 252, 252, 252, 252, 252, 252, 252, 252, 252, 252, 252, 252, 252,
+252, 252, 252, 252, 252, 252, 252, 252, 252, 252, 252, 252, 252, 252, 252, 252,
+252, 252, 252, 252, 252, 252, 252, 252, 252, 252, 252, 252, 252, 252, 252, 252,
+252, 252, 252, 252, 252, 252, 252, 252, 253, 253, 253, 253, 253, 253, 253, 253,
+253, 253, 253, 253, 253, 253, 253, 253, 253, 253, 253, 253, 253, 253, 253, 253,
+253, 253, 253, 253, 253, 253, 253, 253, 253, 253, 253, 253, 253, 253, 253, 253,
+253, 253, 253, 253, 253, 253, 253, 253, 253, 253, 253, 253, 253, 253, 253, 253,
+253, 254, 254, 254, 254, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
diff --git a/drivers/media/platform/omap3isp/isp.c b/drivers/media/platform/omap3isp/isp.c
new file mode 100644
index 0000000..56e683b
--- /dev/null
+++ b/drivers/media/platform/omap3isp/isp.c
@@ -0,0 +1,2529 @@
+/*
+ * isp.c
+ *
+ * TI OMAP3 ISP - Core
+ *
+ * Copyright (C) 2006-2010 Nokia Corporation
+ * Copyright (C) 2007-2009 Texas Instruments, Inc.
+ *
+ * Contacts: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *	     Sakari Ailus <sakari.ailus@iki.fi>
+ *
+ * Contributors:
+ *	Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *	Sakari Ailus <sakari.ailus@iki.fi>
+ *	David Cohen <dacohen@gmail.com>
+ *	Stanimir Varbanov <svarbanov@mm-sol.com>
+ *	Vimarsh Zutshi <vimarsh.zutshi@gmail.com>
+ *	Tuukka Toivonen <tuukkat76@gmail.com>
+ *	Sergio Aguirre <saaguirre@ti.com>
+ *	Antti Koskipaa <akoskipa@gmail.com>
+ *	Ivan T. Ivanov <iivanov@mm-sol.com>
+ *	RaniSuneela <r-m@ti.com>
+ *	Atanas Filipov <afilipov@mm-sol.com>
+ *	Gjorgji Rosikopulos <grosikopulos@mm-sol.com>
+ *	Hiroshi DOYU <hiroshi.doyu@nokia.com>
+ *	Nayden Kanchev <nkanchev@mm-sol.com>
+ *	Phil Carmody <ext-phil.2.carmody@nokia.com>
+ *	Artem Bityutskiy <artem.bityutskiy@nokia.com>
+ *	Dominic Curran <dcurran@ti.com>
+ *	Ilkka Myllyperkio <ilkka.myllyperkio@sofica.fi>
+ *	Pallavi Kulkarni <p-kulkarni@ti.com>
+ *	Vaibhav Hiremath <hvaibhav@ti.com>
+ *	Mohit Jalori <mjalori@ti.com>
+ *	Sameer Venkatraman <sameerv@ti.com>
+ *	Senthilvadivu Guruswamy <svadivu@ti.com>
+ *	Thara Gopinath <thara@ti.com>
+ *	Toni Leinonen <toni.leinonen@nokia.com>
+ *	Troy Laramy <t-laramy@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <asm/cacheflush.h>
+
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/omap-iommu.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/sched.h>
+#include <linux/vmalloc.h>
+
+#include <asm/dma-iommu.h>
+
+#include <media/v4l2-common.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-of.h>
+
+#include "isp.h"
+#include "ispreg.h"
+#include "ispccdc.h"
+#include "isppreview.h"
+#include "ispresizer.h"
+#include "ispcsi2.h"
+#include "ispccp2.h"
+#include "isph3a.h"
+#include "isphist.h"
+
+static unsigned int autoidle;
+module_param(autoidle, int, 0444);
+MODULE_PARM_DESC(autoidle, "Enable OMAP3ISP AUTOIDLE support");
+
+static void isp_save_ctx(struct isp_device *isp);
+
+static void isp_restore_ctx(struct isp_device *isp);
+
+static const struct isp_res_mapping isp_res_maps[] = {
+	{
+		.isp_rev = ISP_REVISION_2_0,
+		.offset = {
+			/* first MMIO area */
+			0x0000, /* base, len 0x0070 */
+			0x0400, /* ccp2, len 0x01f0 */
+			0x0600, /* ccdc, len 0x00a8 */
+			0x0a00, /* hist, len 0x0048 */
+			0x0c00, /* h3a, len 0x0060 */
+			0x0e00, /* preview, len 0x00a0 */
+			0x1000, /* resizer, len 0x00ac */
+			0x1200, /* sbl, len 0x00fc */
+			/* second MMIO area */
+			0x0000, /* csi2a, len 0x0170 */
+			0x0170, /* csiphy2, len 0x000c */
+		},
+		.phy_type = ISP_PHY_TYPE_3430,
+	},
+	{
+		.isp_rev = ISP_REVISION_15_0,
+		.offset = {
+			/* first MMIO area */
+			0x0000, /* base, len 0x0070 */
+			0x0400, /* ccp2, len 0x01f0 */
+			0x0600, /* ccdc, len 0x00a8 */
+			0x0a00, /* hist, len 0x0048 */
+			0x0c00, /* h3a, len 0x0060 */
+			0x0e00, /* preview, len 0x00a0 */
+			0x1000, /* resizer, len 0x00ac */
+			0x1200, /* sbl, len 0x00fc */
+			/* second MMIO area */
+			0x0000, /* csi2a, len 0x0170 (1st area) */
+			0x0170, /* csiphy2, len 0x000c */
+			0x01c0, /* csi2a, len 0x0040 (2nd area) */
+			0x0400, /* csi2c, len 0x0170 (1st area) */
+			0x0570, /* csiphy1, len 0x000c */
+			0x05c0, /* csi2c, len 0x0040 (2nd area) */
+		},
+		.phy_type = ISP_PHY_TYPE_3630,
+	},
+};
+
+/* Structure for saving/restoring ISP module registers */
+static struct isp_reg isp_reg_list[] = {
+	{OMAP3_ISP_IOMEM_MAIN, ISP_SYSCONFIG, 0},
+	{OMAP3_ISP_IOMEM_MAIN, ISP_CTRL, 0},
+	{OMAP3_ISP_IOMEM_MAIN, ISP_TCTRL_CTRL, 0},
+	{0, ISP_TOK_TERM, 0}
+};
+
+/*
+ * omap3isp_flush - Post pending L3 bus writes by doing a register readback
+ * @isp: OMAP3 ISP device
+ *
+ * In order to force posting of pending writes, we need to write and
+ * readback the same register, in this case the revision register.
+ *
+ * See this link for reference:
+ *   http://www.mail-archive.com/linux-omap@vger.kernel.org/msg08149.html
+ */
+void omap3isp_flush(struct isp_device *isp)
+{
+	isp_reg_writel(isp, 0, OMAP3_ISP_IOMEM_MAIN, ISP_REVISION);
+	isp_reg_readl(isp, OMAP3_ISP_IOMEM_MAIN, ISP_REVISION);
+}
+
+/* -----------------------------------------------------------------------------
+ * XCLK
+ */
+
+#define to_isp_xclk(_hw)	container_of(_hw, struct isp_xclk, hw)
+
+static void isp_xclk_update(struct isp_xclk *xclk, u32 divider)
+{
+	switch (xclk->id) {
+	case ISP_XCLK_A:
+		isp_reg_clr_set(xclk->isp, OMAP3_ISP_IOMEM_MAIN, ISP_TCTRL_CTRL,
+				ISPTCTRL_CTRL_DIVA_MASK,
+				divider << ISPTCTRL_CTRL_DIVA_SHIFT);
+		break;
+	case ISP_XCLK_B:
+		isp_reg_clr_set(xclk->isp, OMAP3_ISP_IOMEM_MAIN, ISP_TCTRL_CTRL,
+				ISPTCTRL_CTRL_DIVB_MASK,
+				divider << ISPTCTRL_CTRL_DIVB_SHIFT);
+		break;
+	}
+}
+
+static int isp_xclk_prepare(struct clk_hw *hw)
+{
+	struct isp_xclk *xclk = to_isp_xclk(hw);
+
+	omap3isp_get(xclk->isp);
+
+	return 0;
+}
+
+static void isp_xclk_unprepare(struct clk_hw *hw)
+{
+	struct isp_xclk *xclk = to_isp_xclk(hw);
+
+	omap3isp_put(xclk->isp);
+}
+
+static int isp_xclk_enable(struct clk_hw *hw)
+{
+	struct isp_xclk *xclk = to_isp_xclk(hw);
+	unsigned long flags;
+
+	spin_lock_irqsave(&xclk->lock, flags);
+	isp_xclk_update(xclk, xclk->divider);
+	xclk->enabled = true;
+	spin_unlock_irqrestore(&xclk->lock, flags);
+
+	return 0;
+}
+
+static void isp_xclk_disable(struct clk_hw *hw)
+{
+	struct isp_xclk *xclk = to_isp_xclk(hw);
+	unsigned long flags;
+
+	spin_lock_irqsave(&xclk->lock, flags);
+	isp_xclk_update(xclk, 0);
+	xclk->enabled = false;
+	spin_unlock_irqrestore(&xclk->lock, flags);
+}
+
+static unsigned long isp_xclk_recalc_rate(struct clk_hw *hw,
+					  unsigned long parent_rate)
+{
+	struct isp_xclk *xclk = to_isp_xclk(hw);
+
+	return parent_rate / xclk->divider;
+}
+
+static u32 isp_xclk_calc_divider(unsigned long *rate, unsigned long parent_rate)
+{
+	u32 divider;
+
+	if (*rate >= parent_rate) {
+		*rate = parent_rate;
+		return ISPTCTRL_CTRL_DIV_BYPASS;
+	}
+
+	if (*rate == 0)
+		*rate = 1;
+
+	divider = DIV_ROUND_CLOSEST(parent_rate, *rate);
+	if (divider >= ISPTCTRL_CTRL_DIV_BYPASS)
+		divider = ISPTCTRL_CTRL_DIV_BYPASS - 1;
+
+	*rate = parent_rate / divider;
+	return divider;
+}
+
+static long isp_xclk_round_rate(struct clk_hw *hw, unsigned long rate,
+				unsigned long *parent_rate)
+{
+	isp_xclk_calc_divider(&rate, *parent_rate);
+	return rate;
+}
+
+static int isp_xclk_set_rate(struct clk_hw *hw, unsigned long rate,
+			     unsigned long parent_rate)
+{
+	struct isp_xclk *xclk = to_isp_xclk(hw);
+	unsigned long flags;
+	u32 divider;
+
+	divider = isp_xclk_calc_divider(&rate, parent_rate);
+
+	spin_lock_irqsave(&xclk->lock, flags);
+
+	xclk->divider = divider;
+	if (xclk->enabled)
+		isp_xclk_update(xclk, divider);
+
+	spin_unlock_irqrestore(&xclk->lock, flags);
+
+	dev_dbg(xclk->isp->dev, "%s: cam_xclk%c set to %lu Hz (div %u)\n",
+		__func__, xclk->id == ISP_XCLK_A ? 'a' : 'b', rate, divider);
+	return 0;
+}
+
+static const struct clk_ops isp_xclk_ops = {
+	.prepare = isp_xclk_prepare,
+	.unprepare = isp_xclk_unprepare,
+	.enable = isp_xclk_enable,
+	.disable = isp_xclk_disable,
+	.recalc_rate = isp_xclk_recalc_rate,
+	.round_rate = isp_xclk_round_rate,
+	.set_rate = isp_xclk_set_rate,
+};
+
+static const char *isp_xclk_parent_name = "cam_mclk";
+
+static const struct clk_init_data isp_xclk_init_data = {
+	.name = "cam_xclk",
+	.ops = &isp_xclk_ops,
+	.parent_names = &isp_xclk_parent_name,
+	.num_parents = 1,
+};
+
+static struct clk *isp_xclk_src_get(struct of_phandle_args *clkspec, void *data)
+{
+	unsigned int idx = clkspec->args[0];
+	struct isp_device *isp = data;
+
+	if (idx >= ARRAY_SIZE(isp->xclks))
+		return ERR_PTR(-ENOENT);
+
+	return isp->xclks[idx].clk;
+}
+
+static int isp_xclk_init(struct isp_device *isp)
+{
+	struct device_node *np = isp->dev->of_node;
+	struct clk_init_data init;
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(isp->xclks); ++i)
+		isp->xclks[i].clk = ERR_PTR(-EINVAL);
+
+	for (i = 0; i < ARRAY_SIZE(isp->xclks); ++i) {
+		struct isp_xclk *xclk = &isp->xclks[i];
+
+		xclk->isp = isp;
+		xclk->id = i == 0 ? ISP_XCLK_A : ISP_XCLK_B;
+		xclk->divider = 1;
+		spin_lock_init(&xclk->lock);
+
+		init.name = i == 0 ? "cam_xclka" : "cam_xclkb";
+		init.ops = &isp_xclk_ops;
+		init.parent_names = &isp_xclk_parent_name;
+		init.num_parents = 1;
+
+		xclk->hw.init = &init;
+		/*
+		 * The first argument is NULL in order to avoid circular
+		 * reference, as this driver takes reference on the
+		 * sensor subdevice modules and the sensors would take
+		 * reference on this module through clk_get().
+		 */
+		xclk->clk = clk_register(NULL, &xclk->hw);
+		if (IS_ERR(xclk->clk))
+			return PTR_ERR(xclk->clk);
+	}
+
+	if (np)
+		of_clk_add_provider(np, isp_xclk_src_get, isp);
+
+	return 0;
+}
+
+static void isp_xclk_cleanup(struct isp_device *isp)
+{
+	struct device_node *np = isp->dev->of_node;
+	unsigned int i;
+
+	if (np)
+		of_clk_del_provider(np);
+
+	for (i = 0; i < ARRAY_SIZE(isp->xclks); ++i) {
+		struct isp_xclk *xclk = &isp->xclks[i];
+
+		if (!IS_ERR(xclk->clk))
+			clk_unregister(xclk->clk);
+	}
+}
+
+/* -----------------------------------------------------------------------------
+ * Interrupts
+ */
+
+/*
+ * isp_enable_interrupts - Enable ISP interrupts.
+ * @isp: OMAP3 ISP device
+ */
+static void isp_enable_interrupts(struct isp_device *isp)
+{
+	static const u32 irq = IRQ0ENABLE_CSIA_IRQ
+			     | IRQ0ENABLE_CSIB_IRQ
+			     | IRQ0ENABLE_CCDC_LSC_PREF_ERR_IRQ
+			     | IRQ0ENABLE_CCDC_LSC_DONE_IRQ
+			     | IRQ0ENABLE_CCDC_VD0_IRQ
+			     | IRQ0ENABLE_CCDC_VD1_IRQ
+			     | IRQ0ENABLE_HS_VS_IRQ
+			     | IRQ0ENABLE_HIST_DONE_IRQ
+			     | IRQ0ENABLE_H3A_AWB_DONE_IRQ
+			     | IRQ0ENABLE_H3A_AF_DONE_IRQ
+			     | IRQ0ENABLE_PRV_DONE_IRQ
+			     | IRQ0ENABLE_RSZ_DONE_IRQ;
+
+	isp_reg_writel(isp, irq, OMAP3_ISP_IOMEM_MAIN, ISP_IRQ0STATUS);
+	isp_reg_writel(isp, irq, OMAP3_ISP_IOMEM_MAIN, ISP_IRQ0ENABLE);
+}
+
+/*
+ * isp_disable_interrupts - Disable ISP interrupts.
+ * @isp: OMAP3 ISP device
+ */
+static void isp_disable_interrupts(struct isp_device *isp)
+{
+	isp_reg_writel(isp, 0, OMAP3_ISP_IOMEM_MAIN, ISP_IRQ0ENABLE);
+}
+
+/*
+ * isp_core_init - ISP core settings
+ * @isp: OMAP3 ISP device
+ * @idle: Consider idle state.
+ *
+ * Set the power settings for the ISP and SBL bus and configure the HS/VS
+ * interrupt source.
+ *
+ * We need to configure the HS/VS interrupt source before interrupts get
+ * enabled, as the sensor might be free-running and the ISP default setting
+ * (HS edge) would put an unnecessary burden on the CPU.
+ */
+static void isp_core_init(struct isp_device *isp, int idle)
+{
+	isp_reg_writel(isp,
+		       ((idle ? ISP_SYSCONFIG_MIDLEMODE_SMARTSTANDBY :
+				ISP_SYSCONFIG_MIDLEMODE_FORCESTANDBY) <<
+			ISP_SYSCONFIG_MIDLEMODE_SHIFT) |
+			((isp->revision == ISP_REVISION_15_0) ?
+			  ISP_SYSCONFIG_AUTOIDLE : 0),
+		       OMAP3_ISP_IOMEM_MAIN, ISP_SYSCONFIG);
+
+	isp_reg_writel(isp,
+		       (isp->autoidle ? ISPCTRL_SBL_AUTOIDLE : 0) |
+		       ISPCTRL_SYNC_DETECT_VSRISE,
+		       OMAP3_ISP_IOMEM_MAIN, ISP_CTRL);
+}
+
+/*
+ * Configure the bridge and lane shifter. Valid inputs are
+ *
+ * CCDC_INPUT_PARALLEL: Parallel interface
+ * CCDC_INPUT_CSI2A: CSI2a receiver
+ * CCDC_INPUT_CCP2B: CCP2b receiver
+ * CCDC_INPUT_CSI2C: CSI2c receiver
+ *
+ * The bridge and lane shifter are configured according to the selected input
+ * and the ISP platform data.
+ */
+void omap3isp_configure_bridge(struct isp_device *isp,
+			       enum ccdc_input_entity input,
+			       const struct isp_parallel_cfg *parcfg,
+			       unsigned int shift, unsigned int bridge)
+{
+	u32 ispctrl_val;
+
+	ispctrl_val  = isp_reg_readl(isp, OMAP3_ISP_IOMEM_MAIN, ISP_CTRL);
+	ispctrl_val &= ~ISPCTRL_SHIFT_MASK;
+	ispctrl_val &= ~ISPCTRL_PAR_CLK_POL_INV;
+	ispctrl_val &= ~ISPCTRL_PAR_SER_CLK_SEL_MASK;
+	ispctrl_val &= ~ISPCTRL_PAR_BRIDGE_MASK;
+	ispctrl_val |= bridge;
+
+	switch (input) {
+	case CCDC_INPUT_PARALLEL:
+		ispctrl_val |= ISPCTRL_PAR_SER_CLK_SEL_PARALLEL;
+		ispctrl_val |= parcfg->clk_pol << ISPCTRL_PAR_CLK_POL_SHIFT;
+		shift += parcfg->data_lane_shift * 2;
+		break;
+
+	case CCDC_INPUT_CSI2A:
+		ispctrl_val |= ISPCTRL_PAR_SER_CLK_SEL_CSIA;
+		break;
+
+	case CCDC_INPUT_CCP2B:
+		ispctrl_val |= ISPCTRL_PAR_SER_CLK_SEL_CSIB;
+		break;
+
+	case CCDC_INPUT_CSI2C:
+		ispctrl_val |= ISPCTRL_PAR_SER_CLK_SEL_CSIC;
+		break;
+
+	default:
+		return;
+	}
+
+	ispctrl_val |= ((shift/2) << ISPCTRL_SHIFT_SHIFT) & ISPCTRL_SHIFT_MASK;
+
+	isp_reg_writel(isp, ispctrl_val, OMAP3_ISP_IOMEM_MAIN, ISP_CTRL);
+}
+
+void omap3isp_hist_dma_done(struct isp_device *isp)
+{
+	if (omap3isp_ccdc_busy(&isp->isp_ccdc) ||
+	    omap3isp_stat_pcr_busy(&isp->isp_hist)) {
+		/* Histogram cannot be enabled in this frame anymore */
+		atomic_set(&isp->isp_hist.buf_err, 1);
+		dev_dbg(isp->dev, "hist: Out of synchronization with "
+				  "CCDC. Ignoring next buffer.\n");
+	}
+}
+
+static inline void isp_isr_dbg(struct isp_device *isp, u32 irqstatus)
+{
+	static const char *name[] = {
+		"CSIA_IRQ",
+		"res1",
+		"res2",
+		"CSIB_LCM_IRQ",
+		"CSIB_IRQ",
+		"res5",
+		"res6",
+		"res7",
+		"CCDC_VD0_IRQ",
+		"CCDC_VD1_IRQ",
+		"CCDC_VD2_IRQ",
+		"CCDC_ERR_IRQ",
+		"H3A_AF_DONE_IRQ",
+		"H3A_AWB_DONE_IRQ",
+		"res14",
+		"res15",
+		"HIST_DONE_IRQ",
+		"CCDC_LSC_DONE",
+		"CCDC_LSC_PREFETCH_COMPLETED",
+		"CCDC_LSC_PREFETCH_ERROR",
+		"PRV_DONE_IRQ",
+		"CBUFF_IRQ",
+		"res22",
+		"res23",
+		"RSZ_DONE_IRQ",
+		"OVF_IRQ",
+		"res26",
+		"res27",
+		"MMU_ERR_IRQ",
+		"OCP_ERR_IRQ",
+		"SEC_ERR_IRQ",
+		"HS_VS_IRQ",
+	};
+	int i;
+
+	dev_dbg(isp->dev, "ISP IRQ: ");
+
+	for (i = 0; i < ARRAY_SIZE(name); i++) {
+		if ((1 << i) & irqstatus)
+			printk(KERN_CONT "%s ", name[i]);
+	}
+	printk(KERN_CONT "\n");
+}
+
+static void isp_isr_sbl(struct isp_device *isp)
+{
+	struct device *dev = isp->dev;
+	struct isp_pipeline *pipe;
+	u32 sbl_pcr;
+
+	/*
+	 * Handle shared buffer logic overflows for video buffers.
+	 * ISPSBL_PCR_CCDCPRV_2_RSZ_OVF can be safely ignored.
+	 */
+	sbl_pcr = isp_reg_readl(isp, OMAP3_ISP_IOMEM_SBL, ISPSBL_PCR);
+	isp_reg_writel(isp, sbl_pcr, OMAP3_ISP_IOMEM_SBL, ISPSBL_PCR);
+	sbl_pcr &= ~ISPSBL_PCR_CCDCPRV_2_RSZ_OVF;
+
+	if (sbl_pcr)
+		dev_dbg(dev, "SBL overflow (PCR = 0x%08x)\n", sbl_pcr);
+
+	if (sbl_pcr & ISPSBL_PCR_CSIB_WBL_OVF) {
+		pipe = to_isp_pipeline(&isp->isp_ccp2.subdev.entity);
+		if (pipe != NULL)
+			pipe->error = true;
+	}
+
+	if (sbl_pcr & ISPSBL_PCR_CSIA_WBL_OVF) {
+		pipe = to_isp_pipeline(&isp->isp_csi2a.subdev.entity);
+		if (pipe != NULL)
+			pipe->error = true;
+	}
+
+	if (sbl_pcr & ISPSBL_PCR_CCDC_WBL_OVF) {
+		pipe = to_isp_pipeline(&isp->isp_ccdc.subdev.entity);
+		if (pipe != NULL)
+			pipe->error = true;
+	}
+
+	if (sbl_pcr & ISPSBL_PCR_PRV_WBL_OVF) {
+		pipe = to_isp_pipeline(&isp->isp_prev.subdev.entity);
+		if (pipe != NULL)
+			pipe->error = true;
+	}
+
+	if (sbl_pcr & (ISPSBL_PCR_RSZ1_WBL_OVF
+		       | ISPSBL_PCR_RSZ2_WBL_OVF
+		       | ISPSBL_PCR_RSZ3_WBL_OVF
+		       | ISPSBL_PCR_RSZ4_WBL_OVF)) {
+		pipe = to_isp_pipeline(&isp->isp_res.subdev.entity);
+		if (pipe != NULL)
+			pipe->error = true;
+	}
+
+	if (sbl_pcr & ISPSBL_PCR_H3A_AF_WBL_OVF)
+		omap3isp_stat_sbl_overflow(&isp->isp_af);
+
+	if (sbl_pcr & ISPSBL_PCR_H3A_AEAWB_WBL_OVF)
+		omap3isp_stat_sbl_overflow(&isp->isp_aewb);
+}
+
+/*
+ * isp_isr - Interrupt Service Routine for Camera ISP module.
+ * @irq: Not used currently.
+ * @_isp: Pointer to the OMAP3 ISP device
+ *
+ * Handles the corresponding callback if plugged in.
+ */
+static irqreturn_t isp_isr(int irq, void *_isp)
+{
+	static const u32 ccdc_events = IRQ0STATUS_CCDC_LSC_PREF_ERR_IRQ |
+				       IRQ0STATUS_CCDC_LSC_DONE_IRQ |
+				       IRQ0STATUS_CCDC_VD0_IRQ |
+				       IRQ0STATUS_CCDC_VD1_IRQ |
+				       IRQ0STATUS_HS_VS_IRQ;
+	struct isp_device *isp = _isp;
+	u32 irqstatus;
+
+	irqstatus = isp_reg_readl(isp, OMAP3_ISP_IOMEM_MAIN, ISP_IRQ0STATUS);
+	isp_reg_writel(isp, irqstatus, OMAP3_ISP_IOMEM_MAIN, ISP_IRQ0STATUS);
+
+	isp_isr_sbl(isp);
+
+	if (irqstatus & IRQ0STATUS_CSIA_IRQ)
+		omap3isp_csi2_isr(&isp->isp_csi2a);
+
+	if (irqstatus & IRQ0STATUS_CSIB_IRQ)
+		omap3isp_ccp2_isr(&isp->isp_ccp2);
+
+	if (irqstatus & IRQ0STATUS_CCDC_VD0_IRQ) {
+		if (isp->isp_ccdc.output & CCDC_OUTPUT_PREVIEW)
+			omap3isp_preview_isr_frame_sync(&isp->isp_prev);
+		if (isp->isp_ccdc.output & CCDC_OUTPUT_RESIZER)
+			omap3isp_resizer_isr_frame_sync(&isp->isp_res);
+		omap3isp_stat_isr_frame_sync(&isp->isp_aewb);
+		omap3isp_stat_isr_frame_sync(&isp->isp_af);
+		omap3isp_stat_isr_frame_sync(&isp->isp_hist);
+	}
+
+	if (irqstatus & ccdc_events)
+		omap3isp_ccdc_isr(&isp->isp_ccdc, irqstatus & ccdc_events);
+
+	if (irqstatus & IRQ0STATUS_PRV_DONE_IRQ) {
+		if (isp->isp_prev.output & PREVIEW_OUTPUT_RESIZER)
+			omap3isp_resizer_isr_frame_sync(&isp->isp_res);
+		omap3isp_preview_isr(&isp->isp_prev);
+	}
+
+	if (irqstatus & IRQ0STATUS_RSZ_DONE_IRQ)
+		omap3isp_resizer_isr(&isp->isp_res);
+
+	if (irqstatus & IRQ0STATUS_H3A_AWB_DONE_IRQ)
+		omap3isp_stat_isr(&isp->isp_aewb);
+
+	if (irqstatus & IRQ0STATUS_H3A_AF_DONE_IRQ)
+		omap3isp_stat_isr(&isp->isp_af);
+
+	if (irqstatus & IRQ0STATUS_HIST_DONE_IRQ)
+		omap3isp_stat_isr(&isp->isp_hist);
+
+	omap3isp_flush(isp);
+
+#if defined(DEBUG) && defined(ISP_ISR_DEBUG)
+	isp_isr_dbg(isp, irqstatus);
+#endif
+
+	return IRQ_HANDLED;
+}
+
+/* -----------------------------------------------------------------------------
+ * Pipeline power management
+ *
+ * Entities must be powered up when part of a pipeline that contains at least
+ * one open video device node.
+ *
+ * To achieve this use the entity use_count field to track the number of users.
+ * For entities corresponding to video device nodes the use_count field stores
+ * the users count of the node. For entities corresponding to subdevs the
+ * use_count field stores the total number of users of all video device nodes
+ * in the pipeline.
+ *
+ * The omap3isp_pipeline_pm_use() function must be called in the open() and
+ * close() handlers of video device nodes. It increments or decrements the use
+ * count of all subdev entities in the pipeline.
+ *
+ * To react to link management on powered pipelines, the link setup notification
+ * callback updates the use count of all entities in the source and sink sides
+ * of the link.
+ */
+
+/*
+ * isp_pipeline_pm_use_count - Count the number of users of a pipeline
+ * @entity: The entity
+ *
+ * Return the total number of users of all video device nodes in the pipeline.
+ */
+static int isp_pipeline_pm_use_count(struct media_entity *entity)
+{
+	struct media_entity_graph graph;
+	int use = 0;
+
+	media_entity_graph_walk_start(&graph, entity);
+
+	while ((entity = media_entity_graph_walk_next(&graph))) {
+		if (media_entity_type(entity) == MEDIA_ENT_T_DEVNODE)
+			use += entity->use_count;
+	}
+
+	return use;
+}
+
+/*
+ * isp_pipeline_pm_power_one - Apply power change to an entity
+ * @entity: The entity
+ * @change: Use count change
+ *
+ * Change the entity use count by @change. If the entity is a subdev update its
+ * power state by calling the core::s_power operation when the use count goes
+ * from 0 to != 0 or from != 0 to 0.
+ *
+ * Return 0 on success or a negative error code on failure.
+ */
+static int isp_pipeline_pm_power_one(struct media_entity *entity, int change)
+{
+	struct v4l2_subdev *subdev;
+	int ret;
+
+	subdev = media_entity_type(entity) == MEDIA_ENT_T_V4L2_SUBDEV
+	       ? media_entity_to_v4l2_subdev(entity) : NULL;
+
+	if (entity->use_count == 0 && change > 0 && subdev != NULL) {
+		ret = v4l2_subdev_call(subdev, core, s_power, 1);
+		if (ret < 0 && ret != -ENOIOCTLCMD)
+			return ret;
+	}
+
+	entity->use_count += change;
+	WARN_ON(entity->use_count < 0);
+
+	if (entity->use_count == 0 && change < 0 && subdev != NULL)
+		v4l2_subdev_call(subdev, core, s_power, 0);
+
+	return 0;
+}
+
+/*
+ * isp_pipeline_pm_power - Apply power change to all entities in a pipeline
+ * @entity: The entity
+ * @change: Use count change
+ *
+ * Walk the pipeline to update the use count and the power state of all non-node
+ * entities.
+ *
+ * Return 0 on success or a negative error code on failure.
+ */
+static int isp_pipeline_pm_power(struct media_entity *entity, int change)
+{
+	struct media_entity_graph graph;
+	struct media_entity *first = entity;
+	int ret = 0;
+
+	if (!change)
+		return 0;
+
+	media_entity_graph_walk_start(&graph, entity);
+
+	while (!ret && (entity = media_entity_graph_walk_next(&graph)))
+		if (media_entity_type(entity) != MEDIA_ENT_T_DEVNODE)
+			ret = isp_pipeline_pm_power_one(entity, change);
+
+	if (!ret)
+		return 0;
+
+	media_entity_graph_walk_start(&graph, first);
+
+	while ((first = media_entity_graph_walk_next(&graph))
+	       && first != entity)
+		if (media_entity_type(first) != MEDIA_ENT_T_DEVNODE)
+			isp_pipeline_pm_power_one(first, -change);
+
+	return ret;
+}
+
+/*
+ * omap3isp_pipeline_pm_use - Update the use count of an entity
+ * @entity: The entity
+ * @use: Use (1) or stop using (0) the entity
+ *
+ * Update the use count of all entities in the pipeline and power entities on or
+ * off accordingly.
+ *
+ * Return 0 on success or a negative error code on failure. Powering entities
+ * off is assumed to never fail. No failure can occur when the use parameter is
+ * set to 0.
+ */
+int omap3isp_pipeline_pm_use(struct media_entity *entity, int use)
+{
+	int change = use ? 1 : -1;
+	int ret;
+
+	mutex_lock(&entity->parent->graph_mutex);
+
+	/* Apply use count to node. */
+	entity->use_count += change;
+	WARN_ON(entity->use_count < 0);
+
+	/* Apply power change to connected non-nodes. */
+	ret = isp_pipeline_pm_power(entity, change);
+	if (ret < 0)
+		entity->use_count -= change;
+
+	mutex_unlock(&entity->parent->graph_mutex);
+
+	return ret;
+}
+
+/*
+ * isp_pipeline_link_notify - Link management notification callback
+ * @link: The link
+ * @flags: New link flags that will be applied
+ * @notification: The link's state change notification type (MEDIA_DEV_NOTIFY_*)
+ *
+ * React to link management on powered pipelines by updating the use count of
+ * all entities in the source and sink sides of the link. Entities are powered
+ * on or off accordingly.
+ *
+ * Return 0 on success or a negative error code on failure. Powering entities
+ * off is assumed to never fail. This function will not fail for disconnection
+ * events.
+ */
+static int isp_pipeline_link_notify(struct media_link *link, u32 flags,
+				    unsigned int notification)
+{
+	struct media_entity *source = link->source->entity;
+	struct media_entity *sink = link->sink->entity;
+	int source_use = isp_pipeline_pm_use_count(source);
+	int sink_use = isp_pipeline_pm_use_count(sink);
+	int ret;
+
+	if (notification == MEDIA_DEV_NOTIFY_POST_LINK_CH &&
+	    !(flags & MEDIA_LNK_FL_ENABLED)) {
+		/* Powering off entities is assumed to never fail. */
+		isp_pipeline_pm_power(source, -sink_use);
+		isp_pipeline_pm_power(sink, -source_use);
+		return 0;
+	}
+
+	if (notification == MEDIA_DEV_NOTIFY_PRE_LINK_CH &&
+		(flags & MEDIA_LNK_FL_ENABLED)) {
+
+		ret = isp_pipeline_pm_power(source, sink_use);
+		if (ret < 0)
+			return ret;
+
+		ret = isp_pipeline_pm_power(sink, source_use);
+		if (ret < 0)
+			isp_pipeline_pm_power(source, -sink_use);
+
+		return ret;
+	}
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * Pipeline stream management
+ */
+
+/*
+ * isp_pipeline_enable - Enable streaming on a pipeline
+ * @pipe: ISP pipeline
+ * @mode: Stream mode (single shot or continuous)
+ *
+ * Walk the entities chain starting at the pipeline output video node and start
+ * all modules in the chain in the given mode.
+ *
+ * Return 0 if successful, or the return value of the failed video::s_stream
+ * operation otherwise.
+ */
+static int isp_pipeline_enable(struct isp_pipeline *pipe,
+			       enum isp_pipeline_stream_state mode)
+{
+	struct isp_device *isp = pipe->output->isp;
+	struct media_entity *entity;
+	struct media_pad *pad;
+	struct v4l2_subdev *subdev;
+	unsigned long flags;
+	int ret;
+
+	/* Refuse to start streaming if an entity included in the pipeline has
+	 * crashed. This check must be performed before the loop below to avoid
+	 * starting entities if the pipeline won't start anyway (those entities
+	 * would then likely fail to stop, making the problem worse).
+	 */
+	if (pipe->entities & isp->crashed)
+		return -EIO;
+
+	spin_lock_irqsave(&pipe->lock, flags);
+	pipe->state &= ~(ISP_PIPELINE_IDLE_INPUT | ISP_PIPELINE_IDLE_OUTPUT);
+	spin_unlock_irqrestore(&pipe->lock, flags);
+
+	pipe->do_propagation = false;
+
+	entity = &pipe->output->video.entity;
+	while (1) {
+		pad = &entity->pads[0];
+		if (!(pad->flags & MEDIA_PAD_FL_SINK))
+			break;
+
+		pad = media_entity_remote_pad(pad);
+		if (pad == NULL ||
+		    media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+			break;
+
+		entity = pad->entity;
+		subdev = media_entity_to_v4l2_subdev(entity);
+
+		ret = v4l2_subdev_call(subdev, video, s_stream, mode);
+		if (ret < 0 && ret != -ENOIOCTLCMD)
+			return ret;
+
+		if (subdev == &isp->isp_ccdc.subdev) {
+			v4l2_subdev_call(&isp->isp_aewb.subdev, video,
+					s_stream, mode);
+			v4l2_subdev_call(&isp->isp_af.subdev, video,
+					s_stream, mode);
+			v4l2_subdev_call(&isp->isp_hist.subdev, video,
+					s_stream, mode);
+			pipe->do_propagation = true;
+		}
+	}
+
+	return 0;
+}
+
+static int isp_pipeline_wait_resizer(struct isp_device *isp)
+{
+	return omap3isp_resizer_busy(&isp->isp_res);
+}
+
+static int isp_pipeline_wait_preview(struct isp_device *isp)
+{
+	return omap3isp_preview_busy(&isp->isp_prev);
+}
+
+static int isp_pipeline_wait_ccdc(struct isp_device *isp)
+{
+	return omap3isp_stat_busy(&isp->isp_af)
+	    || omap3isp_stat_busy(&isp->isp_aewb)
+	    || omap3isp_stat_busy(&isp->isp_hist)
+	    || omap3isp_ccdc_busy(&isp->isp_ccdc);
+}
+
+#define ISP_STOP_TIMEOUT	msecs_to_jiffies(1000)
+
+static int isp_pipeline_wait(struct isp_device *isp,
+			     int(*busy)(struct isp_device *isp))
+{
+	unsigned long timeout = jiffies + ISP_STOP_TIMEOUT;
+
+	while (!time_after(jiffies, timeout)) {
+		if (!busy(isp))
+			return 0;
+	}
+
+	return 1;
+}
+
+/*
+ * isp_pipeline_disable - Disable streaming on a pipeline
+ * @pipe: ISP pipeline
+ *
+ * Walk the entities chain starting at the pipeline output video node and stop
+ * all modules in the chain. Wait synchronously for the modules to be stopped if
+ * necessary.
+ *
+ * Return 0 if all modules have been properly stopped, or -ETIMEDOUT if a module
+ * can't be stopped (in which case a software reset of the ISP is probably
+ * necessary).
+ */
+static int isp_pipeline_disable(struct isp_pipeline *pipe)
+{
+	struct isp_device *isp = pipe->output->isp;
+	struct media_entity *entity;
+	struct media_pad *pad;
+	struct v4l2_subdev *subdev;
+	int failure = 0;
+	int ret;
+
+	/*
+	 * We need to stop all the modules after CCDC first or they'll
+	 * never stop since they may not get a full frame from CCDC.
+	 */
+	entity = &pipe->output->video.entity;
+	while (1) {
+		pad = &entity->pads[0];
+		if (!(pad->flags & MEDIA_PAD_FL_SINK))
+			break;
+
+		pad = media_entity_remote_pad(pad);
+		if (pad == NULL ||
+		    media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+			break;
+
+		entity = pad->entity;
+		subdev = media_entity_to_v4l2_subdev(entity);
+
+		if (subdev == &isp->isp_ccdc.subdev) {
+			v4l2_subdev_call(&isp->isp_aewb.subdev,
+					 video, s_stream, 0);
+			v4l2_subdev_call(&isp->isp_af.subdev,
+					 video, s_stream, 0);
+			v4l2_subdev_call(&isp->isp_hist.subdev,
+					 video, s_stream, 0);
+		}
+
+		ret = v4l2_subdev_call(subdev, video, s_stream, 0);
+
+		if (subdev == &isp->isp_res.subdev)
+			ret |= isp_pipeline_wait(isp, isp_pipeline_wait_resizer);
+		else if (subdev == &isp->isp_prev.subdev)
+			ret |= isp_pipeline_wait(isp, isp_pipeline_wait_preview);
+		else if (subdev == &isp->isp_ccdc.subdev)
+			ret |= isp_pipeline_wait(isp, isp_pipeline_wait_ccdc);
+
+		/* Handle stop failures. An entity that fails to stop can
+		 * usually just be restarted. Flag the stop failure nonetheless
+		 * to trigger an ISP reset the next time the device is released,
+		 * just in case.
+		 *
+		 * The preview engine is a special case. A failure to stop can
+		 * mean a hardware crash. When that happens the preview engine
+		 * won't respond to read/write operations on the L4 bus anymore,
+		 * resulting in a bus fault and a kernel oops next time it gets
+		 * accessed. Mark it as crashed to prevent pipelines including
+		 * it from being started.
+		 */
+		if (ret) {
+			dev_info(isp->dev, "Unable to stop %s\n", subdev->name);
+			isp->stop_failure = true;
+			if (subdev == &isp->isp_prev.subdev)
+				isp->crashed |= 1U << subdev->entity.id;
+			failure = -ETIMEDOUT;
+		}
+	}
+
+	return failure;
+}
+
+/*
+ * omap3isp_pipeline_set_stream - Enable/disable streaming on a pipeline
+ * @pipe: ISP pipeline
+ * @state: Stream state (stopped, single shot or continuous)
+ *
+ * Set the pipeline to the given stream state. Pipelines can be started in
+ * single-shot or continuous mode.
+ *
+ * Return 0 if successful, or the return value of the failed video::s_stream
+ * operation otherwise. The pipeline state is not updated when the operation
+ * fails, except when stopping the pipeline.
+ */
+int omap3isp_pipeline_set_stream(struct isp_pipeline *pipe,
+				 enum isp_pipeline_stream_state state)
+{
+	int ret;
+
+	if (state == ISP_PIPELINE_STREAM_STOPPED)
+		ret = isp_pipeline_disable(pipe);
+	else
+		ret = isp_pipeline_enable(pipe, state);
+
+	if (ret == 0 || state == ISP_PIPELINE_STREAM_STOPPED)
+		pipe->stream_state = state;
+
+	return ret;
+}
+
+/*
+ * omap3isp_pipeline_cancel_stream - Cancel stream on a pipeline
+ * @pipe: ISP pipeline
+ *
+ * Cancelling a stream mark all buffers on all video nodes in the pipeline as
+ * erroneous and makes sure no new buffer can be queued. This function is called
+ * when a fatal error that prevents any further operation on the pipeline
+ * occurs.
+ */
+void omap3isp_pipeline_cancel_stream(struct isp_pipeline *pipe)
+{
+	if (pipe->input)
+		omap3isp_video_cancel_stream(pipe->input);
+	if (pipe->output)
+		omap3isp_video_cancel_stream(pipe->output);
+}
+
+/*
+ * isp_pipeline_resume - Resume streaming on a pipeline
+ * @pipe: ISP pipeline
+ *
+ * Resume video output and input and re-enable pipeline.
+ */
+static void isp_pipeline_resume(struct isp_pipeline *pipe)
+{
+	int singleshot = pipe->stream_state == ISP_PIPELINE_STREAM_SINGLESHOT;
+
+	omap3isp_video_resume(pipe->output, !singleshot);
+	if (singleshot)
+		omap3isp_video_resume(pipe->input, 0);
+	isp_pipeline_enable(pipe, pipe->stream_state);
+}
+
+/*
+ * isp_pipeline_suspend - Suspend streaming on a pipeline
+ * @pipe: ISP pipeline
+ *
+ * Suspend pipeline.
+ */
+static void isp_pipeline_suspend(struct isp_pipeline *pipe)
+{
+	isp_pipeline_disable(pipe);
+}
+
+/*
+ * isp_pipeline_is_last - Verify if entity has an enabled link to the output
+ * 			  video node
+ * @me: ISP module's media entity
+ *
+ * Returns 1 if the entity has an enabled link to the output video node or 0
+ * otherwise. It's true only while pipeline can have no more than one output
+ * node.
+ */
+static int isp_pipeline_is_last(struct media_entity *me)
+{
+	struct isp_pipeline *pipe;
+	struct media_pad *pad;
+
+	if (!me->pipe)
+		return 0;
+	pipe = to_isp_pipeline(me);
+	if (pipe->stream_state == ISP_PIPELINE_STREAM_STOPPED)
+		return 0;
+	pad = media_entity_remote_pad(&pipe->output->pad);
+	return pad->entity == me;
+}
+
+/*
+ * isp_suspend_module_pipeline - Suspend pipeline to which belongs the module
+ * @me: ISP module's media entity
+ *
+ * Suspend the whole pipeline if module's entity has an enabled link to the
+ * output video node. It works only while pipeline can have no more than one
+ * output node.
+ */
+static void isp_suspend_module_pipeline(struct media_entity *me)
+{
+	if (isp_pipeline_is_last(me))
+		isp_pipeline_suspend(to_isp_pipeline(me));
+}
+
+/*
+ * isp_resume_module_pipeline - Resume pipeline to which belongs the module
+ * @me: ISP module's media entity
+ *
+ * Resume the whole pipeline if module's entity has an enabled link to the
+ * output video node. It works only while pipeline can have no more than one
+ * output node.
+ */
+static void isp_resume_module_pipeline(struct media_entity *me)
+{
+	if (isp_pipeline_is_last(me))
+		isp_pipeline_resume(to_isp_pipeline(me));
+}
+
+/*
+ * isp_suspend_modules - Suspend ISP submodules.
+ * @isp: OMAP3 ISP device
+ *
+ * Returns 0 if suspend left in idle state all the submodules properly,
+ * or returns 1 if a general Reset is required to suspend the submodules.
+ */
+static int isp_suspend_modules(struct isp_device *isp)
+{
+	unsigned long timeout;
+
+	omap3isp_stat_suspend(&isp->isp_aewb);
+	omap3isp_stat_suspend(&isp->isp_af);
+	omap3isp_stat_suspend(&isp->isp_hist);
+	isp_suspend_module_pipeline(&isp->isp_res.subdev.entity);
+	isp_suspend_module_pipeline(&isp->isp_prev.subdev.entity);
+	isp_suspend_module_pipeline(&isp->isp_ccdc.subdev.entity);
+	isp_suspend_module_pipeline(&isp->isp_csi2a.subdev.entity);
+	isp_suspend_module_pipeline(&isp->isp_ccp2.subdev.entity);
+
+	timeout = jiffies + ISP_STOP_TIMEOUT;
+	while (omap3isp_stat_busy(&isp->isp_af)
+	    || omap3isp_stat_busy(&isp->isp_aewb)
+	    || omap3isp_stat_busy(&isp->isp_hist)
+	    || omap3isp_preview_busy(&isp->isp_prev)
+	    || omap3isp_resizer_busy(&isp->isp_res)
+	    || omap3isp_ccdc_busy(&isp->isp_ccdc)) {
+		if (time_after(jiffies, timeout)) {
+			dev_info(isp->dev, "can't stop modules.\n");
+			return 1;
+		}
+		msleep(1);
+	}
+
+	return 0;
+}
+
+/*
+ * isp_resume_modules - Resume ISP submodules.
+ * @isp: OMAP3 ISP device
+ */
+static void isp_resume_modules(struct isp_device *isp)
+{
+	omap3isp_stat_resume(&isp->isp_aewb);
+	omap3isp_stat_resume(&isp->isp_af);
+	omap3isp_stat_resume(&isp->isp_hist);
+	isp_resume_module_pipeline(&isp->isp_res.subdev.entity);
+	isp_resume_module_pipeline(&isp->isp_prev.subdev.entity);
+	isp_resume_module_pipeline(&isp->isp_ccdc.subdev.entity);
+	isp_resume_module_pipeline(&isp->isp_csi2a.subdev.entity);
+	isp_resume_module_pipeline(&isp->isp_ccp2.subdev.entity);
+}
+
+/*
+ * isp_reset - Reset ISP with a timeout wait for idle.
+ * @isp: OMAP3 ISP device
+ */
+static int isp_reset(struct isp_device *isp)
+{
+	unsigned long timeout = 0;
+
+	isp_reg_writel(isp,
+		       isp_reg_readl(isp, OMAP3_ISP_IOMEM_MAIN, ISP_SYSCONFIG)
+		       | ISP_SYSCONFIG_SOFTRESET,
+		       OMAP3_ISP_IOMEM_MAIN, ISP_SYSCONFIG);
+	while (!(isp_reg_readl(isp, OMAP3_ISP_IOMEM_MAIN,
+			       ISP_SYSSTATUS) & 0x1)) {
+		if (timeout++ > 10000) {
+			dev_alert(isp->dev, "cannot reset ISP\n");
+			return -ETIMEDOUT;
+		}
+		udelay(1);
+	}
+
+	isp->stop_failure = false;
+	isp->crashed = 0;
+	return 0;
+}
+
+/*
+ * isp_save_context - Saves the values of the ISP module registers.
+ * @isp: OMAP3 ISP device
+ * @reg_list: Structure containing pairs of register address and value to
+ *            modify on OMAP.
+ */
+static void
+isp_save_context(struct isp_device *isp, struct isp_reg *reg_list)
+{
+	struct isp_reg *next = reg_list;
+
+	for (; next->reg != ISP_TOK_TERM; next++)
+		next->val = isp_reg_readl(isp, next->mmio_range, next->reg);
+}
+
+/*
+ * isp_restore_context - Restores the values of the ISP module registers.
+ * @isp: OMAP3 ISP device
+ * @reg_list: Structure containing pairs of register address and value to
+ *            modify on OMAP.
+ */
+static void
+isp_restore_context(struct isp_device *isp, struct isp_reg *reg_list)
+{
+	struct isp_reg *next = reg_list;
+
+	for (; next->reg != ISP_TOK_TERM; next++)
+		isp_reg_writel(isp, next->val, next->mmio_range, next->reg);
+}
+
+/*
+ * isp_save_ctx - Saves ISP, CCDC, HIST, H3A, PREV, RESZ & MMU context.
+ * @isp: OMAP3 ISP device
+ *
+ * Routine for saving the context of each module in the ISP.
+ * CCDC, HIST, H3A, PREV, RESZ and MMU.
+ */
+static void isp_save_ctx(struct isp_device *isp)
+{
+	isp_save_context(isp, isp_reg_list);
+	omap_iommu_save_ctx(isp->dev);
+}
+
+/*
+ * isp_restore_ctx - Restores ISP, CCDC, HIST, H3A, PREV, RESZ & MMU context.
+ * @isp: OMAP3 ISP device
+ *
+ * Routine for restoring the context of each module in the ISP.
+ * CCDC, HIST, H3A, PREV, RESZ and MMU.
+ */
+static void isp_restore_ctx(struct isp_device *isp)
+{
+	isp_restore_context(isp, isp_reg_list);
+	omap_iommu_restore_ctx(isp->dev);
+	omap3isp_ccdc_restore_context(isp);
+	omap3isp_preview_restore_context(isp);
+}
+
+/* -----------------------------------------------------------------------------
+ * SBL resources management
+ */
+#define OMAP3_ISP_SBL_READ	(OMAP3_ISP_SBL_CSI1_READ | \
+				 OMAP3_ISP_SBL_CCDC_LSC_READ | \
+				 OMAP3_ISP_SBL_PREVIEW_READ | \
+				 OMAP3_ISP_SBL_RESIZER_READ)
+#define OMAP3_ISP_SBL_WRITE	(OMAP3_ISP_SBL_CSI1_WRITE | \
+				 OMAP3_ISP_SBL_CSI2A_WRITE | \
+				 OMAP3_ISP_SBL_CSI2C_WRITE | \
+				 OMAP3_ISP_SBL_CCDC_WRITE | \
+				 OMAP3_ISP_SBL_PREVIEW_WRITE)
+
+void omap3isp_sbl_enable(struct isp_device *isp, enum isp_sbl_resource res)
+{
+	u32 sbl = 0;
+
+	isp->sbl_resources |= res;
+
+	if (isp->sbl_resources & OMAP3_ISP_SBL_CSI1_READ)
+		sbl |= ISPCTRL_SBL_SHARED_RPORTA;
+
+	if (isp->sbl_resources & OMAP3_ISP_SBL_CCDC_LSC_READ)
+		sbl |= ISPCTRL_SBL_SHARED_RPORTB;
+
+	if (isp->sbl_resources & OMAP3_ISP_SBL_CSI2C_WRITE)
+		sbl |= ISPCTRL_SBL_SHARED_WPORTC;
+
+	if (isp->sbl_resources & OMAP3_ISP_SBL_RESIZER_WRITE)
+		sbl |= ISPCTRL_SBL_WR0_RAM_EN;
+
+	if (isp->sbl_resources & OMAP3_ISP_SBL_WRITE)
+		sbl |= ISPCTRL_SBL_WR1_RAM_EN;
+
+	if (isp->sbl_resources & OMAP3_ISP_SBL_READ)
+		sbl |= ISPCTRL_SBL_RD_RAM_EN;
+
+	isp_reg_set(isp, OMAP3_ISP_IOMEM_MAIN, ISP_CTRL, sbl);
+}
+
+void omap3isp_sbl_disable(struct isp_device *isp, enum isp_sbl_resource res)
+{
+	u32 sbl = 0;
+
+	isp->sbl_resources &= ~res;
+
+	if (!(isp->sbl_resources & OMAP3_ISP_SBL_CSI1_READ))
+		sbl |= ISPCTRL_SBL_SHARED_RPORTA;
+
+	if (!(isp->sbl_resources & OMAP3_ISP_SBL_CCDC_LSC_READ))
+		sbl |= ISPCTRL_SBL_SHARED_RPORTB;
+
+	if (!(isp->sbl_resources & OMAP3_ISP_SBL_CSI2C_WRITE))
+		sbl |= ISPCTRL_SBL_SHARED_WPORTC;
+
+	if (!(isp->sbl_resources & OMAP3_ISP_SBL_RESIZER_WRITE))
+		sbl |= ISPCTRL_SBL_WR0_RAM_EN;
+
+	if (!(isp->sbl_resources & OMAP3_ISP_SBL_WRITE))
+		sbl |= ISPCTRL_SBL_WR1_RAM_EN;
+
+	if (!(isp->sbl_resources & OMAP3_ISP_SBL_READ))
+		sbl |= ISPCTRL_SBL_RD_RAM_EN;
+
+	isp_reg_clr(isp, OMAP3_ISP_IOMEM_MAIN, ISP_CTRL, sbl);
+}
+
+/*
+ * isp_module_sync_idle - Helper to sync module with its idle state
+ * @me: ISP submodule's media entity
+ * @wait: ISP submodule's wait queue for streamoff/interrupt synchronization
+ * @stopping: flag which tells module wants to stop
+ *
+ * This function checks if ISP submodule needs to wait for next interrupt. If
+ * yes, makes the caller to sleep while waiting for such event.
+ */
+int omap3isp_module_sync_idle(struct media_entity *me, wait_queue_head_t *wait,
+			      atomic_t *stopping)
+{
+	struct isp_pipeline *pipe = to_isp_pipeline(me);
+
+	if (pipe->stream_state == ISP_PIPELINE_STREAM_STOPPED ||
+	    (pipe->stream_state == ISP_PIPELINE_STREAM_SINGLESHOT &&
+	     !isp_pipeline_ready(pipe)))
+		return 0;
+
+	/*
+	 * atomic_set() doesn't include memory barrier on ARM platform for SMP
+	 * scenario. We'll call it here to avoid race conditions.
+	 */
+	atomic_set(stopping, 1);
+	smp_mb();
+
+	/*
+	 * If module is the last one, it's writing to memory. In this case,
+	 * it's necessary to check if the module is already paused due to
+	 * DMA queue underrun or if it has to wait for next interrupt to be
+	 * idle.
+	 * If it isn't the last one, the function won't sleep but *stopping
+	 * will still be set to warn next submodule caller's interrupt the
+	 * module wants to be idle.
+	 */
+	if (isp_pipeline_is_last(me)) {
+		struct isp_video *video = pipe->output;
+		unsigned long flags;
+		spin_lock_irqsave(&video->irqlock, flags);
+		if (video->dmaqueue_flags & ISP_VIDEO_DMAQUEUE_UNDERRUN) {
+			spin_unlock_irqrestore(&video->irqlock, flags);
+			atomic_set(stopping, 0);
+			smp_mb();
+			return 0;
+		}
+		spin_unlock_irqrestore(&video->irqlock, flags);
+		if (!wait_event_timeout(*wait, !atomic_read(stopping),
+					msecs_to_jiffies(1000))) {
+			atomic_set(stopping, 0);
+			smp_mb();
+			return -ETIMEDOUT;
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * omap3isp_module_sync_is_stopping - Helper to verify if module was stopping
+ * @wait: ISP submodule's wait queue for streamoff/interrupt synchronization
+ * @stopping: flag which tells module wants to stop
+ *
+ * This function checks if ISP submodule was stopping. In case of yes, it
+ * notices the caller by setting stopping to 0 and waking up the wait queue.
+ * Returns 1 if it was stopping or 0 otherwise.
+ */
+int omap3isp_module_sync_is_stopping(wait_queue_head_t *wait,
+				     atomic_t *stopping)
+{
+	if (atomic_cmpxchg(stopping, 1, 0)) {
+		wake_up(wait);
+		return 1;
+	}
+
+	return 0;
+}
+
+/* --------------------------------------------------------------------------
+ * Clock management
+ */
+
+#define ISPCTRL_CLKS_MASK	(ISPCTRL_H3A_CLK_EN | \
+				 ISPCTRL_HIST_CLK_EN | \
+				 ISPCTRL_RSZ_CLK_EN | \
+				 (ISPCTRL_CCDC_CLK_EN | ISPCTRL_CCDC_RAM_EN) | \
+				 (ISPCTRL_PREV_CLK_EN | ISPCTRL_PREV_RAM_EN))
+
+static void __isp_subclk_update(struct isp_device *isp)
+{
+	u32 clk = 0;
+
+	/* AEWB and AF share the same clock. */
+	if (isp->subclk_resources &
+	    (OMAP3_ISP_SUBCLK_AEWB | OMAP3_ISP_SUBCLK_AF))
+		clk |= ISPCTRL_H3A_CLK_EN;
+
+	if (isp->subclk_resources & OMAP3_ISP_SUBCLK_HIST)
+		clk |= ISPCTRL_HIST_CLK_EN;
+
+	if (isp->subclk_resources & OMAP3_ISP_SUBCLK_RESIZER)
+		clk |= ISPCTRL_RSZ_CLK_EN;
+
+	/* NOTE: For CCDC & Preview submodules, we need to affect internal
+	 *       RAM as well.
+	 */
+	if (isp->subclk_resources & OMAP3_ISP_SUBCLK_CCDC)
+		clk |= ISPCTRL_CCDC_CLK_EN | ISPCTRL_CCDC_RAM_EN;
+
+	if (isp->subclk_resources & OMAP3_ISP_SUBCLK_PREVIEW)
+		clk |= ISPCTRL_PREV_CLK_EN | ISPCTRL_PREV_RAM_EN;
+
+	isp_reg_clr_set(isp, OMAP3_ISP_IOMEM_MAIN, ISP_CTRL,
+			ISPCTRL_CLKS_MASK, clk);
+}
+
+void omap3isp_subclk_enable(struct isp_device *isp,
+			    enum isp_subclk_resource res)
+{
+	isp->subclk_resources |= res;
+
+	__isp_subclk_update(isp);
+}
+
+void omap3isp_subclk_disable(struct isp_device *isp,
+			     enum isp_subclk_resource res)
+{
+	isp->subclk_resources &= ~res;
+
+	__isp_subclk_update(isp);
+}
+
+/*
+ * isp_enable_clocks - Enable ISP clocks
+ * @isp: OMAP3 ISP device
+ *
+ * Return 0 if successful, or clk_prepare_enable return value if any of them
+ * fails.
+ */
+static int isp_enable_clocks(struct isp_device *isp)
+{
+	int r;
+	unsigned long rate;
+
+	r = clk_prepare_enable(isp->clock[ISP_CLK_CAM_ICK]);
+	if (r) {
+		dev_err(isp->dev, "failed to enable cam_ick clock\n");
+		goto out_clk_enable_ick;
+	}
+	r = clk_set_rate(isp->clock[ISP_CLK_CAM_MCLK], CM_CAM_MCLK_HZ);
+	if (r) {
+		dev_err(isp->dev, "clk_set_rate for cam_mclk failed\n");
+		goto out_clk_enable_mclk;
+	}
+	r = clk_prepare_enable(isp->clock[ISP_CLK_CAM_MCLK]);
+	if (r) {
+		dev_err(isp->dev, "failed to enable cam_mclk clock\n");
+		goto out_clk_enable_mclk;
+	}
+	rate = clk_get_rate(isp->clock[ISP_CLK_CAM_MCLK]);
+	if (rate != CM_CAM_MCLK_HZ)
+		dev_warn(isp->dev, "unexpected cam_mclk rate:\n"
+				   " expected : %d\n"
+				   " actual   : %ld\n", CM_CAM_MCLK_HZ, rate);
+	r = clk_prepare_enable(isp->clock[ISP_CLK_CSI2_FCK]);
+	if (r) {
+		dev_err(isp->dev, "failed to enable csi2_fck clock\n");
+		goto out_clk_enable_csi2_fclk;
+	}
+	return 0;
+
+out_clk_enable_csi2_fclk:
+	clk_disable_unprepare(isp->clock[ISP_CLK_CAM_MCLK]);
+out_clk_enable_mclk:
+	clk_disable_unprepare(isp->clock[ISP_CLK_CAM_ICK]);
+out_clk_enable_ick:
+	return r;
+}
+
+/*
+ * isp_disable_clocks - Disable ISP clocks
+ * @isp: OMAP3 ISP device
+ */
+static void isp_disable_clocks(struct isp_device *isp)
+{
+	clk_disable_unprepare(isp->clock[ISP_CLK_CAM_ICK]);
+	clk_disable_unprepare(isp->clock[ISP_CLK_CAM_MCLK]);
+	clk_disable_unprepare(isp->clock[ISP_CLK_CSI2_FCK]);
+}
+
+static const char *isp_clocks[] = {
+	"cam_ick",
+	"cam_mclk",
+	"csi2_96m_fck",
+	"l3_ick",
+};
+
+static int isp_get_clocks(struct isp_device *isp)
+{
+	struct clk *clk;
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(isp_clocks); ++i) {
+		clk = devm_clk_get(isp->dev, isp_clocks[i]);
+		if (IS_ERR(clk)) {
+			dev_err(isp->dev, "clk_get %s failed\n", isp_clocks[i]);
+			return PTR_ERR(clk);
+		}
+
+		isp->clock[i] = clk;
+	}
+
+	return 0;
+}
+
+/*
+ * omap3isp_get - Acquire the ISP resource.
+ *
+ * Initializes the clocks for the first acquire.
+ *
+ * Increment the reference count on the ISP. If the first reference is taken,
+ * enable clocks and power-up all submodules.
+ *
+ * Return a pointer to the ISP device structure, or NULL if an error occurred.
+ */
+static struct isp_device *__omap3isp_get(struct isp_device *isp, bool irq)
+{
+	struct isp_device *__isp = isp;
+
+	if (isp == NULL)
+		return NULL;
+
+	mutex_lock(&isp->isp_mutex);
+	if (isp->ref_count > 0)
+		goto out;
+
+	if (isp_enable_clocks(isp) < 0) {
+		__isp = NULL;
+		goto out;
+	}
+
+	/* We don't want to restore context before saving it! */
+	if (isp->has_context)
+		isp_restore_ctx(isp);
+
+	if (irq)
+		isp_enable_interrupts(isp);
+
+out:
+	if (__isp != NULL)
+		isp->ref_count++;
+	mutex_unlock(&isp->isp_mutex);
+
+	return __isp;
+}
+
+struct isp_device *omap3isp_get(struct isp_device *isp)
+{
+	return __omap3isp_get(isp, true);
+}
+
+/*
+ * omap3isp_put - Release the ISP
+ *
+ * Decrement the reference count on the ISP. If the last reference is released,
+ * power-down all submodules, disable clocks and free temporary buffers.
+ */
+static void __omap3isp_put(struct isp_device *isp, bool save_ctx)
+{
+	if (isp == NULL)
+		return;
+
+	mutex_lock(&isp->isp_mutex);
+	BUG_ON(isp->ref_count == 0);
+	if (--isp->ref_count == 0) {
+		isp_disable_interrupts(isp);
+		if (save_ctx) {
+			isp_save_ctx(isp);
+			isp->has_context = 1;
+		}
+		/* Reset the ISP if an entity has failed to stop. This is the
+		 * only way to recover from such conditions.
+		 */
+		if (isp->crashed || isp->stop_failure)
+			isp_reset(isp);
+		isp_disable_clocks(isp);
+	}
+	mutex_unlock(&isp->isp_mutex);
+}
+
+void omap3isp_put(struct isp_device *isp)
+{
+	__omap3isp_put(isp, true);
+}
+
+/* --------------------------------------------------------------------------
+ * Platform device driver
+ */
+
+/*
+ * omap3isp_print_status - Prints the values of the ISP Control Module registers
+ * @isp: OMAP3 ISP device
+ */
+#define ISP_PRINT_REGISTER(isp, name)\
+	dev_dbg(isp->dev, "###ISP " #name "=0x%08x\n", \
+		isp_reg_readl(isp, OMAP3_ISP_IOMEM_MAIN, ISP_##name))
+#define SBL_PRINT_REGISTER(isp, name)\
+	dev_dbg(isp->dev, "###SBL " #name "=0x%08x\n", \
+		isp_reg_readl(isp, OMAP3_ISP_IOMEM_SBL, ISPSBL_##name))
+
+void omap3isp_print_status(struct isp_device *isp)
+{
+	dev_dbg(isp->dev, "-------------ISP Register dump--------------\n");
+
+	ISP_PRINT_REGISTER(isp, SYSCONFIG);
+	ISP_PRINT_REGISTER(isp, SYSSTATUS);
+	ISP_PRINT_REGISTER(isp, IRQ0ENABLE);
+	ISP_PRINT_REGISTER(isp, IRQ0STATUS);
+	ISP_PRINT_REGISTER(isp, TCTRL_GRESET_LENGTH);
+	ISP_PRINT_REGISTER(isp, TCTRL_PSTRB_REPLAY);
+	ISP_PRINT_REGISTER(isp, CTRL);
+	ISP_PRINT_REGISTER(isp, TCTRL_CTRL);
+	ISP_PRINT_REGISTER(isp, TCTRL_FRAME);
+	ISP_PRINT_REGISTER(isp, TCTRL_PSTRB_DELAY);
+	ISP_PRINT_REGISTER(isp, TCTRL_STRB_DELAY);
+	ISP_PRINT_REGISTER(isp, TCTRL_SHUT_DELAY);
+	ISP_PRINT_REGISTER(isp, TCTRL_PSTRB_LENGTH);
+	ISP_PRINT_REGISTER(isp, TCTRL_STRB_LENGTH);
+	ISP_PRINT_REGISTER(isp, TCTRL_SHUT_LENGTH);
+
+	SBL_PRINT_REGISTER(isp, PCR);
+	SBL_PRINT_REGISTER(isp, SDR_REQ_EXP);
+
+	dev_dbg(isp->dev, "--------------------------------------------\n");
+}
+
+#ifdef CONFIG_PM
+
+/*
+ * Power management support.
+ *
+ * As the ISP can't properly handle an input video stream interruption on a non
+ * frame boundary, the ISP pipelines need to be stopped before sensors get
+ * suspended. However, as suspending the sensors can require a running clock,
+ * which can be provided by the ISP, the ISP can't be completely suspended
+ * before the sensor.
+ *
+ * To solve this problem power management support is split into prepare/complete
+ * and suspend/resume operations. The pipelines are stopped in prepare() and the
+ * ISP clocks get disabled in suspend(). Similarly, the clocks are reenabled in
+ * resume(), and the the pipelines are restarted in complete().
+ *
+ * TODO: PM dependencies between the ISP and sensors are not modelled explicitly
+ * yet.
+ */
+static int isp_pm_prepare(struct device *dev)
+{
+	struct isp_device *isp = dev_get_drvdata(dev);
+	int reset;
+
+	WARN_ON(mutex_is_locked(&isp->isp_mutex));
+
+	if (isp->ref_count == 0)
+		return 0;
+
+	reset = isp_suspend_modules(isp);
+	isp_disable_interrupts(isp);
+	isp_save_ctx(isp);
+	if (reset)
+		isp_reset(isp);
+
+	return 0;
+}
+
+static int isp_pm_suspend(struct device *dev)
+{
+	struct isp_device *isp = dev_get_drvdata(dev);
+
+	WARN_ON(mutex_is_locked(&isp->isp_mutex));
+
+	if (isp->ref_count)
+		isp_disable_clocks(isp);
+
+	return 0;
+}
+
+static int isp_pm_resume(struct device *dev)
+{
+	struct isp_device *isp = dev_get_drvdata(dev);
+
+	if (isp->ref_count == 0)
+		return 0;
+
+	return isp_enable_clocks(isp);
+}
+
+static void isp_pm_complete(struct device *dev)
+{
+	struct isp_device *isp = dev_get_drvdata(dev);
+
+	if (isp->ref_count == 0)
+		return;
+
+	isp_restore_ctx(isp);
+	isp_enable_interrupts(isp);
+	isp_resume_modules(isp);
+}
+
+#else
+
+#define isp_pm_prepare	NULL
+#define isp_pm_suspend	NULL
+#define isp_pm_resume	NULL
+#define isp_pm_complete	NULL
+
+#endif /* CONFIG_PM */
+
+static void isp_unregister_entities(struct isp_device *isp)
+{
+	omap3isp_csi2_unregister_entities(&isp->isp_csi2a);
+	omap3isp_ccp2_unregister_entities(&isp->isp_ccp2);
+	omap3isp_ccdc_unregister_entities(&isp->isp_ccdc);
+	omap3isp_preview_unregister_entities(&isp->isp_prev);
+	omap3isp_resizer_unregister_entities(&isp->isp_res);
+	omap3isp_stat_unregister_entities(&isp->isp_aewb);
+	omap3isp_stat_unregister_entities(&isp->isp_af);
+	omap3isp_stat_unregister_entities(&isp->isp_hist);
+
+	v4l2_device_unregister(&isp->v4l2_dev);
+	media_device_unregister(&isp->media_dev);
+}
+
+static int isp_link_entity(
+	struct isp_device *isp, struct media_entity *entity,
+	enum isp_interface_type interface)
+{
+	struct media_entity *input;
+	unsigned int flags;
+	unsigned int pad;
+	unsigned int i;
+
+	/* Connect the sensor to the correct interface module.
+	 * Parallel sensors are connected directly to the CCDC, while
+	 * serial sensors are connected to the CSI2a, CCP2b or CSI2c
+	 * receiver through CSIPHY1 or CSIPHY2.
+	 */
+	switch (interface) {
+	case ISP_INTERFACE_PARALLEL:
+		input = &isp->isp_ccdc.subdev.entity;
+		pad = CCDC_PAD_SINK;
+		flags = 0;
+		break;
+
+	case ISP_INTERFACE_CSI2A_PHY2:
+		input = &isp->isp_csi2a.subdev.entity;
+		pad = CSI2_PAD_SINK;
+		flags = MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED;
+		break;
+
+	case ISP_INTERFACE_CCP2B_PHY1:
+	case ISP_INTERFACE_CCP2B_PHY2:
+		input = &isp->isp_ccp2.subdev.entity;
+		pad = CCP2_PAD_SINK;
+		flags = 0;
+		break;
+
+	case ISP_INTERFACE_CSI2C_PHY1:
+		input = &isp->isp_csi2c.subdev.entity;
+		pad = CSI2_PAD_SINK;
+		flags = MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED;
+		break;
+
+	default:
+		dev_err(isp->dev, "%s: invalid interface type %u\n", __func__,
+			interface);
+		return -EINVAL;
+	}
+
+	/*
+	 * Not all interfaces are available on all revisions of the
+	 * ISP. The sub-devices of those interfaces aren't initialised
+	 * in such a case. Check this by ensuring the num_pads is
+	 * non-zero.
+	 */
+	if (!input->num_pads) {
+		dev_err(isp->dev, "%s: invalid input %u\n", entity->name,
+			interface);
+		return -EINVAL;
+	}
+
+	for (i = 0; i < entity->num_pads; i++) {
+		if (entity->pads[i].flags & MEDIA_PAD_FL_SOURCE)
+			break;
+	}
+	if (i == entity->num_pads) {
+		dev_err(isp->dev, "%s: no source pad in external entity\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	return media_entity_create_link(entity, i, input, pad, flags);
+}
+
+static int isp_register_entities(struct isp_device *isp)
+{
+	int ret;
+
+	isp->media_dev.dev = isp->dev;
+	strlcpy(isp->media_dev.model, "TI OMAP3 ISP",
+		sizeof(isp->media_dev.model));
+	isp->media_dev.hw_revision = isp->revision;
+	isp->media_dev.link_notify = isp_pipeline_link_notify;
+	ret = media_device_register(&isp->media_dev);
+	if (ret < 0) {
+		dev_err(isp->dev, "%s: Media device registration failed (%d)\n",
+			__func__, ret);
+		return ret;
+	}
+
+	isp->v4l2_dev.mdev = &isp->media_dev;
+	ret = v4l2_device_register(isp->dev, &isp->v4l2_dev);
+	if (ret < 0) {
+		dev_err(isp->dev, "%s: V4L2 device registration failed (%d)\n",
+			__func__, ret);
+		goto done;
+	}
+
+	/* Register internal entities */
+	ret = omap3isp_ccp2_register_entities(&isp->isp_ccp2, &isp->v4l2_dev);
+	if (ret < 0)
+		goto done;
+
+	ret = omap3isp_csi2_register_entities(&isp->isp_csi2a, &isp->v4l2_dev);
+	if (ret < 0)
+		goto done;
+
+	ret = omap3isp_ccdc_register_entities(&isp->isp_ccdc, &isp->v4l2_dev);
+	if (ret < 0)
+		goto done;
+
+	ret = omap3isp_preview_register_entities(&isp->isp_prev,
+						 &isp->v4l2_dev);
+	if (ret < 0)
+		goto done;
+
+	ret = omap3isp_resizer_register_entities(&isp->isp_res, &isp->v4l2_dev);
+	if (ret < 0)
+		goto done;
+
+	ret = omap3isp_stat_register_entities(&isp->isp_aewb, &isp->v4l2_dev);
+	if (ret < 0)
+		goto done;
+
+	ret = omap3isp_stat_register_entities(&isp->isp_af, &isp->v4l2_dev);
+	if (ret < 0)
+		goto done;
+
+	ret = omap3isp_stat_register_entities(&isp->isp_hist, &isp->v4l2_dev);
+	if (ret < 0)
+		goto done;
+
+done:
+	if (ret < 0)
+		isp_unregister_entities(isp);
+
+	return ret;
+}
+
+static void isp_cleanup_modules(struct isp_device *isp)
+{
+	omap3isp_h3a_aewb_cleanup(isp);
+	omap3isp_h3a_af_cleanup(isp);
+	omap3isp_hist_cleanup(isp);
+	omap3isp_resizer_cleanup(isp);
+	omap3isp_preview_cleanup(isp);
+	omap3isp_ccdc_cleanup(isp);
+	omap3isp_ccp2_cleanup(isp);
+	omap3isp_csi2_cleanup(isp);
+}
+
+static int isp_initialize_modules(struct isp_device *isp)
+{
+	int ret;
+
+	ret = omap3isp_csiphy_init(isp);
+	if (ret < 0) {
+		dev_err(isp->dev, "CSI PHY initialization failed\n");
+		goto error_csiphy;
+	}
+
+	ret = omap3isp_csi2_init(isp);
+	if (ret < 0) {
+		dev_err(isp->dev, "CSI2 initialization failed\n");
+		goto error_csi2;
+	}
+
+	ret = omap3isp_ccp2_init(isp);
+	if (ret < 0) {
+		dev_err(isp->dev, "CCP2 initialization failed\n");
+		goto error_ccp2;
+	}
+
+	ret = omap3isp_ccdc_init(isp);
+	if (ret < 0) {
+		dev_err(isp->dev, "CCDC initialization failed\n");
+		goto error_ccdc;
+	}
+
+	ret = omap3isp_preview_init(isp);
+	if (ret < 0) {
+		dev_err(isp->dev, "Preview initialization failed\n");
+		goto error_preview;
+	}
+
+	ret = omap3isp_resizer_init(isp);
+	if (ret < 0) {
+		dev_err(isp->dev, "Resizer initialization failed\n");
+		goto error_resizer;
+	}
+
+	ret = omap3isp_hist_init(isp);
+	if (ret < 0) {
+		dev_err(isp->dev, "Histogram initialization failed\n");
+		goto error_hist;
+	}
+
+	ret = omap3isp_h3a_aewb_init(isp);
+	if (ret < 0) {
+		dev_err(isp->dev, "H3A AEWB initialization failed\n");
+		goto error_h3a_aewb;
+	}
+
+	ret = omap3isp_h3a_af_init(isp);
+	if (ret < 0) {
+		dev_err(isp->dev, "H3A AF initialization failed\n");
+		goto error_h3a_af;
+	}
+
+	/* Connect the submodules. */
+	ret = media_entity_create_link(
+			&isp->isp_csi2a.subdev.entity, CSI2_PAD_SOURCE,
+			&isp->isp_ccdc.subdev.entity, CCDC_PAD_SINK, 0);
+	if (ret < 0)
+		goto error_link;
+
+	ret = media_entity_create_link(
+			&isp->isp_ccp2.subdev.entity, CCP2_PAD_SOURCE,
+			&isp->isp_ccdc.subdev.entity, CCDC_PAD_SINK, 0);
+	if (ret < 0)
+		goto error_link;
+
+	ret = media_entity_create_link(
+			&isp->isp_ccdc.subdev.entity, CCDC_PAD_SOURCE_VP,
+			&isp->isp_prev.subdev.entity, PREV_PAD_SINK, 0);
+	if (ret < 0)
+		goto error_link;
+
+	ret = media_entity_create_link(
+			&isp->isp_ccdc.subdev.entity, CCDC_PAD_SOURCE_OF,
+			&isp->isp_res.subdev.entity, RESZ_PAD_SINK, 0);
+	if (ret < 0)
+		goto error_link;
+
+	ret = media_entity_create_link(
+			&isp->isp_prev.subdev.entity, PREV_PAD_SOURCE,
+			&isp->isp_res.subdev.entity, RESZ_PAD_SINK, 0);
+	if (ret < 0)
+		goto error_link;
+
+	ret = media_entity_create_link(
+			&isp->isp_ccdc.subdev.entity, CCDC_PAD_SOURCE_VP,
+			&isp->isp_aewb.subdev.entity, 0,
+			MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE);
+	if (ret < 0)
+		goto error_link;
+
+	ret = media_entity_create_link(
+			&isp->isp_ccdc.subdev.entity, CCDC_PAD_SOURCE_VP,
+			&isp->isp_af.subdev.entity, 0,
+			MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE);
+	if (ret < 0)
+		goto error_link;
+
+	ret = media_entity_create_link(
+			&isp->isp_ccdc.subdev.entity, CCDC_PAD_SOURCE_VP,
+			&isp->isp_hist.subdev.entity, 0,
+			MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE);
+	if (ret < 0)
+		goto error_link;
+
+	return 0;
+
+error_link:
+	omap3isp_h3a_af_cleanup(isp);
+error_h3a_af:
+	omap3isp_h3a_aewb_cleanup(isp);
+error_h3a_aewb:
+	omap3isp_hist_cleanup(isp);
+error_hist:
+	omap3isp_resizer_cleanup(isp);
+error_resizer:
+	omap3isp_preview_cleanup(isp);
+error_preview:
+	omap3isp_ccdc_cleanup(isp);
+error_ccdc:
+	omap3isp_ccp2_cleanup(isp);
+error_ccp2:
+	omap3isp_csi2_cleanup(isp);
+error_csi2:
+error_csiphy:
+	return ret;
+}
+
+static void isp_detach_iommu(struct isp_device *isp)
+{
+	arm_iommu_release_mapping(isp->mapping);
+	isp->mapping = NULL;
+	iommu_group_remove_device(isp->dev);
+}
+
+static int isp_attach_iommu(struct isp_device *isp)
+{
+	struct dma_iommu_mapping *mapping;
+	struct iommu_group *group;
+	int ret;
+
+	/* Create a device group and add the device to it. */
+	group = iommu_group_alloc();
+	if (IS_ERR(group)) {
+		dev_err(isp->dev, "failed to allocate IOMMU group\n");
+		return PTR_ERR(group);
+	}
+
+	ret = iommu_group_add_device(group, isp->dev);
+	iommu_group_put(group);
+
+	if (ret < 0) {
+		dev_err(isp->dev, "failed to add device to IPMMU group\n");
+		return ret;
+	}
+
+	/*
+	 * Create the ARM mapping, used by the ARM DMA mapping core to allocate
+	 * VAs. This will allocate a corresponding IOMMU domain.
+	 */
+	mapping = arm_iommu_create_mapping(&platform_bus_type, SZ_1G, SZ_2G);
+	if (IS_ERR(mapping)) {
+		dev_err(isp->dev, "failed to create ARM IOMMU mapping\n");
+		ret = PTR_ERR(mapping);
+		goto error;
+	}
+
+	isp->mapping = mapping;
+
+	/* Attach the ARM VA mapping to the device. */
+	ret = arm_iommu_attach_device(isp->dev, mapping);
+	if (ret < 0) {
+		dev_err(isp->dev, "failed to attach device to VA mapping\n");
+		goto error;
+	}
+
+	return 0;
+
+error:
+	isp_detach_iommu(isp);
+	return ret;
+}
+
+/*
+ * isp_remove - Remove ISP platform device
+ * @pdev: Pointer to ISP platform device
+ *
+ * Always returns 0.
+ */
+static int isp_remove(struct platform_device *pdev)
+{
+	struct isp_device *isp = platform_get_drvdata(pdev);
+
+	v4l2_async_notifier_unregister(&isp->notifier);
+	isp_unregister_entities(isp);
+	isp_cleanup_modules(isp);
+	isp_xclk_cleanup(isp);
+
+	__omap3isp_get(isp, false);
+	isp_detach_iommu(isp);
+	__omap3isp_put(isp, false);
+
+	return 0;
+}
+
+enum isp_of_phy {
+	ISP_OF_PHY_PARALLEL = 0,
+	ISP_OF_PHY_CSIPHY1,
+	ISP_OF_PHY_CSIPHY2,
+};
+
+static int isp_of_parse_node(struct device *dev, struct device_node *node,
+			     struct isp_async_subdev *isd)
+{
+	struct isp_bus_cfg *buscfg = &isd->bus;
+	struct v4l2_of_endpoint vep;
+	unsigned int i;
+
+	v4l2_of_parse_endpoint(node, &vep);
+
+	dev_dbg(dev, "parsing endpoint %s, interface %u\n", node->full_name,
+		vep.base.port);
+
+	switch (vep.base.port) {
+	case ISP_OF_PHY_PARALLEL:
+		buscfg->interface = ISP_INTERFACE_PARALLEL;
+		buscfg->bus.parallel.data_lane_shift =
+			vep.bus.parallel.data_shift;
+		buscfg->bus.parallel.clk_pol =
+			!!(vep.bus.parallel.flags
+			   & V4L2_MBUS_PCLK_SAMPLE_FALLING);
+		buscfg->bus.parallel.hs_pol =
+			!!(vep.bus.parallel.flags & V4L2_MBUS_VSYNC_ACTIVE_LOW);
+		buscfg->bus.parallel.vs_pol =
+			!!(vep.bus.parallel.flags & V4L2_MBUS_HSYNC_ACTIVE_LOW);
+		buscfg->bus.parallel.fld_pol =
+			!!(vep.bus.parallel.flags & V4L2_MBUS_FIELD_EVEN_LOW);
+		buscfg->bus.parallel.data_pol =
+			!!(vep.bus.parallel.flags & V4L2_MBUS_DATA_ACTIVE_LOW);
+		break;
+
+	case ISP_OF_PHY_CSIPHY1:
+	case ISP_OF_PHY_CSIPHY2:
+		/* FIXME: always assume CSI-2 for now. */
+		switch (vep.base.port) {
+		case ISP_OF_PHY_CSIPHY1:
+			buscfg->interface = ISP_INTERFACE_CSI2C_PHY1;
+			break;
+		case ISP_OF_PHY_CSIPHY2:
+			buscfg->interface = ISP_INTERFACE_CSI2A_PHY2;
+			break;
+		}
+		buscfg->bus.csi2.lanecfg.clk.pos = vep.bus.mipi_csi2.clock_lane;
+		buscfg->bus.csi2.lanecfg.clk.pol =
+			vep.bus.mipi_csi2.lane_polarities[0];
+		dev_dbg(dev, "clock lane polarity %u, pos %u\n",
+			buscfg->bus.csi2.lanecfg.clk.pol,
+			buscfg->bus.csi2.lanecfg.clk.pos);
+
+		for (i = 0; i < ISP_CSIPHY2_NUM_DATA_LANES; i++) {
+			buscfg->bus.csi2.lanecfg.data[i].pos =
+				vep.bus.mipi_csi2.data_lanes[i];
+			buscfg->bus.csi2.lanecfg.data[i].pol =
+				vep.bus.mipi_csi2.lane_polarities[i + 1];
+			dev_dbg(dev, "data lane %u polarity %u, pos %u\n", i,
+				buscfg->bus.csi2.lanecfg.data[i].pol,
+				buscfg->bus.csi2.lanecfg.data[i].pos);
+		}
+
+		/*
+		 * FIXME: now we assume the CRC is always there.
+		 * Implement a way to obtain this information from the
+		 * sensor. Frame descriptors, perhaps?
+		 */
+		buscfg->bus.csi2.crc = 1;
+		break;
+
+	default:
+		dev_warn(dev, "%s: invalid interface %u\n", node->full_name,
+			 vep.base.port);
+		break;
+	}
+
+	return 0;
+}
+
+static int isp_of_parse_nodes(struct device *dev,
+			      struct v4l2_async_notifier *notifier)
+{
+	struct device_node *node = NULL;
+
+	notifier->subdevs = devm_kcalloc(
+		dev, ISP_MAX_SUBDEVS, sizeof(*notifier->subdevs), GFP_KERNEL);
+	if (!notifier->subdevs)
+		return -ENOMEM;
+
+	while (notifier->num_subdevs < ISP_MAX_SUBDEVS &&
+	       (node = of_graph_get_next_endpoint(dev->of_node, node))) {
+		struct isp_async_subdev *isd;
+
+		isd = devm_kzalloc(dev, sizeof(*isd), GFP_KERNEL);
+		if (!isd) {
+			of_node_put(node);
+			return -ENOMEM;
+		}
+
+		notifier->subdevs[notifier->num_subdevs] = &isd->asd;
+
+		if (isp_of_parse_node(dev, node, isd)) {
+			of_node_put(node);
+			return -EINVAL;
+		}
+
+		isd->asd.match.of.node = of_graph_get_remote_port_parent(node);
+		of_node_put(node);
+		if (!isd->asd.match.of.node) {
+			dev_warn(dev, "bad remote port parent\n");
+			return -EINVAL;
+		}
+
+		isd->asd.match_type = V4L2_ASYNC_MATCH_OF;
+		notifier->num_subdevs++;
+	}
+
+	return notifier->num_subdevs;
+}
+
+static int isp_subdev_notifier_bound(struct v4l2_async_notifier *async,
+				     struct v4l2_subdev *subdev,
+				     struct v4l2_async_subdev *asd)
+{
+	struct isp_device *isp = container_of(async, struct isp_device,
+					      notifier);
+	struct isp_async_subdev *isd =
+		container_of(asd, struct isp_async_subdev, asd);
+	int ret;
+
+	ret = isp_link_entity(isp, &subdev->entity, isd->bus.interface);
+	if (ret < 0)
+		return ret;
+
+	isd->sd = subdev;
+	isd->sd->host_priv = &isd->bus;
+
+	return ret;
+}
+
+static int isp_subdev_notifier_complete(struct v4l2_async_notifier *async)
+{
+	struct isp_device *isp = container_of(async, struct isp_device,
+					      notifier);
+
+	return v4l2_device_register_subdev_nodes(&isp->v4l2_dev);
+}
+
+/*
+ * isp_probe - Probe ISP platform device
+ * @pdev: Pointer to ISP platform device
+ *
+ * Returns 0 if successful,
+ *   -ENOMEM if no memory available,
+ *   -ENODEV if no platform device resources found
+ *     or no space for remapping registers,
+ *   -EINVAL if couldn't install ISR,
+ *   or clk_get return error value.
+ */
+static int isp_probe(struct platform_device *pdev)
+{
+	struct isp_device *isp;
+	struct resource *mem;
+	int ret;
+	int i, m;
+
+	isp = devm_kzalloc(&pdev->dev, sizeof(*isp), GFP_KERNEL);
+	if (!isp) {
+		dev_err(&pdev->dev, "could not allocate memory\n");
+		return -ENOMEM;
+	}
+
+	ret = of_property_read_u32(pdev->dev.of_node, "ti,phy-type",
+				   &isp->phy_type);
+	if (ret)
+		return ret;
+
+	isp->syscon = syscon_regmap_lookup_by_phandle(pdev->dev.of_node,
+						      "syscon");
+	if (IS_ERR(isp->syscon))
+		return PTR_ERR(isp->syscon);
+
+	ret = of_property_read_u32_index(pdev->dev.of_node, "syscon", 1,
+					 &isp->syscon_offset);
+	if (ret)
+		return ret;
+
+	ret = isp_of_parse_nodes(&pdev->dev, &isp->notifier);
+	if (ret < 0)
+		return ret;
+
+	isp->autoidle = autoidle;
+
+	mutex_init(&isp->isp_mutex);
+	spin_lock_init(&isp->stat_lock);
+
+	isp->dev = &pdev->dev;
+	isp->ref_count = 0;
+
+	ret = dma_coerce_mask_and_coherent(isp->dev, DMA_BIT_MASK(32));
+	if (ret)
+		goto error;
+
+	platform_set_drvdata(pdev, isp);
+
+	/* Regulators */
+	isp->isp_csiphy1.vdd = devm_regulator_get(&pdev->dev, "vdd-csiphy1");
+	isp->isp_csiphy2.vdd = devm_regulator_get(&pdev->dev, "vdd-csiphy2");
+
+	/* Clocks
+	 *
+	 * The ISP clock tree is revision-dependent. We thus need to enable ICLK
+	 * manually to read the revision before calling __omap3isp_get().
+	 *
+	 * Start by mapping the ISP MMIO area, which is in two pieces.
+	 * The ISP IOMMU is in between. Map both now, and fill in the
+	 * ISP revision specific portions a little later in the
+	 * function.
+	 */
+	for (i = 0; i < 2; i++) {
+		unsigned int map_idx = i ? OMAP3_ISP_IOMEM_CSI2A_REGS1 : 0;
+
+		mem = platform_get_resource(pdev, IORESOURCE_MEM, i);
+		isp->mmio_base[map_idx] =
+			devm_ioremap_resource(isp->dev, mem);
+		if (IS_ERR(isp->mmio_base[map_idx]))
+			return PTR_ERR(isp->mmio_base[map_idx]);
+	}
+
+	ret = isp_get_clocks(isp);
+	if (ret < 0)
+		goto error;
+
+	ret = clk_enable(isp->clock[ISP_CLK_CAM_ICK]);
+	if (ret < 0)
+		goto error;
+
+	isp->revision = isp_reg_readl(isp, OMAP3_ISP_IOMEM_MAIN, ISP_REVISION);
+	dev_info(isp->dev, "Revision %d.%d found\n",
+		 (isp->revision & 0xf0) >> 4, isp->revision & 0x0f);
+
+	clk_disable(isp->clock[ISP_CLK_CAM_ICK]);
+
+	if (__omap3isp_get(isp, false) == NULL) {
+		ret = -ENODEV;
+		goto error;
+	}
+
+	ret = isp_reset(isp);
+	if (ret < 0)
+		goto error_isp;
+
+	ret = isp_xclk_init(isp);
+	if (ret < 0)
+		goto error_isp;
+
+	/* Memory resources */
+	for (m = 0; m < ARRAY_SIZE(isp_res_maps); m++)
+		if (isp->revision == isp_res_maps[m].isp_rev)
+			break;
+
+	if (m == ARRAY_SIZE(isp_res_maps)) {
+		dev_err(isp->dev, "No resource map found for ISP rev %d.%d\n",
+			(isp->revision & 0xf0) >> 4, isp->revision & 0xf);
+		ret = -ENODEV;
+		goto error_isp;
+	}
+
+	for (i = 1; i < OMAP3_ISP_IOMEM_CSI2A_REGS1; i++)
+		isp->mmio_base[i] =
+			isp->mmio_base[0] + isp_res_maps[m].offset[i];
+
+	for (i = OMAP3_ISP_IOMEM_CSIPHY2; i < OMAP3_ISP_IOMEM_LAST; i++)
+		isp->mmio_base[i] =
+			isp->mmio_base[OMAP3_ISP_IOMEM_CSI2A_REGS1]
+			+ isp_res_maps[m].offset[i];
+
+	isp->mmio_hist_base_phys =
+		mem->start + isp_res_maps[m].offset[OMAP3_ISP_IOMEM_HIST];
+
+	/* IOMMU */
+	ret = isp_attach_iommu(isp);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "unable to attach to IOMMU\n");
+		goto error_isp;
+	}
+
+	/* Interrupt */
+	isp->irq_num = platform_get_irq(pdev, 0);
+	if (isp->irq_num <= 0) {
+		dev_err(isp->dev, "No IRQ resource\n");
+		ret = -ENODEV;
+		goto error_iommu;
+	}
+
+	if (devm_request_irq(isp->dev, isp->irq_num, isp_isr, IRQF_SHARED,
+			     "OMAP3 ISP", isp)) {
+		dev_err(isp->dev, "Unable to request IRQ\n");
+		ret = -EINVAL;
+		goto error_iommu;
+	}
+
+	/* Entities */
+	ret = isp_initialize_modules(isp);
+	if (ret < 0)
+		goto error_iommu;
+
+	ret = isp_register_entities(isp);
+	if (ret < 0)
+		goto error_modules;
+
+	isp->notifier.bound = isp_subdev_notifier_bound;
+	isp->notifier.complete = isp_subdev_notifier_complete;
+
+	ret = v4l2_async_notifier_register(&isp->v4l2_dev, &isp->notifier);
+	if (ret)
+		goto error_register_entities;
+
+	isp_core_init(isp, 1);
+	omap3isp_put(isp);
+
+	return 0;
+
+error_register_entities:
+	isp_unregister_entities(isp);
+error_modules:
+	isp_cleanup_modules(isp);
+error_iommu:
+	isp_detach_iommu(isp);
+error_isp:
+	isp_xclk_cleanup(isp);
+	__omap3isp_put(isp, false);
+error:
+	mutex_destroy(&isp->isp_mutex);
+
+	return ret;
+}
+
+static const struct dev_pm_ops omap3isp_pm_ops = {
+	.prepare = isp_pm_prepare,
+	.suspend = isp_pm_suspend,
+	.resume = isp_pm_resume,
+	.complete = isp_pm_complete,
+};
+
+static struct platform_device_id omap3isp_id_table[] = {
+	{ "omap3isp", 0 },
+	{ },
+};
+MODULE_DEVICE_TABLE(platform, omap3isp_id_table);
+
+static const struct of_device_id omap3isp_of_table[] = {
+	{ .compatible = "ti,omap3-isp" },
+	{ },
+};
+
+static struct platform_driver omap3isp_driver = {
+	.probe = isp_probe,
+	.remove = isp_remove,
+	.id_table = omap3isp_id_table,
+	.driver = {
+		.name = "omap3isp",
+		.pm	= &omap3isp_pm_ops,
+		.of_match_table = omap3isp_of_table,
+	},
+};
+
+module_platform_driver(omap3isp_driver);
+
+MODULE_AUTHOR("Nokia Corporation");
+MODULE_DESCRIPTION("TI OMAP3 ISP driver");
+MODULE_LICENSE("GPL");
+MODULE_VERSION(ISP_VIDEO_DRIVER_VERSION);
diff --git a/drivers/media/platform/omap3isp/isp.h b/drivers/media/platform/omap3isp/isp.h
new file mode 100644
index 0000000..5acc2e6
--- /dev/null
+++ b/drivers/media/platform/omap3isp/isp.h
@@ -0,0 +1,366 @@
+/*
+ * isp.h
+ *
+ * TI OMAP3 ISP - Core
+ *
+ * Copyright (C) 2009-2010 Nokia Corporation
+ * Copyright (C) 2009 Texas Instruments, Inc.
+ *
+ * Contacts: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *	     Sakari Ailus <sakari.ailus@iki.fi>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef OMAP3_ISP_CORE_H
+#define OMAP3_ISP_CORE_H
+
+#include <media/v4l2-async.h>
+#include <media/v4l2-device.h>
+#include <linux/clk-provider.h>
+#include <linux/device.h>
+#include <linux/io.h>
+#include <linux/iommu.h>
+#include <linux/platform_device.h>
+#include <linux/wait.h>
+
+#include "omap3isp.h"
+#include "ispstat.h"
+#include "ispccdc.h"
+#include "ispreg.h"
+#include "ispresizer.h"
+#include "isppreview.h"
+#include "ispcsiphy.h"
+#include "ispcsi2.h"
+#include "ispccp2.h"
+
+#define ISP_TOK_TERM		0xFFFFFFFF	/*
+						 * terminating token for ISP
+						 * modules reg list
+						 */
+#define to_isp_device(ptr_module)				\
+	container_of(ptr_module, struct isp_device, isp_##ptr_module)
+#define to_device(ptr_module)						\
+	(to_isp_device(ptr_module)->dev)
+
+enum isp_mem_resources {
+	OMAP3_ISP_IOMEM_MAIN,
+	OMAP3_ISP_IOMEM_CCP2,
+	OMAP3_ISP_IOMEM_CCDC,
+	OMAP3_ISP_IOMEM_HIST,
+	OMAP3_ISP_IOMEM_H3A,
+	OMAP3_ISP_IOMEM_PREV,
+	OMAP3_ISP_IOMEM_RESZ,
+	OMAP3_ISP_IOMEM_SBL,
+	OMAP3_ISP_IOMEM_CSI2A_REGS1,
+	OMAP3_ISP_IOMEM_CSIPHY2,
+	OMAP3_ISP_IOMEM_CSI2A_REGS2,
+	OMAP3_ISP_IOMEM_CSI2C_REGS1,
+	OMAP3_ISP_IOMEM_CSIPHY1,
+	OMAP3_ISP_IOMEM_CSI2C_REGS2,
+	OMAP3_ISP_IOMEM_LAST
+};
+
+enum isp_sbl_resource {
+	OMAP3_ISP_SBL_CSI1_READ		= 0x1,
+	OMAP3_ISP_SBL_CSI1_WRITE	= 0x2,
+	OMAP3_ISP_SBL_CSI2A_WRITE	= 0x4,
+	OMAP3_ISP_SBL_CSI2C_WRITE	= 0x8,
+	OMAP3_ISP_SBL_CCDC_LSC_READ	= 0x10,
+	OMAP3_ISP_SBL_CCDC_WRITE	= 0x20,
+	OMAP3_ISP_SBL_PREVIEW_READ	= 0x40,
+	OMAP3_ISP_SBL_PREVIEW_WRITE	= 0x80,
+	OMAP3_ISP_SBL_RESIZER_READ	= 0x100,
+	OMAP3_ISP_SBL_RESIZER_WRITE	= 0x200,
+};
+
+enum isp_subclk_resource {
+	OMAP3_ISP_SUBCLK_CCDC		= (1 << 0),
+	OMAP3_ISP_SUBCLK_AEWB		= (1 << 1),
+	OMAP3_ISP_SUBCLK_AF		= (1 << 2),
+	OMAP3_ISP_SUBCLK_HIST		= (1 << 3),
+	OMAP3_ISP_SUBCLK_PREVIEW	= (1 << 4),
+	OMAP3_ISP_SUBCLK_RESIZER	= (1 << 5),
+};
+
+/* ISP: OMAP 34xx ES 1.0 */
+#define ISP_REVISION_1_0		0x10
+/* ISP2: OMAP 34xx ES 2.0, 2.1 and 3.0 */
+#define ISP_REVISION_2_0		0x20
+/* ISP2P: OMAP 36xx */
+#define ISP_REVISION_15_0		0xF0
+
+#define ISP_PHY_TYPE_3430		0
+#define ISP_PHY_TYPE_3630		1
+
+struct regmap;
+
+/*
+ * struct isp_res_mapping - Map ISP io resources to ISP revision.
+ * @isp_rev: ISP_REVISION_x_x
+ * @offset: register offsets of various ISP sub-blocks
+ * @phy_type: ISP_PHY_TYPE_{3430,3630}
+ */
+struct isp_res_mapping {
+	u32 isp_rev;
+	u32 offset[OMAP3_ISP_IOMEM_LAST];
+	u32 phy_type;
+};
+
+/*
+ * struct isp_reg - Structure for ISP register values.
+ * @reg: 32-bit Register address.
+ * @val: 32-bit Register value.
+ */
+struct isp_reg {
+	enum isp_mem_resources mmio_range;
+	u32 reg;
+	u32 val;
+};
+
+enum isp_xclk_id {
+	ISP_XCLK_A,
+	ISP_XCLK_B,
+};
+
+struct isp_xclk {
+	struct isp_device *isp;
+	struct clk_hw hw;
+	struct clk *clk;
+	enum isp_xclk_id id;
+
+	spinlock_t lock;	/* Protects enabled and divider */
+	bool enabled;
+	unsigned int divider;
+};
+
+/*
+ * struct isp_device - ISP device structure.
+ * @dev: Device pointer specific to the OMAP3 ISP.
+ * @revision: Stores current ISP module revision.
+ * @irq_num: Currently used IRQ number.
+ * @mmio_base: Array with kernel base addresses for ioremapped ISP register
+ *             regions.
+ * @mmio_hist_base_phys: Physical L4 bus address for ISP hist block register
+ *			 region.
+ * @syscon: Regmap for the syscon register space
+ * @syscon_offset: Offset of the CSIPHY control register in syscon
+ * @phy_type: ISP_PHY_TYPE_{3430,3630}
+ * @mapping: IOMMU mapping
+ * @stat_lock: Spinlock for handling statistics
+ * @isp_mutex: Mutex for serializing requests to ISP.
+ * @stop_failure: Indicates that an entity failed to stop.
+ * @crashed: Bitmask of crashed entities (indexed by entity ID)
+ * @has_context: Context has been saved at least once and can be restored.
+ * @ref_count: Reference count for handling multiple ISP requests.
+ * @cam_ick: Pointer to camera interface clock structure.
+ * @cam_mclk: Pointer to camera functional clock structure.
+ * @csi2_fck: Pointer to camera CSI2 complexIO clock structure.
+ * @l3_ick: Pointer to OMAP3 L3 bus interface clock.
+ * @xclks: External clocks provided by the ISP
+ * @irq: Currently attached ISP ISR callbacks information structure.
+ * @isp_af: Pointer to current settings for ISP AutoFocus SCM.
+ * @isp_hist: Pointer to current settings for ISP Histogram SCM.
+ * @isp_h3a: Pointer to current settings for ISP Auto Exposure and
+ *           White Balance SCM.
+ * @isp_res: Pointer to current settings for ISP Resizer.
+ * @isp_prev: Pointer to current settings for ISP Preview.
+ * @isp_ccdc: Pointer to current settings for ISP CCDC.
+ * @platform_cb: ISP driver callback function pointers for platform code
+ *
+ * This structure is used to store the OMAP ISP Information.
+ */
+struct isp_device {
+	struct v4l2_device v4l2_dev;
+	struct v4l2_async_notifier notifier;
+	struct media_device media_dev;
+	struct device *dev;
+	u32 revision;
+
+	/* platform HW resources */
+	unsigned int irq_num;
+
+	void __iomem *mmio_base[OMAP3_ISP_IOMEM_LAST];
+	unsigned long mmio_hist_base_phys;
+	struct regmap *syscon;
+	u32 syscon_offset;
+	u32 phy_type;
+
+	struct dma_iommu_mapping *mapping;
+
+	/* ISP Obj */
+	spinlock_t stat_lock;	/* common lock for statistic drivers */
+	struct mutex isp_mutex;	/* For handling ref_count field */
+	bool stop_failure;
+	u32 crashed;
+	int has_context;
+	int ref_count;
+	unsigned int autoidle;
+#define ISP_CLK_CAM_ICK		0
+#define ISP_CLK_CAM_MCLK	1
+#define ISP_CLK_CSI2_FCK	2
+#define ISP_CLK_L3_ICK		3
+	struct clk *clock[4];
+	struct isp_xclk xclks[2];
+
+	/* ISP modules */
+	struct ispstat isp_af;
+	struct ispstat isp_aewb;
+	struct ispstat isp_hist;
+	struct isp_res_device isp_res;
+	struct isp_prev_device isp_prev;
+	struct isp_ccdc_device isp_ccdc;
+	struct isp_csi2_device isp_csi2a;
+	struct isp_csi2_device isp_csi2c;
+	struct isp_ccp2_device isp_ccp2;
+	struct isp_csiphy isp_csiphy1;
+	struct isp_csiphy isp_csiphy2;
+
+	unsigned int sbl_resources;
+	unsigned int subclk_resources;
+
+#define ISP_MAX_SUBDEVS		8
+	struct v4l2_subdev *subdevs[ISP_MAX_SUBDEVS];
+};
+
+struct isp_async_subdev {
+	struct v4l2_subdev *sd;
+	struct isp_bus_cfg bus;
+	struct v4l2_async_subdev asd;
+};
+
+#define v4l2_dev_to_isp_device(dev) \
+	container_of(dev, struct isp_device, v4l2_dev)
+
+void omap3isp_hist_dma_done(struct isp_device *isp);
+
+void omap3isp_flush(struct isp_device *isp);
+
+int omap3isp_module_sync_idle(struct media_entity *me, wait_queue_head_t *wait,
+			      atomic_t *stopping);
+
+int omap3isp_module_sync_is_stopping(wait_queue_head_t *wait,
+				     atomic_t *stopping);
+
+int omap3isp_pipeline_set_stream(struct isp_pipeline *pipe,
+				 enum isp_pipeline_stream_state state);
+void omap3isp_pipeline_cancel_stream(struct isp_pipeline *pipe);
+void omap3isp_configure_bridge(struct isp_device *isp,
+			       enum ccdc_input_entity input,
+			       const struct isp_parallel_cfg *buscfg,
+			       unsigned int shift, unsigned int bridge);
+
+struct isp_device *omap3isp_get(struct isp_device *isp);
+void omap3isp_put(struct isp_device *isp);
+
+void omap3isp_print_status(struct isp_device *isp);
+
+void omap3isp_sbl_enable(struct isp_device *isp, enum isp_sbl_resource res);
+void omap3isp_sbl_disable(struct isp_device *isp, enum isp_sbl_resource res);
+
+void omap3isp_subclk_enable(struct isp_device *isp,
+			    enum isp_subclk_resource res);
+void omap3isp_subclk_disable(struct isp_device *isp,
+			     enum isp_subclk_resource res);
+
+int omap3isp_pipeline_pm_use(struct media_entity *entity, int use);
+
+int omap3isp_register_entities(struct platform_device *pdev,
+			       struct v4l2_device *v4l2_dev);
+void omap3isp_unregister_entities(struct platform_device *pdev);
+
+/*
+ * isp_reg_readl - Read value of an OMAP3 ISP register
+ * @isp: Device pointer specific to the OMAP3 ISP.
+ * @isp_mmio_range: Range to which the register offset refers to.
+ * @reg_offset: Register offset to read from.
+ *
+ * Returns an unsigned 32 bit value with the required register contents.
+ */
+static inline
+u32 isp_reg_readl(struct isp_device *isp, enum isp_mem_resources isp_mmio_range,
+		  u32 reg_offset)
+{
+	return __raw_readl(isp->mmio_base[isp_mmio_range] + reg_offset);
+}
+
+/*
+ * isp_reg_writel - Write value to an OMAP3 ISP register
+ * @isp: Device pointer specific to the OMAP3 ISP.
+ * @reg_value: 32 bit value to write to the register.
+ * @isp_mmio_range: Range to which the register offset refers to.
+ * @reg_offset: Register offset to write into.
+ */
+static inline
+void isp_reg_writel(struct isp_device *isp, u32 reg_value,
+		    enum isp_mem_resources isp_mmio_range, u32 reg_offset)
+{
+	__raw_writel(reg_value, isp->mmio_base[isp_mmio_range] + reg_offset);
+}
+
+/*
+ * isp_reg_clr - Clear individual bits in an OMAP3 ISP register
+ * @isp: Device pointer specific to the OMAP3 ISP.
+ * @mmio_range: Range to which the register offset refers to.
+ * @reg: Register offset to work on.
+ * @clr_bits: 32 bit value which would be cleared in the register.
+ */
+static inline
+void isp_reg_clr(struct isp_device *isp, enum isp_mem_resources mmio_range,
+		 u32 reg, u32 clr_bits)
+{
+	u32 v = isp_reg_readl(isp, mmio_range, reg);
+
+	isp_reg_writel(isp, v & ~clr_bits, mmio_range, reg);
+}
+
+/*
+ * isp_reg_set - Set individual bits in an OMAP3 ISP register
+ * @isp: Device pointer specific to the OMAP3 ISP.
+ * @mmio_range: Range to which the register offset refers to.
+ * @reg: Register offset to work on.
+ * @set_bits: 32 bit value which would be set in the register.
+ */
+static inline
+void isp_reg_set(struct isp_device *isp, enum isp_mem_resources mmio_range,
+		 u32 reg, u32 set_bits)
+{
+	u32 v = isp_reg_readl(isp, mmio_range, reg);
+
+	isp_reg_writel(isp, v | set_bits, mmio_range, reg);
+}
+
+/*
+ * isp_reg_clr_set - Clear and set invidial bits in an OMAP3 ISP register
+ * @isp: Device pointer specific to the OMAP3 ISP.
+ * @mmio_range: Range to which the register offset refers to.
+ * @reg: Register offset to work on.
+ * @clr_bits: 32 bit value which would be cleared in the register.
+ * @set_bits: 32 bit value which would be set in the register.
+ *
+ * The clear operation is done first, and then the set operation.
+ */
+static inline
+void isp_reg_clr_set(struct isp_device *isp, enum isp_mem_resources mmio_range,
+		     u32 reg, u32 clr_bits, u32 set_bits)
+{
+	u32 v = isp_reg_readl(isp, mmio_range, reg);
+
+	isp_reg_writel(isp, (v & ~clr_bits) | set_bits, mmio_range, reg);
+}
+
+static inline enum v4l2_buf_type
+isp_pad_buffer_type(const struct v4l2_subdev *subdev, int pad)
+{
+	if (pad >= subdev->entity.num_pads)
+		return 0;
+
+	if (subdev->entity.pads[pad].flags & MEDIA_PAD_FL_SINK)
+		return V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	else
+		return V4L2_BUF_TYPE_VIDEO_CAPTURE;
+}
+
+#endif	/* OMAP3_ISP_CORE_H */
diff --git a/drivers/media/platform/omap3isp/ispccdc.c b/drivers/media/platform/omap3isp/ispccdc.c
new file mode 100644
index 0000000..a6a61cc
--- /dev/null
+++ b/drivers/media/platform/omap3isp/ispccdc.c
@@ -0,0 +1,2746 @@
+/*
+ * ispccdc.c
+ *
+ * TI OMAP3 ISP - CCDC module
+ *
+ * Copyright (C) 2009-2010 Nokia Corporation
+ * Copyright (C) 2009 Texas Instruments, Inc.
+ *
+ * Contacts: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *	     Sakari Ailus <sakari.ailus@iki.fi>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/uaccess.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/mm.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <media/v4l2-event.h>
+
+#include "isp.h"
+#include "ispreg.h"
+#include "ispccdc.h"
+
+#define CCDC_MIN_WIDTH		32
+#define CCDC_MIN_HEIGHT		32
+
+static struct v4l2_mbus_framefmt *
+__ccdc_get_format(struct isp_ccdc_device *ccdc, struct v4l2_subdev_pad_config *cfg,
+		  unsigned int pad, enum v4l2_subdev_format_whence which);
+
+static const unsigned int ccdc_fmts[] = {
+	MEDIA_BUS_FMT_Y8_1X8,
+	MEDIA_BUS_FMT_Y10_1X10,
+	MEDIA_BUS_FMT_Y12_1X12,
+	MEDIA_BUS_FMT_SGRBG8_1X8,
+	MEDIA_BUS_FMT_SRGGB8_1X8,
+	MEDIA_BUS_FMT_SBGGR8_1X8,
+	MEDIA_BUS_FMT_SGBRG8_1X8,
+	MEDIA_BUS_FMT_SGRBG10_1X10,
+	MEDIA_BUS_FMT_SRGGB10_1X10,
+	MEDIA_BUS_FMT_SBGGR10_1X10,
+	MEDIA_BUS_FMT_SGBRG10_1X10,
+	MEDIA_BUS_FMT_SGRBG12_1X12,
+	MEDIA_BUS_FMT_SRGGB12_1X12,
+	MEDIA_BUS_FMT_SBGGR12_1X12,
+	MEDIA_BUS_FMT_SGBRG12_1X12,
+	MEDIA_BUS_FMT_YUYV8_2X8,
+	MEDIA_BUS_FMT_UYVY8_2X8,
+};
+
+/*
+ * ccdc_print_status - Print current CCDC Module register values.
+ * @ccdc: Pointer to ISP CCDC device.
+ *
+ * Also prints other debug information stored in the CCDC module.
+ */
+#define CCDC_PRINT_REGISTER(isp, name)\
+	dev_dbg(isp->dev, "###CCDC " #name "=0x%08x\n", \
+		isp_reg_readl(isp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_##name))
+
+static void ccdc_print_status(struct isp_ccdc_device *ccdc)
+{
+	struct isp_device *isp = to_isp_device(ccdc);
+
+	dev_dbg(isp->dev, "-------------CCDC Register dump-------------\n");
+
+	CCDC_PRINT_REGISTER(isp, PCR);
+	CCDC_PRINT_REGISTER(isp, SYN_MODE);
+	CCDC_PRINT_REGISTER(isp, HD_VD_WID);
+	CCDC_PRINT_REGISTER(isp, PIX_LINES);
+	CCDC_PRINT_REGISTER(isp, HORZ_INFO);
+	CCDC_PRINT_REGISTER(isp, VERT_START);
+	CCDC_PRINT_REGISTER(isp, VERT_LINES);
+	CCDC_PRINT_REGISTER(isp, CULLING);
+	CCDC_PRINT_REGISTER(isp, HSIZE_OFF);
+	CCDC_PRINT_REGISTER(isp, SDOFST);
+	CCDC_PRINT_REGISTER(isp, SDR_ADDR);
+	CCDC_PRINT_REGISTER(isp, CLAMP);
+	CCDC_PRINT_REGISTER(isp, DCSUB);
+	CCDC_PRINT_REGISTER(isp, COLPTN);
+	CCDC_PRINT_REGISTER(isp, BLKCMP);
+	CCDC_PRINT_REGISTER(isp, FPC);
+	CCDC_PRINT_REGISTER(isp, FPC_ADDR);
+	CCDC_PRINT_REGISTER(isp, VDINT);
+	CCDC_PRINT_REGISTER(isp, ALAW);
+	CCDC_PRINT_REGISTER(isp, REC656IF);
+	CCDC_PRINT_REGISTER(isp, CFG);
+	CCDC_PRINT_REGISTER(isp, FMTCFG);
+	CCDC_PRINT_REGISTER(isp, FMT_HORZ);
+	CCDC_PRINT_REGISTER(isp, FMT_VERT);
+	CCDC_PRINT_REGISTER(isp, PRGEVEN0);
+	CCDC_PRINT_REGISTER(isp, PRGEVEN1);
+	CCDC_PRINT_REGISTER(isp, PRGODD0);
+	CCDC_PRINT_REGISTER(isp, PRGODD1);
+	CCDC_PRINT_REGISTER(isp, VP_OUT);
+	CCDC_PRINT_REGISTER(isp, LSC_CONFIG);
+	CCDC_PRINT_REGISTER(isp, LSC_INITIAL);
+	CCDC_PRINT_REGISTER(isp, LSC_TABLE_BASE);
+	CCDC_PRINT_REGISTER(isp, LSC_TABLE_OFFSET);
+
+	dev_dbg(isp->dev, "--------------------------------------------\n");
+}
+
+/*
+ * omap3isp_ccdc_busy - Get busy state of the CCDC.
+ * @ccdc: Pointer to ISP CCDC device.
+ */
+int omap3isp_ccdc_busy(struct isp_ccdc_device *ccdc)
+{
+	struct isp_device *isp = to_isp_device(ccdc);
+
+	return isp_reg_readl(isp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_PCR) &
+		ISPCCDC_PCR_BUSY;
+}
+
+/* -----------------------------------------------------------------------------
+ * Lens Shading Compensation
+ */
+
+/*
+ * ccdc_lsc_validate_config - Check that LSC configuration is valid.
+ * @ccdc: Pointer to ISP CCDC device.
+ * @lsc_cfg: the LSC configuration to check.
+ *
+ * Returns 0 if the LSC configuration is valid, or -EINVAL if invalid.
+ */
+static int ccdc_lsc_validate_config(struct isp_ccdc_device *ccdc,
+				    struct omap3isp_ccdc_lsc_config *lsc_cfg)
+{
+	struct isp_device *isp = to_isp_device(ccdc);
+	struct v4l2_mbus_framefmt *format;
+	unsigned int paxel_width, paxel_height;
+	unsigned int paxel_shift_x, paxel_shift_y;
+	unsigned int min_width, min_height, min_size;
+	unsigned int input_width, input_height;
+
+	paxel_shift_x = lsc_cfg->gain_mode_m;
+	paxel_shift_y = lsc_cfg->gain_mode_n;
+
+	if ((paxel_shift_x < 2) || (paxel_shift_x > 6) ||
+	    (paxel_shift_y < 2) || (paxel_shift_y > 6)) {
+		dev_dbg(isp->dev, "CCDC: LSC: Invalid paxel size\n");
+		return -EINVAL;
+	}
+
+	if (lsc_cfg->offset & 3) {
+		dev_dbg(isp->dev, "CCDC: LSC: Offset must be a multiple of "
+			"4\n");
+		return -EINVAL;
+	}
+
+	if ((lsc_cfg->initial_x & 1) || (lsc_cfg->initial_y & 1)) {
+		dev_dbg(isp->dev, "CCDC: LSC: initial_x and y must be even\n");
+		return -EINVAL;
+	}
+
+	format = __ccdc_get_format(ccdc, NULL, CCDC_PAD_SINK,
+				   V4L2_SUBDEV_FORMAT_ACTIVE);
+	input_width = format->width;
+	input_height = format->height;
+
+	/* Calculate minimum bytesize for validation */
+	paxel_width = 1 << paxel_shift_x;
+	min_width = ((input_width + lsc_cfg->initial_x + paxel_width - 1)
+		     >> paxel_shift_x) + 1;
+
+	paxel_height = 1 << paxel_shift_y;
+	min_height = ((input_height + lsc_cfg->initial_y + paxel_height - 1)
+		     >> paxel_shift_y) + 1;
+
+	min_size = 4 * min_width * min_height;
+	if (min_size > lsc_cfg->size) {
+		dev_dbg(isp->dev, "CCDC: LSC: too small table\n");
+		return -EINVAL;
+	}
+	if (lsc_cfg->offset < (min_width * 4)) {
+		dev_dbg(isp->dev, "CCDC: LSC: Offset is too small\n");
+		return -EINVAL;
+	}
+	if ((lsc_cfg->size / lsc_cfg->offset) < min_height) {
+		dev_dbg(isp->dev, "CCDC: LSC: Wrong size/offset combination\n");
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/*
+ * ccdc_lsc_program_table - Program Lens Shading Compensation table address.
+ * @ccdc: Pointer to ISP CCDC device.
+ */
+static void ccdc_lsc_program_table(struct isp_ccdc_device *ccdc,
+				   dma_addr_t addr)
+{
+	isp_reg_writel(to_isp_device(ccdc), addr,
+		       OMAP3_ISP_IOMEM_CCDC, ISPCCDC_LSC_TABLE_BASE);
+}
+
+/*
+ * ccdc_lsc_setup_regs - Configures the lens shading compensation module
+ * @ccdc: Pointer to ISP CCDC device.
+ */
+static void ccdc_lsc_setup_regs(struct isp_ccdc_device *ccdc,
+				struct omap3isp_ccdc_lsc_config *cfg)
+{
+	struct isp_device *isp = to_isp_device(ccdc);
+	int reg;
+
+	isp_reg_writel(isp, cfg->offset, OMAP3_ISP_IOMEM_CCDC,
+		       ISPCCDC_LSC_TABLE_OFFSET);
+
+	reg = 0;
+	reg |= cfg->gain_mode_n << ISPCCDC_LSC_GAIN_MODE_N_SHIFT;
+	reg |= cfg->gain_mode_m << ISPCCDC_LSC_GAIN_MODE_M_SHIFT;
+	reg |= cfg->gain_format << ISPCCDC_LSC_GAIN_FORMAT_SHIFT;
+	isp_reg_writel(isp, reg, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_LSC_CONFIG);
+
+	reg = 0;
+	reg &= ~ISPCCDC_LSC_INITIAL_X_MASK;
+	reg |= cfg->initial_x << ISPCCDC_LSC_INITIAL_X_SHIFT;
+	reg &= ~ISPCCDC_LSC_INITIAL_Y_MASK;
+	reg |= cfg->initial_y << ISPCCDC_LSC_INITIAL_Y_SHIFT;
+	isp_reg_writel(isp, reg, OMAP3_ISP_IOMEM_CCDC,
+		       ISPCCDC_LSC_INITIAL);
+}
+
+static int ccdc_lsc_wait_prefetch(struct isp_ccdc_device *ccdc)
+{
+	struct isp_device *isp = to_isp_device(ccdc);
+	unsigned int wait;
+
+	isp_reg_writel(isp, IRQ0STATUS_CCDC_LSC_PREF_COMP_IRQ,
+		       OMAP3_ISP_IOMEM_MAIN, ISP_IRQ0STATUS);
+
+	/* timeout 1 ms */
+	for (wait = 0; wait < 1000; wait++) {
+		if (isp_reg_readl(isp, OMAP3_ISP_IOMEM_MAIN, ISP_IRQ0STATUS) &
+				  IRQ0STATUS_CCDC_LSC_PREF_COMP_IRQ) {
+			isp_reg_writel(isp, IRQ0STATUS_CCDC_LSC_PREF_COMP_IRQ,
+				       OMAP3_ISP_IOMEM_MAIN, ISP_IRQ0STATUS);
+			return 0;
+		}
+
+		rmb();
+		udelay(1);
+	}
+
+	return -ETIMEDOUT;
+}
+
+/*
+ * __ccdc_lsc_enable - Enables/Disables the Lens Shading Compensation module.
+ * @ccdc: Pointer to ISP CCDC device.
+ * @enable: 0 Disables LSC, 1 Enables LSC.
+ */
+static int __ccdc_lsc_enable(struct isp_ccdc_device *ccdc, int enable)
+{
+	struct isp_device *isp = to_isp_device(ccdc);
+	const struct v4l2_mbus_framefmt *format =
+		__ccdc_get_format(ccdc, NULL, CCDC_PAD_SINK,
+				  V4L2_SUBDEV_FORMAT_ACTIVE);
+
+	if ((format->code != MEDIA_BUS_FMT_SGRBG10_1X10) &&
+	    (format->code != MEDIA_BUS_FMT_SRGGB10_1X10) &&
+	    (format->code != MEDIA_BUS_FMT_SBGGR10_1X10) &&
+	    (format->code != MEDIA_BUS_FMT_SGBRG10_1X10))
+		return -EINVAL;
+
+	if (enable)
+		omap3isp_sbl_enable(isp, OMAP3_ISP_SBL_CCDC_LSC_READ);
+
+	isp_reg_clr_set(isp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_LSC_CONFIG,
+			ISPCCDC_LSC_ENABLE, enable ? ISPCCDC_LSC_ENABLE : 0);
+
+	if (enable) {
+		if (ccdc_lsc_wait_prefetch(ccdc) < 0) {
+			isp_reg_clr(isp, OMAP3_ISP_IOMEM_CCDC,
+				    ISPCCDC_LSC_CONFIG, ISPCCDC_LSC_ENABLE);
+			ccdc->lsc.state = LSC_STATE_STOPPED;
+			dev_warn(to_device(ccdc), "LSC prefetch timeout\n");
+			return -ETIMEDOUT;
+		}
+		ccdc->lsc.state = LSC_STATE_RUNNING;
+	} else {
+		ccdc->lsc.state = LSC_STATE_STOPPING;
+	}
+
+	return 0;
+}
+
+static int ccdc_lsc_busy(struct isp_ccdc_device *ccdc)
+{
+	struct isp_device *isp = to_isp_device(ccdc);
+
+	return isp_reg_readl(isp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_LSC_CONFIG) &
+			     ISPCCDC_LSC_BUSY;
+}
+
+/* __ccdc_lsc_configure - Apply a new configuration to the LSC engine
+ * @ccdc: Pointer to ISP CCDC device
+ * @req: New configuration request
+ *
+ * context: in_interrupt()
+ */
+static int __ccdc_lsc_configure(struct isp_ccdc_device *ccdc,
+				struct ispccdc_lsc_config_req *req)
+{
+	if (!req->enable)
+		return -EINVAL;
+
+	if (ccdc_lsc_validate_config(ccdc, &req->config) < 0) {
+		dev_dbg(to_device(ccdc), "Discard LSC configuration\n");
+		return -EINVAL;
+	}
+
+	if (ccdc_lsc_busy(ccdc))
+		return -EBUSY;
+
+	ccdc_lsc_setup_regs(ccdc, &req->config);
+	ccdc_lsc_program_table(ccdc, req->table.dma);
+	return 0;
+}
+
+/*
+ * ccdc_lsc_error_handler - Handle LSC prefetch error scenario.
+ * @ccdc: Pointer to ISP CCDC device.
+ *
+ * Disables LSC, and defers enablement to shadow registers update time.
+ */
+static void ccdc_lsc_error_handler(struct isp_ccdc_device *ccdc)
+{
+	struct isp_device *isp = to_isp_device(ccdc);
+	/*
+	 * From OMAP3 TRM: When this event is pending, the module
+	 * goes into transparent mode (output =input). Normal
+	 * operation can be resumed at the start of the next frame
+	 * after:
+	 *  1) Clearing this event
+	 *  2) Disabling the LSC module
+	 *  3) Enabling it
+	 */
+	isp_reg_clr(isp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_LSC_CONFIG,
+		    ISPCCDC_LSC_ENABLE);
+	ccdc->lsc.state = LSC_STATE_STOPPED;
+}
+
+static void ccdc_lsc_free_request(struct isp_ccdc_device *ccdc,
+				  struct ispccdc_lsc_config_req *req)
+{
+	struct isp_device *isp = to_isp_device(ccdc);
+
+	if (req == NULL)
+		return;
+
+	if (req->table.addr) {
+		sg_free_table(&req->table.sgt);
+		dma_free_coherent(isp->dev, req->config.size, req->table.addr,
+				  req->table.dma);
+	}
+
+	kfree(req);
+}
+
+static void ccdc_lsc_free_queue(struct isp_ccdc_device *ccdc,
+				struct list_head *queue)
+{
+	struct ispccdc_lsc_config_req *req, *n;
+	unsigned long flags;
+
+	spin_lock_irqsave(&ccdc->lsc.req_lock, flags);
+	list_for_each_entry_safe(req, n, queue, list) {
+		list_del(&req->list);
+		spin_unlock_irqrestore(&ccdc->lsc.req_lock, flags);
+		ccdc_lsc_free_request(ccdc, req);
+		spin_lock_irqsave(&ccdc->lsc.req_lock, flags);
+	}
+	spin_unlock_irqrestore(&ccdc->lsc.req_lock, flags);
+}
+
+static void ccdc_lsc_free_table_work(struct work_struct *work)
+{
+	struct isp_ccdc_device *ccdc;
+	struct ispccdc_lsc *lsc;
+
+	lsc = container_of(work, struct ispccdc_lsc, table_work);
+	ccdc = container_of(lsc, struct isp_ccdc_device, lsc);
+
+	ccdc_lsc_free_queue(ccdc, &lsc->free_queue);
+}
+
+/*
+ * ccdc_lsc_config - Configure the LSC module from a userspace request
+ *
+ * Store the request LSC configuration in the LSC engine request pointer. The
+ * configuration will be applied to the hardware when the CCDC will be enabled,
+ * or at the next LSC interrupt if the CCDC is already running.
+ */
+static int ccdc_lsc_config(struct isp_ccdc_device *ccdc,
+			   struct omap3isp_ccdc_update_config *config)
+{
+	struct isp_device *isp = to_isp_device(ccdc);
+	struct ispccdc_lsc_config_req *req;
+	unsigned long flags;
+	u16 update;
+	int ret;
+
+	update = config->update &
+		 (OMAP3ISP_CCDC_CONFIG_LSC | OMAP3ISP_CCDC_TBL_LSC);
+	if (!update)
+		return 0;
+
+	if (update != (OMAP3ISP_CCDC_CONFIG_LSC | OMAP3ISP_CCDC_TBL_LSC)) {
+		dev_dbg(to_device(ccdc), "%s: Both LSC configuration and table "
+			"need to be supplied\n", __func__);
+		return -EINVAL;
+	}
+
+	req = kzalloc(sizeof(*req), GFP_KERNEL);
+	if (req == NULL)
+		return -ENOMEM;
+
+	if (config->flag & OMAP3ISP_CCDC_CONFIG_LSC) {
+		if (copy_from_user(&req->config, config->lsc_cfg,
+				   sizeof(req->config))) {
+			ret = -EFAULT;
+			goto done;
+		}
+
+		req->enable = 1;
+
+		req->table.addr = dma_alloc_coherent(isp->dev, req->config.size,
+						     &req->table.dma,
+						     GFP_KERNEL);
+		if (req->table.addr == NULL) {
+			ret = -ENOMEM;
+			goto done;
+		}
+
+		ret = dma_get_sgtable(isp->dev, &req->table.sgt,
+				      req->table.addr, req->table.dma,
+				      req->config.size);
+		if (ret < 0)
+			goto done;
+
+		dma_sync_sg_for_cpu(isp->dev, req->table.sgt.sgl,
+				    req->table.sgt.nents, DMA_TO_DEVICE);
+
+		if (copy_from_user(req->table.addr, config->lsc,
+				   req->config.size)) {
+			ret = -EFAULT;
+			goto done;
+		}
+
+		dma_sync_sg_for_device(isp->dev, req->table.sgt.sgl,
+				       req->table.sgt.nents, DMA_TO_DEVICE);
+	}
+
+	spin_lock_irqsave(&ccdc->lsc.req_lock, flags);
+	if (ccdc->lsc.request) {
+		list_add_tail(&ccdc->lsc.request->list, &ccdc->lsc.free_queue);
+		schedule_work(&ccdc->lsc.table_work);
+	}
+	ccdc->lsc.request = req;
+	spin_unlock_irqrestore(&ccdc->lsc.req_lock, flags);
+
+	ret = 0;
+
+done:
+	if (ret < 0)
+		ccdc_lsc_free_request(ccdc, req);
+
+	return ret;
+}
+
+static inline int ccdc_lsc_is_configured(struct isp_ccdc_device *ccdc)
+{
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&ccdc->lsc.req_lock, flags);
+	ret = ccdc->lsc.active != NULL;
+	spin_unlock_irqrestore(&ccdc->lsc.req_lock, flags);
+
+	return ret;
+}
+
+static int ccdc_lsc_enable(struct isp_ccdc_device *ccdc)
+{
+	struct ispccdc_lsc *lsc = &ccdc->lsc;
+
+	if (lsc->state != LSC_STATE_STOPPED)
+		return -EINVAL;
+
+	if (lsc->active) {
+		list_add_tail(&lsc->active->list, &lsc->free_queue);
+		lsc->active = NULL;
+	}
+
+	if (__ccdc_lsc_configure(ccdc, lsc->request) < 0) {
+		omap3isp_sbl_disable(to_isp_device(ccdc),
+				OMAP3_ISP_SBL_CCDC_LSC_READ);
+		list_add_tail(&lsc->request->list, &lsc->free_queue);
+		lsc->request = NULL;
+		goto done;
+	}
+
+	lsc->active = lsc->request;
+	lsc->request = NULL;
+	__ccdc_lsc_enable(ccdc, 1);
+
+done:
+	if (!list_empty(&lsc->free_queue))
+		schedule_work(&lsc->table_work);
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * Parameters configuration
+ */
+
+/*
+ * ccdc_configure_clamp - Configure optical-black or digital clamping
+ * @ccdc: Pointer to ISP CCDC device.
+ *
+ * The CCDC performs either optical-black or digital clamp. Configure and enable
+ * the selected clamp method.
+ */
+static void ccdc_configure_clamp(struct isp_ccdc_device *ccdc)
+{
+	struct isp_device *isp = to_isp_device(ccdc);
+	u32 clamp;
+
+	if (ccdc->obclamp) {
+		clamp  = ccdc->clamp.obgain << ISPCCDC_CLAMP_OBGAIN_SHIFT;
+		clamp |= ccdc->clamp.oblen << ISPCCDC_CLAMP_OBSLEN_SHIFT;
+		clamp |= ccdc->clamp.oblines << ISPCCDC_CLAMP_OBSLN_SHIFT;
+		clamp |= ccdc->clamp.obstpixel << ISPCCDC_CLAMP_OBST_SHIFT;
+		isp_reg_writel(isp, clamp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_CLAMP);
+	} else {
+		isp_reg_writel(isp, ccdc->clamp.dcsubval,
+			       OMAP3_ISP_IOMEM_CCDC, ISPCCDC_DCSUB);
+	}
+
+	isp_reg_clr_set(isp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_CLAMP,
+			ISPCCDC_CLAMP_CLAMPEN,
+			ccdc->obclamp ? ISPCCDC_CLAMP_CLAMPEN : 0);
+}
+
+/*
+ * ccdc_configure_fpc - Configure Faulty Pixel Correction
+ * @ccdc: Pointer to ISP CCDC device.
+ */
+static void ccdc_configure_fpc(struct isp_ccdc_device *ccdc)
+{
+	struct isp_device *isp = to_isp_device(ccdc);
+
+	isp_reg_clr(isp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_FPC, ISPCCDC_FPC_FPCEN);
+
+	if (!ccdc->fpc_en)
+		return;
+
+	isp_reg_writel(isp, ccdc->fpc.dma, OMAP3_ISP_IOMEM_CCDC,
+		       ISPCCDC_FPC_ADDR);
+	/* The FPNUM field must be set before enabling FPC. */
+	isp_reg_writel(isp, (ccdc->fpc.fpnum << ISPCCDC_FPC_FPNUM_SHIFT),
+		       OMAP3_ISP_IOMEM_CCDC, ISPCCDC_FPC);
+	isp_reg_writel(isp, (ccdc->fpc.fpnum << ISPCCDC_FPC_FPNUM_SHIFT) |
+		       ISPCCDC_FPC_FPCEN, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_FPC);
+}
+
+/*
+ * ccdc_configure_black_comp - Configure Black Level Compensation.
+ * @ccdc: Pointer to ISP CCDC device.
+ */
+static void ccdc_configure_black_comp(struct isp_ccdc_device *ccdc)
+{
+	struct isp_device *isp = to_isp_device(ccdc);
+	u32 blcomp;
+
+	blcomp  = ccdc->blcomp.b_mg << ISPCCDC_BLKCMP_B_MG_SHIFT;
+	blcomp |= ccdc->blcomp.gb_g << ISPCCDC_BLKCMP_GB_G_SHIFT;
+	blcomp |= ccdc->blcomp.gr_cy << ISPCCDC_BLKCMP_GR_CY_SHIFT;
+	blcomp |= ccdc->blcomp.r_ye << ISPCCDC_BLKCMP_R_YE_SHIFT;
+
+	isp_reg_writel(isp, blcomp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_BLKCMP);
+}
+
+/*
+ * ccdc_configure_lpf - Configure Low-Pass Filter (LPF).
+ * @ccdc: Pointer to ISP CCDC device.
+ */
+static void ccdc_configure_lpf(struct isp_ccdc_device *ccdc)
+{
+	struct isp_device *isp = to_isp_device(ccdc);
+
+	isp_reg_clr_set(isp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_SYN_MODE,
+			ISPCCDC_SYN_MODE_LPF,
+			ccdc->lpf ? ISPCCDC_SYN_MODE_LPF : 0);
+}
+
+/*
+ * ccdc_configure_alaw - Configure A-law compression.
+ * @ccdc: Pointer to ISP CCDC device.
+ */
+static void ccdc_configure_alaw(struct isp_ccdc_device *ccdc)
+{
+	struct isp_device *isp = to_isp_device(ccdc);
+	const struct isp_format_info *info;
+	u32 alaw = 0;
+
+	info = omap3isp_video_format_info(ccdc->formats[CCDC_PAD_SINK].code);
+
+	switch (info->width) {
+	case 8:
+		return;
+
+	case 10:
+		alaw = ISPCCDC_ALAW_GWDI_9_0;
+		break;
+	case 11:
+		alaw = ISPCCDC_ALAW_GWDI_10_1;
+		break;
+	case 12:
+		alaw = ISPCCDC_ALAW_GWDI_11_2;
+		break;
+	case 13:
+		alaw = ISPCCDC_ALAW_GWDI_12_3;
+		break;
+	}
+
+	if (ccdc->alaw)
+		alaw |= ISPCCDC_ALAW_CCDTBL;
+
+	isp_reg_writel(isp, alaw, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_ALAW);
+}
+
+/*
+ * ccdc_config_imgattr - Configure sensor image specific attributes.
+ * @ccdc: Pointer to ISP CCDC device.
+ * @colptn: Color pattern of the sensor.
+ */
+static void ccdc_config_imgattr(struct isp_ccdc_device *ccdc, u32 colptn)
+{
+	struct isp_device *isp = to_isp_device(ccdc);
+
+	isp_reg_writel(isp, colptn, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_COLPTN);
+}
+
+/*
+ * ccdc_config - Set CCDC configuration from userspace
+ * @ccdc: Pointer to ISP CCDC device.
+ * @ccdc_struct: Structure containing CCDC configuration sent from userspace.
+ *
+ * Returns 0 if successful, -EINVAL if the pointer to the configuration
+ * structure is null, or the copy_from_user function fails to copy user space
+ * memory to kernel space memory.
+ */
+static int ccdc_config(struct isp_ccdc_device *ccdc,
+		       struct omap3isp_ccdc_update_config *ccdc_struct)
+{
+	struct isp_device *isp = to_isp_device(ccdc);
+	unsigned long flags;
+
+	spin_lock_irqsave(&ccdc->lock, flags);
+	ccdc->shadow_update = 1;
+	spin_unlock_irqrestore(&ccdc->lock, flags);
+
+	if (OMAP3ISP_CCDC_ALAW & ccdc_struct->update) {
+		ccdc->alaw = !!(OMAP3ISP_CCDC_ALAW & ccdc_struct->flag);
+		ccdc->update |= OMAP3ISP_CCDC_ALAW;
+	}
+
+	if (OMAP3ISP_CCDC_LPF & ccdc_struct->update) {
+		ccdc->lpf = !!(OMAP3ISP_CCDC_LPF & ccdc_struct->flag);
+		ccdc->update |= OMAP3ISP_CCDC_LPF;
+	}
+
+	if (OMAP3ISP_CCDC_BLCLAMP & ccdc_struct->update) {
+		if (copy_from_user(&ccdc->clamp, ccdc_struct->bclamp,
+				   sizeof(ccdc->clamp))) {
+			ccdc->shadow_update = 0;
+			return -EFAULT;
+		}
+
+		ccdc->obclamp = !!(OMAP3ISP_CCDC_BLCLAMP & ccdc_struct->flag);
+		ccdc->update |= OMAP3ISP_CCDC_BLCLAMP;
+	}
+
+	if (OMAP3ISP_CCDC_BCOMP & ccdc_struct->update) {
+		if (copy_from_user(&ccdc->blcomp, ccdc_struct->blcomp,
+				   sizeof(ccdc->blcomp))) {
+			ccdc->shadow_update = 0;
+			return -EFAULT;
+		}
+
+		ccdc->update |= OMAP3ISP_CCDC_BCOMP;
+	}
+
+	ccdc->shadow_update = 0;
+
+	if (OMAP3ISP_CCDC_FPC & ccdc_struct->update) {
+		struct omap3isp_ccdc_fpc fpc;
+		struct ispccdc_fpc fpc_old = { .addr = NULL, };
+		struct ispccdc_fpc fpc_new;
+		u32 size;
+
+		if (ccdc->state != ISP_PIPELINE_STREAM_STOPPED)
+			return -EBUSY;
+
+		ccdc->fpc_en = !!(OMAP3ISP_CCDC_FPC & ccdc_struct->flag);
+
+		if (ccdc->fpc_en) {
+			if (copy_from_user(&fpc, ccdc_struct->fpc, sizeof(fpc)))
+				return -EFAULT;
+
+			size = fpc.fpnum * 4;
+
+			/*
+			 * The table address must be 64-bytes aligned, which is
+			 * guaranteed by dma_alloc_coherent().
+			 */
+			fpc_new.fpnum = fpc.fpnum;
+			fpc_new.addr = dma_alloc_coherent(isp->dev, size,
+							  &fpc_new.dma,
+							  GFP_KERNEL);
+			if (fpc_new.addr == NULL)
+				return -ENOMEM;
+
+			if (copy_from_user(fpc_new.addr,
+					   (__force void __user *)fpc.fpcaddr,
+					   size)) {
+				dma_free_coherent(isp->dev, size, fpc_new.addr,
+						  fpc_new.dma);
+				return -EFAULT;
+			}
+
+			fpc_old = ccdc->fpc;
+			ccdc->fpc = fpc_new;
+		}
+
+		ccdc_configure_fpc(ccdc);
+
+		if (fpc_old.addr != NULL)
+			dma_free_coherent(isp->dev, fpc_old.fpnum * 4,
+					  fpc_old.addr, fpc_old.dma);
+	}
+
+	return ccdc_lsc_config(ccdc, ccdc_struct);
+}
+
+static void ccdc_apply_controls(struct isp_ccdc_device *ccdc)
+{
+	if (ccdc->update & OMAP3ISP_CCDC_ALAW) {
+		ccdc_configure_alaw(ccdc);
+		ccdc->update &= ~OMAP3ISP_CCDC_ALAW;
+	}
+
+	if (ccdc->update & OMAP3ISP_CCDC_LPF) {
+		ccdc_configure_lpf(ccdc);
+		ccdc->update &= ~OMAP3ISP_CCDC_LPF;
+	}
+
+	if (ccdc->update & OMAP3ISP_CCDC_BLCLAMP) {
+		ccdc_configure_clamp(ccdc);
+		ccdc->update &= ~OMAP3ISP_CCDC_BLCLAMP;
+	}
+
+	if (ccdc->update & OMAP3ISP_CCDC_BCOMP) {
+		ccdc_configure_black_comp(ccdc);
+		ccdc->update &= ~OMAP3ISP_CCDC_BCOMP;
+	}
+}
+
+/*
+ * omap3isp_ccdc_restore_context - Restore values of the CCDC module registers
+ * @isp: Pointer to ISP device
+ */
+void omap3isp_ccdc_restore_context(struct isp_device *isp)
+{
+	struct isp_ccdc_device *ccdc = &isp->isp_ccdc;
+
+	isp_reg_set(isp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_CFG, ISPCCDC_CFG_VDLC);
+
+	ccdc->update = OMAP3ISP_CCDC_ALAW | OMAP3ISP_CCDC_LPF
+		     | OMAP3ISP_CCDC_BLCLAMP | OMAP3ISP_CCDC_BCOMP;
+	ccdc_apply_controls(ccdc);
+	ccdc_configure_fpc(ccdc);
+}
+
+/* -----------------------------------------------------------------------------
+ * Format- and pipeline-related configuration helpers
+ */
+
+/*
+ * ccdc_config_vp - Configure the Video Port.
+ * @ccdc: Pointer to ISP CCDC device.
+ */
+static void ccdc_config_vp(struct isp_ccdc_device *ccdc)
+{
+	struct isp_pipeline *pipe = to_isp_pipeline(&ccdc->subdev.entity);
+	struct isp_device *isp = to_isp_device(ccdc);
+	const struct isp_format_info *info;
+	struct v4l2_mbus_framefmt *format;
+	unsigned long l3_ick = pipe->l3_ick;
+	unsigned int max_div = isp->revision == ISP_REVISION_15_0 ? 64 : 8;
+	unsigned int div = 0;
+	u32 fmtcfg = ISPCCDC_FMTCFG_VPEN;
+
+	format = &ccdc->formats[CCDC_PAD_SOURCE_VP];
+
+	if (!format->code) {
+		/* Disable the video port when the input format isn't supported.
+		 * This is indicated by a pixel code set to 0.
+		 */
+		isp_reg_writel(isp, 0, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_FMTCFG);
+		return;
+	}
+
+	isp_reg_writel(isp, (0 << ISPCCDC_FMT_HORZ_FMTSPH_SHIFT) |
+		       (format->width << ISPCCDC_FMT_HORZ_FMTLNH_SHIFT),
+		       OMAP3_ISP_IOMEM_CCDC, ISPCCDC_FMT_HORZ);
+	isp_reg_writel(isp, (0 << ISPCCDC_FMT_VERT_FMTSLV_SHIFT) |
+		       ((format->height + 1) << ISPCCDC_FMT_VERT_FMTLNV_SHIFT),
+		       OMAP3_ISP_IOMEM_CCDC, ISPCCDC_FMT_VERT);
+
+	isp_reg_writel(isp, (format->width << ISPCCDC_VP_OUT_HORZ_NUM_SHIFT) |
+		       (format->height << ISPCCDC_VP_OUT_VERT_NUM_SHIFT),
+		       OMAP3_ISP_IOMEM_CCDC, ISPCCDC_VP_OUT);
+
+	info = omap3isp_video_format_info(ccdc->formats[CCDC_PAD_SINK].code);
+
+	switch (info->width) {
+	case 8:
+	case 10:
+		fmtcfg |= ISPCCDC_FMTCFG_VPIN_9_0;
+		break;
+	case 11:
+		fmtcfg |= ISPCCDC_FMTCFG_VPIN_10_1;
+		break;
+	case 12:
+		fmtcfg |= ISPCCDC_FMTCFG_VPIN_11_2;
+		break;
+	case 13:
+		fmtcfg |= ISPCCDC_FMTCFG_VPIN_12_3;
+		break;
+	}
+
+	if (pipe->input)
+		div = DIV_ROUND_UP(l3_ick, pipe->max_rate);
+	else if (pipe->external_rate)
+		div = l3_ick / pipe->external_rate;
+
+	div = clamp(div, 2U, max_div);
+	fmtcfg |= (div - 2) << ISPCCDC_FMTCFG_VPIF_FRQ_SHIFT;
+
+	isp_reg_writel(isp, fmtcfg, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_FMTCFG);
+}
+
+/*
+ * ccdc_config_outlineoffset - Configure memory saving output line offset
+ * @ccdc: Pointer to ISP CCDC device.
+ * @bpl: Number of bytes per line when stored in memory.
+ * @field: Field order when storing interlaced formats in memory.
+ *
+ * Configure the offsets for the line output control:
+ *
+ * - The horizontal line offset is defined as the number of bytes between the
+ *   start of two consecutive lines in memory. Set it to the given bytes per
+ *   line value.
+ *
+ * - The field offset value is defined as the number of lines to offset the
+ *   start of the field identified by FID = 1. Set it to one.
+ *
+ * - The line offset values are defined as the number of lines (as defined by
+ *   the horizontal line offset) between the start of two consecutive lines for
+ *   all combinations of odd/even lines in odd/even fields. When interleaving
+ *   fields set them all to two lines, and to one line otherwise.
+ */
+static void ccdc_config_outlineoffset(struct isp_ccdc_device *ccdc,
+				      unsigned int bpl,
+				      enum v4l2_field field)
+{
+	struct isp_device *isp = to_isp_device(ccdc);
+	u32 sdofst = 0;
+
+	isp_reg_writel(isp, bpl & 0xffff, OMAP3_ISP_IOMEM_CCDC,
+		       ISPCCDC_HSIZE_OFF);
+
+	switch (field) {
+	case V4L2_FIELD_INTERLACED_TB:
+	case V4L2_FIELD_INTERLACED_BT:
+		/* When interleaving fields in memory offset field one by one
+		 * line and set the line offset to two lines.
+		 */
+		sdofst |= (1 << ISPCCDC_SDOFST_LOFST0_SHIFT)
+		       |  (1 << ISPCCDC_SDOFST_LOFST1_SHIFT)
+		       |  (1 << ISPCCDC_SDOFST_LOFST2_SHIFT)
+		       |  (1 << ISPCCDC_SDOFST_LOFST3_SHIFT);
+		break;
+
+	default:
+		/* In all other cases set the line offsets to one line. */
+		break;
+	}
+
+	isp_reg_writel(isp, sdofst, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_SDOFST);
+}
+
+/*
+ * ccdc_set_outaddr - Set memory address to save output image
+ * @ccdc: Pointer to ISP CCDC device.
+ * @addr: ISP MMU Mapped 32-bit memory address aligned on 32 byte boundary.
+ *
+ * Sets the memory address where the output will be saved.
+ */
+static void ccdc_set_outaddr(struct isp_ccdc_device *ccdc, u32 addr)
+{
+	struct isp_device *isp = to_isp_device(ccdc);
+
+	isp_reg_writel(isp, addr, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_SDR_ADDR);
+}
+
+/*
+ * omap3isp_ccdc_max_rate - Calculate maximum input data rate based on the input
+ * @ccdc: Pointer to ISP CCDC device.
+ * @max_rate: Maximum calculated data rate.
+ *
+ * Returns in *max_rate less value between calculated and passed
+ */
+void omap3isp_ccdc_max_rate(struct isp_ccdc_device *ccdc,
+			    unsigned int *max_rate)
+{
+	struct isp_pipeline *pipe = to_isp_pipeline(&ccdc->subdev.entity);
+	unsigned int rate;
+
+	if (pipe == NULL)
+		return;
+
+	/*
+	 * TRM says that for parallel sensors the maximum data rate
+	 * should be 90% form L3/2 clock, otherwise just L3/2.
+	 */
+	if (ccdc->input == CCDC_INPUT_PARALLEL)
+		rate = pipe->l3_ick / 2 * 9 / 10;
+	else
+		rate = pipe->l3_ick / 2;
+
+	*max_rate = min(*max_rate, rate);
+}
+
+/*
+ * ccdc_config_sync_if - Set CCDC sync interface configuration
+ * @ccdc: Pointer to ISP CCDC device.
+ * @parcfg: Parallel interface platform data (may be NULL)
+ * @data_size: Data size
+ */
+static void ccdc_config_sync_if(struct isp_ccdc_device *ccdc,
+				struct isp_parallel_cfg *parcfg,
+				unsigned int data_size)
+{
+	struct isp_device *isp = to_isp_device(ccdc);
+	const struct v4l2_mbus_framefmt *format;
+	u32 syn_mode = ISPCCDC_SYN_MODE_VDHDEN;
+
+	format = &ccdc->formats[CCDC_PAD_SINK];
+
+	if (format->code == MEDIA_BUS_FMT_YUYV8_2X8 ||
+	    format->code == MEDIA_BUS_FMT_UYVY8_2X8) {
+		/* According to the OMAP3 TRM the input mode only affects SYNC
+		 * mode, enabling BT.656 mode should take precedence. However,
+		 * in practice setting the input mode to YCbCr data on 8 bits
+		 * seems to be required in BT.656 mode. In SYNC mode set it to
+		 * YCbCr on 16 bits as the bridge is enabled in that case.
+		 */
+		if (ccdc->bt656)
+			syn_mode |= ISPCCDC_SYN_MODE_INPMOD_YCBCR8;
+		else
+			syn_mode |= ISPCCDC_SYN_MODE_INPMOD_YCBCR16;
+	}
+
+	switch (data_size) {
+	case 8:
+		syn_mode |= ISPCCDC_SYN_MODE_DATSIZ_8;
+		break;
+	case 10:
+		syn_mode |= ISPCCDC_SYN_MODE_DATSIZ_10;
+		break;
+	case 11:
+		syn_mode |= ISPCCDC_SYN_MODE_DATSIZ_11;
+		break;
+	case 12:
+		syn_mode |= ISPCCDC_SYN_MODE_DATSIZ_12;
+		break;
+	}
+
+	if (parcfg && parcfg->data_pol)
+		syn_mode |= ISPCCDC_SYN_MODE_DATAPOL;
+
+	if (parcfg && parcfg->hs_pol)
+		syn_mode |= ISPCCDC_SYN_MODE_HDPOL;
+
+	/* The polarity of the vertical sync signal output by the BT.656
+	 * decoder is not documented and seems to be active low.
+	 */
+	if ((parcfg && parcfg->vs_pol) || ccdc->bt656)
+		syn_mode |= ISPCCDC_SYN_MODE_VDPOL;
+
+	if (parcfg && parcfg->fld_pol)
+		syn_mode |= ISPCCDC_SYN_MODE_FLDPOL;
+
+	isp_reg_writel(isp, syn_mode, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_SYN_MODE);
+
+	/* The CCDC_CFG.Y8POS bit is used in YCbCr8 input mode only. The
+	 * hardware seems to ignore it in all other input modes.
+	 */
+	if (format->code == MEDIA_BUS_FMT_UYVY8_2X8)
+		isp_reg_set(isp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_CFG,
+			    ISPCCDC_CFG_Y8POS);
+	else
+		isp_reg_clr(isp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_CFG,
+			    ISPCCDC_CFG_Y8POS);
+
+	/* Enable or disable BT.656 mode, including error correction for the
+	 * synchronization codes.
+	 */
+	if (ccdc->bt656)
+		isp_reg_set(isp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_REC656IF,
+			    ISPCCDC_REC656IF_R656ON | ISPCCDC_REC656IF_ECCFVH);
+	else
+		isp_reg_clr(isp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_REC656IF,
+			    ISPCCDC_REC656IF_R656ON | ISPCCDC_REC656IF_ECCFVH);
+
+}
+
+/* CCDC formats descriptions */
+static const u32 ccdc_sgrbg_pattern =
+	ISPCCDC_COLPTN_Gr_Cy << ISPCCDC_COLPTN_CP0PLC0_SHIFT |
+	ISPCCDC_COLPTN_R_Ye  << ISPCCDC_COLPTN_CP0PLC1_SHIFT |
+	ISPCCDC_COLPTN_Gr_Cy << ISPCCDC_COLPTN_CP0PLC2_SHIFT |
+	ISPCCDC_COLPTN_R_Ye  << ISPCCDC_COLPTN_CP0PLC3_SHIFT |
+	ISPCCDC_COLPTN_B_Mg  << ISPCCDC_COLPTN_CP1PLC0_SHIFT |
+	ISPCCDC_COLPTN_Gb_G  << ISPCCDC_COLPTN_CP1PLC1_SHIFT |
+	ISPCCDC_COLPTN_B_Mg  << ISPCCDC_COLPTN_CP1PLC2_SHIFT |
+	ISPCCDC_COLPTN_Gb_G  << ISPCCDC_COLPTN_CP1PLC3_SHIFT |
+	ISPCCDC_COLPTN_Gr_Cy << ISPCCDC_COLPTN_CP2PLC0_SHIFT |
+	ISPCCDC_COLPTN_R_Ye  << ISPCCDC_COLPTN_CP2PLC1_SHIFT |
+	ISPCCDC_COLPTN_Gr_Cy << ISPCCDC_COLPTN_CP2PLC2_SHIFT |
+	ISPCCDC_COLPTN_R_Ye  << ISPCCDC_COLPTN_CP2PLC3_SHIFT |
+	ISPCCDC_COLPTN_B_Mg  << ISPCCDC_COLPTN_CP3PLC0_SHIFT |
+	ISPCCDC_COLPTN_Gb_G  << ISPCCDC_COLPTN_CP3PLC1_SHIFT |
+	ISPCCDC_COLPTN_B_Mg  << ISPCCDC_COLPTN_CP3PLC2_SHIFT |
+	ISPCCDC_COLPTN_Gb_G  << ISPCCDC_COLPTN_CP3PLC3_SHIFT;
+
+static const u32 ccdc_srggb_pattern =
+	ISPCCDC_COLPTN_R_Ye  << ISPCCDC_COLPTN_CP0PLC0_SHIFT |
+	ISPCCDC_COLPTN_Gr_Cy << ISPCCDC_COLPTN_CP0PLC1_SHIFT |
+	ISPCCDC_COLPTN_R_Ye  << ISPCCDC_COLPTN_CP0PLC2_SHIFT |
+	ISPCCDC_COLPTN_Gr_Cy << ISPCCDC_COLPTN_CP0PLC3_SHIFT |
+	ISPCCDC_COLPTN_Gb_G  << ISPCCDC_COLPTN_CP1PLC0_SHIFT |
+	ISPCCDC_COLPTN_B_Mg  << ISPCCDC_COLPTN_CP1PLC1_SHIFT |
+	ISPCCDC_COLPTN_Gb_G  << ISPCCDC_COLPTN_CP1PLC2_SHIFT |
+	ISPCCDC_COLPTN_B_Mg  << ISPCCDC_COLPTN_CP1PLC3_SHIFT |
+	ISPCCDC_COLPTN_R_Ye  << ISPCCDC_COLPTN_CP2PLC0_SHIFT |
+	ISPCCDC_COLPTN_Gr_Cy << ISPCCDC_COLPTN_CP2PLC1_SHIFT |
+	ISPCCDC_COLPTN_R_Ye  << ISPCCDC_COLPTN_CP2PLC2_SHIFT |
+	ISPCCDC_COLPTN_Gr_Cy << ISPCCDC_COLPTN_CP2PLC3_SHIFT |
+	ISPCCDC_COLPTN_Gb_G  << ISPCCDC_COLPTN_CP3PLC0_SHIFT |
+	ISPCCDC_COLPTN_B_Mg  << ISPCCDC_COLPTN_CP3PLC1_SHIFT |
+	ISPCCDC_COLPTN_Gb_G  << ISPCCDC_COLPTN_CP3PLC2_SHIFT |
+	ISPCCDC_COLPTN_B_Mg  << ISPCCDC_COLPTN_CP3PLC3_SHIFT;
+
+static const u32 ccdc_sbggr_pattern =
+	ISPCCDC_COLPTN_B_Mg  << ISPCCDC_COLPTN_CP0PLC0_SHIFT |
+	ISPCCDC_COLPTN_Gb_G  << ISPCCDC_COLPTN_CP0PLC1_SHIFT |
+	ISPCCDC_COLPTN_B_Mg  << ISPCCDC_COLPTN_CP0PLC2_SHIFT |
+	ISPCCDC_COLPTN_Gb_G  << ISPCCDC_COLPTN_CP0PLC3_SHIFT |
+	ISPCCDC_COLPTN_Gr_Cy << ISPCCDC_COLPTN_CP1PLC0_SHIFT |
+	ISPCCDC_COLPTN_R_Ye  << ISPCCDC_COLPTN_CP1PLC1_SHIFT |
+	ISPCCDC_COLPTN_Gr_Cy << ISPCCDC_COLPTN_CP1PLC2_SHIFT |
+	ISPCCDC_COLPTN_R_Ye  << ISPCCDC_COLPTN_CP1PLC3_SHIFT |
+	ISPCCDC_COLPTN_B_Mg  << ISPCCDC_COLPTN_CP2PLC0_SHIFT |
+	ISPCCDC_COLPTN_Gb_G  << ISPCCDC_COLPTN_CP2PLC1_SHIFT |
+	ISPCCDC_COLPTN_B_Mg  << ISPCCDC_COLPTN_CP2PLC2_SHIFT |
+	ISPCCDC_COLPTN_Gb_G  << ISPCCDC_COLPTN_CP2PLC3_SHIFT |
+	ISPCCDC_COLPTN_Gr_Cy << ISPCCDC_COLPTN_CP3PLC0_SHIFT |
+	ISPCCDC_COLPTN_R_Ye  << ISPCCDC_COLPTN_CP3PLC1_SHIFT |
+	ISPCCDC_COLPTN_Gr_Cy << ISPCCDC_COLPTN_CP3PLC2_SHIFT |
+	ISPCCDC_COLPTN_R_Ye  << ISPCCDC_COLPTN_CP3PLC3_SHIFT;
+
+static const u32 ccdc_sgbrg_pattern =
+	ISPCCDC_COLPTN_Gb_G  << ISPCCDC_COLPTN_CP0PLC0_SHIFT |
+	ISPCCDC_COLPTN_B_Mg  << ISPCCDC_COLPTN_CP0PLC1_SHIFT |
+	ISPCCDC_COLPTN_Gb_G  << ISPCCDC_COLPTN_CP0PLC2_SHIFT |
+	ISPCCDC_COLPTN_B_Mg  << ISPCCDC_COLPTN_CP0PLC3_SHIFT |
+	ISPCCDC_COLPTN_R_Ye  << ISPCCDC_COLPTN_CP1PLC0_SHIFT |
+	ISPCCDC_COLPTN_Gr_Cy << ISPCCDC_COLPTN_CP1PLC1_SHIFT |
+	ISPCCDC_COLPTN_R_Ye  << ISPCCDC_COLPTN_CP1PLC2_SHIFT |
+	ISPCCDC_COLPTN_Gr_Cy << ISPCCDC_COLPTN_CP1PLC3_SHIFT |
+	ISPCCDC_COLPTN_Gb_G  << ISPCCDC_COLPTN_CP2PLC0_SHIFT |
+	ISPCCDC_COLPTN_B_Mg  << ISPCCDC_COLPTN_CP2PLC1_SHIFT |
+	ISPCCDC_COLPTN_Gb_G  << ISPCCDC_COLPTN_CP2PLC2_SHIFT |
+	ISPCCDC_COLPTN_B_Mg  << ISPCCDC_COLPTN_CP2PLC3_SHIFT |
+	ISPCCDC_COLPTN_R_Ye  << ISPCCDC_COLPTN_CP3PLC0_SHIFT |
+	ISPCCDC_COLPTN_Gr_Cy << ISPCCDC_COLPTN_CP3PLC1_SHIFT |
+	ISPCCDC_COLPTN_R_Ye  << ISPCCDC_COLPTN_CP3PLC2_SHIFT |
+	ISPCCDC_COLPTN_Gr_Cy << ISPCCDC_COLPTN_CP3PLC3_SHIFT;
+
+static void ccdc_configure(struct isp_ccdc_device *ccdc)
+{
+	struct isp_device *isp = to_isp_device(ccdc);
+	struct isp_parallel_cfg *parcfg = NULL;
+	struct v4l2_subdev *sensor;
+	struct v4l2_mbus_framefmt *format;
+	const struct v4l2_rect *crop;
+	const struct isp_format_info *fmt_info;
+	struct v4l2_subdev_format fmt_src;
+	unsigned int depth_out;
+	unsigned int depth_in = 0;
+	struct media_pad *pad;
+	unsigned long flags;
+	unsigned int bridge;
+	unsigned int shift;
+	unsigned int nph;
+	unsigned int sph;
+	u32 syn_mode;
+	u32 ccdc_pattern;
+
+	ccdc->bt656 = false;
+	ccdc->fields = 0;
+
+	pad = media_entity_remote_pad(&ccdc->pads[CCDC_PAD_SINK]);
+	sensor = media_entity_to_v4l2_subdev(pad->entity);
+	if (ccdc->input == CCDC_INPUT_PARALLEL) {
+		struct v4l2_mbus_config cfg;
+		int ret;
+
+		ret = v4l2_subdev_call(sensor, video, g_mbus_config, &cfg);
+		if (!ret)
+			ccdc->bt656 = cfg.type == V4L2_MBUS_BT656;
+
+		parcfg = &((struct isp_bus_cfg *)sensor->host_priv)
+			->bus.parallel;
+	}
+
+	/* CCDC_PAD_SINK */
+	format = &ccdc->formats[CCDC_PAD_SINK];
+
+	/* Compute the lane shifter shift value and enable the bridge when the
+	 * input format is a non-BT.656 YUV variant.
+	 */
+	fmt_src.pad = pad->index;
+	fmt_src.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	if (!v4l2_subdev_call(sensor, pad, get_fmt, NULL, &fmt_src)) {
+		fmt_info = omap3isp_video_format_info(fmt_src.format.code);
+		depth_in = fmt_info->width;
+	}
+
+	fmt_info = omap3isp_video_format_info(format->code);
+	depth_out = fmt_info->width;
+	shift = depth_in - depth_out;
+
+	if (ccdc->bt656)
+		bridge = ISPCTRL_PAR_BRIDGE_DISABLE;
+	else if (fmt_info->code == MEDIA_BUS_FMT_YUYV8_2X8)
+		bridge = ISPCTRL_PAR_BRIDGE_LENDIAN;
+	else if (fmt_info->code == MEDIA_BUS_FMT_UYVY8_2X8)
+		bridge = ISPCTRL_PAR_BRIDGE_BENDIAN;
+	else
+		bridge = ISPCTRL_PAR_BRIDGE_DISABLE;
+
+	omap3isp_configure_bridge(isp, ccdc->input, parcfg, shift, bridge);
+
+	/* Configure the sync interface. */
+	ccdc_config_sync_if(ccdc, parcfg, depth_out);
+
+	syn_mode = isp_reg_readl(isp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_SYN_MODE);
+
+	/* Use the raw, unprocessed data when writing to memory. The H3A and
+	 * histogram modules are still fed with lens shading corrected data.
+	 */
+	syn_mode &= ~ISPCCDC_SYN_MODE_VP2SDR;
+
+	if (ccdc->output & CCDC_OUTPUT_MEMORY)
+		syn_mode |= ISPCCDC_SYN_MODE_WEN;
+	else
+		syn_mode &= ~ISPCCDC_SYN_MODE_WEN;
+
+	if (ccdc->output & CCDC_OUTPUT_RESIZER)
+		syn_mode |= ISPCCDC_SYN_MODE_SDR2RSZ;
+	else
+		syn_mode &= ~ISPCCDC_SYN_MODE_SDR2RSZ;
+
+	/* Mosaic filter */
+	switch (format->code) {
+	case MEDIA_BUS_FMT_SRGGB10_1X10:
+	case MEDIA_BUS_FMT_SRGGB12_1X12:
+		ccdc_pattern = ccdc_srggb_pattern;
+		break;
+	case MEDIA_BUS_FMT_SBGGR10_1X10:
+	case MEDIA_BUS_FMT_SBGGR12_1X12:
+		ccdc_pattern = ccdc_sbggr_pattern;
+		break;
+	case MEDIA_BUS_FMT_SGBRG10_1X10:
+	case MEDIA_BUS_FMT_SGBRG12_1X12:
+		ccdc_pattern = ccdc_sgbrg_pattern;
+		break;
+	default:
+		/* Use GRBG */
+		ccdc_pattern = ccdc_sgrbg_pattern;
+		break;
+	}
+	ccdc_config_imgattr(ccdc, ccdc_pattern);
+
+	/* Generate VD0 on the last line of the image and VD1 on the
+	 * 2/3 height line.
+	 */
+	isp_reg_writel(isp, ((format->height - 2) << ISPCCDC_VDINT_0_SHIFT) |
+		       ((format->height * 2 / 3) << ISPCCDC_VDINT_1_SHIFT),
+		       OMAP3_ISP_IOMEM_CCDC, ISPCCDC_VDINT);
+
+	/* CCDC_PAD_SOURCE_OF */
+	format = &ccdc->formats[CCDC_PAD_SOURCE_OF];
+	crop = &ccdc->crop;
+
+	/* The horizontal coordinates are expressed in pixel clock cycles. We
+	 * need two cycles per pixel in BT.656 mode, and one cycle per pixel in
+	 * SYNC mode regardless of the format as the bridge is enabled for YUV
+	 * formats in that case.
+	 */
+	if (ccdc->bt656) {
+		sph = crop->left * 2;
+		nph = crop->width * 2 - 1;
+	} else {
+		sph = crop->left;
+		nph = crop->width - 1;
+	}
+
+	isp_reg_writel(isp, (sph << ISPCCDC_HORZ_INFO_SPH_SHIFT) |
+		       (nph << ISPCCDC_HORZ_INFO_NPH_SHIFT),
+		       OMAP3_ISP_IOMEM_CCDC, ISPCCDC_HORZ_INFO);
+	isp_reg_writel(isp, (crop->top << ISPCCDC_VERT_START_SLV0_SHIFT) |
+		       (crop->top << ISPCCDC_VERT_START_SLV1_SHIFT),
+		       OMAP3_ISP_IOMEM_CCDC, ISPCCDC_VERT_START);
+	isp_reg_writel(isp, (crop->height - 1)
+			<< ISPCCDC_VERT_LINES_NLV_SHIFT,
+		       OMAP3_ISP_IOMEM_CCDC, ISPCCDC_VERT_LINES);
+
+	ccdc_config_outlineoffset(ccdc, ccdc->video_out.bpl_value,
+				  format->field);
+
+	/* When interleaving fields enable processing of the field input signal.
+	 * This will cause the line output control module to apply the field
+	 * offset to field 1.
+	 */
+	if (ccdc->formats[CCDC_PAD_SINK].field == V4L2_FIELD_ALTERNATE &&
+	    (format->field == V4L2_FIELD_INTERLACED_TB ||
+	     format->field == V4L2_FIELD_INTERLACED_BT))
+		syn_mode |= ISPCCDC_SYN_MODE_FLDMODE;
+
+	/* The CCDC outputs data in UYVY order by default. Swap bytes to get
+	 * YUYV.
+	 */
+	if (format->code == MEDIA_BUS_FMT_YUYV8_1X16)
+		isp_reg_set(isp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_CFG,
+			    ISPCCDC_CFG_BSWD);
+	else
+		isp_reg_clr(isp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_CFG,
+			    ISPCCDC_CFG_BSWD);
+
+	/* Use PACK8 mode for 1byte per pixel formats. Check for BT.656 mode
+	 * explicitly as the driver reports 1X16 instead of 2X8 at the OF pad
+	 * for simplicity.
+	 */
+	if (omap3isp_video_format_info(format->code)->width <= 8 || ccdc->bt656)
+		syn_mode |= ISPCCDC_SYN_MODE_PACK8;
+	else
+		syn_mode &= ~ISPCCDC_SYN_MODE_PACK8;
+
+	isp_reg_writel(isp, syn_mode, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_SYN_MODE);
+
+	/* CCDC_PAD_SOURCE_VP */
+	ccdc_config_vp(ccdc);
+
+	/* Lens shading correction. */
+	spin_lock_irqsave(&ccdc->lsc.req_lock, flags);
+	if (ccdc->lsc.request == NULL)
+		goto unlock;
+
+	WARN_ON(ccdc->lsc.active);
+
+	/* Get last good LSC configuration. If it is not supported for
+	 * the current active resolution discard it.
+	 */
+	if (ccdc->lsc.active == NULL &&
+	    __ccdc_lsc_configure(ccdc, ccdc->lsc.request) == 0) {
+		ccdc->lsc.active = ccdc->lsc.request;
+	} else {
+		list_add_tail(&ccdc->lsc.request->list, &ccdc->lsc.free_queue);
+		schedule_work(&ccdc->lsc.table_work);
+	}
+
+	ccdc->lsc.request = NULL;
+
+unlock:
+	spin_unlock_irqrestore(&ccdc->lsc.req_lock, flags);
+
+	ccdc_apply_controls(ccdc);
+}
+
+static void __ccdc_enable(struct isp_ccdc_device *ccdc, int enable)
+{
+	struct isp_device *isp = to_isp_device(ccdc);
+
+	isp_reg_clr_set(isp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_PCR,
+			ISPCCDC_PCR_EN, enable ? ISPCCDC_PCR_EN : 0);
+
+	ccdc->running = enable;
+}
+
+static int ccdc_disable(struct isp_ccdc_device *ccdc)
+{
+	unsigned long flags;
+	int ret = 0;
+
+	spin_lock_irqsave(&ccdc->lock, flags);
+	if (ccdc->state == ISP_PIPELINE_STREAM_CONTINUOUS)
+		ccdc->stopping = CCDC_STOP_REQUEST;
+	if (!ccdc->running)
+		ccdc->stopping = CCDC_STOP_FINISHED;
+	spin_unlock_irqrestore(&ccdc->lock, flags);
+
+	ret = wait_event_timeout(ccdc->wait,
+				 ccdc->stopping == CCDC_STOP_FINISHED,
+				 msecs_to_jiffies(2000));
+	if (ret == 0) {
+		ret = -ETIMEDOUT;
+		dev_warn(to_device(ccdc), "CCDC stop timeout!\n");
+	}
+
+	omap3isp_sbl_disable(to_isp_device(ccdc), OMAP3_ISP_SBL_CCDC_LSC_READ);
+
+	mutex_lock(&ccdc->ioctl_lock);
+	ccdc_lsc_free_request(ccdc, ccdc->lsc.request);
+	ccdc->lsc.request = ccdc->lsc.active;
+	ccdc->lsc.active = NULL;
+	cancel_work_sync(&ccdc->lsc.table_work);
+	ccdc_lsc_free_queue(ccdc, &ccdc->lsc.free_queue);
+	mutex_unlock(&ccdc->ioctl_lock);
+
+	ccdc->stopping = CCDC_STOP_NOT_REQUESTED;
+
+	return ret > 0 ? 0 : ret;
+}
+
+static void ccdc_enable(struct isp_ccdc_device *ccdc)
+{
+	if (ccdc_lsc_is_configured(ccdc))
+		__ccdc_lsc_enable(ccdc, 1);
+	__ccdc_enable(ccdc, 1);
+}
+
+/* -----------------------------------------------------------------------------
+ * Interrupt handling
+ */
+
+/*
+ * ccdc_sbl_busy - Poll idle state of CCDC and related SBL memory write bits
+ * @ccdc: Pointer to ISP CCDC device.
+ *
+ * Returns zero if the CCDC is idle and the image has been written to
+ * memory, too.
+ */
+static int ccdc_sbl_busy(struct isp_ccdc_device *ccdc)
+{
+	struct isp_device *isp = to_isp_device(ccdc);
+
+	return omap3isp_ccdc_busy(ccdc)
+		| (isp_reg_readl(isp, OMAP3_ISP_IOMEM_SBL, ISPSBL_CCDC_WR_0) &
+		   ISPSBL_CCDC_WR_0_DATA_READY)
+		| (isp_reg_readl(isp, OMAP3_ISP_IOMEM_SBL, ISPSBL_CCDC_WR_1) &
+		   ISPSBL_CCDC_WR_0_DATA_READY)
+		| (isp_reg_readl(isp, OMAP3_ISP_IOMEM_SBL, ISPSBL_CCDC_WR_2) &
+		   ISPSBL_CCDC_WR_0_DATA_READY)
+		| (isp_reg_readl(isp, OMAP3_ISP_IOMEM_SBL, ISPSBL_CCDC_WR_3) &
+		   ISPSBL_CCDC_WR_0_DATA_READY);
+}
+
+/*
+ * ccdc_sbl_wait_idle - Wait until the CCDC and related SBL are idle
+ * @ccdc: Pointer to ISP CCDC device.
+ * @max_wait: Max retry count in us for wait for idle/busy transition.
+ */
+static int ccdc_sbl_wait_idle(struct isp_ccdc_device *ccdc,
+			      unsigned int max_wait)
+{
+	unsigned int wait = 0;
+
+	if (max_wait == 0)
+		max_wait = 10000; /* 10 ms */
+
+	for (wait = 0; wait <= max_wait; wait++) {
+		if (!ccdc_sbl_busy(ccdc))
+			return 0;
+
+		rmb();
+		udelay(1);
+	}
+
+	return -EBUSY;
+}
+
+/* ccdc_handle_stopping - Handle CCDC and/or LSC stopping sequence
+ * @ccdc: Pointer to ISP CCDC device.
+ * @event: Pointing which event trigger handler
+ *
+ * Return 1 when the event and stopping request combination is satisfied,
+ * zero otherwise.
+ */
+static int ccdc_handle_stopping(struct isp_ccdc_device *ccdc, u32 event)
+{
+	int rval = 0;
+
+	switch ((ccdc->stopping & 3) | event) {
+	case CCDC_STOP_REQUEST | CCDC_EVENT_VD1:
+		if (ccdc->lsc.state != LSC_STATE_STOPPED)
+			__ccdc_lsc_enable(ccdc, 0);
+		__ccdc_enable(ccdc, 0);
+		ccdc->stopping = CCDC_STOP_EXECUTED;
+		return 1;
+
+	case CCDC_STOP_EXECUTED | CCDC_EVENT_VD0:
+		ccdc->stopping |= CCDC_STOP_CCDC_FINISHED;
+		if (ccdc->lsc.state == LSC_STATE_STOPPED)
+			ccdc->stopping |= CCDC_STOP_LSC_FINISHED;
+		rval = 1;
+		break;
+
+	case CCDC_STOP_EXECUTED | CCDC_EVENT_LSC_DONE:
+		ccdc->stopping |= CCDC_STOP_LSC_FINISHED;
+		rval = 1;
+		break;
+
+	case CCDC_STOP_EXECUTED | CCDC_EVENT_VD1:
+		return 1;
+	}
+
+	if (ccdc->stopping == CCDC_STOP_FINISHED) {
+		wake_up(&ccdc->wait);
+		rval = 1;
+	}
+
+	return rval;
+}
+
+static void ccdc_hs_vs_isr(struct isp_ccdc_device *ccdc)
+{
+	struct isp_pipeline *pipe = to_isp_pipeline(&ccdc->subdev.entity);
+	struct video_device *vdev = ccdc->subdev.devnode;
+	struct v4l2_event event;
+
+	/* Frame number propagation */
+	atomic_inc(&pipe->frame_number);
+
+	memset(&event, 0, sizeof(event));
+	event.type = V4L2_EVENT_FRAME_SYNC;
+	event.u.frame_sync.frame_sequence = atomic_read(&pipe->frame_number);
+
+	v4l2_event_queue(vdev, &event);
+}
+
+/*
+ * ccdc_lsc_isr - Handle LSC events
+ * @ccdc: Pointer to ISP CCDC device.
+ * @events: LSC events
+ */
+static void ccdc_lsc_isr(struct isp_ccdc_device *ccdc, u32 events)
+{
+	unsigned long flags;
+
+	if (events & IRQ0STATUS_CCDC_LSC_PREF_ERR_IRQ) {
+		struct isp_pipeline *pipe =
+			to_isp_pipeline(&ccdc->subdev.entity);
+
+		ccdc_lsc_error_handler(ccdc);
+		pipe->error = true;
+		dev_dbg(to_device(ccdc), "lsc prefetch error\n");
+	}
+
+	if (!(events & IRQ0STATUS_CCDC_LSC_DONE_IRQ))
+		return;
+
+	/* LSC_DONE interrupt occur, there are two cases
+	 * 1. stopping for reconfiguration
+	 * 2. stopping because of STREAM OFF command
+	 */
+	spin_lock_irqsave(&ccdc->lsc.req_lock, flags);
+
+	if (ccdc->lsc.state == LSC_STATE_STOPPING)
+		ccdc->lsc.state = LSC_STATE_STOPPED;
+
+	if (ccdc_handle_stopping(ccdc, CCDC_EVENT_LSC_DONE))
+		goto done;
+
+	if (ccdc->lsc.state != LSC_STATE_RECONFIG)
+		goto done;
+
+	/* LSC is in STOPPING state, change to the new state */
+	ccdc->lsc.state = LSC_STATE_STOPPED;
+
+	/* This is an exception. Start of frame and LSC_DONE interrupt
+	 * have been received on the same time. Skip this event and wait
+	 * for better times.
+	 */
+	if (events & IRQ0STATUS_HS_VS_IRQ)
+		goto done;
+
+	/* The LSC engine is stopped at this point. Enable it if there's a
+	 * pending request.
+	 */
+	if (ccdc->lsc.request == NULL)
+		goto done;
+
+	ccdc_lsc_enable(ccdc);
+
+done:
+	spin_unlock_irqrestore(&ccdc->lsc.req_lock, flags);
+}
+
+/*
+ * Check whether the CCDC has captured all fields necessary to complete the
+ * buffer.
+ */
+static bool ccdc_has_all_fields(struct isp_ccdc_device *ccdc)
+{
+	struct isp_pipeline *pipe = to_isp_pipeline(&ccdc->subdev.entity);
+	struct isp_device *isp = to_isp_device(ccdc);
+	enum v4l2_field of_field = ccdc->formats[CCDC_PAD_SOURCE_OF].field;
+	enum v4l2_field field;
+
+	/* When the input is progressive fields don't matter. */
+	if (of_field == V4L2_FIELD_NONE)
+		return true;
+
+	/* Read the current field identifier. */
+	field = isp_reg_readl(isp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_SYN_MODE)
+	      & ISPCCDC_SYN_MODE_FLDSTAT
+	      ? V4L2_FIELD_BOTTOM : V4L2_FIELD_TOP;
+
+	/* When capturing fields in alternate order just store the current field
+	 * identifier in the pipeline.
+	 */
+	if (of_field == V4L2_FIELD_ALTERNATE) {
+		pipe->field = field;
+		return true;
+	}
+
+	/* The format is interlaced. Make sure we've captured both fields. */
+	ccdc->fields |= field == V4L2_FIELD_BOTTOM
+		      ? CCDC_FIELD_BOTTOM : CCDC_FIELD_TOP;
+
+	if (ccdc->fields != CCDC_FIELD_BOTH)
+		return false;
+
+	/* Verify that the field just captured corresponds to the last field
+	 * needed based on the desired field order.
+	 */
+	if ((of_field == V4L2_FIELD_INTERLACED_TB && field == V4L2_FIELD_TOP) ||
+	    (of_field == V4L2_FIELD_INTERLACED_BT && field == V4L2_FIELD_BOTTOM))
+		return false;
+
+	/* The buffer can be completed, reset the fields for the next buffer. */
+	ccdc->fields = 0;
+
+	return true;
+}
+
+static int ccdc_isr_buffer(struct isp_ccdc_device *ccdc)
+{
+	struct isp_pipeline *pipe = to_isp_pipeline(&ccdc->subdev.entity);
+	struct isp_device *isp = to_isp_device(ccdc);
+	struct isp_buffer *buffer;
+
+	/* The CCDC generates VD0 interrupts even when disabled (the datasheet
+	 * doesn't explicitly state if that's supposed to happen or not, so it
+	 * can be considered as a hardware bug or as a feature, but we have to
+	 * deal with it anyway). Disabling the CCDC when no buffer is available
+	 * would thus not be enough, we need to handle the situation explicitly.
+	 */
+	if (list_empty(&ccdc->video_out.dmaqueue))
+		return 0;
+
+	/* We're in continuous mode, and memory writes were disabled due to a
+	 * buffer underrun. Reenable them now that we have a buffer. The buffer
+	 * address has been set in ccdc_video_queue.
+	 */
+	if (ccdc->state == ISP_PIPELINE_STREAM_CONTINUOUS && ccdc->underrun) {
+		ccdc->underrun = 0;
+		return 1;
+	}
+
+	/* Wait for the CCDC to become idle. */
+	if (ccdc_sbl_wait_idle(ccdc, 1000)) {
+		dev_info(isp->dev, "CCDC won't become idle!\n");
+		isp->crashed |= 1U << ccdc->subdev.entity.id;
+		omap3isp_pipeline_cancel_stream(pipe);
+		return 0;
+	}
+
+	if (!ccdc_has_all_fields(ccdc))
+		return 1;
+
+	buffer = omap3isp_video_buffer_next(&ccdc->video_out);
+	if (buffer != NULL)
+		ccdc_set_outaddr(ccdc, buffer->dma);
+
+	pipe->state |= ISP_PIPELINE_IDLE_OUTPUT;
+
+	if (ccdc->state == ISP_PIPELINE_STREAM_SINGLESHOT &&
+	    isp_pipeline_ready(pipe))
+		omap3isp_pipeline_set_stream(pipe,
+					ISP_PIPELINE_STREAM_SINGLESHOT);
+
+	return buffer != NULL;
+}
+
+/*
+ * ccdc_vd0_isr - Handle VD0 event
+ * @ccdc: Pointer to ISP CCDC device.
+ *
+ * Executes LSC deferred enablement before next frame starts.
+ */
+static void ccdc_vd0_isr(struct isp_ccdc_device *ccdc)
+{
+	unsigned long flags;
+	int restart = 0;
+
+	/* In BT.656 mode the CCDC doesn't generate an HS/VS interrupt. We thus
+	 * need to increment the frame counter here.
+	 */
+	if (ccdc->bt656) {
+		struct isp_pipeline *pipe =
+			to_isp_pipeline(&ccdc->subdev.entity);
+
+		atomic_inc(&pipe->frame_number);
+	}
+
+	/* Emulate a VD1 interrupt for BT.656 mode, as we can't stop the CCDC in
+	 * the VD1 interrupt handler in that mode without risking a CCDC stall
+	 * if a short frame is received.
+	 */
+	if (ccdc->bt656) {
+		spin_lock_irqsave(&ccdc->lock, flags);
+		if (ccdc->state == ISP_PIPELINE_STREAM_CONTINUOUS &&
+		    ccdc->output & CCDC_OUTPUT_MEMORY) {
+			if (ccdc->lsc.state != LSC_STATE_STOPPED)
+				__ccdc_lsc_enable(ccdc, 0);
+			__ccdc_enable(ccdc, 0);
+		}
+		ccdc_handle_stopping(ccdc, CCDC_EVENT_VD1);
+		spin_unlock_irqrestore(&ccdc->lock, flags);
+	}
+
+	if (ccdc->output & CCDC_OUTPUT_MEMORY)
+		restart = ccdc_isr_buffer(ccdc);
+
+	spin_lock_irqsave(&ccdc->lock, flags);
+
+	if (ccdc_handle_stopping(ccdc, CCDC_EVENT_VD0)) {
+		spin_unlock_irqrestore(&ccdc->lock, flags);
+		return;
+	}
+
+	if (!ccdc->shadow_update)
+		ccdc_apply_controls(ccdc);
+	spin_unlock_irqrestore(&ccdc->lock, flags);
+
+	if (restart)
+		ccdc_enable(ccdc);
+}
+
+/*
+ * ccdc_vd1_isr - Handle VD1 event
+ * @ccdc: Pointer to ISP CCDC device.
+ */
+static void ccdc_vd1_isr(struct isp_ccdc_device *ccdc)
+{
+	unsigned long flags;
+
+	/* In BT.656 mode the synchronization signals are generated by the CCDC
+	 * from the embedded sync codes. The VD0 and VD1 interrupts are thus
+	 * only triggered when the CCDC is enabled, unlike external sync mode
+	 * where the line counter runs even when the CCDC is stopped. We can't
+	 * disable the CCDC at VD1 time, as no VD0 interrupt would be generated
+	 * for a short frame, which would result in the CCDC being stopped and
+	 * no VD interrupt generated anymore. The CCDC is stopped from the VD0
+	 * interrupt handler instead for BT.656.
+	 */
+	if (ccdc->bt656)
+		return;
+
+	spin_lock_irqsave(&ccdc->lsc.req_lock, flags);
+
+	/*
+	 * Depending on the CCDC pipeline state, CCDC stopping should be
+	 * handled differently. In SINGLESHOT we emulate an internal CCDC
+	 * stopping because the CCDC hw works only in continuous mode.
+	 * When CONTINUOUS pipeline state is used and the CCDC writes it's
+	 * data to memory the CCDC and LSC are stopped immediately but
+	 * without change the CCDC stopping state machine. The CCDC
+	 * stopping state machine should be used only when user request
+	 * for stopping is received (SINGLESHOT is an exeption).
+	 */
+	switch (ccdc->state) {
+	case ISP_PIPELINE_STREAM_SINGLESHOT:
+		ccdc->stopping = CCDC_STOP_REQUEST;
+		break;
+
+	case ISP_PIPELINE_STREAM_CONTINUOUS:
+		if (ccdc->output & CCDC_OUTPUT_MEMORY) {
+			if (ccdc->lsc.state != LSC_STATE_STOPPED)
+				__ccdc_lsc_enable(ccdc, 0);
+			__ccdc_enable(ccdc, 0);
+		}
+		break;
+
+	case ISP_PIPELINE_STREAM_STOPPED:
+		break;
+	}
+
+	if (ccdc_handle_stopping(ccdc, CCDC_EVENT_VD1))
+		goto done;
+
+	if (ccdc->lsc.request == NULL)
+		goto done;
+
+	/*
+	 * LSC need to be reconfigured. Stop it here and on next LSC_DONE IRQ
+	 * do the appropriate changes in registers
+	 */
+	if (ccdc->lsc.state == LSC_STATE_RUNNING) {
+		__ccdc_lsc_enable(ccdc, 0);
+		ccdc->lsc.state = LSC_STATE_RECONFIG;
+		goto done;
+	}
+
+	/* LSC has been in STOPPED state, enable it */
+	if (ccdc->lsc.state == LSC_STATE_STOPPED)
+		ccdc_lsc_enable(ccdc);
+
+done:
+	spin_unlock_irqrestore(&ccdc->lsc.req_lock, flags);
+}
+
+/*
+ * omap3isp_ccdc_isr - Configure CCDC during interframe time.
+ * @ccdc: Pointer to ISP CCDC device.
+ * @events: CCDC events
+ */
+int omap3isp_ccdc_isr(struct isp_ccdc_device *ccdc, u32 events)
+{
+	if (ccdc->state == ISP_PIPELINE_STREAM_STOPPED)
+		return 0;
+
+	if (events & IRQ0STATUS_CCDC_VD1_IRQ)
+		ccdc_vd1_isr(ccdc);
+
+	ccdc_lsc_isr(ccdc, events);
+
+	if (events & IRQ0STATUS_CCDC_VD0_IRQ)
+		ccdc_vd0_isr(ccdc);
+
+	if (events & IRQ0STATUS_HS_VS_IRQ)
+		ccdc_hs_vs_isr(ccdc);
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * ISP video operations
+ */
+
+static int ccdc_video_queue(struct isp_video *video, struct isp_buffer *buffer)
+{
+	struct isp_ccdc_device *ccdc = &video->isp->isp_ccdc;
+	unsigned long flags;
+	bool restart = false;
+
+	if (!(ccdc->output & CCDC_OUTPUT_MEMORY))
+		return -ENODEV;
+
+	ccdc_set_outaddr(ccdc, buffer->dma);
+
+	/* We now have a buffer queued on the output, restart the pipeline
+	 * on the next CCDC interrupt if running in continuous mode (or when
+	 * starting the stream) in external sync mode, or immediately in BT.656
+	 * sync mode as no CCDC interrupt is generated when the CCDC is stopped
+	 * in that case.
+	 */
+	spin_lock_irqsave(&ccdc->lock, flags);
+	if (ccdc->state == ISP_PIPELINE_STREAM_CONTINUOUS && !ccdc->running &&
+	    ccdc->bt656)
+		restart = true;
+	else
+		ccdc->underrun = 1;
+	spin_unlock_irqrestore(&ccdc->lock, flags);
+
+	if (restart)
+		ccdc_enable(ccdc);
+
+	return 0;
+}
+
+static const struct isp_video_operations ccdc_video_ops = {
+	.queue = ccdc_video_queue,
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev operations
+ */
+
+/*
+ * ccdc_ioctl - CCDC module private ioctl's
+ * @sd: ISP CCDC V4L2 subdevice
+ * @cmd: ioctl command
+ * @arg: ioctl argument
+ *
+ * Return 0 on success or a negative error code otherwise.
+ */
+static long ccdc_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
+{
+	struct isp_ccdc_device *ccdc = v4l2_get_subdevdata(sd);
+	int ret;
+
+	switch (cmd) {
+	case VIDIOC_OMAP3ISP_CCDC_CFG:
+		mutex_lock(&ccdc->ioctl_lock);
+		ret = ccdc_config(ccdc, arg);
+		mutex_unlock(&ccdc->ioctl_lock);
+		break;
+
+	default:
+		return -ENOIOCTLCMD;
+	}
+
+	return ret;
+}
+
+static int ccdc_subscribe_event(struct v4l2_subdev *sd, struct v4l2_fh *fh,
+				struct v4l2_event_subscription *sub)
+{
+	if (sub->type != V4L2_EVENT_FRAME_SYNC)
+		return -EINVAL;
+
+	/* line number is zero at frame start */
+	if (sub->id != 0)
+		return -EINVAL;
+
+	return v4l2_event_subscribe(fh, sub, OMAP3ISP_CCDC_NEVENTS, NULL);
+}
+
+static int ccdc_unsubscribe_event(struct v4l2_subdev *sd, struct v4l2_fh *fh,
+				  struct v4l2_event_subscription *sub)
+{
+	return v4l2_event_unsubscribe(fh, sub);
+}
+
+/*
+ * ccdc_set_stream - Enable/Disable streaming on the CCDC module
+ * @sd: ISP CCDC V4L2 subdevice
+ * @enable: Enable/disable stream
+ *
+ * When writing to memory, the CCDC hardware can't be enabled without a memory
+ * buffer to write to. As the s_stream operation is called in response to a
+ * STREAMON call without any buffer queued yet, just update the enabled field
+ * and return immediately. The CCDC will be enabled in ccdc_isr_buffer().
+ *
+ * When not writing to memory enable the CCDC immediately.
+ */
+static int ccdc_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct isp_ccdc_device *ccdc = v4l2_get_subdevdata(sd);
+	struct isp_device *isp = to_isp_device(ccdc);
+	int ret = 0;
+
+	if (ccdc->state == ISP_PIPELINE_STREAM_STOPPED) {
+		if (enable == ISP_PIPELINE_STREAM_STOPPED)
+			return 0;
+
+		omap3isp_subclk_enable(isp, OMAP3_ISP_SUBCLK_CCDC);
+		isp_reg_set(isp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_CFG,
+			    ISPCCDC_CFG_VDLC);
+
+		ccdc_configure(ccdc);
+
+		ccdc_print_status(ccdc);
+	}
+
+	switch (enable) {
+	case ISP_PIPELINE_STREAM_CONTINUOUS:
+		if (ccdc->output & CCDC_OUTPUT_MEMORY)
+			omap3isp_sbl_enable(isp, OMAP3_ISP_SBL_CCDC_WRITE);
+
+		if (ccdc->underrun || !(ccdc->output & CCDC_OUTPUT_MEMORY))
+			ccdc_enable(ccdc);
+
+		ccdc->underrun = 0;
+		break;
+
+	case ISP_PIPELINE_STREAM_SINGLESHOT:
+		if (ccdc->output & CCDC_OUTPUT_MEMORY &&
+		    ccdc->state != ISP_PIPELINE_STREAM_SINGLESHOT)
+			omap3isp_sbl_enable(isp, OMAP3_ISP_SBL_CCDC_WRITE);
+
+		ccdc_enable(ccdc);
+		break;
+
+	case ISP_PIPELINE_STREAM_STOPPED:
+		ret = ccdc_disable(ccdc);
+		if (ccdc->output & CCDC_OUTPUT_MEMORY)
+			omap3isp_sbl_disable(isp, OMAP3_ISP_SBL_CCDC_WRITE);
+		omap3isp_subclk_disable(isp, OMAP3_ISP_SUBCLK_CCDC);
+		ccdc->underrun = 0;
+		break;
+	}
+
+	ccdc->state = enable;
+	return ret;
+}
+
+static struct v4l2_mbus_framefmt *
+__ccdc_get_format(struct isp_ccdc_device *ccdc, struct v4l2_subdev_pad_config *cfg,
+		  unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(&ccdc->subdev, cfg, pad);
+	else
+		return &ccdc->formats[pad];
+}
+
+static struct v4l2_rect *
+__ccdc_get_crop(struct isp_ccdc_device *ccdc, struct v4l2_subdev_pad_config *cfg,
+		enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_crop(&ccdc->subdev, cfg, CCDC_PAD_SOURCE_OF);
+	else
+		return &ccdc->crop;
+}
+
+/*
+ * ccdc_try_format - Try video format on a pad
+ * @ccdc: ISP CCDC device
+ * @cfg : V4L2 subdev pad configuration
+ * @pad: Pad number
+ * @fmt: Format
+ */
+static void
+ccdc_try_format(struct isp_ccdc_device *ccdc, struct v4l2_subdev_pad_config *cfg,
+		unsigned int pad, struct v4l2_mbus_framefmt *fmt,
+		enum v4l2_subdev_format_whence which)
+{
+	const struct isp_format_info *info;
+	u32 pixelcode;
+	unsigned int width = fmt->width;
+	unsigned int height = fmt->height;
+	struct v4l2_rect *crop;
+	enum v4l2_field field;
+	unsigned int i;
+
+	switch (pad) {
+	case CCDC_PAD_SINK:
+		for (i = 0; i < ARRAY_SIZE(ccdc_fmts); i++) {
+			if (fmt->code == ccdc_fmts[i])
+				break;
+		}
+
+		/* If not found, use SGRBG10 as default */
+		if (i >= ARRAY_SIZE(ccdc_fmts))
+			fmt->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+
+		/* Clamp the input size. */
+		fmt->width = clamp_t(u32, width, 32, 4096);
+		fmt->height = clamp_t(u32, height, 32, 4096);
+
+		/* Default to progressive field order. */
+		if (fmt->field == V4L2_FIELD_ANY)
+			fmt->field = V4L2_FIELD_NONE;
+
+		break;
+
+	case CCDC_PAD_SOURCE_OF:
+		pixelcode = fmt->code;
+		field = fmt->field;
+		*fmt = *__ccdc_get_format(ccdc, cfg, CCDC_PAD_SINK, which);
+
+		/* In SYNC mode the bridge converts YUV formats from 2X8 to
+		 * 1X16. In BT.656 no such conversion occurs. As we don't know
+		 * at this point whether the source will use SYNC or BT.656 mode
+		 * let's pretend the conversion always occurs. The CCDC will be
+		 * configured to pack bytes in BT.656, hiding the inaccuracy.
+		 * In all cases bytes can be swapped.
+		 */
+		if (fmt->code == MEDIA_BUS_FMT_YUYV8_2X8 ||
+		    fmt->code == MEDIA_BUS_FMT_UYVY8_2X8) {
+			/* Use the user requested format if YUV. */
+			if (pixelcode == MEDIA_BUS_FMT_YUYV8_2X8 ||
+			    pixelcode == MEDIA_BUS_FMT_UYVY8_2X8 ||
+			    pixelcode == MEDIA_BUS_FMT_YUYV8_1X16 ||
+			    pixelcode == MEDIA_BUS_FMT_UYVY8_1X16)
+				fmt->code = pixelcode;
+
+			if (fmt->code == MEDIA_BUS_FMT_YUYV8_2X8)
+				fmt->code = MEDIA_BUS_FMT_YUYV8_1X16;
+			else if (fmt->code == MEDIA_BUS_FMT_UYVY8_2X8)
+				fmt->code = MEDIA_BUS_FMT_UYVY8_1X16;
+		}
+
+		/* Hardcode the output size to the crop rectangle size. */
+		crop = __ccdc_get_crop(ccdc, cfg, which);
+		fmt->width = crop->width;
+		fmt->height = crop->height;
+
+		/* When input format is interlaced with alternating fields the
+		 * CCDC can interleave the fields.
+		 */
+		if (fmt->field == V4L2_FIELD_ALTERNATE &&
+		    (field == V4L2_FIELD_INTERLACED_TB ||
+		     field == V4L2_FIELD_INTERLACED_BT)) {
+			fmt->field = field;
+			fmt->height *= 2;
+		}
+
+		break;
+
+	case CCDC_PAD_SOURCE_VP:
+		*fmt = *__ccdc_get_format(ccdc, cfg, CCDC_PAD_SINK, which);
+
+		/* The video port interface truncates the data to 10 bits. */
+		info = omap3isp_video_format_info(fmt->code);
+		fmt->code = info->truncated;
+
+		/* YUV formats are not supported by the video port. */
+		if (fmt->code == MEDIA_BUS_FMT_YUYV8_2X8 ||
+		    fmt->code == MEDIA_BUS_FMT_UYVY8_2X8)
+			fmt->code = 0;
+
+		/* The number of lines that can be clocked out from the video
+		 * port output must be at least one line less than the number
+		 * of input lines.
+		 */
+		fmt->width = clamp_t(u32, width, 32, fmt->width);
+		fmt->height = clamp_t(u32, height, 32, fmt->height - 1);
+		break;
+	}
+
+	/* Data is written to memory unpacked, each 10-bit or 12-bit pixel is
+	 * stored on 2 bytes.
+	 */
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+}
+
+/*
+ * ccdc_try_crop - Validate a crop rectangle
+ * @ccdc: ISP CCDC device
+ * @sink: format on the sink pad
+ * @crop: crop rectangle to be validated
+ */
+static void ccdc_try_crop(struct isp_ccdc_device *ccdc,
+			  const struct v4l2_mbus_framefmt *sink,
+			  struct v4l2_rect *crop)
+{
+	const struct isp_format_info *info;
+	unsigned int max_width;
+
+	/* For Bayer formats, restrict left/top and width/height to even values
+	 * to keep the Bayer pattern.
+	 */
+	info = omap3isp_video_format_info(sink->code);
+	if (info->flavor != MEDIA_BUS_FMT_Y8_1X8) {
+		crop->left &= ~1;
+		crop->top &= ~1;
+	}
+
+	crop->left = clamp_t(u32, crop->left, 0, sink->width - CCDC_MIN_WIDTH);
+	crop->top = clamp_t(u32, crop->top, 0, sink->height - CCDC_MIN_HEIGHT);
+
+	/* The data formatter truncates the number of horizontal output pixels
+	 * to a multiple of 16. To avoid clipping data, allow callers to request
+	 * an output size bigger than the input size up to the nearest multiple
+	 * of 16.
+	 */
+	max_width = (sink->width - crop->left + 15) & ~15;
+	crop->width = clamp_t(u32, crop->width, CCDC_MIN_WIDTH, max_width)
+		    & ~15;
+	crop->height = clamp_t(u32, crop->height, CCDC_MIN_HEIGHT,
+			       sink->height - crop->top);
+
+	/* Odd width/height values don't make sense for Bayer formats. */
+	if (info->flavor != MEDIA_BUS_FMT_Y8_1X8) {
+		crop->width &= ~1;
+		crop->height &= ~1;
+	}
+}
+
+/*
+ * ccdc_enum_mbus_code - Handle pixel format enumeration
+ * @sd     : pointer to v4l2 subdev structure
+ * @cfg : V4L2 subdev pad configuration
+ * @code   : pointer to v4l2_subdev_mbus_code_enum structure
+ * return -EINVAL or zero on success
+ */
+static int ccdc_enum_mbus_code(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_pad_config *cfg,
+			       struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct isp_ccdc_device *ccdc = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	switch (code->pad) {
+	case CCDC_PAD_SINK:
+		if (code->index >= ARRAY_SIZE(ccdc_fmts))
+			return -EINVAL;
+
+		code->code = ccdc_fmts[code->index];
+		break;
+
+	case CCDC_PAD_SOURCE_OF:
+		format = __ccdc_get_format(ccdc, cfg, code->pad,
+					   code->which);
+
+		if (format->code == MEDIA_BUS_FMT_YUYV8_2X8 ||
+		    format->code == MEDIA_BUS_FMT_UYVY8_2X8) {
+			/* In YUV mode the CCDC can swap bytes. */
+			if (code->index == 0)
+				code->code = MEDIA_BUS_FMT_YUYV8_1X16;
+			else if (code->index == 1)
+				code->code = MEDIA_BUS_FMT_UYVY8_1X16;
+			else
+				return -EINVAL;
+		} else {
+			/* In raw mode, no configurable format confversion is
+			 * available.
+			 */
+			if (code->index == 0)
+				code->code = format->code;
+			else
+				return -EINVAL;
+		}
+		break;
+
+	case CCDC_PAD_SOURCE_VP:
+		/* The CCDC supports no configurable format conversion
+		 * compatible with the video port. Enumerate a single output
+		 * format code.
+		 */
+		if (code->index != 0)
+			return -EINVAL;
+
+		format = __ccdc_get_format(ccdc, cfg, code->pad,
+					   code->which);
+
+		/* A pixel code equal to 0 means that the video port doesn't
+		 * support the input format. Don't enumerate any pixel code.
+		 */
+		if (format->code == 0)
+			return -EINVAL;
+
+		code->code = format->code;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int ccdc_enum_frame_size(struct v4l2_subdev *sd,
+				struct v4l2_subdev_pad_config *cfg,
+				struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct isp_ccdc_device *ccdc = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt format;
+
+	if (fse->index != 0)
+		return -EINVAL;
+
+	format.code = fse->code;
+	format.width = 1;
+	format.height = 1;
+	ccdc_try_format(ccdc, cfg, fse->pad, &format, fse->which);
+	fse->min_width = format.width;
+	fse->min_height = format.height;
+
+	if (format.code != fse->code)
+		return -EINVAL;
+
+	format.code = fse->code;
+	format.width = -1;
+	format.height = -1;
+	ccdc_try_format(ccdc, cfg, fse->pad, &format, fse->which);
+	fse->max_width = format.width;
+	fse->max_height = format.height;
+
+	return 0;
+}
+
+/*
+ * ccdc_get_selection - Retrieve a selection rectangle on a pad
+ * @sd: ISP CCDC V4L2 subdevice
+ * @cfg: V4L2 subdev pad configuration
+ * @sel: Selection rectangle
+ *
+ * The only supported rectangles are the crop rectangles on the output formatter
+ * source pad.
+ *
+ * Return 0 on success or a negative error code otherwise.
+ */
+static int ccdc_get_selection(struct v4l2_subdev *sd, struct v4l2_subdev_pad_config *cfg,
+			      struct v4l2_subdev_selection *sel)
+{
+	struct isp_ccdc_device *ccdc = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	if (sel->pad != CCDC_PAD_SOURCE_OF)
+		return -EINVAL;
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = INT_MAX;
+		sel->r.height = INT_MAX;
+
+		format = __ccdc_get_format(ccdc, cfg, CCDC_PAD_SINK, sel->which);
+		ccdc_try_crop(ccdc, format, &sel->r);
+		break;
+
+	case V4L2_SEL_TGT_CROP:
+		sel->r = *__ccdc_get_crop(ccdc, cfg, sel->which);
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/*
+ * ccdc_set_selection - Set a selection rectangle on a pad
+ * @sd: ISP CCDC V4L2 subdevice
+ * @cfg: V4L2 subdev pad configuration
+ * @sel: Selection rectangle
+ *
+ * The only supported rectangle is the actual crop rectangle on the output
+ * formatter source pad.
+ *
+ * Return 0 on success or a negative error code otherwise.
+ */
+static int ccdc_set_selection(struct v4l2_subdev *sd, struct v4l2_subdev_pad_config *cfg,
+			      struct v4l2_subdev_selection *sel)
+{
+	struct isp_ccdc_device *ccdc = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	if (sel->target != V4L2_SEL_TGT_CROP ||
+	    sel->pad != CCDC_PAD_SOURCE_OF)
+		return -EINVAL;
+
+	/* The crop rectangle can't be changed while streaming. */
+	if (ccdc->state != ISP_PIPELINE_STREAM_STOPPED)
+		return -EBUSY;
+
+	/* Modifying the crop rectangle always changes the format on the source
+	 * pad. If the KEEP_CONFIG flag is set, just return the current crop
+	 * rectangle.
+	 */
+	if (sel->flags & V4L2_SEL_FLAG_KEEP_CONFIG) {
+		sel->r = *__ccdc_get_crop(ccdc, cfg, sel->which);
+		return 0;
+	}
+
+	format = __ccdc_get_format(ccdc, cfg, CCDC_PAD_SINK, sel->which);
+	ccdc_try_crop(ccdc, format, &sel->r);
+	*__ccdc_get_crop(ccdc, cfg, sel->which) = sel->r;
+
+	/* Update the source format. */
+	format = __ccdc_get_format(ccdc, cfg, CCDC_PAD_SOURCE_OF, sel->which);
+	ccdc_try_format(ccdc, cfg, CCDC_PAD_SOURCE_OF, format, sel->which);
+
+	return 0;
+}
+
+/*
+ * ccdc_get_format - Retrieve the video format on a pad
+ * @sd : ISP CCDC V4L2 subdevice
+ * @cfg: V4L2 subdev pad configuration
+ * @fmt: Format
+ *
+ * Return 0 on success or -EINVAL if the pad is invalid or doesn't correspond
+ * to the format type.
+ */
+static int ccdc_get_format(struct v4l2_subdev *sd, struct v4l2_subdev_pad_config *cfg,
+			   struct v4l2_subdev_format *fmt)
+{
+	struct isp_ccdc_device *ccdc = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __ccdc_get_format(ccdc, cfg, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
+
+	fmt->format = *format;
+	return 0;
+}
+
+/*
+ * ccdc_set_format - Set the video format on a pad
+ * @sd : ISP CCDC V4L2 subdevice
+ * @cfg: V4L2 subdev pad configuration
+ * @fmt: Format
+ *
+ * Return 0 on success or -EINVAL if the pad is invalid or doesn't correspond
+ * to the format type.
+ */
+static int ccdc_set_format(struct v4l2_subdev *sd, struct v4l2_subdev_pad_config *cfg,
+			   struct v4l2_subdev_format *fmt)
+{
+	struct isp_ccdc_device *ccdc = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+	struct v4l2_rect *crop;
+
+	format = __ccdc_get_format(ccdc, cfg, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
+
+	ccdc_try_format(ccdc, cfg, fmt->pad, &fmt->format, fmt->which);
+	*format = fmt->format;
+
+	/* Propagate the format from sink to source */
+	if (fmt->pad == CCDC_PAD_SINK) {
+		/* Reset the crop rectangle. */
+		crop = __ccdc_get_crop(ccdc, cfg, fmt->which);
+		crop->left = 0;
+		crop->top = 0;
+		crop->width = fmt->format.width;
+		crop->height = fmt->format.height;
+
+		ccdc_try_crop(ccdc, &fmt->format, crop);
+
+		/* Update the source formats. */
+		format = __ccdc_get_format(ccdc, cfg, CCDC_PAD_SOURCE_OF,
+					   fmt->which);
+		*format = fmt->format;
+		ccdc_try_format(ccdc, cfg, CCDC_PAD_SOURCE_OF, format,
+				fmt->which);
+
+		format = __ccdc_get_format(ccdc, cfg, CCDC_PAD_SOURCE_VP,
+					   fmt->which);
+		*format = fmt->format;
+		ccdc_try_format(ccdc, cfg, CCDC_PAD_SOURCE_VP, format,
+				fmt->which);
+	}
+
+	return 0;
+}
+
+/*
+ * Decide whether desired output pixel code can be obtained with
+ * the lane shifter by shifting the input pixel code.
+ * @in: input pixelcode to shifter
+ * @out: output pixelcode from shifter
+ * @additional_shift: # of bits the sensor's LSB is offset from CAMEXT[0]
+ *
+ * return true if the combination is possible
+ * return false otherwise
+ */
+static bool ccdc_is_shiftable(u32 in, u32 out, unsigned int additional_shift)
+{
+	const struct isp_format_info *in_info, *out_info;
+
+	if (in == out)
+		return true;
+
+	in_info = omap3isp_video_format_info(in);
+	out_info = omap3isp_video_format_info(out);
+
+	if ((in_info->flavor == 0) || (out_info->flavor == 0))
+		return false;
+
+	if (in_info->flavor != out_info->flavor)
+		return false;
+
+	return in_info->width - out_info->width + additional_shift <= 6;
+}
+
+static int ccdc_link_validate(struct v4l2_subdev *sd,
+			      struct media_link *link,
+			      struct v4l2_subdev_format *source_fmt,
+			      struct v4l2_subdev_format *sink_fmt)
+{
+	struct isp_ccdc_device *ccdc = v4l2_get_subdevdata(sd);
+	unsigned long parallel_shift;
+
+	/* Check if the two ends match */
+	if (source_fmt->format.width != sink_fmt->format.width ||
+	    source_fmt->format.height != sink_fmt->format.height)
+		return -EPIPE;
+
+	/* We've got a parallel sensor here. */
+	if (ccdc->input == CCDC_INPUT_PARALLEL) {
+		struct isp_parallel_cfg *parcfg =
+			&((struct isp_bus_cfg *)
+			  media_entity_to_v4l2_subdev(link->source->entity)
+			  ->host_priv)->bus.parallel;
+		parallel_shift = parcfg->data_lane_shift * 2;
+	} else {
+		parallel_shift = 0;
+	}
+
+	/* Lane shifter may be used to drop bits on CCDC sink pad */
+	if (!ccdc_is_shiftable(source_fmt->format.code,
+			       sink_fmt->format.code, parallel_shift))
+		return -EPIPE;
+
+	return 0;
+}
+
+/*
+ * ccdc_init_formats - Initialize formats on all pads
+ * @sd: ISP CCDC V4L2 subdevice
+ * @fh: V4L2 subdev file handle
+ *
+ * Initialize all pad formats with default values. If fh is not NULL, try
+ * formats are initialized on the file handle. Otherwise active formats are
+ * initialized on the device.
+ */
+static int ccdc_init_formats(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_subdev_format format;
+
+	memset(&format, 0, sizeof(format));
+	format.pad = CCDC_PAD_SINK;
+	format.which = fh ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE;
+	format.format.code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	format.format.width = 4096;
+	format.format.height = 4096;
+	ccdc_set_format(sd, fh ? fh->pad : NULL, &format);
+
+	return 0;
+}
+
+/* V4L2 subdev core operations */
+static const struct v4l2_subdev_core_ops ccdc_v4l2_core_ops = {
+	.ioctl = ccdc_ioctl,
+	.subscribe_event = ccdc_subscribe_event,
+	.unsubscribe_event = ccdc_unsubscribe_event,
+};
+
+/* V4L2 subdev video operations */
+static const struct v4l2_subdev_video_ops ccdc_v4l2_video_ops = {
+	.s_stream = ccdc_set_stream,
+};
+
+/* V4L2 subdev pad operations */
+static const struct v4l2_subdev_pad_ops ccdc_v4l2_pad_ops = {
+	.enum_mbus_code = ccdc_enum_mbus_code,
+	.enum_frame_size = ccdc_enum_frame_size,
+	.get_fmt = ccdc_get_format,
+	.set_fmt = ccdc_set_format,
+	.get_selection = ccdc_get_selection,
+	.set_selection = ccdc_set_selection,
+	.link_validate = ccdc_link_validate,
+};
+
+/* V4L2 subdev operations */
+static const struct v4l2_subdev_ops ccdc_v4l2_ops = {
+	.core = &ccdc_v4l2_core_ops,
+	.video = &ccdc_v4l2_video_ops,
+	.pad = &ccdc_v4l2_pad_ops,
+};
+
+/* V4L2 subdev internal operations */
+static const struct v4l2_subdev_internal_ops ccdc_v4l2_internal_ops = {
+	.open = ccdc_init_formats,
+};
+
+/* -----------------------------------------------------------------------------
+ * Media entity operations
+ */
+
+/*
+ * ccdc_link_setup - Setup CCDC connections
+ * @entity: CCDC media entity
+ * @local: Pad at the local end of the link
+ * @remote: Pad at the remote end of the link
+ * @flags: Link flags
+ *
+ * return -EINVAL or zero on success
+ */
+static int ccdc_link_setup(struct media_entity *entity,
+			   const struct media_pad *local,
+			   const struct media_pad *remote, u32 flags)
+{
+	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+	struct isp_ccdc_device *ccdc = v4l2_get_subdevdata(sd);
+	struct isp_device *isp = to_isp_device(ccdc);
+
+	switch (local->index | media_entity_type(remote->entity)) {
+	case CCDC_PAD_SINK | MEDIA_ENT_T_V4L2_SUBDEV:
+		/* Read from the sensor (parallel interface), CCP2, CSI2a or
+		 * CSI2c.
+		 */
+		if (!(flags & MEDIA_LNK_FL_ENABLED)) {
+			ccdc->input = CCDC_INPUT_NONE;
+			break;
+		}
+
+		if (ccdc->input != CCDC_INPUT_NONE)
+			return -EBUSY;
+
+		if (remote->entity == &isp->isp_ccp2.subdev.entity)
+			ccdc->input = CCDC_INPUT_CCP2B;
+		else if (remote->entity == &isp->isp_csi2a.subdev.entity)
+			ccdc->input = CCDC_INPUT_CSI2A;
+		else if (remote->entity == &isp->isp_csi2c.subdev.entity)
+			ccdc->input = CCDC_INPUT_CSI2C;
+		else
+			ccdc->input = CCDC_INPUT_PARALLEL;
+
+		break;
+
+	/*
+	 * The ISP core doesn't support pipelines with multiple video outputs.
+	 * Revisit this when it will be implemented, and return -EBUSY for now.
+	 */
+
+	case CCDC_PAD_SOURCE_VP | MEDIA_ENT_T_V4L2_SUBDEV:
+		/* Write to preview engine, histogram and H3A. When none of
+		 * those links are active, the video port can be disabled.
+		 */
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (ccdc->output & ~CCDC_OUTPUT_PREVIEW)
+				return -EBUSY;
+			ccdc->output |= CCDC_OUTPUT_PREVIEW;
+		} else {
+			ccdc->output &= ~CCDC_OUTPUT_PREVIEW;
+		}
+		break;
+
+	case CCDC_PAD_SOURCE_OF | MEDIA_ENT_T_DEVNODE:
+		/* Write to memory */
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (ccdc->output & ~CCDC_OUTPUT_MEMORY)
+				return -EBUSY;
+			ccdc->output |= CCDC_OUTPUT_MEMORY;
+		} else {
+			ccdc->output &= ~CCDC_OUTPUT_MEMORY;
+		}
+		break;
+
+	case CCDC_PAD_SOURCE_OF | MEDIA_ENT_T_V4L2_SUBDEV:
+		/* Write to resizer */
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (ccdc->output & ~CCDC_OUTPUT_RESIZER)
+				return -EBUSY;
+			ccdc->output |= CCDC_OUTPUT_RESIZER;
+		} else {
+			ccdc->output &= ~CCDC_OUTPUT_RESIZER;
+		}
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* media operations */
+static const struct media_entity_operations ccdc_media_ops = {
+	.link_setup = ccdc_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+void omap3isp_ccdc_unregister_entities(struct isp_ccdc_device *ccdc)
+{
+	v4l2_device_unregister_subdev(&ccdc->subdev);
+	omap3isp_video_unregister(&ccdc->video_out);
+}
+
+int omap3isp_ccdc_register_entities(struct isp_ccdc_device *ccdc,
+	struct v4l2_device *vdev)
+{
+	int ret;
+
+	/* Register the subdev and video node. */
+	ret = v4l2_device_register_subdev(vdev, &ccdc->subdev);
+	if (ret < 0)
+		goto error;
+
+	ret = omap3isp_video_register(&ccdc->video_out, vdev);
+	if (ret < 0)
+		goto error;
+
+	return 0;
+
+error:
+	omap3isp_ccdc_unregister_entities(ccdc);
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * ISP CCDC initialisation and cleanup
+ */
+
+/*
+ * ccdc_init_entities - Initialize V4L2 subdev and media entity
+ * @ccdc: ISP CCDC module
+ *
+ * Return 0 on success and a negative error code on failure.
+ */
+static int ccdc_init_entities(struct isp_ccdc_device *ccdc)
+{
+	struct v4l2_subdev *sd = &ccdc->subdev;
+	struct media_pad *pads = ccdc->pads;
+	struct media_entity *me = &sd->entity;
+	int ret;
+
+	ccdc->input = CCDC_INPUT_NONE;
+
+	v4l2_subdev_init(sd, &ccdc_v4l2_ops);
+	sd->internal_ops = &ccdc_v4l2_internal_ops;
+	strlcpy(sd->name, "OMAP3 ISP CCDC", sizeof(sd->name));
+	sd->grp_id = 1 << 16;	/* group ID for isp subdevs */
+	v4l2_set_subdevdata(sd, ccdc);
+	sd->flags |= V4L2_SUBDEV_FL_HAS_EVENTS | V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	pads[CCDC_PAD_SINK].flags = MEDIA_PAD_FL_SINK
+				    | MEDIA_PAD_FL_MUST_CONNECT;
+	pads[CCDC_PAD_SOURCE_VP].flags = MEDIA_PAD_FL_SOURCE;
+	pads[CCDC_PAD_SOURCE_OF].flags = MEDIA_PAD_FL_SOURCE;
+
+	me->ops = &ccdc_media_ops;
+	ret = media_entity_init(me, CCDC_PADS_NUM, pads, 0);
+	if (ret < 0)
+		return ret;
+
+	ccdc_init_formats(sd, NULL);
+
+	ccdc->video_out.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	ccdc->video_out.ops = &ccdc_video_ops;
+	ccdc->video_out.isp = to_isp_device(ccdc);
+	ccdc->video_out.capture_mem = PAGE_ALIGN(4096 * 4096) * 3;
+	ccdc->video_out.bpl_alignment = 32;
+
+	ret = omap3isp_video_init(&ccdc->video_out, "CCDC");
+	if (ret < 0)
+		goto error_video;
+
+	/* Connect the CCDC subdev to the video node. */
+	ret = media_entity_create_link(&ccdc->subdev.entity, CCDC_PAD_SOURCE_OF,
+			&ccdc->video_out.video.entity, 0, 0);
+	if (ret < 0)
+		goto error_link;
+
+	return 0;
+
+error_link:
+	omap3isp_video_cleanup(&ccdc->video_out);
+error_video:
+	media_entity_cleanup(me);
+	return ret;
+}
+
+/*
+ * omap3isp_ccdc_init - CCDC module initialization.
+ * @isp: Device pointer specific to the OMAP3 ISP.
+ *
+ * TODO: Get the initialisation values from platform data.
+ *
+ * Return 0 on success or a negative error code otherwise.
+ */
+int omap3isp_ccdc_init(struct isp_device *isp)
+{
+	struct isp_ccdc_device *ccdc = &isp->isp_ccdc;
+	int ret;
+
+	spin_lock_init(&ccdc->lock);
+	init_waitqueue_head(&ccdc->wait);
+	mutex_init(&ccdc->ioctl_lock);
+
+	ccdc->stopping = CCDC_STOP_NOT_REQUESTED;
+
+	INIT_WORK(&ccdc->lsc.table_work, ccdc_lsc_free_table_work);
+	ccdc->lsc.state = LSC_STATE_STOPPED;
+	INIT_LIST_HEAD(&ccdc->lsc.free_queue);
+	spin_lock_init(&ccdc->lsc.req_lock);
+
+	ccdc->clamp.oblen = 0;
+	ccdc->clamp.dcsubval = 0;
+
+	ccdc->update = OMAP3ISP_CCDC_BLCLAMP;
+	ccdc_apply_controls(ccdc);
+
+	ret = ccdc_init_entities(ccdc);
+	if (ret < 0) {
+		mutex_destroy(&ccdc->ioctl_lock);
+		return ret;
+	}
+
+	return 0;
+}
+
+/*
+ * omap3isp_ccdc_cleanup - CCDC module cleanup.
+ * @isp: Device pointer specific to the OMAP3 ISP.
+ */
+void omap3isp_ccdc_cleanup(struct isp_device *isp)
+{
+	struct isp_ccdc_device *ccdc = &isp->isp_ccdc;
+
+	omap3isp_video_cleanup(&ccdc->video_out);
+	media_entity_cleanup(&ccdc->subdev.entity);
+
+	/* Free LSC requests. As the CCDC is stopped there's no active request,
+	 * so only the pending request and the free queue need to be handled.
+	 */
+	ccdc_lsc_free_request(ccdc, ccdc->lsc.request);
+	cancel_work_sync(&ccdc->lsc.table_work);
+	ccdc_lsc_free_queue(ccdc, &ccdc->lsc.free_queue);
+
+	if (ccdc->fpc.addr != NULL)
+		dma_free_coherent(isp->dev, ccdc->fpc.fpnum * 4, ccdc->fpc.addr,
+				  ccdc->fpc.dma);
+
+	mutex_destroy(&ccdc->ioctl_lock);
+}
diff --git a/drivers/media/platform/omap3isp/ispccdc.h b/drivers/media/platform/omap3isp/ispccdc.h
new file mode 100644
index 0000000..3440a70
--- /dev/null
+++ b/drivers/media/platform/omap3isp/ispccdc.h
@@ -0,0 +1,177 @@
+/*
+ * ispccdc.h
+ *
+ * TI OMAP3 ISP - CCDC module
+ *
+ * Copyright (C) 2009-2010 Nokia Corporation
+ * Copyright (C) 2009 Texas Instruments, Inc.
+ *
+ * Contacts: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *	     Sakari Ailus <sakari.ailus@iki.fi>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef OMAP3_ISP_CCDC_H
+#define OMAP3_ISP_CCDC_H
+
+#include <linux/omap3isp.h>
+#include <linux/workqueue.h>
+
+#include "ispvideo.h"
+
+enum ccdc_input_entity {
+	CCDC_INPUT_NONE,
+	CCDC_INPUT_PARALLEL,
+	CCDC_INPUT_CSI2A,
+	CCDC_INPUT_CCP2B,
+	CCDC_INPUT_CSI2C
+};
+
+#define CCDC_OUTPUT_MEMORY	(1 << 0)
+#define CCDC_OUTPUT_PREVIEW	(1 << 1)
+#define CCDC_OUTPUT_RESIZER	(1 << 2)
+
+#define	OMAP3ISP_CCDC_NEVENTS	16
+
+struct ispccdc_fpc {
+	void *addr;
+	dma_addr_t dma;
+	unsigned int fpnum;
+};
+
+enum ispccdc_lsc_state {
+	LSC_STATE_STOPPED = 0,
+	LSC_STATE_STOPPING = 1,
+	LSC_STATE_RUNNING = 2,
+	LSC_STATE_RECONFIG = 3,
+};
+
+struct ispccdc_lsc_config_req {
+	struct list_head list;
+	struct omap3isp_ccdc_lsc_config config;
+	unsigned char enable;
+
+	struct {
+		void *addr;
+		dma_addr_t dma;
+		struct sg_table sgt;
+	} table;
+};
+
+/*
+ * ispccdc_lsc - CCDC LSC parameters
+ */
+struct ispccdc_lsc {
+	enum ispccdc_lsc_state state;
+	struct work_struct table_work;
+
+	/* LSC queue of configurations */
+	spinlock_t req_lock;
+	struct ispccdc_lsc_config_req *request;	/* requested configuration */
+	struct ispccdc_lsc_config_req *active;	/* active configuration */
+	struct list_head free_queue;	/* configurations for freeing */
+};
+
+#define CCDC_STOP_NOT_REQUESTED		0x00
+#define CCDC_STOP_REQUEST		0x01
+#define CCDC_STOP_EXECUTED		(0x02 | CCDC_STOP_REQUEST)
+#define CCDC_STOP_CCDC_FINISHED		0x04
+#define CCDC_STOP_LSC_FINISHED		0x08
+#define CCDC_STOP_FINISHED		\
+	(CCDC_STOP_EXECUTED | CCDC_STOP_CCDC_FINISHED | CCDC_STOP_LSC_FINISHED)
+
+#define CCDC_EVENT_VD1			0x10
+#define CCDC_EVENT_VD0			0x20
+#define CCDC_EVENT_LSC_DONE		0x40
+
+/* Sink and source CCDC pads */
+#define CCDC_PAD_SINK			0
+#define CCDC_PAD_SOURCE_OF		1
+#define CCDC_PAD_SOURCE_VP		2
+#define CCDC_PADS_NUM			3
+
+#define CCDC_FIELD_TOP			1
+#define CCDC_FIELD_BOTTOM		2
+#define CCDC_FIELD_BOTH			3
+
+/*
+ * struct isp_ccdc_device - Structure for the CCDC module to store its own
+ *			    information
+ * @subdev: V4L2 subdevice
+ * @pads: Sink and source media entity pads
+ * @formats: Active video formats
+ * @crop: Active crop rectangle on the OF source pad
+ * @input: Active input
+ * @output: Active outputs
+ * @video_out: Output video node
+ * @alaw: A-law compression enabled (1) or disabled (0)
+ * @lpf: Low pass filter enabled (1) or disabled (0)
+ * @obclamp: Optical-black clamp enabled (1) or disabled (0)
+ * @fpc_en: Faulty pixels correction enabled (1) or disabled (0)
+ * @blcomp: Black level compensation configuration
+ * @clamp: Optical-black or digital clamp configuration
+ * @fpc: Faulty pixels correction configuration
+ * @lsc: Lens shading compensation configuration
+ * @update: Bitmask of controls to update during the next interrupt
+ * @shadow_update: Controls update in progress by userspace
+ * @bt656: Whether the input interface uses BT.656 synchronization
+ * @fields: The fields (CCDC_FIELD_*) stored in the current buffer
+ * @underrun: A buffer underrun occurred and a new buffer has been queued
+ * @state: Streaming state
+ * @lock: Serializes shadow_update with interrupt handler
+ * @wait: Wait queue used to stop the module
+ * @stopping: Stopping state
+ * @running: Is the CCDC hardware running
+ * @ioctl_lock: Serializes ioctl calls and LSC requests freeing
+ */
+struct isp_ccdc_device {
+	struct v4l2_subdev subdev;
+	struct media_pad pads[CCDC_PADS_NUM];
+	struct v4l2_mbus_framefmt formats[CCDC_PADS_NUM];
+	struct v4l2_rect crop;
+
+	enum ccdc_input_entity input;
+	unsigned int output;
+	struct isp_video video_out;
+
+	unsigned int alaw:1,
+		     lpf:1,
+		     obclamp:1,
+		     fpc_en:1;
+	struct omap3isp_ccdc_blcomp blcomp;
+	struct omap3isp_ccdc_bclamp clamp;
+	struct ispccdc_fpc fpc;
+	struct ispccdc_lsc lsc;
+	unsigned int update;
+	unsigned int shadow_update;
+
+	bool bt656;
+	unsigned int fields;
+
+	unsigned int underrun:1;
+	enum isp_pipeline_stream_state state;
+	spinlock_t lock;
+	wait_queue_head_t wait;
+	unsigned int stopping;
+	bool running;
+	struct mutex ioctl_lock;
+};
+
+struct isp_device;
+
+int omap3isp_ccdc_init(struct isp_device *isp);
+void omap3isp_ccdc_cleanup(struct isp_device *isp);
+int omap3isp_ccdc_register_entities(struct isp_ccdc_device *ccdc,
+	struct v4l2_device *vdev);
+void omap3isp_ccdc_unregister_entities(struct isp_ccdc_device *ccdc);
+
+int omap3isp_ccdc_busy(struct isp_ccdc_device *isp_ccdc);
+int omap3isp_ccdc_isr(struct isp_ccdc_device *isp_ccdc, u32 events);
+void omap3isp_ccdc_restore_context(struct isp_device *isp);
+void omap3isp_ccdc_max_rate(struct isp_ccdc_device *ccdc,
+	unsigned int *max_rate);
+
+#endif	/* OMAP3_ISP_CCDC_H */
diff --git a/drivers/media/platform/omap3isp/ispccp2.c b/drivers/media/platform/omap3isp/ispccp2.c
new file mode 100644
index 0000000..38e6a97
--- /dev/null
+++ b/drivers/media/platform/omap3isp/ispccp2.c
@@ -0,0 +1,1169 @@
+/*
+ * ispccp2.c
+ *
+ * TI OMAP3 ISP - CCP2 module
+ *
+ * Copyright (C) 2010 Nokia Corporation
+ * Copyright (C) 2010 Texas Instruments, Inc.
+ *
+ * Contacts: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *	     Sakari Ailus <sakari.ailus@iki.fi>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/mm.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/uaccess.h>
+#include <linux/regulator/consumer.h>
+
+#include "isp.h"
+#include "ispreg.h"
+#include "ispccp2.h"
+
+/* Number of LCX channels */
+#define CCP2_LCx_CHANS_NUM			3
+/* Max/Min size for CCP2 video port */
+#define ISPCCP2_DAT_START_MIN			0
+#define ISPCCP2_DAT_START_MAX			4095
+#define ISPCCP2_DAT_SIZE_MIN			0
+#define ISPCCP2_DAT_SIZE_MAX			4095
+#define ISPCCP2_VPCLK_FRACDIV			65536
+#define ISPCCP2_LCx_CTRL_FORMAT_RAW8_DPCM10_VP	0x12
+#define ISPCCP2_LCx_CTRL_FORMAT_RAW10_VP	0x16
+/* Max/Min size for CCP2 memory channel */
+#define ISPCCP2_LCM_HSIZE_COUNT_MIN		16
+#define ISPCCP2_LCM_HSIZE_COUNT_MAX		8191
+#define ISPCCP2_LCM_HSIZE_SKIP_MIN		0
+#define ISPCCP2_LCM_HSIZE_SKIP_MAX		8191
+#define ISPCCP2_LCM_VSIZE_MIN			1
+#define ISPCCP2_LCM_VSIZE_MAX			8191
+#define ISPCCP2_LCM_HWORDS_MIN			1
+#define ISPCCP2_LCM_HWORDS_MAX			4095
+#define ISPCCP2_LCM_CTRL_BURST_SIZE_32X		5
+#define ISPCCP2_LCM_CTRL_READ_THROTTLE_FULL	0
+#define ISPCCP2_LCM_CTRL_SRC_DECOMPR_DPCM10	2
+#define ISPCCP2_LCM_CTRL_SRC_FORMAT_RAW8	2
+#define ISPCCP2_LCM_CTRL_SRC_FORMAT_RAW10	3
+#define ISPCCP2_LCM_CTRL_DST_FORMAT_RAW10	3
+#define ISPCCP2_LCM_CTRL_DST_PORT_VP		0
+#define ISPCCP2_LCM_CTRL_DST_PORT_MEM		1
+
+/* Set only the required bits */
+#define BIT_SET(var, shift, mask, val)			\
+	do {						\
+		var = ((var) & ~((mask) << (shift)))	\
+			| ((val) << (shift));		\
+	} while (0)
+
+/*
+ * ccp2_print_status - Print current CCP2 module register values.
+ */
+#define CCP2_PRINT_REGISTER(isp, name)\
+	dev_dbg(isp->dev, "###CCP2 " #name "=0x%08x\n", \
+		isp_reg_readl(isp, OMAP3_ISP_IOMEM_CCP2, ISPCCP2_##name))
+
+static void ccp2_print_status(struct isp_ccp2_device *ccp2)
+{
+	struct isp_device *isp = to_isp_device(ccp2);
+
+	dev_dbg(isp->dev, "-------------CCP2 Register dump-------------\n");
+
+	CCP2_PRINT_REGISTER(isp, SYSCONFIG);
+	CCP2_PRINT_REGISTER(isp, SYSSTATUS);
+	CCP2_PRINT_REGISTER(isp, LC01_IRQENABLE);
+	CCP2_PRINT_REGISTER(isp, LC01_IRQSTATUS);
+	CCP2_PRINT_REGISTER(isp, LC23_IRQENABLE);
+	CCP2_PRINT_REGISTER(isp, LC23_IRQSTATUS);
+	CCP2_PRINT_REGISTER(isp, LCM_IRQENABLE);
+	CCP2_PRINT_REGISTER(isp, LCM_IRQSTATUS);
+	CCP2_PRINT_REGISTER(isp, CTRL);
+	CCP2_PRINT_REGISTER(isp, LCx_CTRL(0));
+	CCP2_PRINT_REGISTER(isp, LCx_CODE(0));
+	CCP2_PRINT_REGISTER(isp, LCx_STAT_START(0));
+	CCP2_PRINT_REGISTER(isp, LCx_STAT_SIZE(0));
+	CCP2_PRINT_REGISTER(isp, LCx_SOF_ADDR(0));
+	CCP2_PRINT_REGISTER(isp, LCx_EOF_ADDR(0));
+	CCP2_PRINT_REGISTER(isp, LCx_DAT_START(0));
+	CCP2_PRINT_REGISTER(isp, LCx_DAT_SIZE(0));
+	CCP2_PRINT_REGISTER(isp, LCx_DAT_PING_ADDR(0));
+	CCP2_PRINT_REGISTER(isp, LCx_DAT_PONG_ADDR(0));
+	CCP2_PRINT_REGISTER(isp, LCx_DAT_OFST(0));
+	CCP2_PRINT_REGISTER(isp, LCM_CTRL);
+	CCP2_PRINT_REGISTER(isp, LCM_VSIZE);
+	CCP2_PRINT_REGISTER(isp, LCM_HSIZE);
+	CCP2_PRINT_REGISTER(isp, LCM_PREFETCH);
+	CCP2_PRINT_REGISTER(isp, LCM_SRC_ADDR);
+	CCP2_PRINT_REGISTER(isp, LCM_SRC_OFST);
+	CCP2_PRINT_REGISTER(isp, LCM_DST_ADDR);
+	CCP2_PRINT_REGISTER(isp, LCM_DST_OFST);
+
+	dev_dbg(isp->dev, "--------------------------------------------\n");
+}
+
+/*
+ * ccp2_reset - Reset the CCP2
+ * @ccp2: pointer to ISP CCP2 device
+ */
+static void ccp2_reset(struct isp_ccp2_device *ccp2)
+{
+	struct isp_device *isp = to_isp_device(ccp2);
+	int i = 0;
+
+	/* Reset the CSI1/CCP2B and wait for reset to complete */
+	isp_reg_set(isp, OMAP3_ISP_IOMEM_CCP2, ISPCCP2_SYSCONFIG,
+		    ISPCCP2_SYSCONFIG_SOFT_RESET);
+	while (!(isp_reg_readl(isp, OMAP3_ISP_IOMEM_CCP2, ISPCCP2_SYSSTATUS) &
+		 ISPCCP2_SYSSTATUS_RESET_DONE)) {
+		udelay(10);
+		if (i++ > 10) {  /* try read 10 times */
+			dev_warn(isp->dev,
+				"omap3_isp: timeout waiting for ccp2 reset\n");
+			break;
+		}
+	}
+}
+
+/*
+ * ccp2_pwr_cfg - Configure the power mode settings
+ * @ccp2: pointer to ISP CCP2 device
+ */
+static void ccp2_pwr_cfg(struct isp_ccp2_device *ccp2)
+{
+	struct isp_device *isp = to_isp_device(ccp2);
+
+	isp_reg_writel(isp, ISPCCP2_SYSCONFIG_MSTANDBY_MODE_SMART |
+			((isp->revision == ISP_REVISION_15_0 && isp->autoidle) ?
+			  ISPCCP2_SYSCONFIG_AUTO_IDLE : 0),
+		       OMAP3_ISP_IOMEM_CCP2, ISPCCP2_SYSCONFIG);
+}
+
+/*
+ * ccp2_if_enable - Enable CCP2 interface.
+ * @ccp2: pointer to ISP CCP2 device
+ * @enable: enable/disable flag
+ */
+static int ccp2_if_enable(struct isp_ccp2_device *ccp2, u8 enable)
+{
+	struct isp_device *isp = to_isp_device(ccp2);
+	int ret;
+	int i;
+
+	if (enable && ccp2->vdds_csib) {
+		ret = regulator_enable(ccp2->vdds_csib);
+		if (ret < 0)
+			return ret;
+	}
+
+	/* Enable/Disable all the LCx channels */
+	for (i = 0; i < CCP2_LCx_CHANS_NUM; i++)
+		isp_reg_clr_set(isp, OMAP3_ISP_IOMEM_CCP2, ISPCCP2_LCx_CTRL(i),
+				ISPCCP2_LCx_CTRL_CHAN_EN,
+				enable ? ISPCCP2_LCx_CTRL_CHAN_EN : 0);
+
+	/* Enable/Disable ccp2 interface in ccp2 mode */
+	isp_reg_clr_set(isp, OMAP3_ISP_IOMEM_CCP2, ISPCCP2_CTRL,
+			ISPCCP2_CTRL_MODE | ISPCCP2_CTRL_IF_EN,
+			enable ? (ISPCCP2_CTRL_MODE | ISPCCP2_CTRL_IF_EN) : 0);
+
+	if (!enable && ccp2->vdds_csib)
+		regulator_disable(ccp2->vdds_csib);
+
+	return 0;
+}
+
+/*
+ * ccp2_mem_enable - Enable CCP2 memory interface.
+ * @ccp2: pointer to ISP CCP2 device
+ * @enable: enable/disable flag
+ */
+static void ccp2_mem_enable(struct isp_ccp2_device *ccp2, u8 enable)
+{
+	struct isp_device *isp = to_isp_device(ccp2);
+
+	if (enable)
+		ccp2_if_enable(ccp2, 0);
+
+	/* Enable/Disable ccp2 interface in ccp2 mode */
+	isp_reg_clr_set(isp, OMAP3_ISP_IOMEM_CCP2, ISPCCP2_CTRL,
+			ISPCCP2_CTRL_MODE, enable ? ISPCCP2_CTRL_MODE : 0);
+
+	isp_reg_clr_set(isp, OMAP3_ISP_IOMEM_CCP2, ISPCCP2_LCM_CTRL,
+			ISPCCP2_LCM_CTRL_CHAN_EN,
+			enable ? ISPCCP2_LCM_CTRL_CHAN_EN : 0);
+}
+
+/*
+ * ccp2_phyif_config - Initialize CCP2 phy interface config
+ * @ccp2: Pointer to ISP CCP2 device
+ * @buscfg: CCP2 platform data
+ *
+ * Configure the CCP2 physical interface module from platform data.
+ *
+ * Returns -EIO if strobe is chosen in CSI1 mode, or 0 on success.
+ */
+static int ccp2_phyif_config(struct isp_ccp2_device *ccp2,
+			     const struct isp_ccp2_cfg *buscfg)
+{
+	struct isp_device *isp = to_isp_device(ccp2);
+	u32 val;
+
+	/* CCP2B mode */
+	val = isp_reg_readl(isp, OMAP3_ISP_IOMEM_CCP2, ISPCCP2_CTRL) |
+			    ISPCCP2_CTRL_IO_OUT_SEL | ISPCCP2_CTRL_MODE;
+	/* Data/strobe physical layer */
+	BIT_SET(val, ISPCCP2_CTRL_PHY_SEL_SHIFT, ISPCCP2_CTRL_PHY_SEL_MASK,
+		buscfg->phy_layer);
+	BIT_SET(val, ISPCCP2_CTRL_INV_SHIFT, ISPCCP2_CTRL_INV_MASK,
+		buscfg->strobe_clk_pol);
+	isp_reg_writel(isp, val, OMAP3_ISP_IOMEM_CCP2, ISPCCP2_CTRL);
+
+	val = isp_reg_readl(isp, OMAP3_ISP_IOMEM_CCP2, ISPCCP2_CTRL);
+	if (!(val & ISPCCP2_CTRL_MODE)) {
+		if (buscfg->ccp2_mode == ISP_CCP2_MODE_CCP2)
+			dev_warn(isp->dev, "OMAP3 CCP2 bus not available\n");
+		if (buscfg->phy_layer == ISP_CCP2_PHY_DATA_STROBE)
+			/* Strobe mode requires CCP2 */
+			return -EIO;
+	}
+
+	return 0;
+}
+
+/*
+ * ccp2_vp_config - Initialize CCP2 video port interface.
+ * @ccp2: Pointer to ISP CCP2 device
+ * @vpclk_div: Video port divisor
+ *
+ * Configure the CCP2 video port with the given clock divisor. The valid divisor
+ * values depend on the ISP revision:
+ *
+ * - revision 1.0 and 2.0	1 to 4
+ * - revision 15.0		1 to 65536
+ *
+ * The exact divisor value used might differ from the requested value, as ISP
+ * revision 15.0 represent the divisor by 65536 divided by an integer.
+ */
+static void ccp2_vp_config(struct isp_ccp2_device *ccp2,
+			   unsigned int vpclk_div)
+{
+	struct isp_device *isp = to_isp_device(ccp2);
+	u32 val;
+
+	/* ISPCCP2_CTRL Video port */
+	val = isp_reg_readl(isp, OMAP3_ISP_IOMEM_CCP2, ISPCCP2_CTRL);
+	val |= ISPCCP2_CTRL_VP_ONLY_EN;	/* Disable the memory write port */
+
+	if (isp->revision == ISP_REVISION_15_0) {
+		vpclk_div = clamp_t(unsigned int, vpclk_div, 1, 65536);
+		vpclk_div = min(ISPCCP2_VPCLK_FRACDIV / vpclk_div, 65535U);
+		BIT_SET(val, ISPCCP2_CTRL_VPCLK_DIV_SHIFT,
+			ISPCCP2_CTRL_VPCLK_DIV_MASK, vpclk_div);
+	} else {
+		vpclk_div = clamp_t(unsigned int, vpclk_div, 1, 4);
+		BIT_SET(val, ISPCCP2_CTRL_VP_OUT_CTRL_SHIFT,
+			ISPCCP2_CTRL_VP_OUT_CTRL_MASK, vpclk_div - 1);
+	}
+
+	isp_reg_writel(isp, val, OMAP3_ISP_IOMEM_CCP2, ISPCCP2_CTRL);
+}
+
+/*
+ * ccp2_lcx_config - Initialize CCP2 logical channel interface.
+ * @ccp2: Pointer to ISP CCP2 device
+ * @config: Pointer to ISP LCx config structure.
+ *
+ * This will analyze the parameters passed by the interface config
+ * and configure CSI1/CCP2 logical channel
+ *
+ */
+static void ccp2_lcx_config(struct isp_ccp2_device *ccp2,
+			    struct isp_interface_lcx_config *config)
+{
+	struct isp_device *isp = to_isp_device(ccp2);
+	u32 val, format;
+
+	switch (config->format) {
+	case MEDIA_BUS_FMT_SGRBG10_DPCM8_1X8:
+		format = ISPCCP2_LCx_CTRL_FORMAT_RAW8_DPCM10_VP;
+		break;
+	case MEDIA_BUS_FMT_SGRBG10_1X10:
+	default:
+		format = ISPCCP2_LCx_CTRL_FORMAT_RAW10_VP;	/* RAW10+VP */
+		break;
+	}
+	/* ISPCCP2_LCx_CTRL logical channel #0 */
+	val = isp_reg_readl(isp, OMAP3_ISP_IOMEM_CCP2, ISPCCP2_LCx_CTRL(0))
+			    | (ISPCCP2_LCx_CTRL_REGION_EN); /* Region */
+
+	if (isp->revision == ISP_REVISION_15_0) {
+		/* CRC */
+		BIT_SET(val, ISPCCP2_LCx_CTRL_CRC_SHIFT_15_0,
+			ISPCCP2_LCx_CTRL_CRC_MASK,
+			config->crc);
+		/* Format = RAW10+VP or RAW8+DPCM10+VP*/
+		BIT_SET(val, ISPCCP2_LCx_CTRL_FORMAT_SHIFT_15_0,
+			ISPCCP2_LCx_CTRL_FORMAT_MASK_15_0, format);
+	} else {
+		BIT_SET(val, ISPCCP2_LCx_CTRL_CRC_SHIFT,
+			ISPCCP2_LCx_CTRL_CRC_MASK,
+			config->crc);
+
+		BIT_SET(val, ISPCCP2_LCx_CTRL_FORMAT_SHIFT,
+			ISPCCP2_LCx_CTRL_FORMAT_MASK, format);
+	}
+	isp_reg_writel(isp, val, OMAP3_ISP_IOMEM_CCP2, ISPCCP2_LCx_CTRL(0));
+
+	/* ISPCCP2_DAT_START for logical channel #0 */
+	isp_reg_writel(isp, config->data_start << ISPCCP2_LCx_DAT_SHIFT,
+		       OMAP3_ISP_IOMEM_CCP2, ISPCCP2_LCx_DAT_START(0));
+
+	/* ISPCCP2_DAT_SIZE for logical channel #0 */
+	isp_reg_writel(isp, config->data_size << ISPCCP2_LCx_DAT_SHIFT,
+		       OMAP3_ISP_IOMEM_CCP2, ISPCCP2_LCx_DAT_SIZE(0));
+
+	/* Enable error IRQs for logical channel #0 */
+	val = ISPCCP2_LC01_IRQSTATUS_LC0_FIFO_OVF_IRQ |
+	      ISPCCP2_LC01_IRQSTATUS_LC0_CRC_IRQ |
+	      ISPCCP2_LC01_IRQSTATUS_LC0_FSP_IRQ |
+	      ISPCCP2_LC01_IRQSTATUS_LC0_FW_IRQ |
+	      ISPCCP2_LC01_IRQSTATUS_LC0_FSC_IRQ |
+	      ISPCCP2_LC01_IRQSTATUS_LC0_SSC_IRQ;
+
+	isp_reg_writel(isp, val, OMAP3_ISP_IOMEM_CCP2, ISPCCP2_LC01_IRQSTATUS);
+	isp_reg_set(isp, OMAP3_ISP_IOMEM_CCP2, ISPCCP2_LC01_IRQENABLE, val);
+}
+
+/*
+ * ccp2_if_configure - Configure ccp2 with data from sensor
+ * @ccp2: Pointer to ISP CCP2 device
+ *
+ * Return 0 on success or a negative error code
+ */
+static int ccp2_if_configure(struct isp_ccp2_device *ccp2)
+{
+	const struct isp_bus_cfg *buscfg;
+	struct v4l2_mbus_framefmt *format;
+	struct media_pad *pad;
+	struct v4l2_subdev *sensor;
+	u32 lines = 0;
+	int ret;
+
+	ccp2_pwr_cfg(ccp2);
+
+	pad = media_entity_remote_pad(&ccp2->pads[CCP2_PAD_SINK]);
+	sensor = media_entity_to_v4l2_subdev(pad->entity);
+	buscfg = sensor->host_priv;
+
+	ret = ccp2_phyif_config(ccp2, &buscfg->bus.ccp2);
+	if (ret < 0)
+		return ret;
+
+	ccp2_vp_config(ccp2, buscfg->bus.ccp2.vpclk_div + 1);
+
+	v4l2_subdev_call(sensor, sensor, g_skip_top_lines, &lines);
+
+	format = &ccp2->formats[CCP2_PAD_SINK];
+
+	ccp2->if_cfg.data_start = lines;
+	ccp2->if_cfg.crc = buscfg->bus.ccp2.crc;
+	ccp2->if_cfg.format = format->code;
+	ccp2->if_cfg.data_size = format->height;
+
+	ccp2_lcx_config(ccp2, &ccp2->if_cfg);
+
+	return 0;
+}
+
+static int ccp2_adjust_bandwidth(struct isp_ccp2_device *ccp2)
+{
+	struct isp_pipeline *pipe = to_isp_pipeline(&ccp2->subdev.entity);
+	struct isp_device *isp = to_isp_device(ccp2);
+	const struct v4l2_mbus_framefmt *ofmt = &ccp2->formats[CCP2_PAD_SOURCE];
+	unsigned long l3_ick = pipe->l3_ick;
+	struct v4l2_fract *timeperframe;
+	unsigned int vpclk_div = 2;
+	unsigned int value;
+	u64 bound;
+	u64 area;
+
+	/* Compute the minimum clock divisor, based on the pipeline maximum
+	 * data rate. This is an absolute lower bound if we don't want SBL
+	 * overflows, so round the value up.
+	 */
+	vpclk_div = max_t(unsigned int, DIV_ROUND_UP(l3_ick, pipe->max_rate),
+			  vpclk_div);
+
+	/* Compute the maximum clock divisor, based on the requested frame rate.
+	 * This is a soft lower bound to achieve a frame rate equal or higher
+	 * than the requested value, so round the value down.
+	 */
+	timeperframe = &pipe->max_timeperframe;
+
+	if (timeperframe->numerator) {
+		area = ofmt->width * ofmt->height;
+		bound = div_u64(area * timeperframe->denominator,
+				timeperframe->numerator);
+		value = min_t(u64, bound, l3_ick);
+		vpclk_div = max_t(unsigned int, l3_ick / value, vpclk_div);
+	}
+
+	dev_dbg(isp->dev, "%s: minimum clock divisor = %u\n", __func__,
+		vpclk_div);
+
+	return vpclk_div;
+}
+
+/*
+ * ccp2_mem_configure - Initialize CCP2 memory input/output interface
+ * @ccp2: Pointer to ISP CCP2 device
+ * @config: Pointer to ISP mem interface config structure
+ *
+ * This will analyze the parameters passed by the interface config
+ * structure, and configure the respective registers for proper
+ * CSI1/CCP2 memory input.
+ */
+static void ccp2_mem_configure(struct isp_ccp2_device *ccp2,
+			       struct isp_interface_mem_config *config)
+{
+	struct isp_device *isp = to_isp_device(ccp2);
+	u32 sink_pixcode = ccp2->formats[CCP2_PAD_SINK].code;
+	u32 source_pixcode = ccp2->formats[CCP2_PAD_SOURCE].code;
+	unsigned int dpcm_decompress = 0;
+	u32 val, hwords;
+
+	if (sink_pixcode != source_pixcode &&
+	    sink_pixcode == MEDIA_BUS_FMT_SGRBG10_DPCM8_1X8)
+		dpcm_decompress = 1;
+
+	ccp2_pwr_cfg(ccp2);
+
+	/* Hsize, Skip */
+	isp_reg_writel(isp, ISPCCP2_LCM_HSIZE_SKIP_MIN |
+		       (config->hsize_count << ISPCCP2_LCM_HSIZE_SHIFT),
+		       OMAP3_ISP_IOMEM_CCP2, ISPCCP2_LCM_HSIZE);
+
+	/* Vsize, no. of lines */
+	isp_reg_writel(isp, config->vsize_count << ISPCCP2_LCM_VSIZE_SHIFT,
+		       OMAP3_ISP_IOMEM_CCP2, ISPCCP2_LCM_VSIZE);
+
+	if (ccp2->video_in.bpl_padding == 0)
+		config->src_ofst = 0;
+	else
+		config->src_ofst = ccp2->video_in.bpl_value;
+
+	isp_reg_writel(isp, config->src_ofst, OMAP3_ISP_IOMEM_CCP2,
+		       ISPCCP2_LCM_SRC_OFST);
+
+	/* Source and Destination formats */
+	val = ISPCCP2_LCM_CTRL_DST_FORMAT_RAW10 <<
+	      ISPCCP2_LCM_CTRL_DST_FORMAT_SHIFT;
+
+	if (dpcm_decompress) {
+		/* source format is RAW8 */
+		val |= ISPCCP2_LCM_CTRL_SRC_FORMAT_RAW8 <<
+		       ISPCCP2_LCM_CTRL_SRC_FORMAT_SHIFT;
+
+		/* RAW8 + DPCM10 - simple predictor */
+		val |= ISPCCP2_LCM_CTRL_SRC_DPCM_PRED;
+
+		/* enable source DPCM decompression */
+		val |= ISPCCP2_LCM_CTRL_SRC_DECOMPR_DPCM10 <<
+		       ISPCCP2_LCM_CTRL_SRC_DECOMPR_SHIFT;
+	} else {
+		/* source format is RAW10 */
+		val |= ISPCCP2_LCM_CTRL_SRC_FORMAT_RAW10 <<
+		       ISPCCP2_LCM_CTRL_SRC_FORMAT_SHIFT;
+	}
+
+	/* Burst size to 32x64 */
+	val |= ISPCCP2_LCM_CTRL_BURST_SIZE_32X <<
+	       ISPCCP2_LCM_CTRL_BURST_SIZE_SHIFT;
+
+	isp_reg_writel(isp, val, OMAP3_ISP_IOMEM_CCP2, ISPCCP2_LCM_CTRL);
+
+	/* Prefetch setup */
+	if (dpcm_decompress)
+		hwords = (ISPCCP2_LCM_HSIZE_SKIP_MIN +
+			  config->hsize_count) >> 3;
+	else
+		hwords = (ISPCCP2_LCM_HSIZE_SKIP_MIN +
+			  config->hsize_count) >> 2;
+
+	isp_reg_writel(isp, hwords << ISPCCP2_LCM_PREFETCH_SHIFT,
+		       OMAP3_ISP_IOMEM_CCP2, ISPCCP2_LCM_PREFETCH);
+
+	/* Video port */
+	isp_reg_set(isp, OMAP3_ISP_IOMEM_CCP2, ISPCCP2_CTRL,
+		    ISPCCP2_CTRL_IO_OUT_SEL | ISPCCP2_CTRL_MODE);
+	ccp2_vp_config(ccp2, ccp2_adjust_bandwidth(ccp2));
+
+	/* Clear LCM interrupts */
+	isp_reg_writel(isp, ISPCCP2_LCM_IRQSTATUS_OCPERROR_IRQ |
+		       ISPCCP2_LCM_IRQSTATUS_EOF_IRQ,
+		       OMAP3_ISP_IOMEM_CCP2, ISPCCP2_LCM_IRQSTATUS);
+
+	/* Enable LCM interrupts */
+	isp_reg_set(isp, OMAP3_ISP_IOMEM_CCP2, ISPCCP2_LCM_IRQENABLE,
+		    ISPCCP2_LCM_IRQSTATUS_EOF_IRQ |
+		    ISPCCP2_LCM_IRQSTATUS_OCPERROR_IRQ);
+}
+
+/*
+ * ccp2_set_inaddr - Sets memory address of input frame.
+ * @ccp2: Pointer to ISP CCP2 device
+ * @addr: 32bit memory address aligned on 32byte boundary.
+ *
+ * Configures the memory address from which the input frame is to be read.
+ */
+static void ccp2_set_inaddr(struct isp_ccp2_device *ccp2, u32 addr)
+{
+	struct isp_device *isp = to_isp_device(ccp2);
+
+	isp_reg_writel(isp, addr, OMAP3_ISP_IOMEM_CCP2, ISPCCP2_LCM_SRC_ADDR);
+}
+
+/* -----------------------------------------------------------------------------
+ * Interrupt handling
+ */
+
+static void ccp2_isr_buffer(struct isp_ccp2_device *ccp2)
+{
+	struct isp_pipeline *pipe = to_isp_pipeline(&ccp2->subdev.entity);
+	struct isp_buffer *buffer;
+
+	buffer = omap3isp_video_buffer_next(&ccp2->video_in);
+	if (buffer != NULL)
+		ccp2_set_inaddr(ccp2, buffer->dma);
+
+	pipe->state |= ISP_PIPELINE_IDLE_INPUT;
+
+	if (ccp2->state == ISP_PIPELINE_STREAM_SINGLESHOT) {
+		if (isp_pipeline_ready(pipe))
+			omap3isp_pipeline_set_stream(pipe,
+						ISP_PIPELINE_STREAM_SINGLESHOT);
+	}
+}
+
+/*
+ * omap3isp_ccp2_isr - Handle ISP CCP2 interrupts
+ * @ccp2: Pointer to ISP CCP2 device
+ *
+ * This will handle the CCP2 interrupts
+ */
+void omap3isp_ccp2_isr(struct isp_ccp2_device *ccp2)
+{
+	struct isp_pipeline *pipe = to_isp_pipeline(&ccp2->subdev.entity);
+	struct isp_device *isp = to_isp_device(ccp2);
+	static const u32 ISPCCP2_LC01_ERROR =
+		ISPCCP2_LC01_IRQSTATUS_LC0_FIFO_OVF_IRQ |
+		ISPCCP2_LC01_IRQSTATUS_LC0_CRC_IRQ |
+		ISPCCP2_LC01_IRQSTATUS_LC0_FSP_IRQ |
+		ISPCCP2_LC01_IRQSTATUS_LC0_FW_IRQ |
+		ISPCCP2_LC01_IRQSTATUS_LC0_FSC_IRQ |
+		ISPCCP2_LC01_IRQSTATUS_LC0_SSC_IRQ;
+	u32 lcx_irqstatus, lcm_irqstatus;
+
+	/* First clear the interrupts */
+	lcx_irqstatus = isp_reg_readl(isp, OMAP3_ISP_IOMEM_CCP2,
+				      ISPCCP2_LC01_IRQSTATUS);
+	isp_reg_writel(isp, lcx_irqstatus, OMAP3_ISP_IOMEM_CCP2,
+		       ISPCCP2_LC01_IRQSTATUS);
+
+	lcm_irqstatus = isp_reg_readl(isp, OMAP3_ISP_IOMEM_CCP2,
+				      ISPCCP2_LCM_IRQSTATUS);
+	isp_reg_writel(isp, lcm_irqstatus, OMAP3_ISP_IOMEM_CCP2,
+		       ISPCCP2_LCM_IRQSTATUS);
+	/* Errors */
+	if (lcx_irqstatus & ISPCCP2_LC01_ERROR) {
+		pipe->error = true;
+		dev_dbg(isp->dev, "CCP2 err:%x\n", lcx_irqstatus);
+		return;
+	}
+
+	if (lcm_irqstatus & ISPCCP2_LCM_IRQSTATUS_OCPERROR_IRQ) {
+		pipe->error = true;
+		dev_dbg(isp->dev, "CCP2 OCP err:%x\n", lcm_irqstatus);
+	}
+
+	if (omap3isp_module_sync_is_stopping(&ccp2->wait, &ccp2->stopping))
+		return;
+
+	/* Handle queued buffers on frame end interrupts */
+	if (lcm_irqstatus & ISPCCP2_LCM_IRQSTATUS_EOF_IRQ)
+		ccp2_isr_buffer(ccp2);
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev operations
+ */
+
+static const unsigned int ccp2_fmts[] = {
+	MEDIA_BUS_FMT_SGRBG10_1X10,
+	MEDIA_BUS_FMT_SGRBG10_DPCM8_1X8,
+};
+
+/*
+ * __ccp2_get_format - helper function for getting ccp2 format
+ * @ccp2  : Pointer to ISP CCP2 device
+ * @cfg: V4L2 subdev pad configuration
+ * @pad   : pad number
+ * @which : wanted subdev format
+ * return format structure or NULL on error
+ */
+static struct v4l2_mbus_framefmt *
+__ccp2_get_format(struct isp_ccp2_device *ccp2, struct v4l2_subdev_pad_config *cfg,
+		     unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(&ccp2->subdev, cfg, pad);
+	else
+		return &ccp2->formats[pad];
+}
+
+/*
+ * ccp2_try_format - Handle try format by pad subdev method
+ * @ccp2  : Pointer to ISP CCP2 device
+ * @cfg: V4L2 subdev pad configuration
+ * @pad   : pad num
+ * @fmt   : pointer to v4l2 mbus format structure
+ * @which : wanted subdev format
+ */
+static void ccp2_try_format(struct isp_ccp2_device *ccp2,
+			       struct v4l2_subdev_pad_config *cfg, unsigned int pad,
+			       struct v4l2_mbus_framefmt *fmt,
+			       enum v4l2_subdev_format_whence which)
+{
+	struct v4l2_mbus_framefmt *format;
+
+	switch (pad) {
+	case CCP2_PAD_SINK:
+		if (fmt->code != MEDIA_BUS_FMT_SGRBG10_DPCM8_1X8)
+			fmt->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+
+		if (ccp2->input == CCP2_INPUT_SENSOR) {
+			fmt->width = clamp_t(u32, fmt->width,
+					     ISPCCP2_DAT_START_MIN,
+					     ISPCCP2_DAT_START_MAX);
+			fmt->height = clamp_t(u32, fmt->height,
+					      ISPCCP2_DAT_SIZE_MIN,
+					      ISPCCP2_DAT_SIZE_MAX);
+		} else if (ccp2->input == CCP2_INPUT_MEMORY) {
+			fmt->width = clamp_t(u32, fmt->width,
+					     ISPCCP2_LCM_HSIZE_COUNT_MIN,
+					     ISPCCP2_LCM_HSIZE_COUNT_MAX);
+			fmt->height = clamp_t(u32, fmt->height,
+					      ISPCCP2_LCM_VSIZE_MIN,
+					      ISPCCP2_LCM_VSIZE_MAX);
+		}
+		break;
+
+	case CCP2_PAD_SOURCE:
+		/* Source format - copy sink format and change pixel code
+		 * to SGRBG10_1X10 as we don't support CCP2 write to memory.
+		 * When CCP2 write to memory feature will be added this
+		 * should be changed properly.
+		 */
+		format = __ccp2_get_format(ccp2, cfg, CCP2_PAD_SINK, which);
+		memcpy(fmt, format, sizeof(*fmt));
+		fmt->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+		break;
+	}
+
+	fmt->field = V4L2_FIELD_NONE;
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+}
+
+/*
+ * ccp2_enum_mbus_code - Handle pixel format enumeration
+ * @sd     : pointer to v4l2 subdev structure
+ * @cfg: V4L2 subdev pad configuration
+ * @code   : pointer to v4l2_subdev_mbus_code_enum structure
+ * return -EINVAL or zero on success
+ */
+static int ccp2_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_pad_config *cfg,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct isp_ccp2_device *ccp2 = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	if (code->pad == CCP2_PAD_SINK) {
+		if (code->index >= ARRAY_SIZE(ccp2_fmts))
+			return -EINVAL;
+
+		code->code = ccp2_fmts[code->index];
+	} else {
+		if (code->index != 0)
+			return -EINVAL;
+
+		format = __ccp2_get_format(ccp2, cfg, CCP2_PAD_SINK,
+					      code->which);
+		code->code = format->code;
+	}
+
+	return 0;
+}
+
+static int ccp2_enum_frame_size(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_pad_config *cfg,
+				   struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct isp_ccp2_device *ccp2 = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt format;
+
+	if (fse->index != 0)
+		return -EINVAL;
+
+	format.code = fse->code;
+	format.width = 1;
+	format.height = 1;
+	ccp2_try_format(ccp2, cfg, fse->pad, &format, fse->which);
+	fse->min_width = format.width;
+	fse->min_height = format.height;
+
+	if (format.code != fse->code)
+		return -EINVAL;
+
+	format.code = fse->code;
+	format.width = -1;
+	format.height = -1;
+	ccp2_try_format(ccp2, cfg, fse->pad, &format, fse->which);
+	fse->max_width = format.width;
+	fse->max_height = format.height;
+
+	return 0;
+}
+
+/*
+ * ccp2_get_format - Handle get format by pads subdev method
+ * @sd    : pointer to v4l2 subdev structure
+ * @cfg: V4L2 subdev pad configuration
+ * @fmt   : pointer to v4l2 subdev format structure
+ * return -EINVAL or zero on success
+ */
+static int ccp2_get_format(struct v4l2_subdev *sd, struct v4l2_subdev_pad_config *cfg,
+			      struct v4l2_subdev_format *fmt)
+{
+	struct isp_ccp2_device *ccp2 = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __ccp2_get_format(ccp2, cfg, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
+
+	fmt->format = *format;
+	return 0;
+}
+
+/*
+ * ccp2_set_format - Handle set format by pads subdev method
+ * @sd    : pointer to v4l2 subdev structure
+ * @cfg: V4L2 subdev pad configuration
+ * @fmt   : pointer to v4l2 subdev format structure
+ * returns zero
+ */
+static int ccp2_set_format(struct v4l2_subdev *sd, struct v4l2_subdev_pad_config *cfg,
+			      struct v4l2_subdev_format *fmt)
+{
+	struct isp_ccp2_device *ccp2 = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __ccp2_get_format(ccp2, cfg, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
+
+	ccp2_try_format(ccp2, cfg, fmt->pad, &fmt->format, fmt->which);
+	*format = fmt->format;
+
+	/* Propagate the format from sink to source */
+	if (fmt->pad == CCP2_PAD_SINK) {
+		format = __ccp2_get_format(ccp2, cfg, CCP2_PAD_SOURCE,
+					   fmt->which);
+		*format = fmt->format;
+		ccp2_try_format(ccp2, cfg, CCP2_PAD_SOURCE, format, fmt->which);
+	}
+
+	return 0;
+}
+
+/*
+ * ccp2_init_formats - Initialize formats on all pads
+ * @sd: ISP CCP2 V4L2 subdevice
+ * @fh: V4L2 subdev file handle
+ *
+ * Initialize all pad formats with default values. If fh is not NULL, try
+ * formats are initialized on the file handle. Otherwise active formats are
+ * initialized on the device.
+ */
+static int ccp2_init_formats(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_subdev_format format;
+
+	memset(&format, 0, sizeof(format));
+	format.pad = CCP2_PAD_SINK;
+	format.which = fh ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE;
+	format.format.code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	format.format.width = 4096;
+	format.format.height = 4096;
+	ccp2_set_format(sd, fh ? fh->pad : NULL, &format);
+
+	return 0;
+}
+
+/*
+ * ccp2_s_stream - Enable/Disable streaming on ccp2 subdev
+ * @sd    : pointer to v4l2 subdev structure
+ * @enable: 1 == Enable, 0 == Disable
+ * return zero
+ */
+static int ccp2_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct isp_ccp2_device *ccp2 = v4l2_get_subdevdata(sd);
+	struct isp_device *isp = to_isp_device(ccp2);
+	struct device *dev = to_device(ccp2);
+	int ret;
+
+	if (ccp2->state == ISP_PIPELINE_STREAM_STOPPED) {
+		if (enable == ISP_PIPELINE_STREAM_STOPPED)
+			return 0;
+		atomic_set(&ccp2->stopping, 0);
+	}
+
+	switch (enable) {
+	case ISP_PIPELINE_STREAM_CONTINUOUS:
+		if (ccp2->phy) {
+			ret = omap3isp_csiphy_acquire(ccp2->phy);
+			if (ret < 0)
+				return ret;
+		}
+
+		ccp2_if_configure(ccp2);
+		ccp2_print_status(ccp2);
+
+		/* Enable CSI1/CCP2 interface */
+		ret = ccp2_if_enable(ccp2, 1);
+		if (ret < 0) {
+			if (ccp2->phy)
+				omap3isp_csiphy_release(ccp2->phy);
+			return ret;
+		}
+		break;
+
+	case ISP_PIPELINE_STREAM_SINGLESHOT:
+		if (ccp2->state != ISP_PIPELINE_STREAM_SINGLESHOT) {
+			struct v4l2_mbus_framefmt *format;
+
+			format = &ccp2->formats[CCP2_PAD_SINK];
+
+			ccp2->mem_cfg.hsize_count = format->width;
+			ccp2->mem_cfg.vsize_count = format->height;
+			ccp2->mem_cfg.src_ofst = 0;
+
+			ccp2_mem_configure(ccp2, &ccp2->mem_cfg);
+			omap3isp_sbl_enable(isp, OMAP3_ISP_SBL_CSI1_READ);
+			ccp2_print_status(ccp2);
+		}
+		ccp2_mem_enable(ccp2, 1);
+		break;
+
+	case ISP_PIPELINE_STREAM_STOPPED:
+		if (omap3isp_module_sync_idle(&sd->entity, &ccp2->wait,
+					      &ccp2->stopping))
+			dev_dbg(dev, "%s: module stop timeout.\n", sd->name);
+		if (ccp2->input == CCP2_INPUT_MEMORY) {
+			ccp2_mem_enable(ccp2, 0);
+			omap3isp_sbl_disable(isp, OMAP3_ISP_SBL_CSI1_READ);
+		} else if (ccp2->input == CCP2_INPUT_SENSOR) {
+			/* Disable CSI1/CCP2 interface */
+			ccp2_if_enable(ccp2, 0);
+			if (ccp2->phy)
+				omap3isp_csiphy_release(ccp2->phy);
+		}
+		break;
+	}
+
+	ccp2->state = enable;
+	return 0;
+}
+
+/* subdev video operations */
+static const struct v4l2_subdev_video_ops ccp2_sd_video_ops = {
+	.s_stream = ccp2_s_stream,
+};
+
+/* subdev pad operations */
+static const struct v4l2_subdev_pad_ops ccp2_sd_pad_ops = {
+	.enum_mbus_code = ccp2_enum_mbus_code,
+	.enum_frame_size = ccp2_enum_frame_size,
+	.get_fmt = ccp2_get_format,
+	.set_fmt = ccp2_set_format,
+};
+
+/* subdev operations */
+static const struct v4l2_subdev_ops ccp2_sd_ops = {
+	.video = &ccp2_sd_video_ops,
+	.pad = &ccp2_sd_pad_ops,
+};
+
+/* subdev internal operations */
+static const struct v4l2_subdev_internal_ops ccp2_sd_internal_ops = {
+	.open = ccp2_init_formats,
+};
+
+/* --------------------------------------------------------------------------
+ * ISP ccp2 video device node
+ */
+
+/*
+ * ccp2_video_queue - Queue video buffer.
+ * @video : Pointer to isp video structure
+ * @buffer: Pointer to isp_buffer structure
+ * return -EIO or zero on success
+ */
+static int ccp2_video_queue(struct isp_video *video, struct isp_buffer *buffer)
+{
+	struct isp_ccp2_device *ccp2 = &video->isp->isp_ccp2;
+
+	ccp2_set_inaddr(ccp2, buffer->dma);
+	return 0;
+}
+
+static const struct isp_video_operations ccp2_video_ops = {
+	.queue = ccp2_video_queue,
+};
+
+/* -----------------------------------------------------------------------------
+ * Media entity operations
+ */
+
+/*
+ * ccp2_link_setup - Setup ccp2 connections.
+ * @entity : Pointer to media entity structure
+ * @local  : Pointer to local pad array
+ * @remote : Pointer to remote pad array
+ * @flags  : Link flags
+ * return -EINVAL on error or zero on success
+ */
+static int ccp2_link_setup(struct media_entity *entity,
+			   const struct media_pad *local,
+			   const struct media_pad *remote, u32 flags)
+{
+	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+	struct isp_ccp2_device *ccp2 = v4l2_get_subdevdata(sd);
+
+	switch (local->index | media_entity_type(remote->entity)) {
+	case CCP2_PAD_SINK | MEDIA_ENT_T_DEVNODE:
+		/* read from memory */
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (ccp2->input == CCP2_INPUT_SENSOR)
+				return -EBUSY;
+			ccp2->input = CCP2_INPUT_MEMORY;
+		} else {
+			if (ccp2->input == CCP2_INPUT_MEMORY)
+				ccp2->input = CCP2_INPUT_NONE;
+		}
+		break;
+
+	case CCP2_PAD_SINK | MEDIA_ENT_T_V4L2_SUBDEV:
+		/* read from sensor/phy */
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (ccp2->input == CCP2_INPUT_MEMORY)
+				return -EBUSY;
+			ccp2->input = CCP2_INPUT_SENSOR;
+		} else {
+			if (ccp2->input == CCP2_INPUT_SENSOR)
+				ccp2->input = CCP2_INPUT_NONE;
+		} break;
+
+	case CCP2_PAD_SOURCE | MEDIA_ENT_T_V4L2_SUBDEV:
+		/* write to video port/ccdc */
+		if (flags & MEDIA_LNK_FL_ENABLED)
+			ccp2->output = CCP2_OUTPUT_CCDC;
+		else
+			ccp2->output = CCP2_OUTPUT_NONE;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* media operations */
+static const struct media_entity_operations ccp2_media_ops = {
+	.link_setup = ccp2_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+/*
+ * omap3isp_ccp2_unregister_entities - Unregister media entities: subdev
+ * @ccp2: Pointer to ISP CCP2 device
+ */
+void omap3isp_ccp2_unregister_entities(struct isp_ccp2_device *ccp2)
+{
+	v4l2_device_unregister_subdev(&ccp2->subdev);
+	omap3isp_video_unregister(&ccp2->video_in);
+}
+
+/*
+ * omap3isp_ccp2_register_entities - Register the subdev media entity
+ * @ccp2: Pointer to ISP CCP2 device
+ * @vdev: Pointer to v4l device
+ * return negative error code or zero on success
+ */
+
+int omap3isp_ccp2_register_entities(struct isp_ccp2_device *ccp2,
+				    struct v4l2_device *vdev)
+{
+	int ret;
+
+	/* Register the subdev and video nodes. */
+	ret = v4l2_device_register_subdev(vdev, &ccp2->subdev);
+	if (ret < 0)
+		goto error;
+
+	ret = omap3isp_video_register(&ccp2->video_in, vdev);
+	if (ret < 0)
+		goto error;
+
+	return 0;
+
+error:
+	omap3isp_ccp2_unregister_entities(ccp2);
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * ISP ccp2 initialisation and cleanup
+ */
+
+/*
+ * ccp2_init_entities - Initialize ccp2 subdev and media entity.
+ * @ccp2: Pointer to ISP CCP2 device
+ * return negative error code or zero on success
+ */
+static int ccp2_init_entities(struct isp_ccp2_device *ccp2)
+{
+	struct v4l2_subdev *sd = &ccp2->subdev;
+	struct media_pad *pads = ccp2->pads;
+	struct media_entity *me = &sd->entity;
+	int ret;
+
+	ccp2->input = CCP2_INPUT_NONE;
+	ccp2->output = CCP2_OUTPUT_NONE;
+
+	v4l2_subdev_init(sd, &ccp2_sd_ops);
+	sd->internal_ops = &ccp2_sd_internal_ops;
+	strlcpy(sd->name, "OMAP3 ISP CCP2", sizeof(sd->name));
+	sd->grp_id = 1 << 16;   /* group ID for isp subdevs */
+	v4l2_set_subdevdata(sd, ccp2);
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	pads[CCP2_PAD_SINK].flags = MEDIA_PAD_FL_SINK
+				    | MEDIA_PAD_FL_MUST_CONNECT;
+	pads[CCP2_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
+
+	me->ops = &ccp2_media_ops;
+	ret = media_entity_init(me, CCP2_PADS_NUM, pads, 0);
+	if (ret < 0)
+		return ret;
+
+	ccp2_init_formats(sd, NULL);
+
+	/*
+	 * The CCP2 has weird line alignment requirements, possibly caused by
+	 * DPCM8 decompression. Line length for data read from memory must be a
+	 * multiple of 128 bits (16 bytes) in continuous mode (when no padding
+	 * is present at end of lines). Additionally, if padding is used, the
+	 * padded line length must be a multiple of 32 bytes. To simplify the
+	 * implementation we use a fixed 32 bytes alignment regardless of the
+	 * input format and width. If strict 128 bits alignment support is
+	 * required ispvideo will need to be made aware of this special dual
+	 * alignment requirements.
+	 */
+	ccp2->video_in.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	ccp2->video_in.bpl_alignment = 32;
+	ccp2->video_in.bpl_max = 0xffffffe0;
+	ccp2->video_in.isp = to_isp_device(ccp2);
+	ccp2->video_in.ops = &ccp2_video_ops;
+	ccp2->video_in.capture_mem = PAGE_ALIGN(4096 * 4096) * 3;
+
+	ret = omap3isp_video_init(&ccp2->video_in, "CCP2");
+	if (ret < 0)
+		goto error_video;
+
+	/* Connect the video node to the ccp2 subdev. */
+	ret = media_entity_create_link(&ccp2->video_in.video.entity, 0,
+				       &ccp2->subdev.entity, CCP2_PAD_SINK, 0);
+	if (ret < 0)
+		goto error_link;
+
+	return 0;
+
+error_link:
+	omap3isp_video_cleanup(&ccp2->video_in);
+error_video:
+	media_entity_cleanup(&ccp2->subdev.entity);
+	return ret;
+}
+
+/*
+ * omap3isp_ccp2_init - CCP2 initialization.
+ * @isp : Pointer to ISP device
+ * return negative error code or zero on success
+ */
+int omap3isp_ccp2_init(struct isp_device *isp)
+{
+	struct isp_ccp2_device *ccp2 = &isp->isp_ccp2;
+	int ret;
+
+	init_waitqueue_head(&ccp2->wait);
+
+	/*
+	 * On the OMAP34xx the CSI1 receiver is operated in the CSIb IO
+	 * complex, which is powered by vdds_csib power rail. Hence the
+	 * request for the regulator.
+	 *
+	 * On the OMAP36xx, the CCP2 uses the CSI PHY1 or PHY2, shared with
+	 * the CSI2c or CSI2a receivers. The PHY then needs to be explicitly
+	 * configured.
+	 *
+	 * TODO: Don't hardcode the usage of PHY1 (shared with CSI2c).
+	 */
+	if (isp->revision == ISP_REVISION_2_0) {
+		ccp2->vdds_csib = devm_regulator_get(isp->dev, "vdds_csib");
+		if (IS_ERR(ccp2->vdds_csib)) {
+			dev_dbg(isp->dev,
+				"Could not get regulator vdds_csib\n");
+			ccp2->vdds_csib = NULL;
+		}
+	} else if (isp->revision == ISP_REVISION_15_0) {
+		ccp2->phy = &isp->isp_csiphy1;
+	}
+
+	ret = ccp2_init_entities(ccp2);
+	if (ret < 0)
+		return ret;
+
+	ccp2_reset(ccp2);
+	return 0;
+}
+
+/*
+ * omap3isp_ccp2_cleanup - CCP2 un-initialization
+ * @isp : Pointer to ISP device
+ */
+void omap3isp_ccp2_cleanup(struct isp_device *isp)
+{
+	struct isp_ccp2_device *ccp2 = &isp->isp_ccp2;
+
+	omap3isp_video_cleanup(&ccp2->video_in);
+	media_entity_cleanup(&ccp2->subdev.entity);
+}
diff --git a/drivers/media/platform/omap3isp/ispccp2.h b/drivers/media/platform/omap3isp/ispccp2.h
new file mode 100644
index 0000000..4662bff
--- /dev/null
+++ b/drivers/media/platform/omap3isp/ispccp2.h
@@ -0,0 +1,88 @@
+/*
+ * ispccp2.h
+ *
+ * TI OMAP3 ISP - CCP2 module
+ *
+ * Copyright (C) 2010 Nokia Corporation
+ * Copyright (C) 2010 Texas Instruments, Inc.
+ *
+ * Contacts: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *	     Sakari Ailus <sakari.ailus@iki.fi>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef OMAP3_ISP_CCP2_H
+#define OMAP3_ISP_CCP2_H
+
+#include <linux/videodev2.h>
+
+struct isp_device;
+struct isp_csiphy;
+
+/* Sink and source ccp2 pads */
+#define CCP2_PAD_SINK			0
+#define CCP2_PAD_SOURCE			1
+#define CCP2_PADS_NUM			2
+
+/* CCP2 input media entity */
+enum ccp2_input_entity {
+	CCP2_INPUT_NONE,
+	CCP2_INPUT_SENSOR,
+	CCP2_INPUT_MEMORY,
+};
+
+/* CCP2 output media entity */
+enum ccp2_output_entity {
+	CCP2_OUTPUT_NONE,
+	CCP2_OUTPUT_CCDC,
+	CCP2_OUTPUT_MEMORY,
+};
+
+
+/* Logical channel configuration */
+struct isp_interface_lcx_config {
+	int crc;
+	u32 data_start;
+	u32 data_size;
+	u32 format;
+};
+
+/* Memory channel configuration */
+struct isp_interface_mem_config {
+	u32 dst_port;
+	u32 vsize_count;
+	u32 hsize_count;
+	u32 src_ofst;
+	u32 dst_ofst;
+};
+
+/* CCP2 device */
+struct isp_ccp2_device {
+	struct v4l2_subdev subdev;
+	struct v4l2_mbus_framefmt formats[CCP2_PADS_NUM];
+	struct media_pad pads[CCP2_PADS_NUM];
+
+	enum ccp2_input_entity input;
+	enum ccp2_output_entity output;
+	struct isp_interface_lcx_config if_cfg;
+	struct isp_interface_mem_config mem_cfg;
+	struct isp_video video_in;
+	struct isp_csiphy *phy;
+	struct regulator *vdds_csib;
+	enum isp_pipeline_stream_state state;
+	wait_queue_head_t wait;
+	atomic_t stopping;
+};
+
+/* Function declarations */
+int omap3isp_ccp2_init(struct isp_device *isp);
+void omap3isp_ccp2_cleanup(struct isp_device *isp);
+int omap3isp_ccp2_register_entities(struct isp_ccp2_device *ccp2,
+			struct v4l2_device *vdev);
+void omap3isp_ccp2_unregister_entities(struct isp_ccp2_device *ccp2);
+void omap3isp_ccp2_isr(struct isp_ccp2_device *ccp2);
+
+#endif	/* OMAP3_ISP_CCP2_H */
diff --git a/drivers/media/platform/omap3isp/ispcsi2.c b/drivers/media/platform/omap3isp/ispcsi2.c
new file mode 100644
index 0000000..a78338d
--- /dev/null
+++ b/drivers/media/platform/omap3isp/ispcsi2.c
@@ -0,0 +1,1325 @@
+/*
+ * ispcsi2.c
+ *
+ * TI OMAP3 ISP - CSI2 module
+ *
+ * Copyright (C) 2010 Nokia Corporation
+ * Copyright (C) 2009 Texas Instruments, Inc.
+ *
+ * Contacts: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *	     Sakari Ailus <sakari.ailus@iki.fi>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#include <linux/delay.h>
+#include <media/v4l2-common.h>
+#include <linux/v4l2-mediabus.h>
+#include <linux/mm.h>
+
+#include "isp.h"
+#include "ispreg.h"
+#include "ispcsi2.h"
+
+/*
+ * csi2_if_enable - Enable CSI2 Receiver interface.
+ * @enable: enable flag
+ *
+ */
+static void csi2_if_enable(struct isp_device *isp,
+			   struct isp_csi2_device *csi2, u8 enable)
+{
+	struct isp_csi2_ctrl_cfg *currctrl = &csi2->ctrl;
+
+	isp_reg_clr_set(isp, csi2->regs1, ISPCSI2_CTRL, ISPCSI2_CTRL_IF_EN,
+			enable ? ISPCSI2_CTRL_IF_EN : 0);
+
+	currctrl->if_enable = enable;
+}
+
+/*
+ * csi2_recv_config - CSI2 receiver module configuration.
+ * @currctrl: isp_csi2_ctrl_cfg structure
+ *
+ */
+static void csi2_recv_config(struct isp_device *isp,
+			     struct isp_csi2_device *csi2,
+			     struct isp_csi2_ctrl_cfg *currctrl)
+{
+	u32 reg;
+
+	reg = isp_reg_readl(isp, csi2->regs1, ISPCSI2_CTRL);
+
+	if (currctrl->frame_mode)
+		reg |= ISPCSI2_CTRL_FRAME;
+	else
+		reg &= ~ISPCSI2_CTRL_FRAME;
+
+	if (currctrl->vp_clk_enable)
+		reg |= ISPCSI2_CTRL_VP_CLK_EN;
+	else
+		reg &= ~ISPCSI2_CTRL_VP_CLK_EN;
+
+	if (currctrl->vp_only_enable)
+		reg |= ISPCSI2_CTRL_VP_ONLY_EN;
+	else
+		reg &= ~ISPCSI2_CTRL_VP_ONLY_EN;
+
+	reg &= ~ISPCSI2_CTRL_VP_OUT_CTRL_MASK;
+	reg |= currctrl->vp_out_ctrl << ISPCSI2_CTRL_VP_OUT_CTRL_SHIFT;
+
+	if (currctrl->ecc_enable)
+		reg |= ISPCSI2_CTRL_ECC_EN;
+	else
+		reg &= ~ISPCSI2_CTRL_ECC_EN;
+
+	isp_reg_writel(isp, reg, csi2->regs1, ISPCSI2_CTRL);
+}
+
+static const unsigned int csi2_input_fmts[] = {
+	MEDIA_BUS_FMT_SGRBG10_1X10,
+	MEDIA_BUS_FMT_SGRBG10_DPCM8_1X8,
+	MEDIA_BUS_FMT_SRGGB10_1X10,
+	MEDIA_BUS_FMT_SRGGB10_DPCM8_1X8,
+	MEDIA_BUS_FMT_SBGGR10_1X10,
+	MEDIA_BUS_FMT_SBGGR10_DPCM8_1X8,
+	MEDIA_BUS_FMT_SGBRG10_1X10,
+	MEDIA_BUS_FMT_SGBRG10_DPCM8_1X8,
+	MEDIA_BUS_FMT_YUYV8_2X8,
+};
+
+/* To set the format on the CSI2 requires a mapping function that takes
+ * the following inputs:
+ * - 3 different formats (at this time)
+ * - 2 destinations (mem, vp+mem) (vp only handled separately)
+ * - 2 decompression options (on, off)
+ * - 2 isp revisions (certain format must be handled differently on OMAP3630)
+ * Output should be CSI2 frame format code
+ * Array indices as follows: [format][dest][decompr][is_3630]
+ * Not all combinations are valid. 0 means invalid.
+ */
+static const u16 __csi2_fmt_map[3][2][2][2] = {
+	/* RAW10 formats */
+	{
+		/* Output to memory */
+		{
+			/* No DPCM decompression */
+			{ CSI2_PIX_FMT_RAW10_EXP16, CSI2_PIX_FMT_RAW10_EXP16 },
+			/* DPCM decompression */
+			{ 0, 0 },
+		},
+		/* Output to both */
+		{
+			/* No DPCM decompression */
+			{ CSI2_PIX_FMT_RAW10_EXP16_VP,
+			  CSI2_PIX_FMT_RAW10_EXP16_VP },
+			/* DPCM decompression */
+			{ 0, 0 },
+		},
+	},
+	/* RAW10 DPCM8 formats */
+	{
+		/* Output to memory */
+		{
+			/* No DPCM decompression */
+			{ CSI2_PIX_FMT_RAW8, CSI2_USERDEF_8BIT_DATA1 },
+			/* DPCM decompression */
+			{ CSI2_PIX_FMT_RAW8_DPCM10_EXP16,
+			  CSI2_USERDEF_8BIT_DATA1_DPCM10 },
+		},
+		/* Output to both */
+		{
+			/* No DPCM decompression */
+			{ CSI2_PIX_FMT_RAW8_VP,
+			  CSI2_PIX_FMT_RAW8_VP },
+			/* DPCM decompression */
+			{ CSI2_PIX_FMT_RAW8_DPCM10_VP,
+			  CSI2_USERDEF_8BIT_DATA1_DPCM10_VP },
+		},
+	},
+	/* YUYV8 2X8 formats */
+	{
+		/* Output to memory */
+		{
+			/* No DPCM decompression */
+			{ CSI2_PIX_FMT_YUV422_8BIT,
+			  CSI2_PIX_FMT_YUV422_8BIT },
+			/* DPCM decompression */
+			{ 0, 0 },
+		},
+		/* Output to both */
+		{
+			/* No DPCM decompression */
+			{ CSI2_PIX_FMT_YUV422_8BIT_VP,
+			  CSI2_PIX_FMT_YUV422_8BIT_VP },
+			/* DPCM decompression */
+			{ 0, 0 },
+		},
+	},
+};
+
+/*
+ * csi2_ctx_map_format - Map CSI2 sink media bus format to CSI2 format ID
+ * @csi2: ISP CSI2 device
+ *
+ * Returns CSI2 physical format id
+ */
+static u16 csi2_ctx_map_format(struct isp_csi2_device *csi2)
+{
+	const struct v4l2_mbus_framefmt *fmt = &csi2->formats[CSI2_PAD_SINK];
+	int fmtidx, destidx, is_3630;
+
+	switch (fmt->code) {
+	case MEDIA_BUS_FMT_SGRBG10_1X10:
+	case MEDIA_BUS_FMT_SRGGB10_1X10:
+	case MEDIA_BUS_FMT_SBGGR10_1X10:
+	case MEDIA_BUS_FMT_SGBRG10_1X10:
+		fmtidx = 0;
+		break;
+	case MEDIA_BUS_FMT_SGRBG10_DPCM8_1X8:
+	case MEDIA_BUS_FMT_SRGGB10_DPCM8_1X8:
+	case MEDIA_BUS_FMT_SBGGR10_DPCM8_1X8:
+	case MEDIA_BUS_FMT_SGBRG10_DPCM8_1X8:
+		fmtidx = 1;
+		break;
+	case MEDIA_BUS_FMT_YUYV8_2X8:
+		fmtidx = 2;
+		break;
+	default:
+		WARN(1, KERN_ERR "CSI2: pixel format %08x unsupported!\n",
+		     fmt->code);
+		return 0;
+	}
+
+	if (!(csi2->output & CSI2_OUTPUT_CCDC) &&
+	    !(csi2->output & CSI2_OUTPUT_MEMORY)) {
+		/* Neither output enabled is a valid combination */
+		return CSI2_PIX_FMT_OTHERS;
+	}
+
+	/* If we need to skip frames at the beginning of the stream disable the
+	 * video port to avoid sending the skipped frames to the CCDC.
+	 */
+	destidx = csi2->frame_skip ? 0 : !!(csi2->output & CSI2_OUTPUT_CCDC);
+	is_3630 = csi2->isp->revision == ISP_REVISION_15_0;
+
+	return __csi2_fmt_map[fmtidx][destidx][csi2->dpcm_decompress][is_3630];
+}
+
+/*
+ * csi2_set_outaddr - Set memory address to save output image
+ * @csi2: Pointer to ISP CSI2a device.
+ * @addr: ISP MMU Mapped 32-bit memory address aligned on 32 byte boundary.
+ *
+ * Sets the memory address where the output will be saved.
+ *
+ * Returns 0 if successful, or -EINVAL if the address is not in the 32 byte
+ * boundary.
+ */
+static void csi2_set_outaddr(struct isp_csi2_device *csi2, u32 addr)
+{
+	struct isp_device *isp = csi2->isp;
+	struct isp_csi2_ctx_cfg *ctx = &csi2->contexts[0];
+
+	ctx->ping_addr = addr;
+	ctx->pong_addr = addr;
+	isp_reg_writel(isp, ctx->ping_addr,
+		       csi2->regs1, ISPCSI2_CTX_DAT_PING_ADDR(ctx->ctxnum));
+	isp_reg_writel(isp, ctx->pong_addr,
+		       csi2->regs1, ISPCSI2_CTX_DAT_PONG_ADDR(ctx->ctxnum));
+}
+
+/*
+ * is_usr_def_mapping - Checks whether USER_DEF_MAPPING should
+ *			be enabled by CSI2.
+ * @format_id: mapped format id
+ *
+ */
+static inline int is_usr_def_mapping(u32 format_id)
+{
+	return (format_id & 0x40) ? 1 : 0;
+}
+
+/*
+ * csi2_ctx_enable - Enable specified CSI2 context
+ * @ctxnum: Context number, valid between 0 and 7 values.
+ * @enable: enable
+ *
+ */
+static void csi2_ctx_enable(struct isp_device *isp,
+			    struct isp_csi2_device *csi2, u8 ctxnum, u8 enable)
+{
+	struct isp_csi2_ctx_cfg *ctx = &csi2->contexts[ctxnum];
+	unsigned int skip = 0;
+	u32 reg;
+
+	reg = isp_reg_readl(isp, csi2->regs1, ISPCSI2_CTX_CTRL1(ctxnum));
+
+	if (enable) {
+		if (csi2->frame_skip)
+			skip = csi2->frame_skip;
+		else if (csi2->output & CSI2_OUTPUT_MEMORY)
+			skip = 1;
+
+		reg &= ~ISPCSI2_CTX_CTRL1_COUNT_MASK;
+		reg |= ISPCSI2_CTX_CTRL1_COUNT_UNLOCK
+		    |  (skip << ISPCSI2_CTX_CTRL1_COUNT_SHIFT)
+		    |  ISPCSI2_CTX_CTRL1_CTX_EN;
+	} else {
+		reg &= ~ISPCSI2_CTX_CTRL1_CTX_EN;
+	}
+
+	isp_reg_writel(isp, reg, csi2->regs1, ISPCSI2_CTX_CTRL1(ctxnum));
+	ctx->enabled = enable;
+}
+
+/*
+ * csi2_ctx_config - CSI2 context configuration.
+ * @ctx: context configuration
+ *
+ */
+static void csi2_ctx_config(struct isp_device *isp,
+			    struct isp_csi2_device *csi2,
+			    struct isp_csi2_ctx_cfg *ctx)
+{
+	u32 reg;
+
+	/* Set up CSI2_CTx_CTRL1 */
+	reg = isp_reg_readl(isp, csi2->regs1, ISPCSI2_CTX_CTRL1(ctx->ctxnum));
+
+	if (ctx->eof_enabled)
+		reg |= ISPCSI2_CTX_CTRL1_EOF_EN;
+	else
+		reg &= ~ISPCSI2_CTX_CTRL1_EOF_EN;
+
+	if (ctx->eol_enabled)
+		reg |= ISPCSI2_CTX_CTRL1_EOL_EN;
+	else
+		reg &= ~ISPCSI2_CTX_CTRL1_EOL_EN;
+
+	if (ctx->checksum_enabled)
+		reg |= ISPCSI2_CTX_CTRL1_CS_EN;
+	else
+		reg &= ~ISPCSI2_CTX_CTRL1_CS_EN;
+
+	isp_reg_writel(isp, reg, csi2->regs1, ISPCSI2_CTX_CTRL1(ctx->ctxnum));
+
+	/* Set up CSI2_CTx_CTRL2 */
+	reg = isp_reg_readl(isp, csi2->regs1, ISPCSI2_CTX_CTRL2(ctx->ctxnum));
+
+	reg &= ~(ISPCSI2_CTX_CTRL2_VIRTUAL_ID_MASK);
+	reg |= ctx->virtual_id << ISPCSI2_CTX_CTRL2_VIRTUAL_ID_SHIFT;
+
+	reg &= ~(ISPCSI2_CTX_CTRL2_FORMAT_MASK);
+	reg |= ctx->format_id << ISPCSI2_CTX_CTRL2_FORMAT_SHIFT;
+
+	if (ctx->dpcm_decompress) {
+		if (ctx->dpcm_predictor)
+			reg |= ISPCSI2_CTX_CTRL2_DPCM_PRED;
+		else
+			reg &= ~ISPCSI2_CTX_CTRL2_DPCM_PRED;
+	}
+
+	if (is_usr_def_mapping(ctx->format_id)) {
+		reg &= ~ISPCSI2_CTX_CTRL2_USER_DEF_MAP_MASK;
+		reg |= 2 << ISPCSI2_CTX_CTRL2_USER_DEF_MAP_SHIFT;
+	}
+
+	isp_reg_writel(isp, reg, csi2->regs1, ISPCSI2_CTX_CTRL2(ctx->ctxnum));
+
+	/* Set up CSI2_CTx_CTRL3 */
+	reg = isp_reg_readl(isp, csi2->regs1, ISPCSI2_CTX_CTRL3(ctx->ctxnum));
+	reg &= ~(ISPCSI2_CTX_CTRL3_ALPHA_MASK);
+	reg |= (ctx->alpha << ISPCSI2_CTX_CTRL3_ALPHA_SHIFT);
+
+	isp_reg_writel(isp, reg, csi2->regs1, ISPCSI2_CTX_CTRL3(ctx->ctxnum));
+
+	/* Set up CSI2_CTx_DAT_OFST */
+	reg = isp_reg_readl(isp, csi2->regs1,
+			    ISPCSI2_CTX_DAT_OFST(ctx->ctxnum));
+	reg &= ~ISPCSI2_CTX_DAT_OFST_OFST_MASK;
+	reg |= ctx->data_offset << ISPCSI2_CTX_DAT_OFST_OFST_SHIFT;
+	isp_reg_writel(isp, reg, csi2->regs1,
+		       ISPCSI2_CTX_DAT_OFST(ctx->ctxnum));
+
+	isp_reg_writel(isp, ctx->ping_addr,
+		       csi2->regs1, ISPCSI2_CTX_DAT_PING_ADDR(ctx->ctxnum));
+
+	isp_reg_writel(isp, ctx->pong_addr,
+		       csi2->regs1, ISPCSI2_CTX_DAT_PONG_ADDR(ctx->ctxnum));
+}
+
+/*
+ * csi2_timing_config - CSI2 timing configuration.
+ * @timing: csi2_timing_cfg structure
+ */
+static void csi2_timing_config(struct isp_device *isp,
+			       struct isp_csi2_device *csi2,
+			       struct isp_csi2_timing_cfg *timing)
+{
+	u32 reg;
+
+	reg = isp_reg_readl(isp, csi2->regs1, ISPCSI2_TIMING);
+
+	if (timing->force_rx_mode)
+		reg |= ISPCSI2_TIMING_FORCE_RX_MODE_IO(timing->ionum);
+	else
+		reg &= ~ISPCSI2_TIMING_FORCE_RX_MODE_IO(timing->ionum);
+
+	if (timing->stop_state_16x)
+		reg |= ISPCSI2_TIMING_STOP_STATE_X16_IO(timing->ionum);
+	else
+		reg &= ~ISPCSI2_TIMING_STOP_STATE_X16_IO(timing->ionum);
+
+	if (timing->stop_state_4x)
+		reg |= ISPCSI2_TIMING_STOP_STATE_X4_IO(timing->ionum);
+	else
+		reg &= ~ISPCSI2_TIMING_STOP_STATE_X4_IO(timing->ionum);
+
+	reg &= ~ISPCSI2_TIMING_STOP_STATE_COUNTER_IO_MASK(timing->ionum);
+	reg |= timing->stop_state_counter <<
+	       ISPCSI2_TIMING_STOP_STATE_COUNTER_IO_SHIFT(timing->ionum);
+
+	isp_reg_writel(isp, reg, csi2->regs1, ISPCSI2_TIMING);
+}
+
+/*
+ * csi2_irq_ctx_set - Enables CSI2 Context IRQs.
+ * @enable: Enable/disable CSI2 Context interrupts
+ */
+static void csi2_irq_ctx_set(struct isp_device *isp,
+			     struct isp_csi2_device *csi2, int enable)
+{
+	int i;
+
+	for (i = 0; i < 8; i++) {
+		isp_reg_writel(isp, ISPCSI2_CTX_IRQSTATUS_FE_IRQ, csi2->regs1,
+			       ISPCSI2_CTX_IRQSTATUS(i));
+		if (enable)
+			isp_reg_set(isp, csi2->regs1, ISPCSI2_CTX_IRQENABLE(i),
+				    ISPCSI2_CTX_IRQSTATUS_FE_IRQ);
+		else
+			isp_reg_clr(isp, csi2->regs1, ISPCSI2_CTX_IRQENABLE(i),
+				    ISPCSI2_CTX_IRQSTATUS_FE_IRQ);
+	}
+}
+
+/*
+ * csi2_irq_complexio1_set - Enables CSI2 ComplexIO IRQs.
+ * @enable: Enable/disable CSI2 ComplexIO #1 interrupts
+ */
+static void csi2_irq_complexio1_set(struct isp_device *isp,
+				    struct isp_csi2_device *csi2, int enable)
+{
+	u32 reg;
+	reg = ISPCSI2_PHY_IRQENABLE_STATEALLULPMEXIT |
+		ISPCSI2_PHY_IRQENABLE_STATEALLULPMENTER |
+		ISPCSI2_PHY_IRQENABLE_STATEULPM5 |
+		ISPCSI2_PHY_IRQENABLE_ERRCONTROL5 |
+		ISPCSI2_PHY_IRQENABLE_ERRESC5 |
+		ISPCSI2_PHY_IRQENABLE_ERRSOTSYNCHS5 |
+		ISPCSI2_PHY_IRQENABLE_ERRSOTHS5 |
+		ISPCSI2_PHY_IRQENABLE_STATEULPM4 |
+		ISPCSI2_PHY_IRQENABLE_ERRCONTROL4 |
+		ISPCSI2_PHY_IRQENABLE_ERRESC4 |
+		ISPCSI2_PHY_IRQENABLE_ERRSOTSYNCHS4 |
+		ISPCSI2_PHY_IRQENABLE_ERRSOTHS4 |
+		ISPCSI2_PHY_IRQENABLE_STATEULPM3 |
+		ISPCSI2_PHY_IRQENABLE_ERRCONTROL3 |
+		ISPCSI2_PHY_IRQENABLE_ERRESC3 |
+		ISPCSI2_PHY_IRQENABLE_ERRSOTSYNCHS3 |
+		ISPCSI2_PHY_IRQENABLE_ERRSOTHS3 |
+		ISPCSI2_PHY_IRQENABLE_STATEULPM2 |
+		ISPCSI2_PHY_IRQENABLE_ERRCONTROL2 |
+		ISPCSI2_PHY_IRQENABLE_ERRESC2 |
+		ISPCSI2_PHY_IRQENABLE_ERRSOTSYNCHS2 |
+		ISPCSI2_PHY_IRQENABLE_ERRSOTHS2 |
+		ISPCSI2_PHY_IRQENABLE_STATEULPM1 |
+		ISPCSI2_PHY_IRQENABLE_ERRCONTROL1 |
+		ISPCSI2_PHY_IRQENABLE_ERRESC1 |
+		ISPCSI2_PHY_IRQENABLE_ERRSOTSYNCHS1 |
+		ISPCSI2_PHY_IRQENABLE_ERRSOTHS1;
+	isp_reg_writel(isp, reg, csi2->regs1, ISPCSI2_PHY_IRQSTATUS);
+	if (enable)
+		reg |= isp_reg_readl(isp, csi2->regs1, ISPCSI2_PHY_IRQENABLE);
+	else
+		reg = 0;
+	isp_reg_writel(isp, reg, csi2->regs1, ISPCSI2_PHY_IRQENABLE);
+}
+
+/*
+ * csi2_irq_status_set - Enables CSI2 Status IRQs.
+ * @enable: Enable/disable CSI2 Status interrupts
+ */
+static void csi2_irq_status_set(struct isp_device *isp,
+				struct isp_csi2_device *csi2, int enable)
+{
+	u32 reg;
+	reg = ISPCSI2_IRQSTATUS_OCP_ERR_IRQ |
+		ISPCSI2_IRQSTATUS_SHORT_PACKET_IRQ |
+		ISPCSI2_IRQSTATUS_ECC_CORRECTION_IRQ |
+		ISPCSI2_IRQSTATUS_ECC_NO_CORRECTION_IRQ |
+		ISPCSI2_IRQSTATUS_COMPLEXIO2_ERR_IRQ |
+		ISPCSI2_IRQSTATUS_COMPLEXIO1_ERR_IRQ |
+		ISPCSI2_IRQSTATUS_FIFO_OVF_IRQ |
+		ISPCSI2_IRQSTATUS_CONTEXT(0);
+	isp_reg_writel(isp, reg, csi2->regs1, ISPCSI2_IRQSTATUS);
+	if (enable)
+		reg |= isp_reg_readl(isp, csi2->regs1, ISPCSI2_IRQENABLE);
+	else
+		reg = 0;
+
+	isp_reg_writel(isp, reg, csi2->regs1, ISPCSI2_IRQENABLE);
+}
+
+/*
+ * omap3isp_csi2_reset - Resets the CSI2 module.
+ *
+ * Must be called with the phy lock held.
+ *
+ * Returns 0 if successful, or -EBUSY if power command didn't respond.
+ */
+int omap3isp_csi2_reset(struct isp_csi2_device *csi2)
+{
+	struct isp_device *isp = csi2->isp;
+	u8 soft_reset_retries = 0;
+	u32 reg;
+	int i;
+
+	if (!csi2->available)
+		return -ENODEV;
+
+	if (csi2->phy->phy_in_use)
+		return -EBUSY;
+
+	isp_reg_set(isp, csi2->regs1, ISPCSI2_SYSCONFIG,
+		    ISPCSI2_SYSCONFIG_SOFT_RESET);
+
+	do {
+		reg = isp_reg_readl(isp, csi2->regs1, ISPCSI2_SYSSTATUS) &
+				    ISPCSI2_SYSSTATUS_RESET_DONE;
+		if (reg == ISPCSI2_SYSSTATUS_RESET_DONE)
+			break;
+		soft_reset_retries++;
+		if (soft_reset_retries < 5)
+			udelay(100);
+	} while (soft_reset_retries < 5);
+
+	if (soft_reset_retries == 5) {
+		dev_err(isp->dev, "CSI2: Soft reset try count exceeded!\n");
+		return -EBUSY;
+	}
+
+	if (isp->revision == ISP_REVISION_15_0)
+		isp_reg_set(isp, csi2->regs1, ISPCSI2_PHY_CFG,
+			    ISPCSI2_PHY_CFG_RESET_CTRL);
+
+	i = 100;
+	do {
+		reg = isp_reg_readl(isp, csi2->phy->phy_regs, ISPCSIPHY_REG1)
+		    & ISPCSIPHY_REG1_RESET_DONE_CTRLCLK;
+		if (reg == ISPCSIPHY_REG1_RESET_DONE_CTRLCLK)
+			break;
+		udelay(100);
+	} while (--i > 0);
+
+	if (i == 0) {
+		dev_err(isp->dev,
+			"CSI2: Reset for CSI2_96M_FCLK domain Failed!\n");
+		return -EBUSY;
+	}
+
+	if (isp->autoidle)
+		isp_reg_clr_set(isp, csi2->regs1, ISPCSI2_SYSCONFIG,
+				ISPCSI2_SYSCONFIG_MSTANDBY_MODE_MASK |
+				ISPCSI2_SYSCONFIG_AUTO_IDLE,
+				ISPCSI2_SYSCONFIG_MSTANDBY_MODE_SMART |
+				((isp->revision == ISP_REVISION_15_0) ?
+				 ISPCSI2_SYSCONFIG_AUTO_IDLE : 0));
+	else
+		isp_reg_clr_set(isp, csi2->regs1, ISPCSI2_SYSCONFIG,
+				ISPCSI2_SYSCONFIG_MSTANDBY_MODE_MASK |
+				ISPCSI2_SYSCONFIG_AUTO_IDLE,
+				ISPCSI2_SYSCONFIG_MSTANDBY_MODE_NO);
+
+	return 0;
+}
+
+static int csi2_configure(struct isp_csi2_device *csi2)
+{
+	struct isp_pipeline *pipe = to_isp_pipeline(&csi2->subdev.entity);
+	const struct isp_bus_cfg *buscfg;
+	struct isp_device *isp = csi2->isp;
+	struct isp_csi2_timing_cfg *timing = &csi2->timing[0];
+	struct v4l2_subdev *sensor;
+	struct media_pad *pad;
+
+	/*
+	 * CSI2 fields that can be updated while the context has
+	 * been enabled or the interface has been enabled are not
+	 * updated dynamically currently. So we do not allow to
+	 * reconfigure if either has been enabled
+	 */
+	if (csi2->contexts[0].enabled || csi2->ctrl.if_enable)
+		return -EBUSY;
+
+	pad = media_entity_remote_pad(&csi2->pads[CSI2_PAD_SINK]);
+	sensor = media_entity_to_v4l2_subdev(pad->entity);
+	buscfg = sensor->host_priv;
+
+	csi2->frame_skip = 0;
+	v4l2_subdev_call(sensor, sensor, g_skip_frames, &csi2->frame_skip);
+
+	csi2->ctrl.vp_out_ctrl =
+		clamp_t(unsigned int, pipe->l3_ick / pipe->external_rate - 1,
+			1, 3);
+	dev_dbg(isp->dev, "%s: l3_ick %lu, external_rate %u, vp_out_ctrl %u\n",
+		__func__, pipe->l3_ick,  pipe->external_rate,
+		csi2->ctrl.vp_out_ctrl);
+	csi2->ctrl.frame_mode = ISP_CSI2_FRAME_IMMEDIATE;
+	csi2->ctrl.ecc_enable = buscfg->bus.csi2.crc;
+
+	timing->ionum = 1;
+	timing->force_rx_mode = 1;
+	timing->stop_state_16x = 1;
+	timing->stop_state_4x = 1;
+	timing->stop_state_counter = 0x1FF;
+
+	/*
+	 * The CSI2 receiver can't do any format conversion except DPCM
+	 * decompression, so every set_format call configures both pads
+	 * and enables DPCM decompression as a special case:
+	 */
+	if (csi2->formats[CSI2_PAD_SINK].code !=
+	    csi2->formats[CSI2_PAD_SOURCE].code)
+		csi2->dpcm_decompress = true;
+	else
+		csi2->dpcm_decompress = false;
+
+	csi2->contexts[0].format_id = csi2_ctx_map_format(csi2);
+
+	if (csi2->video_out.bpl_padding == 0)
+		csi2->contexts[0].data_offset = 0;
+	else
+		csi2->contexts[0].data_offset = csi2->video_out.bpl_value;
+
+	/*
+	 * Enable end of frame and end of line signals generation for
+	 * context 0. These signals are generated from CSI2 receiver to
+	 * qualify the last pixel of a frame and the last pixel of a line.
+	 * Without enabling the signals CSI2 receiver writes data to memory
+	 * beyond buffer size and/or data line offset is not handled correctly.
+	 */
+	csi2->contexts[0].eof_enabled = 1;
+	csi2->contexts[0].eol_enabled = 1;
+
+	csi2_irq_complexio1_set(isp, csi2, 1);
+	csi2_irq_ctx_set(isp, csi2, 1);
+	csi2_irq_status_set(isp, csi2, 1);
+
+	/* Set configuration (timings, format and links) */
+	csi2_timing_config(isp, csi2, timing);
+	csi2_recv_config(isp, csi2, &csi2->ctrl);
+	csi2_ctx_config(isp, csi2, &csi2->contexts[0]);
+
+	return 0;
+}
+
+/*
+ * csi2_print_status - Prints CSI2 debug information.
+ */
+#define CSI2_PRINT_REGISTER(isp, regs, name)\
+	dev_dbg(isp->dev, "###CSI2 " #name "=0x%08x\n", \
+		isp_reg_readl(isp, regs, ISPCSI2_##name))
+
+static void csi2_print_status(struct isp_csi2_device *csi2)
+{
+	struct isp_device *isp = csi2->isp;
+
+	if (!csi2->available)
+		return;
+
+	dev_dbg(isp->dev, "-------------CSI2 Register dump-------------\n");
+
+	CSI2_PRINT_REGISTER(isp, csi2->regs1, SYSCONFIG);
+	CSI2_PRINT_REGISTER(isp, csi2->regs1, SYSSTATUS);
+	CSI2_PRINT_REGISTER(isp, csi2->regs1, IRQENABLE);
+	CSI2_PRINT_REGISTER(isp, csi2->regs1, IRQSTATUS);
+	CSI2_PRINT_REGISTER(isp, csi2->regs1, CTRL);
+	CSI2_PRINT_REGISTER(isp, csi2->regs1, DBG_H);
+	CSI2_PRINT_REGISTER(isp, csi2->regs1, GNQ);
+	CSI2_PRINT_REGISTER(isp, csi2->regs1, PHY_CFG);
+	CSI2_PRINT_REGISTER(isp, csi2->regs1, PHY_IRQSTATUS);
+	CSI2_PRINT_REGISTER(isp, csi2->regs1, SHORT_PACKET);
+	CSI2_PRINT_REGISTER(isp, csi2->regs1, PHY_IRQENABLE);
+	CSI2_PRINT_REGISTER(isp, csi2->regs1, DBG_P);
+	CSI2_PRINT_REGISTER(isp, csi2->regs1, TIMING);
+	CSI2_PRINT_REGISTER(isp, csi2->regs1, CTX_CTRL1(0));
+	CSI2_PRINT_REGISTER(isp, csi2->regs1, CTX_CTRL2(0));
+	CSI2_PRINT_REGISTER(isp, csi2->regs1, CTX_DAT_OFST(0));
+	CSI2_PRINT_REGISTER(isp, csi2->regs1, CTX_DAT_PING_ADDR(0));
+	CSI2_PRINT_REGISTER(isp, csi2->regs1, CTX_DAT_PONG_ADDR(0));
+	CSI2_PRINT_REGISTER(isp, csi2->regs1, CTX_IRQENABLE(0));
+	CSI2_PRINT_REGISTER(isp, csi2->regs1, CTX_IRQSTATUS(0));
+	CSI2_PRINT_REGISTER(isp, csi2->regs1, CTX_CTRL3(0));
+
+	dev_dbg(isp->dev, "--------------------------------------------\n");
+}
+
+/* -----------------------------------------------------------------------------
+ * Interrupt handling
+ */
+
+/*
+ * csi2_isr_buffer - Does buffer handling at end-of-frame
+ * when writing to memory.
+ */
+static void csi2_isr_buffer(struct isp_csi2_device *csi2)
+{
+	struct isp_device *isp = csi2->isp;
+	struct isp_buffer *buffer;
+
+	csi2_ctx_enable(isp, csi2, 0, 0);
+
+	buffer = omap3isp_video_buffer_next(&csi2->video_out);
+
+	/*
+	 * Let video queue operation restart engine if there is an underrun
+	 * condition.
+	 */
+	if (buffer == NULL)
+		return;
+
+	csi2_set_outaddr(csi2, buffer->dma);
+	csi2_ctx_enable(isp, csi2, 0, 1);
+}
+
+static void csi2_isr_ctx(struct isp_csi2_device *csi2,
+			 struct isp_csi2_ctx_cfg *ctx)
+{
+	struct isp_device *isp = csi2->isp;
+	unsigned int n = ctx->ctxnum;
+	u32 status;
+
+	status = isp_reg_readl(isp, csi2->regs1, ISPCSI2_CTX_IRQSTATUS(n));
+	isp_reg_writel(isp, status, csi2->regs1, ISPCSI2_CTX_IRQSTATUS(n));
+
+	if (!(status & ISPCSI2_CTX_IRQSTATUS_FE_IRQ))
+		return;
+
+	/* Skip interrupts until we reach the frame skip count. The CSI2 will be
+	 * automatically disabled, as the frame skip count has been programmed
+	 * in the CSI2_CTx_CTRL1::COUNT field, so reenable it.
+	 *
+	 * It would have been nice to rely on the FRAME_NUMBER interrupt instead
+	 * but it turned out that the interrupt is only generated when the CSI2
+	 * writes to memory (the CSI2_CTx_CTRL1::COUNT field is decreased
+	 * correctly and reaches 0 when data is forwarded to the video port only
+	 * but no interrupt arrives). Maybe a CSI2 hardware bug.
+	 */
+	if (csi2->frame_skip) {
+		csi2->frame_skip--;
+		if (csi2->frame_skip == 0) {
+			ctx->format_id = csi2_ctx_map_format(csi2);
+			csi2_ctx_config(isp, csi2, ctx);
+			csi2_ctx_enable(isp, csi2, n, 1);
+		}
+		return;
+	}
+
+	if (csi2->output & CSI2_OUTPUT_MEMORY)
+		csi2_isr_buffer(csi2);
+}
+
+/*
+ * omap3isp_csi2_isr - CSI2 interrupt handling.
+ */
+void omap3isp_csi2_isr(struct isp_csi2_device *csi2)
+{
+	struct isp_pipeline *pipe = to_isp_pipeline(&csi2->subdev.entity);
+	u32 csi2_irqstatus, cpxio1_irqstatus;
+	struct isp_device *isp = csi2->isp;
+
+	if (!csi2->available)
+		return;
+
+	csi2_irqstatus = isp_reg_readl(isp, csi2->regs1, ISPCSI2_IRQSTATUS);
+	isp_reg_writel(isp, csi2_irqstatus, csi2->regs1, ISPCSI2_IRQSTATUS);
+
+	/* Failure Cases */
+	if (csi2_irqstatus & ISPCSI2_IRQSTATUS_COMPLEXIO1_ERR_IRQ) {
+		cpxio1_irqstatus = isp_reg_readl(isp, csi2->regs1,
+						 ISPCSI2_PHY_IRQSTATUS);
+		isp_reg_writel(isp, cpxio1_irqstatus,
+			       csi2->regs1, ISPCSI2_PHY_IRQSTATUS);
+		dev_dbg(isp->dev, "CSI2: ComplexIO Error IRQ "
+			"%x\n", cpxio1_irqstatus);
+		pipe->error = true;
+	}
+
+	if (csi2_irqstatus & (ISPCSI2_IRQSTATUS_OCP_ERR_IRQ |
+			      ISPCSI2_IRQSTATUS_SHORT_PACKET_IRQ |
+			      ISPCSI2_IRQSTATUS_ECC_NO_CORRECTION_IRQ |
+			      ISPCSI2_IRQSTATUS_COMPLEXIO2_ERR_IRQ |
+			      ISPCSI2_IRQSTATUS_FIFO_OVF_IRQ)) {
+		dev_dbg(isp->dev, "CSI2 Err:"
+			" OCP:%d,"
+			" Short_pack:%d,"
+			" ECC:%d,"
+			" CPXIO2:%d,"
+			" FIFO_OVF:%d,"
+			"\n",
+			(csi2_irqstatus &
+			 ISPCSI2_IRQSTATUS_OCP_ERR_IRQ) ? 1 : 0,
+			(csi2_irqstatus &
+			 ISPCSI2_IRQSTATUS_SHORT_PACKET_IRQ) ? 1 : 0,
+			(csi2_irqstatus &
+			 ISPCSI2_IRQSTATUS_ECC_NO_CORRECTION_IRQ) ? 1 : 0,
+			(csi2_irqstatus &
+			 ISPCSI2_IRQSTATUS_COMPLEXIO2_ERR_IRQ) ? 1 : 0,
+			(csi2_irqstatus &
+			 ISPCSI2_IRQSTATUS_FIFO_OVF_IRQ) ? 1 : 0);
+		pipe->error = true;
+	}
+
+	if (omap3isp_module_sync_is_stopping(&csi2->wait, &csi2->stopping))
+		return;
+
+	/* Successful cases */
+	if (csi2_irqstatus & ISPCSI2_IRQSTATUS_CONTEXT(0))
+		csi2_isr_ctx(csi2, &csi2->contexts[0]);
+
+	if (csi2_irqstatus & ISPCSI2_IRQSTATUS_ECC_CORRECTION_IRQ)
+		dev_dbg(isp->dev, "CSI2: ECC correction done\n");
+}
+
+/* -----------------------------------------------------------------------------
+ * ISP video operations
+ */
+
+/*
+ * csi2_queue - Queues the first buffer when using memory output
+ * @video: The video node
+ * @buffer: buffer to queue
+ */
+static int csi2_queue(struct isp_video *video, struct isp_buffer *buffer)
+{
+	struct isp_device *isp = video->isp;
+	struct isp_csi2_device *csi2 = &isp->isp_csi2a;
+
+	csi2_set_outaddr(csi2, buffer->dma);
+
+	/*
+	 * If streaming was enabled before there was a buffer queued
+	 * or underrun happened in the ISR, the hardware was not enabled
+	 * and DMA queue flag ISP_VIDEO_DMAQUEUE_UNDERRUN is still set.
+	 * Enable it now.
+	 */
+	if (csi2->video_out.dmaqueue_flags & ISP_VIDEO_DMAQUEUE_UNDERRUN) {
+		/* Enable / disable context 0 and IRQs */
+		csi2_if_enable(isp, csi2, 1);
+		csi2_ctx_enable(isp, csi2, 0, 1);
+		isp_video_dmaqueue_flags_clr(&csi2->video_out);
+	}
+
+	return 0;
+}
+
+static const struct isp_video_operations csi2_ispvideo_ops = {
+	.queue = csi2_queue,
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev operations
+ */
+
+static struct v4l2_mbus_framefmt *
+__csi2_get_format(struct isp_csi2_device *csi2, struct v4l2_subdev_pad_config *cfg,
+		  unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(&csi2->subdev, cfg, pad);
+	else
+		return &csi2->formats[pad];
+}
+
+static void
+csi2_try_format(struct isp_csi2_device *csi2, struct v4l2_subdev_pad_config *cfg,
+		unsigned int pad, struct v4l2_mbus_framefmt *fmt,
+		enum v4l2_subdev_format_whence which)
+{
+	u32 pixelcode;
+	struct v4l2_mbus_framefmt *format;
+	const struct isp_format_info *info;
+	unsigned int i;
+
+	switch (pad) {
+	case CSI2_PAD_SINK:
+		/* Clamp the width and height to valid range (1-8191). */
+		for (i = 0; i < ARRAY_SIZE(csi2_input_fmts); i++) {
+			if (fmt->code == csi2_input_fmts[i])
+				break;
+		}
+
+		/* If not found, use SGRBG10 as default */
+		if (i >= ARRAY_SIZE(csi2_input_fmts))
+			fmt->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+
+		fmt->width = clamp_t(u32, fmt->width, 1, 8191);
+		fmt->height = clamp_t(u32, fmt->height, 1, 8191);
+		break;
+
+	case CSI2_PAD_SOURCE:
+		/* Source format same as sink format, except for DPCM
+		 * compression.
+		 */
+		pixelcode = fmt->code;
+		format = __csi2_get_format(csi2, cfg, CSI2_PAD_SINK, which);
+		memcpy(fmt, format, sizeof(*fmt));
+
+		/*
+		 * Only Allow DPCM decompression, and check that the
+		 * pattern is preserved
+		 */
+		info = omap3isp_video_format_info(fmt->code);
+		if (info->uncompressed == pixelcode)
+			fmt->code = pixelcode;
+		break;
+	}
+
+	/* RGB, non-interlaced */
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+	fmt->field = V4L2_FIELD_NONE;
+}
+
+/*
+ * csi2_enum_mbus_code - Handle pixel format enumeration
+ * @sd     : pointer to v4l2 subdev structure
+ * @cfg: V4L2 subdev pad configuration
+ * @code   : pointer to v4l2_subdev_mbus_code_enum structure
+ * return -EINVAL or zero on success
+ */
+static int csi2_enum_mbus_code(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_pad_config *cfg,
+			       struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct isp_csi2_device *csi2 = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+	const struct isp_format_info *info;
+
+	if (code->pad == CSI2_PAD_SINK) {
+		if (code->index >= ARRAY_SIZE(csi2_input_fmts))
+			return -EINVAL;
+
+		code->code = csi2_input_fmts[code->index];
+	} else {
+		format = __csi2_get_format(csi2, cfg, CSI2_PAD_SINK,
+					   code->which);
+		switch (code->index) {
+		case 0:
+			/* Passthrough sink pad code */
+			code->code = format->code;
+			break;
+		case 1:
+			/* Uncompressed code */
+			info = omap3isp_video_format_info(format->code);
+			if (info->uncompressed == format->code)
+				return -EINVAL;
+
+			code->code = info->uncompressed;
+			break;
+		default:
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+static int csi2_enum_frame_size(struct v4l2_subdev *sd,
+				struct v4l2_subdev_pad_config *cfg,
+				struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct isp_csi2_device *csi2 = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt format;
+
+	if (fse->index != 0)
+		return -EINVAL;
+
+	format.code = fse->code;
+	format.width = 1;
+	format.height = 1;
+	csi2_try_format(csi2, cfg, fse->pad, &format, fse->which);
+	fse->min_width = format.width;
+	fse->min_height = format.height;
+
+	if (format.code != fse->code)
+		return -EINVAL;
+
+	format.code = fse->code;
+	format.width = -1;
+	format.height = -1;
+	csi2_try_format(csi2, cfg, fse->pad, &format, fse->which);
+	fse->max_width = format.width;
+	fse->max_height = format.height;
+
+	return 0;
+}
+
+/*
+ * csi2_get_format - Handle get format by pads subdev method
+ * @sd : pointer to v4l2 subdev structure
+ * @cfg: V4L2 subdev pad configuration
+ * @fmt: pointer to v4l2 subdev format structure
+ * return -EINVAL or zero on success
+ */
+static int csi2_get_format(struct v4l2_subdev *sd, struct v4l2_subdev_pad_config *cfg,
+			   struct v4l2_subdev_format *fmt)
+{
+	struct isp_csi2_device *csi2 = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __csi2_get_format(csi2, cfg, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
+
+	fmt->format = *format;
+	return 0;
+}
+
+/*
+ * csi2_set_format - Handle set format by pads subdev method
+ * @sd : pointer to v4l2 subdev structure
+ * @cfg: V4L2 subdev pad configuration
+ * @fmt: pointer to v4l2 subdev format structure
+ * return -EINVAL or zero on success
+ */
+static int csi2_set_format(struct v4l2_subdev *sd, struct v4l2_subdev_pad_config *cfg,
+			   struct v4l2_subdev_format *fmt)
+{
+	struct isp_csi2_device *csi2 = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __csi2_get_format(csi2, cfg, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
+
+	csi2_try_format(csi2, cfg, fmt->pad, &fmt->format, fmt->which);
+	*format = fmt->format;
+
+	/* Propagate the format from sink to source */
+	if (fmt->pad == CSI2_PAD_SINK) {
+		format = __csi2_get_format(csi2, cfg, CSI2_PAD_SOURCE,
+					   fmt->which);
+		*format = fmt->format;
+		csi2_try_format(csi2, cfg, CSI2_PAD_SOURCE, format, fmt->which);
+	}
+
+	return 0;
+}
+
+/*
+ * csi2_init_formats - Initialize formats on all pads
+ * @sd: ISP CSI2 V4L2 subdevice
+ * @fh: V4L2 subdev file handle
+ *
+ * Initialize all pad formats with default values. If fh is not NULL, try
+ * formats are initialized on the file handle. Otherwise active formats are
+ * initialized on the device.
+ */
+static int csi2_init_formats(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_subdev_format format;
+
+	memset(&format, 0, sizeof(format));
+	format.pad = CSI2_PAD_SINK;
+	format.which = fh ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE;
+	format.format.code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	format.format.width = 4096;
+	format.format.height = 4096;
+	csi2_set_format(sd, fh ? fh->pad : NULL, &format);
+
+	return 0;
+}
+
+/*
+ * csi2_set_stream - Enable/Disable streaming on the CSI2 module
+ * @sd: ISP CSI2 V4L2 subdevice
+ * @enable: ISP pipeline stream state
+ *
+ * Return 0 on success or a negative error code otherwise.
+ */
+static int csi2_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct isp_csi2_device *csi2 = v4l2_get_subdevdata(sd);
+	struct isp_device *isp = csi2->isp;
+	struct isp_video *video_out = &csi2->video_out;
+
+	switch (enable) {
+	case ISP_PIPELINE_STREAM_CONTINUOUS:
+		if (omap3isp_csiphy_acquire(csi2->phy) < 0)
+			return -ENODEV;
+		if (csi2->output & CSI2_OUTPUT_MEMORY)
+			omap3isp_sbl_enable(isp, OMAP3_ISP_SBL_CSI2A_WRITE);
+		csi2_configure(csi2);
+		csi2_print_status(csi2);
+
+		/*
+		 * When outputting to memory with no buffer available, let the
+		 * buffer queue handler start the hardware. A DMA queue flag
+		 * ISP_VIDEO_DMAQUEUE_QUEUED will be set as soon as there is
+		 * a buffer available.
+		 */
+		if (csi2->output & CSI2_OUTPUT_MEMORY &&
+		    !(video_out->dmaqueue_flags & ISP_VIDEO_DMAQUEUE_QUEUED))
+			break;
+		/* Enable context 0 and IRQs */
+		atomic_set(&csi2->stopping, 0);
+		csi2_ctx_enable(isp, csi2, 0, 1);
+		csi2_if_enable(isp, csi2, 1);
+		isp_video_dmaqueue_flags_clr(video_out);
+		break;
+
+	case ISP_PIPELINE_STREAM_STOPPED:
+		if (csi2->state == ISP_PIPELINE_STREAM_STOPPED)
+			return 0;
+		if (omap3isp_module_sync_idle(&sd->entity, &csi2->wait,
+					      &csi2->stopping))
+			dev_dbg(isp->dev, "%s: module stop timeout.\n",
+				sd->name);
+		csi2_ctx_enable(isp, csi2, 0, 0);
+		csi2_if_enable(isp, csi2, 0);
+		csi2_irq_ctx_set(isp, csi2, 0);
+		omap3isp_csiphy_release(csi2->phy);
+		isp_video_dmaqueue_flags_clr(video_out);
+		omap3isp_sbl_disable(isp, OMAP3_ISP_SBL_CSI2A_WRITE);
+		break;
+	}
+
+	csi2->state = enable;
+	return 0;
+}
+
+/* subdev video operations */
+static const struct v4l2_subdev_video_ops csi2_video_ops = {
+	.s_stream = csi2_set_stream,
+};
+
+/* subdev pad operations */
+static const struct v4l2_subdev_pad_ops csi2_pad_ops = {
+	.enum_mbus_code = csi2_enum_mbus_code,
+	.enum_frame_size = csi2_enum_frame_size,
+	.get_fmt = csi2_get_format,
+	.set_fmt = csi2_set_format,
+};
+
+/* subdev operations */
+static const struct v4l2_subdev_ops csi2_ops = {
+	.video = &csi2_video_ops,
+	.pad = &csi2_pad_ops,
+};
+
+/* subdev internal operations */
+static const struct v4l2_subdev_internal_ops csi2_internal_ops = {
+	.open = csi2_init_formats,
+};
+
+/* -----------------------------------------------------------------------------
+ * Media entity operations
+ */
+
+/*
+ * csi2_link_setup - Setup CSI2 connections.
+ * @entity : Pointer to media entity structure
+ * @local  : Pointer to local pad array
+ * @remote : Pointer to remote pad array
+ * @flags  : Link flags
+ * return -EINVAL or zero on success
+ */
+static int csi2_link_setup(struct media_entity *entity,
+			   const struct media_pad *local,
+			   const struct media_pad *remote, u32 flags)
+{
+	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+	struct isp_csi2_device *csi2 = v4l2_get_subdevdata(sd);
+	struct isp_csi2_ctrl_cfg *ctrl = &csi2->ctrl;
+
+	/*
+	 * The ISP core doesn't support pipelines with multiple video outputs.
+	 * Revisit this when it will be implemented, and return -EBUSY for now.
+	 */
+
+	switch (local->index | media_entity_type(remote->entity)) {
+	case CSI2_PAD_SOURCE | MEDIA_ENT_T_DEVNODE:
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (csi2->output & ~CSI2_OUTPUT_MEMORY)
+				return -EBUSY;
+			csi2->output |= CSI2_OUTPUT_MEMORY;
+		} else {
+			csi2->output &= ~CSI2_OUTPUT_MEMORY;
+		}
+		break;
+
+	case CSI2_PAD_SOURCE | MEDIA_ENT_T_V4L2_SUBDEV:
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (csi2->output & ~CSI2_OUTPUT_CCDC)
+				return -EBUSY;
+			csi2->output |= CSI2_OUTPUT_CCDC;
+		} else {
+			csi2->output &= ~CSI2_OUTPUT_CCDC;
+		}
+		break;
+
+	default:
+		/* Link from camera to CSI2 is fixed... */
+		return -EINVAL;
+	}
+
+	ctrl->vp_only_enable =
+		(csi2->output & CSI2_OUTPUT_MEMORY) ? false : true;
+	ctrl->vp_clk_enable = !!(csi2->output & CSI2_OUTPUT_CCDC);
+
+	return 0;
+}
+
+/* media operations */
+static const struct media_entity_operations csi2_media_ops = {
+	.link_setup = csi2_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+void omap3isp_csi2_unregister_entities(struct isp_csi2_device *csi2)
+{
+	v4l2_device_unregister_subdev(&csi2->subdev);
+	omap3isp_video_unregister(&csi2->video_out);
+}
+
+int omap3isp_csi2_register_entities(struct isp_csi2_device *csi2,
+				    struct v4l2_device *vdev)
+{
+	int ret;
+
+	/* Register the subdev and video nodes. */
+	ret = v4l2_device_register_subdev(vdev, &csi2->subdev);
+	if (ret < 0)
+		goto error;
+
+	ret = omap3isp_video_register(&csi2->video_out, vdev);
+	if (ret < 0)
+		goto error;
+
+	return 0;
+
+error:
+	omap3isp_csi2_unregister_entities(csi2);
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * ISP CSI2 initialisation and cleanup
+ */
+
+/*
+ * csi2_init_entities - Initialize subdev and media entity.
+ * @csi2: Pointer to csi2 structure.
+ * return -ENOMEM or zero on success
+ */
+static int csi2_init_entities(struct isp_csi2_device *csi2)
+{
+	struct v4l2_subdev *sd = &csi2->subdev;
+	struct media_pad *pads = csi2->pads;
+	struct media_entity *me = &sd->entity;
+	int ret;
+
+	v4l2_subdev_init(sd, &csi2_ops);
+	sd->internal_ops = &csi2_internal_ops;
+	strlcpy(sd->name, "OMAP3 ISP CSI2a", sizeof(sd->name));
+
+	sd->grp_id = 1 << 16;	/* group ID for isp subdevs */
+	v4l2_set_subdevdata(sd, csi2);
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	pads[CSI2_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
+	pads[CSI2_PAD_SINK].flags = MEDIA_PAD_FL_SINK
+				    | MEDIA_PAD_FL_MUST_CONNECT;
+
+	me->ops = &csi2_media_ops;
+	ret = media_entity_init(me, CSI2_PADS_NUM, pads, 0);
+	if (ret < 0)
+		return ret;
+
+	csi2_init_formats(sd, NULL);
+
+	/* Video device node */
+	csi2->video_out.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	csi2->video_out.ops = &csi2_ispvideo_ops;
+	csi2->video_out.bpl_alignment = 32;
+	csi2->video_out.bpl_zero_padding = 1;
+	csi2->video_out.bpl_max = 0x1ffe0;
+	csi2->video_out.isp = csi2->isp;
+	csi2->video_out.capture_mem = PAGE_ALIGN(4096 * 4096) * 3;
+
+	ret = omap3isp_video_init(&csi2->video_out, "CSI2a");
+	if (ret < 0)
+		goto error_video;
+
+	/* Connect the CSI2 subdev to the video node. */
+	ret = media_entity_create_link(&csi2->subdev.entity, CSI2_PAD_SOURCE,
+				       &csi2->video_out.video.entity, 0, 0);
+	if (ret < 0)
+		goto error_link;
+
+	return 0;
+
+error_link:
+	omap3isp_video_cleanup(&csi2->video_out);
+error_video:
+	media_entity_cleanup(&csi2->subdev.entity);
+	return ret;
+}
+
+/*
+ * omap3isp_csi2_init - Routine for module driver init
+ */
+int omap3isp_csi2_init(struct isp_device *isp)
+{
+	struct isp_csi2_device *csi2a = &isp->isp_csi2a;
+	struct isp_csi2_device *csi2c = &isp->isp_csi2c;
+	int ret;
+
+	csi2a->isp = isp;
+	csi2a->available = 1;
+	csi2a->regs1 = OMAP3_ISP_IOMEM_CSI2A_REGS1;
+	csi2a->regs2 = OMAP3_ISP_IOMEM_CSI2A_REGS2;
+	csi2a->phy = &isp->isp_csiphy2;
+	csi2a->state = ISP_PIPELINE_STREAM_STOPPED;
+	init_waitqueue_head(&csi2a->wait);
+
+	ret = csi2_init_entities(csi2a);
+	if (ret < 0)
+		return ret;
+
+	if (isp->revision == ISP_REVISION_15_0) {
+		csi2c->isp = isp;
+		csi2c->available = 1;
+		csi2c->regs1 = OMAP3_ISP_IOMEM_CSI2C_REGS1;
+		csi2c->regs2 = OMAP3_ISP_IOMEM_CSI2C_REGS2;
+		csi2c->phy = &isp->isp_csiphy1;
+		csi2c->state = ISP_PIPELINE_STREAM_STOPPED;
+		init_waitqueue_head(&csi2c->wait);
+	}
+
+	return 0;
+}
+
+/*
+ * omap3isp_csi2_cleanup - Routine for module driver cleanup
+ */
+void omap3isp_csi2_cleanup(struct isp_device *isp)
+{
+	struct isp_csi2_device *csi2a = &isp->isp_csi2a;
+
+	omap3isp_video_cleanup(&csi2a->video_out);
+	media_entity_cleanup(&csi2a->subdev.entity);
+}
diff --git a/drivers/media/platform/omap3isp/ispcsi2.h b/drivers/media/platform/omap3isp/ispcsi2.h
new file mode 100644
index 0000000..453ed62
--- /dev/null
+++ b/drivers/media/platform/omap3isp/ispcsi2.h
@@ -0,0 +1,155 @@
+/*
+ * ispcsi2.h
+ *
+ * TI OMAP3 ISP - CSI2 module
+ *
+ * Copyright (C) 2010 Nokia Corporation
+ * Copyright (C) 2009 Texas Instruments, Inc.
+ *
+ * Contacts: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *	     Sakari Ailus <sakari.ailus@iki.fi>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef OMAP3_ISP_CSI2_H
+#define OMAP3_ISP_CSI2_H
+
+#include <linux/types.h>
+#include <linux/videodev2.h>
+
+struct isp_csiphy;
+
+/* This is not an exhaustive list */
+enum isp_csi2_pix_formats {
+	CSI2_PIX_FMT_OTHERS = 0,
+	CSI2_PIX_FMT_YUV422_8BIT = 0x1e,
+	CSI2_PIX_FMT_YUV422_8BIT_VP = 0x9e,
+	CSI2_PIX_FMT_RAW10_EXP16 = 0xab,
+	CSI2_PIX_FMT_RAW10_EXP16_VP = 0x12f,
+	CSI2_PIX_FMT_RAW8 = 0x2a,
+	CSI2_PIX_FMT_RAW8_DPCM10_EXP16 = 0x2aa,
+	CSI2_PIX_FMT_RAW8_DPCM10_VP = 0x32a,
+	CSI2_PIX_FMT_RAW8_VP = 0x12a,
+	CSI2_USERDEF_8BIT_DATA1_DPCM10_VP = 0x340,
+	CSI2_USERDEF_8BIT_DATA1_DPCM10 = 0x2c0,
+	CSI2_USERDEF_8BIT_DATA1 = 0x40,
+};
+
+enum isp_csi2_irqevents {
+	OCP_ERR_IRQ = 0x4000,
+	SHORT_PACKET_IRQ = 0x2000,
+	ECC_CORRECTION_IRQ = 0x1000,
+	ECC_NO_CORRECTION_IRQ = 0x800,
+	COMPLEXIO2_ERR_IRQ = 0x400,
+	COMPLEXIO1_ERR_IRQ = 0x200,
+	FIFO_OVF_IRQ = 0x100,
+	CONTEXT7 = 0x80,
+	CONTEXT6 = 0x40,
+	CONTEXT5 = 0x20,
+	CONTEXT4 = 0x10,
+	CONTEXT3 = 0x8,
+	CONTEXT2 = 0x4,
+	CONTEXT1 = 0x2,
+	CONTEXT0 = 0x1,
+};
+
+enum isp_csi2_ctx_irqevents {
+	CTX_ECC_CORRECTION = 0x100,
+	CTX_LINE_NUMBER = 0x80,
+	CTX_FRAME_NUMBER = 0x40,
+	CTX_CS = 0x20,
+	CTX_LE = 0x8,
+	CTX_LS = 0x4,
+	CTX_FE = 0x2,
+	CTX_FS = 0x1,
+};
+
+enum isp_csi2_frame_mode {
+	ISP_CSI2_FRAME_IMMEDIATE,
+	ISP_CSI2_FRAME_AFTERFEC,
+};
+
+#define ISP_CSI2_MAX_CTX_NUM	7
+
+struct isp_csi2_ctx_cfg {
+	u8 ctxnum;		/* context number 0 - 7 */
+	u8 dpcm_decompress;
+
+	/* Fields in CSI2_CTx_CTRL2 - locked by CSI2_CTx_CTRL1.CTX_EN */
+	u8 virtual_id;
+	u16 format_id;		/* as in CSI2_CTx_CTRL2[9:0] */
+	u8 dpcm_predictor;	/* 1: simple, 0: advanced */
+
+	/* Fields in CSI2_CTx_CTRL1/3 - Shadowed */
+	u16 alpha;
+	u16 data_offset;
+	u32 ping_addr;
+	u32 pong_addr;
+	u8 eof_enabled;
+	u8 eol_enabled;
+	u8 checksum_enabled;
+	u8 enabled;
+};
+
+struct isp_csi2_timing_cfg {
+	u8 ionum;			/* IO1 or IO2 as in CSI2_TIMING */
+	unsigned force_rx_mode:1;
+	unsigned stop_state_16x:1;
+	unsigned stop_state_4x:1;
+	u16 stop_state_counter;
+};
+
+struct isp_csi2_ctrl_cfg {
+	bool vp_clk_enable;
+	bool vp_only_enable;
+	u8 vp_out_ctrl;
+	enum isp_csi2_frame_mode frame_mode;
+	bool ecc_enable;
+	bool if_enable;
+};
+
+#define CSI2_PAD_SINK		0
+#define CSI2_PAD_SOURCE		1
+#define CSI2_PADS_NUM		2
+
+#define CSI2_OUTPUT_CCDC	(1 << 0)
+#define CSI2_OUTPUT_MEMORY	(1 << 1)
+
+struct isp_csi2_device {
+	struct v4l2_subdev subdev;
+	struct media_pad pads[CSI2_PADS_NUM];
+	struct v4l2_mbus_framefmt formats[CSI2_PADS_NUM];
+
+	struct isp_video video_out;
+	struct isp_device *isp;
+
+	u8 available;		/* Is the IP present on the silicon? */
+
+	/* mem resources - enums as defined in enum isp_mem_resources */
+	u8 regs1;
+	u8 regs2;
+
+	u32 output; /* output to CCDC, memory or both? */
+	bool dpcm_decompress;
+	unsigned int frame_skip;
+
+	struct isp_csiphy *phy;
+	struct isp_csi2_ctx_cfg contexts[ISP_CSI2_MAX_CTX_NUM + 1];
+	struct isp_csi2_timing_cfg timing[2];
+	struct isp_csi2_ctrl_cfg ctrl;
+	enum isp_pipeline_stream_state state;
+	wait_queue_head_t wait;
+	atomic_t stopping;
+};
+
+void omap3isp_csi2_isr(struct isp_csi2_device *csi2);
+int omap3isp_csi2_reset(struct isp_csi2_device *csi2);
+int omap3isp_csi2_init(struct isp_device *isp);
+void omap3isp_csi2_cleanup(struct isp_device *isp);
+void omap3isp_csi2_unregister_entities(struct isp_csi2_device *csi2);
+int omap3isp_csi2_register_entities(struct isp_csi2_device *csi2,
+				    struct v4l2_device *vdev);
+#endif	/* OMAP3_ISP_CSI2_H */
diff --git a/drivers/media/platform/omap3isp/ispcsiphy.c b/drivers/media/platform/omap3isp/ispcsiphy.c
new file mode 100644
index 0000000..495447d
--- /dev/null
+++ b/drivers/media/platform/omap3isp/ispcsiphy.c
@@ -0,0 +1,347 @@
+/*
+ * ispcsiphy.c
+ *
+ * TI OMAP3 ISP - CSI PHY module
+ *
+ * Copyright (C) 2010 Nokia Corporation
+ * Copyright (C) 2009 Texas Instruments, Inc.
+ *
+ * Contacts: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *	     Sakari Ailus <sakari.ailus@iki.fi>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/regmap.h>
+#include <linux/regulator/consumer.h>
+
+#include "isp.h"
+#include "ispreg.h"
+#include "ispcsiphy.h"
+
+static void csiphy_routing_cfg_3630(struct isp_csiphy *phy,
+				    enum isp_interface_type iface,
+				    bool ccp2_strobe)
+{
+	u32 reg;
+	u32 shift, mode;
+
+	regmap_read(phy->isp->syscon, phy->isp->syscon_offset, &reg);
+
+	switch (iface) {
+	default:
+	/* Should not happen in practice, but let's keep the compiler happy. */
+	case ISP_INTERFACE_CCP2B_PHY1:
+		reg &= ~OMAP3630_CONTROL_CAMERA_PHY_CTRL_CSI1_RX_SEL_PHY2;
+		shift = OMAP3630_CONTROL_CAMERA_PHY_CTRL_CAMMODE_PHY1_SHIFT;
+		break;
+	case ISP_INTERFACE_CSI2C_PHY1:
+		shift = OMAP3630_CONTROL_CAMERA_PHY_CTRL_CAMMODE_PHY1_SHIFT;
+		mode = OMAP3630_CONTROL_CAMERA_PHY_CTRL_CAMMODE_DPHY;
+		break;
+	case ISP_INTERFACE_CCP2B_PHY2:
+		reg |= OMAP3630_CONTROL_CAMERA_PHY_CTRL_CSI1_RX_SEL_PHY2;
+		shift = OMAP3630_CONTROL_CAMERA_PHY_CTRL_CAMMODE_PHY2_SHIFT;
+		break;
+	case ISP_INTERFACE_CSI2A_PHY2:
+		shift = OMAP3630_CONTROL_CAMERA_PHY_CTRL_CAMMODE_PHY2_SHIFT;
+		mode = OMAP3630_CONTROL_CAMERA_PHY_CTRL_CAMMODE_DPHY;
+		break;
+	}
+
+	/* Select data/clock or data/strobe mode for CCP2 */
+	if (iface == ISP_INTERFACE_CCP2B_PHY1 ||
+	    iface == ISP_INTERFACE_CCP2B_PHY2) {
+		if (ccp2_strobe)
+			mode = OMAP3630_CONTROL_CAMERA_PHY_CTRL_CAMMODE_CCP2_DATA_STROBE;
+		else
+			mode = OMAP3630_CONTROL_CAMERA_PHY_CTRL_CAMMODE_CCP2_DATA_CLOCK;
+	}
+
+	reg &= ~(OMAP3630_CONTROL_CAMERA_PHY_CTRL_CAMMODE_MASK << shift);
+	reg |= mode << shift;
+
+	regmap_write(phy->isp->syscon, phy->isp->syscon_offset, reg);
+}
+
+static void csiphy_routing_cfg_3430(struct isp_csiphy *phy, u32 iface, bool on,
+				    bool ccp2_strobe)
+{
+	u32 csirxfe = OMAP343X_CONTROL_CSIRXFE_PWRDNZ
+		| OMAP343X_CONTROL_CSIRXFE_RESET;
+
+	/* Only the CCP2B on PHY1 is configurable. */
+	if (iface != ISP_INTERFACE_CCP2B_PHY1)
+		return;
+
+	if (!on) {
+		regmap_write(phy->isp->syscon, phy->isp->syscon_offset, 0);
+		return;
+	}
+
+	if (ccp2_strobe)
+		csirxfe |= OMAP343X_CONTROL_CSIRXFE_SELFORM;
+
+	regmap_write(phy->isp->syscon, phy->isp->syscon_offset, csirxfe);
+}
+
+/*
+ * Configure OMAP 3 CSI PHY routing.
+ * @phy: relevant phy device
+ * @iface: ISP_INTERFACE_*
+ * @on: power on or off
+ * @ccp2_strobe: false: data/clock, true: data/strobe
+ *
+ * Note that the underlying routing configuration registers are part of the
+ * control (SCM) register space and part of the CORE power domain on both 3430
+ * and 3630, so they will not hold their contents in off-mode. This isn't an
+ * issue since the MPU power domain is forced on whilst the ISP is in use.
+ */
+static void csiphy_routing_cfg(struct isp_csiphy *phy,
+			       enum isp_interface_type iface, bool on,
+			       bool ccp2_strobe)
+{
+	if (phy->isp->phy_type == ISP_PHY_TYPE_3630 && on)
+		return csiphy_routing_cfg_3630(phy, iface, ccp2_strobe);
+	if (phy->isp->phy_type == ISP_PHY_TYPE_3430)
+		return csiphy_routing_cfg_3430(phy, iface, on, ccp2_strobe);
+}
+
+/*
+ * csiphy_power_autoswitch_enable
+ * @enable: Sets or clears the autoswitch function enable flag.
+ */
+static void csiphy_power_autoswitch_enable(struct isp_csiphy *phy, bool enable)
+{
+	isp_reg_clr_set(phy->isp, phy->cfg_regs, ISPCSI2_PHY_CFG,
+			ISPCSI2_PHY_CFG_PWR_AUTO,
+			enable ? ISPCSI2_PHY_CFG_PWR_AUTO : 0);
+}
+
+/*
+ * csiphy_set_power
+ * @power: Power state to be set.
+ *
+ * Returns 0 if successful, or -EBUSY if the retry count is exceeded.
+ */
+static int csiphy_set_power(struct isp_csiphy *phy, u32 power)
+{
+	u32 reg;
+	u8 retry_count;
+
+	isp_reg_clr_set(phy->isp, phy->cfg_regs, ISPCSI2_PHY_CFG,
+			ISPCSI2_PHY_CFG_PWR_CMD_MASK, power);
+
+	retry_count = 0;
+	do {
+		udelay(50);
+		reg = isp_reg_readl(phy->isp, phy->cfg_regs, ISPCSI2_PHY_CFG) &
+				    ISPCSI2_PHY_CFG_PWR_STATUS_MASK;
+
+		if (reg != power >> 2)
+			retry_count++;
+
+	} while ((reg != power >> 2) && (retry_count < 100));
+
+	if (retry_count == 100) {
+		dev_err(phy->isp->dev, "CSI2 CIO set power failed!\n");
+		return -EBUSY;
+	}
+
+	return 0;
+}
+
+/*
+ * TCLK values are OK at their reset values
+ */
+#define TCLK_TERM	0
+#define TCLK_MISS	1
+#define TCLK_SETTLE	14
+
+static int omap3isp_csiphy_config(struct isp_csiphy *phy)
+{
+	struct isp_csi2_device *csi2 = phy->csi2;
+	struct isp_pipeline *pipe = to_isp_pipeline(&csi2->subdev.entity);
+	struct isp_bus_cfg *buscfg = pipe->external->host_priv;
+	struct isp_csiphy_lanes_cfg *lanes;
+	int csi2_ddrclk_khz;
+	unsigned int used_lanes = 0;
+	unsigned int i;
+	u32 reg;
+
+	if (!buscfg) {
+		struct isp_async_subdev *isd =
+			container_of(pipe->external->asd,
+				     struct isp_async_subdev, asd);
+		buscfg = &isd->bus;
+	}
+
+	if (buscfg->interface == ISP_INTERFACE_CCP2B_PHY1
+	    || buscfg->interface == ISP_INTERFACE_CCP2B_PHY2)
+		lanes = &buscfg->bus.ccp2.lanecfg;
+	else
+		lanes = &buscfg->bus.csi2.lanecfg;
+
+	/* Clock and data lanes verification */
+	for (i = 0; i < phy->num_data_lanes; i++) {
+		if (lanes->data[i].pol > 1 || lanes->data[i].pos > 3)
+			return -EINVAL;
+
+		if (used_lanes & (1 << lanes->data[i].pos))
+			return -EINVAL;
+
+		used_lanes |= 1 << lanes->data[i].pos;
+	}
+
+	if (lanes->clk.pol > 1 || lanes->clk.pos > 3)
+		return -EINVAL;
+
+	if (lanes->clk.pos == 0 || used_lanes & (1 << lanes->clk.pos))
+		return -EINVAL;
+
+	/*
+	 * The PHY configuration is lost in off mode, that's not an
+	 * issue since the MPU power domain is forced on whilst the
+	 * ISP is in use.
+	 */
+	csiphy_routing_cfg(phy, buscfg->interface, true,
+			   buscfg->bus.ccp2.phy_layer);
+
+	/* DPHY timing configuration */
+	/* CSI-2 is DDR and we only count used lanes. */
+	csi2_ddrclk_khz = pipe->external_rate / 1000
+		/ (2 * hweight32(used_lanes)) * pipe->external_width;
+
+	reg = isp_reg_readl(csi2->isp, phy->phy_regs, ISPCSIPHY_REG0);
+
+	reg &= ~(ISPCSIPHY_REG0_THS_TERM_MASK |
+		 ISPCSIPHY_REG0_THS_SETTLE_MASK);
+	/* THS_TERM: Programmed value = ceil(12.5 ns/DDRClk period) - 1. */
+	reg |= (DIV_ROUND_UP(25 * csi2_ddrclk_khz, 2000000) - 1)
+		<< ISPCSIPHY_REG0_THS_TERM_SHIFT;
+	/* THS_SETTLE: Programmed value = ceil(90 ns/DDRClk period) + 3. */
+	reg |= (DIV_ROUND_UP(90 * csi2_ddrclk_khz, 1000000) + 3)
+		<< ISPCSIPHY_REG0_THS_SETTLE_SHIFT;
+
+	isp_reg_writel(csi2->isp, reg, phy->phy_regs, ISPCSIPHY_REG0);
+
+	reg = isp_reg_readl(csi2->isp, phy->phy_regs, ISPCSIPHY_REG1);
+
+	reg &= ~(ISPCSIPHY_REG1_TCLK_TERM_MASK |
+		 ISPCSIPHY_REG1_TCLK_MISS_MASK |
+		 ISPCSIPHY_REG1_TCLK_SETTLE_MASK);
+	reg |= TCLK_TERM << ISPCSIPHY_REG1_TCLK_TERM_SHIFT;
+	reg |= TCLK_MISS << ISPCSIPHY_REG1_TCLK_MISS_SHIFT;
+	reg |= TCLK_SETTLE << ISPCSIPHY_REG1_TCLK_SETTLE_SHIFT;
+
+	isp_reg_writel(csi2->isp, reg, phy->phy_regs, ISPCSIPHY_REG1);
+
+	/* DPHY lane configuration */
+	reg = isp_reg_readl(csi2->isp, phy->cfg_regs, ISPCSI2_PHY_CFG);
+
+	for (i = 0; i < phy->num_data_lanes; i++) {
+		reg &= ~(ISPCSI2_PHY_CFG_DATA_POL_MASK(i + 1) |
+			 ISPCSI2_PHY_CFG_DATA_POSITION_MASK(i + 1));
+		reg |= (lanes->data[i].pol <<
+			ISPCSI2_PHY_CFG_DATA_POL_SHIFT(i + 1));
+		reg |= (lanes->data[i].pos <<
+			ISPCSI2_PHY_CFG_DATA_POSITION_SHIFT(i + 1));
+	}
+
+	reg &= ~(ISPCSI2_PHY_CFG_CLOCK_POL_MASK |
+		 ISPCSI2_PHY_CFG_CLOCK_POSITION_MASK);
+	reg |= lanes->clk.pol << ISPCSI2_PHY_CFG_CLOCK_POL_SHIFT;
+	reg |= lanes->clk.pos << ISPCSI2_PHY_CFG_CLOCK_POSITION_SHIFT;
+
+	isp_reg_writel(csi2->isp, reg, phy->cfg_regs, ISPCSI2_PHY_CFG);
+
+	return 0;
+}
+
+int omap3isp_csiphy_acquire(struct isp_csiphy *phy)
+{
+	int rval;
+
+	if (phy->vdd == NULL) {
+		dev_err(phy->isp->dev, "Power regulator for CSI PHY not "
+			"available\n");
+		return -ENODEV;
+	}
+
+	mutex_lock(&phy->mutex);
+
+	rval = regulator_enable(phy->vdd);
+	if (rval < 0)
+		goto done;
+
+	rval = omap3isp_csi2_reset(phy->csi2);
+	if (rval < 0)
+		goto done;
+
+	rval = omap3isp_csiphy_config(phy);
+	if (rval < 0)
+		goto done;
+
+	rval = csiphy_set_power(phy, ISPCSI2_PHY_CFG_PWR_CMD_ON);
+	if (rval) {
+		regulator_disable(phy->vdd);
+		goto done;
+	}
+
+	csiphy_power_autoswitch_enable(phy, true);
+	phy->phy_in_use = 1;
+
+done:
+	mutex_unlock(&phy->mutex);
+	return rval;
+}
+
+void omap3isp_csiphy_release(struct isp_csiphy *phy)
+{
+	mutex_lock(&phy->mutex);
+	if (phy->phy_in_use) {
+		struct isp_csi2_device *csi2 = phy->csi2;
+		struct isp_pipeline *pipe =
+			to_isp_pipeline(&csi2->subdev.entity);
+		struct isp_bus_cfg *buscfg = pipe->external->host_priv;
+
+		csiphy_routing_cfg(phy, buscfg->interface, false,
+				   buscfg->bus.ccp2.phy_layer);
+		csiphy_power_autoswitch_enable(phy, false);
+		csiphy_set_power(phy, ISPCSI2_PHY_CFG_PWR_CMD_OFF);
+		regulator_disable(phy->vdd);
+		phy->phy_in_use = 0;
+	}
+	mutex_unlock(&phy->mutex);
+}
+
+/*
+ * omap3isp_csiphy_init - Initialize the CSI PHY frontends
+ */
+int omap3isp_csiphy_init(struct isp_device *isp)
+{
+	struct isp_csiphy *phy1 = &isp->isp_csiphy1;
+	struct isp_csiphy *phy2 = &isp->isp_csiphy2;
+
+	phy2->isp = isp;
+	phy2->csi2 = &isp->isp_csi2a;
+	phy2->num_data_lanes = ISP_CSIPHY2_NUM_DATA_LANES;
+	phy2->cfg_regs = OMAP3_ISP_IOMEM_CSI2A_REGS1;
+	phy2->phy_regs = OMAP3_ISP_IOMEM_CSIPHY2;
+	mutex_init(&phy2->mutex);
+
+	if (isp->revision == ISP_REVISION_15_0) {
+		phy1->isp = isp;
+		phy1->csi2 = &isp->isp_csi2c;
+		phy1->num_data_lanes = ISP_CSIPHY1_NUM_DATA_LANES;
+		phy1->cfg_regs = OMAP3_ISP_IOMEM_CSI2C_REGS1;
+		phy1->phy_regs = OMAP3_ISP_IOMEM_CSIPHY1;
+		mutex_init(&phy1->mutex);
+	}
+
+	return 0;
+}
diff --git a/drivers/media/platform/omap3isp/ispcsiphy.h b/drivers/media/platform/omap3isp/ispcsiphy.h
new file mode 100644
index 0000000..28b63b2
--- /dev/null
+++ b/drivers/media/platform/omap3isp/ispcsiphy.h
@@ -0,0 +1,43 @@
+/*
+ * ispcsiphy.h
+ *
+ * TI OMAP3 ISP - CSI PHY module
+ *
+ * Copyright (C) 2010 Nokia Corporation
+ * Copyright (C) 2009 Texas Instruments, Inc.
+ *
+ * Contacts: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *	     Sakari Ailus <sakari.ailus@iki.fi>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef OMAP3_ISP_CSI_PHY_H
+#define OMAP3_ISP_CSI_PHY_H
+
+#include "omap3isp.h"
+
+struct isp_csi2_device;
+struct regulator;
+
+struct isp_csiphy {
+	struct isp_device *isp;
+	struct mutex mutex;	/* serialize csiphy configuration */
+	u8 phy_in_use;
+	struct isp_csi2_device *csi2;
+	struct regulator *vdd;
+
+	/* mem resources - enums as defined in enum isp_mem_resources */
+	unsigned int cfg_regs;
+	unsigned int phy_regs;
+
+	u8 num_data_lanes;	/* number of CSI2 Data Lanes supported */
+};
+
+int omap3isp_csiphy_acquire(struct isp_csiphy *phy);
+void omap3isp_csiphy_release(struct isp_csiphy *phy);
+int omap3isp_csiphy_init(struct isp_device *isp);
+
+#endif	/* OMAP3_ISP_CSI_PHY_H */
diff --git a/drivers/media/platform/omap3isp/isph3a.h b/drivers/media/platform/omap3isp/isph3a.h
new file mode 100644
index 0000000..e5b28d0
--- /dev/null
+++ b/drivers/media/platform/omap3isp/isph3a.h
@@ -0,0 +1,107 @@
+/*
+ * isph3a.h
+ *
+ * TI OMAP3 ISP - H3A AF module
+ *
+ * Copyright (C) 2010 Nokia Corporation
+ * Copyright (C) 2009 Texas Instruments, Inc.
+ *
+ * Contacts: David Cohen <dacohen@gmail.com>
+ *	     Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *	     Sakari Ailus <sakari.ailus@iki.fi>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef OMAP3_ISP_H3A_H
+#define OMAP3_ISP_H3A_H
+
+#include <linux/omap3isp.h>
+
+/*
+ * ----------
+ * -H3A AEWB-
+ * ----------
+ */
+
+#define AEWB_PACKET_SIZE	16
+#define AEWB_SATURATION_LIMIT	0x3ff
+
+/* Flags for changed registers */
+#define PCR_CHNG		(1 << 0)
+#define AEWWIN1_CHNG		(1 << 1)
+#define AEWINSTART_CHNG		(1 << 2)
+#define AEWINBLK_CHNG		(1 << 3)
+#define AEWSUBWIN_CHNG		(1 << 4)
+#define PRV_WBDGAIN_CHNG	(1 << 5)
+#define PRV_WBGAIN_CHNG		(1 << 6)
+
+/* ISPH3A REGISTERS bits */
+#define ISPH3A_PCR_AF_EN	(1 << 0)
+#define ISPH3A_PCR_AF_ALAW_EN	(1 << 1)
+#define ISPH3A_PCR_AF_MED_EN	(1 << 2)
+#define ISPH3A_PCR_AF_BUSY	(1 << 15)
+#define ISPH3A_PCR_AEW_EN	(1 << 16)
+#define ISPH3A_PCR_AEW_ALAW_EN	(1 << 17)
+#define ISPH3A_PCR_AEW_BUSY	(1 << 18)
+#define ISPH3A_PCR_AEW_MASK	(ISPH3A_PCR_AEW_ALAW_EN | \
+				 ISPH3A_PCR_AEW_AVE2LMT_MASK)
+
+/*
+ * --------
+ * -H3A AF-
+ * --------
+ */
+
+/* Peripheral Revision */
+#define AFPID				0x0
+
+#define AFCOEF_OFFSET			0x00000004	/* COEF base address */
+
+/* PCR fields */
+#define AF_BUSYAF			(1 << 15)
+#define AF_FVMODE			(1 << 14)
+#define AF_RGBPOS			(0x7 << 11)
+#define AF_MED_TH			(0xFF << 3)
+#define AF_MED_EN			(1 << 2)
+#define AF_ALAW_EN			(1 << 1)
+#define AF_EN				(1 << 0)
+#define AF_PCR_MASK			(AF_FVMODE | AF_RGBPOS | AF_MED_TH | \
+					 AF_MED_EN | AF_ALAW_EN)
+
+/* AFPAX1 fields */
+#define AF_PAXW				(0x7F << 16)
+#define AF_PAXH				0x7F
+
+/* AFPAX2 fields */
+#define AF_AFINCV			(0xF << 13)
+#define AF_PAXVC			(0x7F << 6)
+#define AF_PAXHC			0x3F
+
+/* AFPAXSTART fields */
+#define AF_PAXSH			(0xFFF<<16)
+#define AF_PAXSV			0xFFF
+
+/* COEFFICIENT MASK */
+#define AF_COEF_MASK0			0xFFF
+#define AF_COEF_MASK1			(0xFFF<<16)
+
+/* BIT SHIFTS */
+#define AF_RGBPOS_SHIFT			11
+#define AF_MED_TH_SHIFT			3
+#define AF_PAXW_SHIFT			16
+#define AF_LINE_INCR_SHIFT		13
+#define AF_VT_COUNT_SHIFT		6
+#define AF_HZ_START_SHIFT		16
+#define AF_COEF_SHIFT			16
+
+/* Init and cleanup functions */
+int omap3isp_h3a_aewb_init(struct isp_device *isp);
+int omap3isp_h3a_af_init(struct isp_device *isp);
+
+void omap3isp_h3a_aewb_cleanup(struct isp_device *isp);
+void omap3isp_h3a_af_cleanup(struct isp_device *isp);
+
+#endif /* OMAP3_ISP_H3A_H */
diff --git a/drivers/media/platform/omap3isp/isph3a_aewb.c b/drivers/media/platform/omap3isp/isph3a_aewb.c
new file mode 100644
index 0000000..ccaf92f
--- /dev/null
+++ b/drivers/media/platform/omap3isp/isph3a_aewb.c
@@ -0,0 +1,341 @@
+/*
+ * isph3a.c
+ *
+ * TI OMAP3 ISP - H3A module
+ *
+ * Copyright (C) 2010 Nokia Corporation
+ * Copyright (C) 2009 Texas Instruments, Inc.
+ *
+ * Contacts: David Cohen <dacohen@gmail.com>
+ *	     Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *	     Sakari Ailus <sakari.ailus@iki.fi>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+
+#include "isp.h"
+#include "isph3a.h"
+#include "ispstat.h"
+
+/*
+ * h3a_aewb_update_regs - Helper function to update h3a registers.
+ */
+static void h3a_aewb_setup_regs(struct ispstat *aewb, void *priv)
+{
+	struct omap3isp_h3a_aewb_config *conf = priv;
+	u32 pcr;
+	u32 win1;
+	u32 start;
+	u32 blk;
+	u32 subwin;
+
+	if (aewb->state == ISPSTAT_DISABLED)
+		return;
+
+	isp_reg_writel(aewb->isp, aewb->active_buf->dma_addr,
+		       OMAP3_ISP_IOMEM_H3A, ISPH3A_AEWBUFST);
+
+	if (!aewb->update)
+		return;
+
+	/* Converting config metadata into reg values */
+	pcr = conf->saturation_limit << ISPH3A_PCR_AEW_AVE2LMT_SHIFT;
+	pcr |= !!conf->alaw_enable << ISPH3A_PCR_AEW_ALAW_EN_SHIFT;
+
+	win1 = ((conf->win_height >> 1) - 1) << ISPH3A_AEWWIN1_WINH_SHIFT;
+	win1 |= ((conf->win_width >> 1) - 1) << ISPH3A_AEWWIN1_WINW_SHIFT;
+	win1 |= (conf->ver_win_count - 1) << ISPH3A_AEWWIN1_WINVC_SHIFT;
+	win1 |= (conf->hor_win_count - 1) << ISPH3A_AEWWIN1_WINHC_SHIFT;
+
+	start = conf->hor_win_start << ISPH3A_AEWINSTART_WINSH_SHIFT;
+	start |= conf->ver_win_start << ISPH3A_AEWINSTART_WINSV_SHIFT;
+
+	blk = conf->blk_ver_win_start << ISPH3A_AEWINBLK_WINSV_SHIFT;
+	blk |= ((conf->blk_win_height >> 1) - 1) << ISPH3A_AEWINBLK_WINH_SHIFT;
+
+	subwin = ((conf->subsample_ver_inc >> 1) - 1) <<
+		 ISPH3A_AEWSUBWIN_AEWINCV_SHIFT;
+	subwin |= ((conf->subsample_hor_inc >> 1) - 1) <<
+		  ISPH3A_AEWSUBWIN_AEWINCH_SHIFT;
+
+	isp_reg_writel(aewb->isp, win1, OMAP3_ISP_IOMEM_H3A, ISPH3A_AEWWIN1);
+	isp_reg_writel(aewb->isp, start, OMAP3_ISP_IOMEM_H3A,
+		       ISPH3A_AEWINSTART);
+	isp_reg_writel(aewb->isp, blk, OMAP3_ISP_IOMEM_H3A, ISPH3A_AEWINBLK);
+	isp_reg_writel(aewb->isp, subwin, OMAP3_ISP_IOMEM_H3A,
+		       ISPH3A_AEWSUBWIN);
+	isp_reg_clr_set(aewb->isp, OMAP3_ISP_IOMEM_H3A, ISPH3A_PCR,
+			ISPH3A_PCR_AEW_MASK, pcr);
+
+	aewb->update = 0;
+	aewb->config_counter += aewb->inc_config;
+	aewb->inc_config = 0;
+	aewb->buf_size = conf->buf_size;
+}
+
+static void h3a_aewb_enable(struct ispstat *aewb, int enable)
+{
+	if (enable) {
+		isp_reg_set(aewb->isp, OMAP3_ISP_IOMEM_H3A, ISPH3A_PCR,
+			    ISPH3A_PCR_AEW_EN);
+		omap3isp_subclk_enable(aewb->isp, OMAP3_ISP_SUBCLK_AEWB);
+	} else {
+		isp_reg_clr(aewb->isp, OMAP3_ISP_IOMEM_H3A, ISPH3A_PCR,
+			    ISPH3A_PCR_AEW_EN);
+		omap3isp_subclk_disable(aewb->isp, OMAP3_ISP_SUBCLK_AEWB);
+	}
+}
+
+static int h3a_aewb_busy(struct ispstat *aewb)
+{
+	return isp_reg_readl(aewb->isp, OMAP3_ISP_IOMEM_H3A, ISPH3A_PCR)
+						& ISPH3A_PCR_BUSYAEAWB;
+}
+
+static u32 h3a_aewb_get_buf_size(struct omap3isp_h3a_aewb_config *conf)
+{
+	/* Number of configured windows + extra row for black data */
+	u32 win_count = (conf->ver_win_count + 1) * conf->hor_win_count;
+
+	/*
+	 * Unsaturated block counts for each 8 windows.
+	 * 1 extra for the last (win_count % 8) windows if win_count is not
+	 * divisible by 8.
+	 */
+	win_count += (win_count + 7) / 8;
+
+	return win_count * AEWB_PACKET_SIZE;
+}
+
+static int h3a_aewb_validate_params(struct ispstat *aewb, void *new_conf)
+{
+	struct omap3isp_h3a_aewb_config *user_cfg = new_conf;
+	u32 buf_size;
+
+	if (unlikely(user_cfg->saturation_limit >
+		     OMAP3ISP_AEWB_MAX_SATURATION_LIM))
+		return -EINVAL;
+
+	if (unlikely(user_cfg->win_height < OMAP3ISP_AEWB_MIN_WIN_H ||
+		     user_cfg->win_height > OMAP3ISP_AEWB_MAX_WIN_H ||
+		     user_cfg->win_height & 0x01))
+		return -EINVAL;
+
+	if (unlikely(user_cfg->win_width < OMAP3ISP_AEWB_MIN_WIN_W ||
+		     user_cfg->win_width > OMAP3ISP_AEWB_MAX_WIN_W ||
+		     user_cfg->win_width & 0x01))
+		return -EINVAL;
+
+	if (unlikely(user_cfg->ver_win_count < OMAP3ISP_AEWB_MIN_WINVC ||
+		     user_cfg->ver_win_count > OMAP3ISP_AEWB_MAX_WINVC))
+		return -EINVAL;
+
+	if (unlikely(user_cfg->hor_win_count < OMAP3ISP_AEWB_MIN_WINHC ||
+		     user_cfg->hor_win_count > OMAP3ISP_AEWB_MAX_WINHC))
+		return -EINVAL;
+
+	if (unlikely(user_cfg->ver_win_start > OMAP3ISP_AEWB_MAX_WINSTART))
+		return -EINVAL;
+
+	if (unlikely(user_cfg->hor_win_start > OMAP3ISP_AEWB_MAX_WINSTART))
+		return -EINVAL;
+
+	if (unlikely(user_cfg->blk_ver_win_start > OMAP3ISP_AEWB_MAX_WINSTART))
+		return -EINVAL;
+
+	if (unlikely(user_cfg->blk_win_height < OMAP3ISP_AEWB_MIN_WIN_H ||
+		     user_cfg->blk_win_height > OMAP3ISP_AEWB_MAX_WIN_H ||
+		     user_cfg->blk_win_height & 0x01))
+		return -EINVAL;
+
+	if (unlikely(user_cfg->subsample_ver_inc < OMAP3ISP_AEWB_MIN_SUB_INC ||
+		     user_cfg->subsample_ver_inc > OMAP3ISP_AEWB_MAX_SUB_INC ||
+		     user_cfg->subsample_ver_inc & 0x01))
+		return -EINVAL;
+
+	if (unlikely(user_cfg->subsample_hor_inc < OMAP3ISP_AEWB_MIN_SUB_INC ||
+		     user_cfg->subsample_hor_inc > OMAP3ISP_AEWB_MAX_SUB_INC ||
+		     user_cfg->subsample_hor_inc & 0x01))
+		return -EINVAL;
+
+	buf_size = h3a_aewb_get_buf_size(user_cfg);
+	if (buf_size > user_cfg->buf_size)
+		user_cfg->buf_size = buf_size;
+	else if (user_cfg->buf_size > OMAP3ISP_AEWB_MAX_BUF_SIZE)
+		user_cfg->buf_size = OMAP3ISP_AEWB_MAX_BUF_SIZE;
+
+	return 0;
+}
+
+/*
+ * h3a_aewb_set_params - Helper function to check & store user given params.
+ * @new_conf: Pointer to AE and AWB parameters struct.
+ *
+ * As most of them are busy-lock registers, need to wait until AEW_BUSY = 0 to
+ * program them during ISR.
+ */
+static void h3a_aewb_set_params(struct ispstat *aewb, void *new_conf)
+{
+	struct omap3isp_h3a_aewb_config *user_cfg = new_conf;
+	struct omap3isp_h3a_aewb_config *cur_cfg = aewb->priv;
+	int update = 0;
+
+	if (cur_cfg->saturation_limit != user_cfg->saturation_limit) {
+		cur_cfg->saturation_limit = user_cfg->saturation_limit;
+		update = 1;
+	}
+	if (cur_cfg->alaw_enable != user_cfg->alaw_enable) {
+		cur_cfg->alaw_enable = user_cfg->alaw_enable;
+		update = 1;
+	}
+	if (cur_cfg->win_height != user_cfg->win_height) {
+		cur_cfg->win_height = user_cfg->win_height;
+		update = 1;
+	}
+	if (cur_cfg->win_width != user_cfg->win_width) {
+		cur_cfg->win_width = user_cfg->win_width;
+		update = 1;
+	}
+	if (cur_cfg->ver_win_count != user_cfg->ver_win_count) {
+		cur_cfg->ver_win_count = user_cfg->ver_win_count;
+		update = 1;
+	}
+	if (cur_cfg->hor_win_count != user_cfg->hor_win_count) {
+		cur_cfg->hor_win_count = user_cfg->hor_win_count;
+		update = 1;
+	}
+	if (cur_cfg->ver_win_start != user_cfg->ver_win_start) {
+		cur_cfg->ver_win_start = user_cfg->ver_win_start;
+		update = 1;
+	}
+	if (cur_cfg->hor_win_start != user_cfg->hor_win_start) {
+		cur_cfg->hor_win_start = user_cfg->hor_win_start;
+		update = 1;
+	}
+	if (cur_cfg->blk_ver_win_start != user_cfg->blk_ver_win_start) {
+		cur_cfg->blk_ver_win_start = user_cfg->blk_ver_win_start;
+		update = 1;
+	}
+	if (cur_cfg->blk_win_height != user_cfg->blk_win_height) {
+		cur_cfg->blk_win_height = user_cfg->blk_win_height;
+		update = 1;
+	}
+	if (cur_cfg->subsample_ver_inc != user_cfg->subsample_ver_inc) {
+		cur_cfg->subsample_ver_inc = user_cfg->subsample_ver_inc;
+		update = 1;
+	}
+	if (cur_cfg->subsample_hor_inc != user_cfg->subsample_hor_inc) {
+		cur_cfg->subsample_hor_inc = user_cfg->subsample_hor_inc;
+		update = 1;
+	}
+
+	if (update || !aewb->configured) {
+		aewb->inc_config++;
+		aewb->update = 1;
+		cur_cfg->buf_size = h3a_aewb_get_buf_size(cur_cfg);
+	}
+}
+
+static long h3a_aewb_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
+{
+	struct ispstat *stat = v4l2_get_subdevdata(sd);
+
+	switch (cmd) {
+	case VIDIOC_OMAP3ISP_AEWB_CFG:
+		return omap3isp_stat_config(stat, arg);
+	case VIDIOC_OMAP3ISP_STAT_REQ:
+		return omap3isp_stat_request_statistics(stat, arg);
+	case VIDIOC_OMAP3ISP_STAT_EN: {
+		unsigned long *en = arg;
+		return omap3isp_stat_enable(stat, !!*en);
+	}
+	}
+
+	return -ENOIOCTLCMD;
+}
+
+static const struct ispstat_ops h3a_aewb_ops = {
+	.validate_params	= h3a_aewb_validate_params,
+	.set_params		= h3a_aewb_set_params,
+	.setup_regs		= h3a_aewb_setup_regs,
+	.enable			= h3a_aewb_enable,
+	.busy			= h3a_aewb_busy,
+};
+
+static const struct v4l2_subdev_core_ops h3a_aewb_subdev_core_ops = {
+	.ioctl = h3a_aewb_ioctl,
+	.subscribe_event = omap3isp_stat_subscribe_event,
+	.unsubscribe_event = omap3isp_stat_unsubscribe_event,
+};
+
+static const struct v4l2_subdev_video_ops h3a_aewb_subdev_video_ops = {
+	.s_stream = omap3isp_stat_s_stream,
+};
+
+static const struct v4l2_subdev_ops h3a_aewb_subdev_ops = {
+	.core = &h3a_aewb_subdev_core_ops,
+	.video = &h3a_aewb_subdev_video_ops,
+};
+
+/*
+ * omap3isp_h3a_aewb_init - Module Initialisation.
+ */
+int omap3isp_h3a_aewb_init(struct isp_device *isp)
+{
+	struct ispstat *aewb = &isp->isp_aewb;
+	struct omap3isp_h3a_aewb_config *aewb_cfg;
+	struct omap3isp_h3a_aewb_config *aewb_recover_cfg;
+
+	aewb_cfg = devm_kzalloc(isp->dev, sizeof(*aewb_cfg), GFP_KERNEL);
+	if (!aewb_cfg)
+		return -ENOMEM;
+
+	aewb->ops = &h3a_aewb_ops;
+	aewb->priv = aewb_cfg;
+	aewb->event_type = V4L2_EVENT_OMAP3ISP_AEWB;
+	aewb->isp = isp;
+
+	/* Set recover state configuration */
+	aewb_recover_cfg = devm_kzalloc(isp->dev, sizeof(*aewb_recover_cfg),
+					GFP_KERNEL);
+	if (!aewb_recover_cfg) {
+		dev_err(aewb->isp->dev, "AEWB: cannot allocate memory for "
+					"recover configuration.\n");
+		return -ENOMEM;
+	}
+
+	aewb_recover_cfg->saturation_limit = OMAP3ISP_AEWB_MAX_SATURATION_LIM;
+	aewb_recover_cfg->win_height = OMAP3ISP_AEWB_MIN_WIN_H;
+	aewb_recover_cfg->win_width = OMAP3ISP_AEWB_MIN_WIN_W;
+	aewb_recover_cfg->ver_win_count = OMAP3ISP_AEWB_MIN_WINVC;
+	aewb_recover_cfg->hor_win_count = OMAP3ISP_AEWB_MIN_WINHC;
+	aewb_recover_cfg->blk_ver_win_start = aewb_recover_cfg->ver_win_start +
+		aewb_recover_cfg->win_height * aewb_recover_cfg->ver_win_count;
+	aewb_recover_cfg->blk_win_height = OMAP3ISP_AEWB_MIN_WIN_H;
+	aewb_recover_cfg->subsample_ver_inc = OMAP3ISP_AEWB_MIN_SUB_INC;
+	aewb_recover_cfg->subsample_hor_inc = OMAP3ISP_AEWB_MIN_SUB_INC;
+
+	if (h3a_aewb_validate_params(aewb, aewb_recover_cfg)) {
+		dev_err(aewb->isp->dev, "AEWB: recover configuration is "
+					"invalid.\n");
+		return -EINVAL;
+	}
+
+	aewb_recover_cfg->buf_size = h3a_aewb_get_buf_size(aewb_recover_cfg);
+	aewb->recover_priv = aewb_recover_cfg;
+
+	return omap3isp_stat_init(aewb, "AEWB", &h3a_aewb_subdev_ops);
+}
+
+/*
+ * omap3isp_h3a_aewb_cleanup - Module exit.
+ */
+void omap3isp_h3a_aewb_cleanup(struct isp_device *isp)
+{
+	omap3isp_stat_cleanup(&isp->isp_aewb);
+}
diff --git a/drivers/media/platform/omap3isp/isph3a_af.c b/drivers/media/platform/omap3isp/isph3a_af.c
new file mode 100644
index 0000000..92937f7
--- /dev/null
+++ b/drivers/media/platform/omap3isp/isph3a_af.c
@@ -0,0 +1,396 @@
+/*
+ * isph3a_af.c
+ *
+ * TI OMAP3 ISP - H3A AF module
+ *
+ * Copyright (C) 2010 Nokia Corporation
+ * Copyright (C) 2009 Texas Instruments, Inc.
+ *
+ * Contacts: David Cohen <dacohen@gmail.com>
+ *	     Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *	     Sakari Ailus <sakari.ailus@iki.fi>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+/* Linux specific include files */
+#include <linux/device.h>
+#include <linux/slab.h>
+
+#include "isp.h"
+#include "isph3a.h"
+#include "ispstat.h"
+
+#define IS_OUT_OF_BOUNDS(value, min, max)		\
+	(((value) < (min)) || ((value) > (max)))
+
+static void h3a_af_setup_regs(struct ispstat *af, void *priv)
+{
+	struct omap3isp_h3a_af_config *conf = priv;
+	u32 pcr;
+	u32 pax1;
+	u32 pax2;
+	u32 paxstart;
+	u32 coef;
+	u32 base_coef_set0;
+	u32 base_coef_set1;
+	int index;
+
+	if (af->state == ISPSTAT_DISABLED)
+		return;
+
+	isp_reg_writel(af->isp, af->active_buf->dma_addr, OMAP3_ISP_IOMEM_H3A,
+		       ISPH3A_AFBUFST);
+
+	if (!af->update)
+		return;
+
+	/* Configure Hardware Registers */
+	pax1 = ((conf->paxel.width >> 1) - 1) << AF_PAXW_SHIFT;
+	/* Set height in AFPAX1 */
+	pax1 |= (conf->paxel.height >> 1) - 1;
+	isp_reg_writel(af->isp, pax1, OMAP3_ISP_IOMEM_H3A, ISPH3A_AFPAX1);
+
+	/* Configure AFPAX2 Register */
+	/* Set Line Increment in AFPAX2 Register */
+	pax2 = ((conf->paxel.line_inc >> 1) - 1) << AF_LINE_INCR_SHIFT;
+	/* Set Vertical Count */
+	pax2 |= (conf->paxel.v_cnt - 1) << AF_VT_COUNT_SHIFT;
+	/* Set Horizontal Count */
+	pax2 |= (conf->paxel.h_cnt - 1);
+	isp_reg_writel(af->isp, pax2, OMAP3_ISP_IOMEM_H3A, ISPH3A_AFPAX2);
+
+	/* Configure PAXSTART Register */
+	/*Configure Horizontal Start */
+	paxstart = conf->paxel.h_start << AF_HZ_START_SHIFT;
+	/* Configure Vertical Start */
+	paxstart |= conf->paxel.v_start;
+	isp_reg_writel(af->isp, paxstart, OMAP3_ISP_IOMEM_H3A,
+		       ISPH3A_AFPAXSTART);
+
+	/*SetIIRSH Register */
+	isp_reg_writel(af->isp, conf->iir.h_start,
+		       OMAP3_ISP_IOMEM_H3A, ISPH3A_AFIIRSH);
+
+	base_coef_set0 = ISPH3A_AFCOEF010;
+	base_coef_set1 = ISPH3A_AFCOEF110;
+	for (index = 0; index <= 8; index += 2) {
+		/*Set IIR Filter0 Coefficients */
+		coef = 0;
+		coef |= conf->iir.coeff_set0[index];
+		coef |= conf->iir.coeff_set0[index + 1] <<
+			AF_COEF_SHIFT;
+		isp_reg_writel(af->isp, coef, OMAP3_ISP_IOMEM_H3A,
+			       base_coef_set0);
+		base_coef_set0 += AFCOEF_OFFSET;
+
+		/*Set IIR Filter1 Coefficients */
+		coef = 0;
+		coef |= conf->iir.coeff_set1[index];
+		coef |= conf->iir.coeff_set1[index + 1] <<
+			AF_COEF_SHIFT;
+		isp_reg_writel(af->isp, coef, OMAP3_ISP_IOMEM_H3A,
+			       base_coef_set1);
+		base_coef_set1 += AFCOEF_OFFSET;
+	}
+	/* set AFCOEF0010 Register */
+	isp_reg_writel(af->isp, conf->iir.coeff_set0[10],
+		       OMAP3_ISP_IOMEM_H3A, ISPH3A_AFCOEF0010);
+	/* set AFCOEF1010 Register */
+	isp_reg_writel(af->isp, conf->iir.coeff_set1[10],
+		       OMAP3_ISP_IOMEM_H3A, ISPH3A_AFCOEF1010);
+
+	/* PCR Register */
+	/* Set RGB Position */
+	pcr = conf->rgb_pos << AF_RGBPOS_SHIFT;
+	/* Set Accumulator Mode */
+	if (conf->fvmode == OMAP3ISP_AF_MODE_PEAK)
+		pcr |= AF_FVMODE;
+	/* Set A-law */
+	if (conf->alaw_enable)
+		pcr |= AF_ALAW_EN;
+	/* HMF Configurations */
+	if (conf->hmf.enable) {
+		/* Enable HMF */
+		pcr |= AF_MED_EN;
+		/* Set Median Threshold */
+		pcr |= conf->hmf.threshold << AF_MED_TH_SHIFT;
+	}
+	/* Set PCR Register */
+	isp_reg_clr_set(af->isp, OMAP3_ISP_IOMEM_H3A, ISPH3A_PCR,
+			AF_PCR_MASK, pcr);
+
+	af->update = 0;
+	af->config_counter += af->inc_config;
+	af->inc_config = 0;
+	af->buf_size = conf->buf_size;
+}
+
+static void h3a_af_enable(struct ispstat *af, int enable)
+{
+	if (enable) {
+		isp_reg_set(af->isp, OMAP3_ISP_IOMEM_H3A, ISPH3A_PCR,
+			    ISPH3A_PCR_AF_EN);
+		omap3isp_subclk_enable(af->isp, OMAP3_ISP_SUBCLK_AF);
+	} else {
+		isp_reg_clr(af->isp, OMAP3_ISP_IOMEM_H3A, ISPH3A_PCR,
+			    ISPH3A_PCR_AF_EN);
+		omap3isp_subclk_disable(af->isp, OMAP3_ISP_SUBCLK_AF);
+	}
+}
+
+static int h3a_af_busy(struct ispstat *af)
+{
+	return isp_reg_readl(af->isp, OMAP3_ISP_IOMEM_H3A, ISPH3A_PCR)
+						& ISPH3A_PCR_BUSYAF;
+}
+
+static u32 h3a_af_get_buf_size(struct omap3isp_h3a_af_config *conf)
+{
+	return conf->paxel.h_cnt * conf->paxel.v_cnt * OMAP3ISP_AF_PAXEL_SIZE;
+}
+
+/* Function to check paxel parameters */
+static int h3a_af_validate_params(struct ispstat *af, void *new_conf)
+{
+	struct omap3isp_h3a_af_config *user_cfg = new_conf;
+	struct omap3isp_h3a_af_paxel *paxel_cfg = &user_cfg->paxel;
+	struct omap3isp_h3a_af_iir *iir_cfg = &user_cfg->iir;
+	int index;
+	u32 buf_size;
+
+	/* Check horizontal Count */
+	if (IS_OUT_OF_BOUNDS(paxel_cfg->h_cnt,
+			     OMAP3ISP_AF_PAXEL_HORIZONTAL_COUNT_MIN,
+			     OMAP3ISP_AF_PAXEL_HORIZONTAL_COUNT_MAX))
+		return -EINVAL;
+
+	/* Check Vertical Count */
+	if (IS_OUT_OF_BOUNDS(paxel_cfg->v_cnt,
+			     OMAP3ISP_AF_PAXEL_VERTICAL_COUNT_MIN,
+			     OMAP3ISP_AF_PAXEL_VERTICAL_COUNT_MAX))
+		return -EINVAL;
+
+	if (IS_OUT_OF_BOUNDS(paxel_cfg->height, OMAP3ISP_AF_PAXEL_HEIGHT_MIN,
+			     OMAP3ISP_AF_PAXEL_HEIGHT_MAX) ||
+	    paxel_cfg->height % 2)
+		return -EINVAL;
+
+	/* Check width */
+	if (IS_OUT_OF_BOUNDS(paxel_cfg->width, OMAP3ISP_AF_PAXEL_WIDTH_MIN,
+			     OMAP3ISP_AF_PAXEL_WIDTH_MAX) ||
+	    paxel_cfg->width % 2)
+		return -EINVAL;
+
+	/* Check Line Increment */
+	if (IS_OUT_OF_BOUNDS(paxel_cfg->line_inc,
+			     OMAP3ISP_AF_PAXEL_INCREMENT_MIN,
+			     OMAP3ISP_AF_PAXEL_INCREMENT_MAX) ||
+	    paxel_cfg->line_inc % 2)
+		return -EINVAL;
+
+	/* Check Horizontal Start */
+	if ((paxel_cfg->h_start < iir_cfg->h_start) ||
+	    IS_OUT_OF_BOUNDS(paxel_cfg->h_start,
+			     OMAP3ISP_AF_PAXEL_HZSTART_MIN,
+			     OMAP3ISP_AF_PAXEL_HZSTART_MAX))
+		return -EINVAL;
+
+	/* Check IIR */
+	for (index = 0; index < OMAP3ISP_AF_NUM_COEF; index++) {
+		if ((iir_cfg->coeff_set0[index]) > OMAP3ISP_AF_COEF_MAX)
+			return -EINVAL;
+
+		if ((iir_cfg->coeff_set1[index]) > OMAP3ISP_AF_COEF_MAX)
+			return -EINVAL;
+	}
+
+	if (IS_OUT_OF_BOUNDS(iir_cfg->h_start, OMAP3ISP_AF_IIRSH_MIN,
+			     OMAP3ISP_AF_IIRSH_MAX))
+		return -EINVAL;
+
+	/* Hack: If paxel size is 12, the 10th AF window may be corrupted */
+	if ((paxel_cfg->h_cnt * paxel_cfg->v_cnt > 9) &&
+	    (paxel_cfg->width * paxel_cfg->height == 12))
+		return -EINVAL;
+
+	buf_size = h3a_af_get_buf_size(user_cfg);
+	if (buf_size > user_cfg->buf_size)
+		/* User buf_size request wasn't enough */
+		user_cfg->buf_size = buf_size;
+	else if (user_cfg->buf_size > OMAP3ISP_AF_MAX_BUF_SIZE)
+		user_cfg->buf_size = OMAP3ISP_AF_MAX_BUF_SIZE;
+
+	return 0;
+}
+
+/* Update local parameters */
+static void h3a_af_set_params(struct ispstat *af, void *new_conf)
+{
+	struct omap3isp_h3a_af_config *user_cfg = new_conf;
+	struct omap3isp_h3a_af_config *cur_cfg = af->priv;
+	int update = 0;
+	int index;
+
+	/* alaw */
+	if (cur_cfg->alaw_enable != user_cfg->alaw_enable) {
+		update = 1;
+		goto out;
+	}
+
+	/* hmf */
+	if (cur_cfg->hmf.enable != user_cfg->hmf.enable) {
+		update = 1;
+		goto out;
+	}
+	if (cur_cfg->hmf.threshold != user_cfg->hmf.threshold) {
+		update = 1;
+		goto out;
+	}
+
+	/* rgbpos */
+	if (cur_cfg->rgb_pos != user_cfg->rgb_pos) {
+		update = 1;
+		goto out;
+	}
+
+	/* iir */
+	if (cur_cfg->iir.h_start != user_cfg->iir.h_start) {
+		update = 1;
+		goto out;
+	}
+	for (index = 0; index < OMAP3ISP_AF_NUM_COEF; index++) {
+		if (cur_cfg->iir.coeff_set0[index] !=
+				user_cfg->iir.coeff_set0[index]) {
+			update = 1;
+			goto out;
+		}
+		if (cur_cfg->iir.coeff_set1[index] !=
+				user_cfg->iir.coeff_set1[index]) {
+			update = 1;
+			goto out;
+		}
+	}
+
+	/* paxel */
+	if ((cur_cfg->paxel.width != user_cfg->paxel.width) ||
+	    (cur_cfg->paxel.height != user_cfg->paxel.height) ||
+	    (cur_cfg->paxel.h_start != user_cfg->paxel.h_start) ||
+	    (cur_cfg->paxel.v_start != user_cfg->paxel.v_start) ||
+	    (cur_cfg->paxel.h_cnt != user_cfg->paxel.h_cnt) ||
+	    (cur_cfg->paxel.v_cnt != user_cfg->paxel.v_cnt) ||
+	    (cur_cfg->paxel.line_inc != user_cfg->paxel.line_inc)) {
+		update = 1;
+		goto out;
+	}
+
+	/* af_mode */
+	if (cur_cfg->fvmode != user_cfg->fvmode)
+		update = 1;
+
+out:
+	if (update || !af->configured) {
+		memcpy(cur_cfg, user_cfg, sizeof(*cur_cfg));
+		af->inc_config++;
+		af->update = 1;
+		/*
+		 * User might be asked for a bigger buffer than necessary for
+		 * this configuration. In order to return the right amount of
+		 * data during buffer request, let's calculate the size here
+		 * instead of stick with user_cfg->buf_size.
+		 */
+		cur_cfg->buf_size = h3a_af_get_buf_size(cur_cfg);
+	}
+}
+
+static long h3a_af_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
+{
+	struct ispstat *stat = v4l2_get_subdevdata(sd);
+
+	switch (cmd) {
+	case VIDIOC_OMAP3ISP_AF_CFG:
+		return omap3isp_stat_config(stat, arg);
+	case VIDIOC_OMAP3ISP_STAT_REQ:
+		return omap3isp_stat_request_statistics(stat, arg);
+	case VIDIOC_OMAP3ISP_STAT_EN: {
+		int *en = arg;
+		return omap3isp_stat_enable(stat, !!*en);
+	}
+	}
+
+	return -ENOIOCTLCMD;
+
+}
+
+static const struct ispstat_ops h3a_af_ops = {
+	.validate_params	= h3a_af_validate_params,
+	.set_params		= h3a_af_set_params,
+	.setup_regs		= h3a_af_setup_regs,
+	.enable			= h3a_af_enable,
+	.busy			= h3a_af_busy,
+};
+
+static const struct v4l2_subdev_core_ops h3a_af_subdev_core_ops = {
+	.ioctl = h3a_af_ioctl,
+	.subscribe_event = omap3isp_stat_subscribe_event,
+	.unsubscribe_event = omap3isp_stat_unsubscribe_event,
+};
+
+static const struct v4l2_subdev_video_ops h3a_af_subdev_video_ops = {
+	.s_stream = omap3isp_stat_s_stream,
+};
+
+static const struct v4l2_subdev_ops h3a_af_subdev_ops = {
+	.core = &h3a_af_subdev_core_ops,
+	.video = &h3a_af_subdev_video_ops,
+};
+
+/* Function to register the AF character device driver. */
+int omap3isp_h3a_af_init(struct isp_device *isp)
+{
+	struct ispstat *af = &isp->isp_af;
+	struct omap3isp_h3a_af_config *af_cfg;
+	struct omap3isp_h3a_af_config *af_recover_cfg;
+
+	af_cfg = devm_kzalloc(isp->dev, sizeof(*af_cfg), GFP_KERNEL);
+	if (af_cfg == NULL)
+		return -ENOMEM;
+
+	af->ops = &h3a_af_ops;
+	af->priv = af_cfg;
+	af->event_type = V4L2_EVENT_OMAP3ISP_AF;
+	af->isp = isp;
+
+	/* Set recover state configuration */
+	af_recover_cfg = devm_kzalloc(isp->dev, sizeof(*af_recover_cfg),
+				      GFP_KERNEL);
+	if (!af_recover_cfg) {
+		dev_err(af->isp->dev, "AF: cannot allocate memory for recover "
+				      "configuration.\n");
+		return -ENOMEM;
+	}
+
+	af_recover_cfg->paxel.h_start = OMAP3ISP_AF_PAXEL_HZSTART_MIN;
+	af_recover_cfg->paxel.width = OMAP3ISP_AF_PAXEL_WIDTH_MIN;
+	af_recover_cfg->paxel.height = OMAP3ISP_AF_PAXEL_HEIGHT_MIN;
+	af_recover_cfg->paxel.h_cnt = OMAP3ISP_AF_PAXEL_HORIZONTAL_COUNT_MIN;
+	af_recover_cfg->paxel.v_cnt = OMAP3ISP_AF_PAXEL_VERTICAL_COUNT_MIN;
+	af_recover_cfg->paxel.line_inc = OMAP3ISP_AF_PAXEL_INCREMENT_MIN;
+	if (h3a_af_validate_params(af, af_recover_cfg)) {
+		dev_err(af->isp->dev, "AF: recover configuration is "
+				      "invalid.\n");
+		return -EINVAL;
+	}
+
+	af_recover_cfg->buf_size = h3a_af_get_buf_size(af_recover_cfg);
+	af->recover_priv = af_recover_cfg;
+
+	return omap3isp_stat_init(af, "AF", &h3a_af_subdev_ops);
+}
+
+void omap3isp_h3a_af_cleanup(struct isp_device *isp)
+{
+	omap3isp_stat_cleanup(&isp->isp_af);
+}
diff --git a/drivers/media/platform/omap3isp/isphist.c b/drivers/media/platform/omap3isp/isphist.c
new file mode 100644
index 0000000..7138b04
--- /dev/null
+++ b/drivers/media/platform/omap3isp/isphist.c
@@ -0,0 +1,536 @@
+/*
+ * isphist.c
+ *
+ * TI OMAP3 ISP - Histogram module
+ *
+ * Copyright (C) 2010 Nokia Corporation
+ * Copyright (C) 2009 Texas Instruments, Inc.
+ *
+ * Contacts: David Cohen <dacohen@gmail.com>
+ *	     Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *	     Sakari Ailus <sakari.ailus@iki.fi>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/dmaengine.h>
+#include <linux/omap-dmaengine.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+
+#include "isp.h"
+#include "ispreg.h"
+#include "isphist.h"
+
+#define HIST_CONFIG_DMA	1
+
+/*
+ * hist_reset_mem - clear Histogram memory before start stats engine.
+ */
+static void hist_reset_mem(struct ispstat *hist)
+{
+	struct isp_device *isp = hist->isp;
+	struct omap3isp_hist_config *conf = hist->priv;
+	unsigned int i;
+
+	isp_reg_writel(isp, 0, OMAP3_ISP_IOMEM_HIST, ISPHIST_ADDR);
+
+	/*
+	 * By setting it, the histogram internal buffer is being cleared at the
+	 * same time it's being read. This bit must be cleared afterwards.
+	 */
+	isp_reg_set(isp, OMAP3_ISP_IOMEM_HIST, ISPHIST_CNT, ISPHIST_CNT_CLEAR);
+
+	/*
+	 * We'll clear 4 words at each iteration for optimization. It avoids
+	 * 3/4 of the jumps. We also know HIST_MEM_SIZE is divisible by 4.
+	 */
+	for (i = OMAP3ISP_HIST_MEM_SIZE / 4; i > 0; i--) {
+		isp_reg_readl(isp, OMAP3_ISP_IOMEM_HIST, ISPHIST_DATA);
+		isp_reg_readl(isp, OMAP3_ISP_IOMEM_HIST, ISPHIST_DATA);
+		isp_reg_readl(isp, OMAP3_ISP_IOMEM_HIST, ISPHIST_DATA);
+		isp_reg_readl(isp, OMAP3_ISP_IOMEM_HIST, ISPHIST_DATA);
+	}
+	isp_reg_clr(isp, OMAP3_ISP_IOMEM_HIST, ISPHIST_CNT, ISPHIST_CNT_CLEAR);
+
+	hist->wait_acc_frames = conf->num_acc_frames;
+}
+
+/*
+ * hist_setup_regs - Helper function to update Histogram registers.
+ */
+static void hist_setup_regs(struct ispstat *hist, void *priv)
+{
+	struct isp_device *isp = hist->isp;
+	struct omap3isp_hist_config *conf = priv;
+	int c;
+	u32 cnt;
+	u32 wb_gain;
+	u32 reg_hor[OMAP3ISP_HIST_MAX_REGIONS];
+	u32 reg_ver[OMAP3ISP_HIST_MAX_REGIONS];
+
+	if (!hist->update || hist->state == ISPSTAT_DISABLED ||
+	    hist->state == ISPSTAT_DISABLING)
+		return;
+
+	cnt = conf->cfa << ISPHIST_CNT_CFA_SHIFT;
+
+	wb_gain = conf->wg[0] << ISPHIST_WB_GAIN_WG00_SHIFT;
+	wb_gain |= conf->wg[1] << ISPHIST_WB_GAIN_WG01_SHIFT;
+	wb_gain |= conf->wg[2] << ISPHIST_WB_GAIN_WG02_SHIFT;
+	if (conf->cfa == OMAP3ISP_HIST_CFA_BAYER)
+		wb_gain |= conf->wg[3] << ISPHIST_WB_GAIN_WG03_SHIFT;
+
+	/* Regions size and position */
+	for (c = 0; c < OMAP3ISP_HIST_MAX_REGIONS; c++) {
+		if (c < conf->num_regions) {
+			reg_hor[c] = (conf->region[c].h_start <<
+				     ISPHIST_REG_START_SHIFT)
+				   | (conf->region[c].h_end <<
+				     ISPHIST_REG_END_SHIFT);
+			reg_ver[c] = (conf->region[c].v_start <<
+				     ISPHIST_REG_START_SHIFT)
+				   | (conf->region[c].v_end <<
+				     ISPHIST_REG_END_SHIFT);
+		} else {
+			reg_hor[c] = 0;
+			reg_ver[c] = 0;
+		}
+	}
+
+	cnt |= conf->hist_bins << ISPHIST_CNT_BINS_SHIFT;
+	switch (conf->hist_bins) {
+	case OMAP3ISP_HIST_BINS_256:
+		cnt |= (ISPHIST_IN_BIT_WIDTH_CCDC - 8) <<
+			ISPHIST_CNT_SHIFT_SHIFT;
+		break;
+	case OMAP3ISP_HIST_BINS_128:
+		cnt |= (ISPHIST_IN_BIT_WIDTH_CCDC - 7) <<
+			ISPHIST_CNT_SHIFT_SHIFT;
+		break;
+	case OMAP3ISP_HIST_BINS_64:
+		cnt |= (ISPHIST_IN_BIT_WIDTH_CCDC - 6) <<
+			ISPHIST_CNT_SHIFT_SHIFT;
+		break;
+	default: /* OMAP3ISP_HIST_BINS_32 */
+		cnt |= (ISPHIST_IN_BIT_WIDTH_CCDC - 5) <<
+			ISPHIST_CNT_SHIFT_SHIFT;
+		break;
+	}
+
+	hist_reset_mem(hist);
+
+	isp_reg_writel(isp, cnt, OMAP3_ISP_IOMEM_HIST, ISPHIST_CNT);
+	isp_reg_writel(isp, wb_gain,  OMAP3_ISP_IOMEM_HIST, ISPHIST_WB_GAIN);
+	isp_reg_writel(isp, reg_hor[0], OMAP3_ISP_IOMEM_HIST, ISPHIST_R0_HORZ);
+	isp_reg_writel(isp, reg_ver[0], OMAP3_ISP_IOMEM_HIST, ISPHIST_R0_VERT);
+	isp_reg_writel(isp, reg_hor[1], OMAP3_ISP_IOMEM_HIST, ISPHIST_R1_HORZ);
+	isp_reg_writel(isp, reg_ver[1], OMAP3_ISP_IOMEM_HIST, ISPHIST_R1_VERT);
+	isp_reg_writel(isp, reg_hor[2], OMAP3_ISP_IOMEM_HIST, ISPHIST_R2_HORZ);
+	isp_reg_writel(isp, reg_ver[2], OMAP3_ISP_IOMEM_HIST, ISPHIST_R2_VERT);
+	isp_reg_writel(isp, reg_hor[3], OMAP3_ISP_IOMEM_HIST, ISPHIST_R3_HORZ);
+	isp_reg_writel(isp, reg_ver[3], OMAP3_ISP_IOMEM_HIST, ISPHIST_R3_VERT);
+
+	hist->update = 0;
+	hist->config_counter += hist->inc_config;
+	hist->inc_config = 0;
+	hist->buf_size = conf->buf_size;
+}
+
+static void hist_enable(struct ispstat *hist, int enable)
+{
+	if (enable) {
+		isp_reg_set(hist->isp, OMAP3_ISP_IOMEM_HIST, ISPHIST_PCR,
+			    ISPHIST_PCR_ENABLE);
+		omap3isp_subclk_enable(hist->isp, OMAP3_ISP_SUBCLK_HIST);
+	} else {
+		isp_reg_clr(hist->isp, OMAP3_ISP_IOMEM_HIST, ISPHIST_PCR,
+			    ISPHIST_PCR_ENABLE);
+		omap3isp_subclk_disable(hist->isp, OMAP3_ISP_SUBCLK_HIST);
+	}
+}
+
+static int hist_busy(struct ispstat *hist)
+{
+	return isp_reg_readl(hist->isp, OMAP3_ISP_IOMEM_HIST, ISPHIST_PCR)
+						& ISPHIST_PCR_BUSY;
+}
+
+static void hist_dma_cb(void *data)
+{
+	struct ispstat *hist = data;
+
+	/* FIXME: The DMA engine API can't report transfer errors :-/ */
+
+	isp_reg_clr(hist->isp, OMAP3_ISP_IOMEM_HIST, ISPHIST_CNT,
+		    ISPHIST_CNT_CLEAR);
+
+	omap3isp_stat_dma_isr(hist);
+	if (hist->state != ISPSTAT_DISABLED)
+		omap3isp_hist_dma_done(hist->isp);
+}
+
+static int hist_buf_dma(struct ispstat *hist)
+{
+	dma_addr_t dma_addr = hist->active_buf->dma_addr;
+	struct dma_async_tx_descriptor *tx;
+	struct dma_slave_config cfg;
+	dma_cookie_t cookie;
+	int ret;
+
+	if (unlikely(!dma_addr)) {
+		dev_dbg(hist->isp->dev, "hist: invalid DMA buffer address\n");
+		goto error;
+	}
+
+	isp_reg_writel(hist->isp, 0, OMAP3_ISP_IOMEM_HIST, ISPHIST_ADDR);
+	isp_reg_set(hist->isp, OMAP3_ISP_IOMEM_HIST, ISPHIST_CNT,
+		    ISPHIST_CNT_CLEAR);
+	omap3isp_flush(hist->isp);
+
+	memset(&cfg, 0, sizeof(cfg));
+	cfg.src_addr = hist->isp->mmio_hist_base_phys + ISPHIST_DATA;
+	cfg.src_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
+	cfg.src_maxburst = hist->buf_size / 4;
+
+	ret = dmaengine_slave_config(hist->dma_ch, &cfg);
+	if (ret < 0) {
+		dev_dbg(hist->isp->dev,
+			"hist: DMA slave configuration failed\n");
+		goto error;
+	}
+
+	tx = dmaengine_prep_slave_single(hist->dma_ch, dma_addr,
+					 hist->buf_size, DMA_DEV_TO_MEM,
+					 DMA_CTRL_ACK);
+	if (tx == NULL) {
+		dev_dbg(hist->isp->dev,
+			"hist: DMA slave preparation failed\n");
+		goto error;
+	}
+
+	tx->callback = hist_dma_cb;
+	tx->callback_param = hist;
+	cookie = tx->tx_submit(tx);
+	if (dma_submit_error(cookie)) {
+		dev_dbg(hist->isp->dev, "hist: DMA submission failed\n");
+		goto error;
+	}
+
+	dma_async_issue_pending(hist->dma_ch);
+
+	return STAT_BUF_WAITING_DMA;
+
+error:
+	hist_reset_mem(hist);
+	return STAT_NO_BUF;
+}
+
+static int hist_buf_pio(struct ispstat *hist)
+{
+	struct isp_device *isp = hist->isp;
+	u32 *buf = hist->active_buf->virt_addr;
+	unsigned int i;
+
+	if (!buf) {
+		dev_dbg(isp->dev, "hist: invalid PIO buffer address\n");
+		hist_reset_mem(hist);
+		return STAT_NO_BUF;
+	}
+
+	isp_reg_writel(isp, 0, OMAP3_ISP_IOMEM_HIST, ISPHIST_ADDR);
+
+	/*
+	 * By setting it, the histogram internal buffer is being cleared at the
+	 * same time it's being read. This bit must be cleared just after all
+	 * data is acquired.
+	 */
+	isp_reg_set(isp, OMAP3_ISP_IOMEM_HIST, ISPHIST_CNT, ISPHIST_CNT_CLEAR);
+
+	/*
+	 * We'll read 4 times a 4-bytes-word at each iteration for
+	 * optimization. It avoids 3/4 of the jumps. We also know buf_size is
+	 * divisible by 16.
+	 */
+	for (i = hist->buf_size / 16; i > 0; i--) {
+		*buf++ = isp_reg_readl(isp, OMAP3_ISP_IOMEM_HIST, ISPHIST_DATA);
+		*buf++ = isp_reg_readl(isp, OMAP3_ISP_IOMEM_HIST, ISPHIST_DATA);
+		*buf++ = isp_reg_readl(isp, OMAP3_ISP_IOMEM_HIST, ISPHIST_DATA);
+		*buf++ = isp_reg_readl(isp, OMAP3_ISP_IOMEM_HIST, ISPHIST_DATA);
+	}
+	isp_reg_clr(hist->isp, OMAP3_ISP_IOMEM_HIST, ISPHIST_CNT,
+		    ISPHIST_CNT_CLEAR);
+
+	return STAT_BUF_DONE;
+}
+
+/*
+ * hist_buf_process - Callback from ISP driver for HIST interrupt.
+ */
+static int hist_buf_process(struct ispstat *hist)
+{
+	struct omap3isp_hist_config *user_cfg = hist->priv;
+	int ret;
+
+	if (atomic_read(&hist->buf_err) || hist->state != ISPSTAT_ENABLED) {
+		hist_reset_mem(hist);
+		return STAT_NO_BUF;
+	}
+
+	if (--(hist->wait_acc_frames))
+		return STAT_NO_BUF;
+
+	if (hist->dma_ch)
+		ret = hist_buf_dma(hist);
+	else
+		ret = hist_buf_pio(hist);
+
+	hist->wait_acc_frames = user_cfg->num_acc_frames;
+
+	return ret;
+}
+
+static u32 hist_get_buf_size(struct omap3isp_hist_config *conf)
+{
+	return OMAP3ISP_HIST_MEM_SIZE_BINS(conf->hist_bins) * conf->num_regions;
+}
+
+/*
+ * hist_validate_params - Helper function to check user given params.
+ * @new_conf: Pointer to user configuration structure.
+ *
+ * Returns 0 on success configuration.
+ */
+static int hist_validate_params(struct ispstat *hist, void *new_conf)
+{
+	struct omap3isp_hist_config *user_cfg = new_conf;
+	int c;
+	u32 buf_size;
+
+	if (user_cfg->cfa > OMAP3ISP_HIST_CFA_FOVEONX3)
+		return -EINVAL;
+
+	/* Regions size and position */
+
+	if ((user_cfg->num_regions < OMAP3ISP_HIST_MIN_REGIONS) ||
+	    (user_cfg->num_regions > OMAP3ISP_HIST_MAX_REGIONS))
+		return -EINVAL;
+
+	/* Regions */
+	for (c = 0; c < user_cfg->num_regions; c++) {
+		if (user_cfg->region[c].h_start & ~ISPHIST_REG_START_END_MASK)
+			return -EINVAL;
+		if (user_cfg->region[c].h_end & ~ISPHIST_REG_START_END_MASK)
+			return -EINVAL;
+		if (user_cfg->region[c].v_start & ~ISPHIST_REG_START_END_MASK)
+			return -EINVAL;
+		if (user_cfg->region[c].v_end & ~ISPHIST_REG_START_END_MASK)
+			return -EINVAL;
+		if (user_cfg->region[c].h_start > user_cfg->region[c].h_end)
+			return -EINVAL;
+		if (user_cfg->region[c].v_start > user_cfg->region[c].v_end)
+			return -EINVAL;
+	}
+
+	switch (user_cfg->num_regions) {
+	case 1:
+		if (user_cfg->hist_bins > OMAP3ISP_HIST_BINS_256)
+			return -EINVAL;
+		break;
+	case 2:
+		if (user_cfg->hist_bins > OMAP3ISP_HIST_BINS_128)
+			return -EINVAL;
+		break;
+	default: /* 3 or 4 */
+		if (user_cfg->hist_bins > OMAP3ISP_HIST_BINS_64)
+			return -EINVAL;
+		break;
+	}
+
+	buf_size = hist_get_buf_size(user_cfg);
+	if (buf_size > user_cfg->buf_size)
+		/* User's buf_size request wasn't enough */
+		user_cfg->buf_size = buf_size;
+	else if (user_cfg->buf_size > OMAP3ISP_HIST_MAX_BUF_SIZE)
+		user_cfg->buf_size = OMAP3ISP_HIST_MAX_BUF_SIZE;
+
+	return 0;
+}
+
+static int hist_comp_params(struct ispstat *hist,
+			    struct omap3isp_hist_config *user_cfg)
+{
+	struct omap3isp_hist_config *cur_cfg = hist->priv;
+	int c;
+
+	if (cur_cfg->cfa != user_cfg->cfa)
+		return 1;
+
+	if (cur_cfg->num_acc_frames != user_cfg->num_acc_frames)
+		return 1;
+
+	if (cur_cfg->hist_bins != user_cfg->hist_bins)
+		return 1;
+
+	for (c = 0; c < OMAP3ISP_HIST_MAX_WG; c++) {
+		if (c == 3 && user_cfg->cfa == OMAP3ISP_HIST_CFA_FOVEONX3)
+			break;
+		else if (cur_cfg->wg[c] != user_cfg->wg[c])
+			return 1;
+	}
+
+	if (cur_cfg->num_regions != user_cfg->num_regions)
+		return 1;
+
+	/* Regions */
+	for (c = 0; c < user_cfg->num_regions; c++) {
+		if (cur_cfg->region[c].h_start != user_cfg->region[c].h_start)
+			return 1;
+		if (cur_cfg->region[c].h_end != user_cfg->region[c].h_end)
+			return 1;
+		if (cur_cfg->region[c].v_start != user_cfg->region[c].v_start)
+			return 1;
+		if (cur_cfg->region[c].v_end != user_cfg->region[c].v_end)
+			return 1;
+	}
+
+	return 0;
+}
+
+/*
+ * hist_update_params - Helper function to check and store user given params.
+ * @new_conf: Pointer to user configuration structure.
+ */
+static void hist_set_params(struct ispstat *hist, void *new_conf)
+{
+	struct omap3isp_hist_config *user_cfg = new_conf;
+	struct omap3isp_hist_config *cur_cfg = hist->priv;
+
+	if (!hist->configured || hist_comp_params(hist, user_cfg)) {
+		memcpy(cur_cfg, user_cfg, sizeof(*user_cfg));
+		if (user_cfg->num_acc_frames == 0)
+			user_cfg->num_acc_frames = 1;
+		hist->inc_config++;
+		hist->update = 1;
+		/*
+		 * User might be asked for a bigger buffer than necessary for
+		 * this configuration. In order to return the right amount of
+		 * data during buffer request, let's calculate the size here
+		 * instead of stick with user_cfg->buf_size.
+		 */
+		cur_cfg->buf_size = hist_get_buf_size(cur_cfg);
+
+	}
+}
+
+static long hist_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
+{
+	struct ispstat *stat = v4l2_get_subdevdata(sd);
+
+	switch (cmd) {
+	case VIDIOC_OMAP3ISP_HIST_CFG:
+		return omap3isp_stat_config(stat, arg);
+	case VIDIOC_OMAP3ISP_STAT_REQ:
+		return omap3isp_stat_request_statistics(stat, arg);
+	case VIDIOC_OMAP3ISP_STAT_EN: {
+		int *en = arg;
+		return omap3isp_stat_enable(stat, !!*en);
+	}
+	}
+
+	return -ENOIOCTLCMD;
+
+}
+
+static const struct ispstat_ops hist_ops = {
+	.validate_params	= hist_validate_params,
+	.set_params		= hist_set_params,
+	.setup_regs		= hist_setup_regs,
+	.enable			= hist_enable,
+	.busy			= hist_busy,
+	.buf_process		= hist_buf_process,
+};
+
+static const struct v4l2_subdev_core_ops hist_subdev_core_ops = {
+	.ioctl = hist_ioctl,
+	.subscribe_event = omap3isp_stat_subscribe_event,
+	.unsubscribe_event = omap3isp_stat_unsubscribe_event,
+};
+
+static const struct v4l2_subdev_video_ops hist_subdev_video_ops = {
+	.s_stream = omap3isp_stat_s_stream,
+};
+
+static const struct v4l2_subdev_ops hist_subdev_ops = {
+	.core = &hist_subdev_core_ops,
+	.video = &hist_subdev_video_ops,
+};
+
+/*
+ * omap3isp_hist_init - Module Initialization.
+ */
+int omap3isp_hist_init(struct isp_device *isp)
+{
+	struct ispstat *hist = &isp->isp_hist;
+	struct omap3isp_hist_config *hist_cfg;
+	int ret = -1;
+
+	hist_cfg = devm_kzalloc(isp->dev, sizeof(*hist_cfg), GFP_KERNEL);
+	if (hist_cfg == NULL)
+		return -ENOMEM;
+
+	hist->isp = isp;
+
+	if (HIST_CONFIG_DMA) {
+		struct platform_device *pdev = to_platform_device(isp->dev);
+		struct resource *res;
+		unsigned int sig = 0;
+		dma_cap_mask_t mask;
+
+		dma_cap_zero(mask);
+		dma_cap_set(DMA_SLAVE, mask);
+
+		res = platform_get_resource_byname(pdev, IORESOURCE_DMA,
+						   "hist");
+		if (res)
+			sig = res->start;
+
+		hist->dma_ch = dma_request_slave_channel_compat(mask,
+				omap_dma_filter_fn, &sig, isp->dev, "hist");
+		if (!hist->dma_ch)
+			dev_warn(isp->dev,
+				 "hist: DMA channel request failed, using PIO\n");
+		else
+			dev_dbg(isp->dev, "hist: using DMA channel %s\n",
+				dma_chan_name(hist->dma_ch));
+	}
+
+	hist->ops = &hist_ops;
+	hist->priv = hist_cfg;
+	hist->event_type = V4L2_EVENT_OMAP3ISP_HIST;
+
+	ret = omap3isp_stat_init(hist, "histogram", &hist_subdev_ops);
+	if (ret) {
+		if (hist->dma_ch)
+			dma_release_channel(hist->dma_ch);
+	}
+
+	return ret;
+}
+
+/*
+ * omap3isp_hist_cleanup - Module cleanup.
+ */
+void omap3isp_hist_cleanup(struct isp_device *isp)
+{
+	struct ispstat *hist = &isp->isp_hist;
+
+	if (hist->dma_ch)
+		dma_release_channel(hist->dma_ch);
+
+	omap3isp_stat_cleanup(hist);
+}
diff --git a/drivers/media/platform/omap3isp/isphist.h b/drivers/media/platform/omap3isp/isphist.h
new file mode 100644
index 0000000..3b54155
--- /dev/null
+++ b/drivers/media/platform/omap3isp/isphist.h
@@ -0,0 +1,30 @@
+/*
+ * isphist.h
+ *
+ * TI OMAP3 ISP - Histogram module
+ *
+ * Copyright (C) 2010 Nokia Corporation
+ * Copyright (C) 2009 Texas Instruments, Inc.
+ *
+ * Contacts: David Cohen <dacohen@gmail.com>
+ *	     Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *	     Sakari Ailus <sakari.ailus@iki.fi>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef OMAP3_ISP_HIST_H
+#define OMAP3_ISP_HIST_H
+
+#include <linux/omap3isp.h>
+
+#define ISPHIST_IN_BIT_WIDTH_CCDC	10
+
+struct isp_device;
+
+int omap3isp_hist_init(struct isp_device *isp);
+void omap3isp_hist_cleanup(struct isp_device *isp);
+
+#endif /* OMAP3_ISP_HIST */
diff --git a/drivers/media/platform/omap3isp/isppreview.c b/drivers/media/platform/omap3isp/isppreview.c
new file mode 100644
index 0000000..1380327
--- /dev/null
+++ b/drivers/media/platform/omap3isp/isppreview.c
@@ -0,0 +1,2360 @@
+/*
+ * isppreview.c
+ *
+ * TI OMAP3 ISP driver - Preview module
+ *
+ * Copyright (C) 2010 Nokia Corporation
+ * Copyright (C) 2009 Texas Instruments, Inc.
+ *
+ * Contacts: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *	     Sakari Ailus <sakari.ailus@iki.fi>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/device.h>
+#include <linux/mm.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/uaccess.h>
+
+#include "isp.h"
+#include "ispreg.h"
+#include "isppreview.h"
+
+/* Default values in Office Fluorescent Light for RGBtoRGB Blending */
+static struct omap3isp_prev_rgbtorgb flr_rgb2rgb = {
+	{	/* RGB-RGB Matrix */
+		{0x01E2, 0x0F30, 0x0FEE},
+		{0x0F9B, 0x01AC, 0x0FB9},
+		{0x0FE0, 0x0EC0, 0x0260}
+	},	/* RGB Offset */
+	{0x0000, 0x0000, 0x0000}
+};
+
+/* Default values in Office Fluorescent Light for RGB to YUV Conversion*/
+static struct omap3isp_prev_csc flr_prev_csc = {
+	{	/* CSC Coef Matrix */
+		{66, 129, 25},
+		{-38, -75, 112},
+		{112, -94 , -18}
+	},	/* CSC Offset */
+	{0x0, 0x0, 0x0}
+};
+
+/* Default values in Office Fluorescent Light for CFA Gradient*/
+#define FLR_CFA_GRADTHRS_HORZ	0x28
+#define FLR_CFA_GRADTHRS_VERT	0x28
+
+/* Default values in Office Fluorescent Light for Chroma Suppression*/
+#define FLR_CSUP_GAIN		0x0D
+#define FLR_CSUP_THRES		0xEB
+
+/* Default values in Office Fluorescent Light for Noise Filter*/
+#define FLR_NF_STRGTH		0x03
+
+/* Default values for White Balance */
+#define FLR_WBAL_DGAIN		0x100
+#define FLR_WBAL_COEF		0x20
+
+/* Default values in Office Fluorescent Light for Black Adjustment*/
+#define FLR_BLKADJ_BLUE		0x0
+#define FLR_BLKADJ_GREEN	0x0
+#define FLR_BLKADJ_RED		0x0
+
+#define DEF_DETECT_CORRECT_VAL	0xe
+
+/*
+ * Margins and image size limits.
+ *
+ * The preview engine crops several rows and columns internally depending on
+ * which filters are enabled. To avoid format changes when the filters are
+ * enabled or disabled (which would prevent them from being turned on or off
+ * during streaming), the driver assumes all filters that can be configured
+ * during streaming are enabled when computing sink crop and source format
+ * limits.
+ *
+ * If a filter is disabled, additional cropping is automatically added at the
+ * preview engine input by the driver to avoid overflow at line and frame end.
+ * This is completely transparent for applications.
+ *
+ * Median filter		4 pixels
+ * Noise filter,
+ * Faulty pixels correction	4 pixels, 4 lines
+ * Color suppression		2 pixels
+ * or luma enhancement
+ * -------------------------------------------------------------
+ * Maximum total		10 pixels, 4 lines
+ *
+ * The color suppression and luma enhancement filters are applied after bayer to
+ * YUV conversion. They thus can crop one pixel on the left and one pixel on the
+ * right side of the image without changing the color pattern. When both those
+ * filters are disabled, the driver must crop the two pixels on the same side of
+ * the image to avoid changing the bayer pattern. The left margin is thus set to
+ * 6 pixels and the right margin to 4 pixels.
+ */
+
+#define PREV_MARGIN_LEFT	6
+#define PREV_MARGIN_RIGHT	4
+#define PREV_MARGIN_TOP		2
+#define PREV_MARGIN_BOTTOM	2
+
+#define PREV_MIN_IN_WIDTH	64
+#define PREV_MIN_IN_HEIGHT	8
+#define PREV_MAX_IN_HEIGHT	16384
+
+#define PREV_MIN_OUT_WIDTH		0
+#define PREV_MIN_OUT_HEIGHT		0
+#define PREV_MAX_OUT_WIDTH_REV_1	1280
+#define PREV_MAX_OUT_WIDTH_REV_2	3300
+#define PREV_MAX_OUT_WIDTH_REV_15	4096
+
+/*
+ * Coefficient Tables for the submodules in Preview.
+ * Array is initialised with the values from.the tables text file.
+ */
+
+/*
+ * CFA Filter Coefficient Table
+ *
+ */
+static u32 cfa_coef_table[4][OMAP3ISP_PREV_CFA_BLK_SIZE] = {
+#include "cfa_coef_table.h"
+};
+
+/*
+ * Default Gamma Correction Table - All components
+ */
+static u32 gamma_table[] = {
+#include "gamma_table.h"
+};
+
+/*
+ * Noise Filter Threshold table
+ */
+static u32 noise_filter_table[] = {
+#include "noise_filter_table.h"
+};
+
+/*
+ * Luminance Enhancement Table
+ */
+static u32 luma_enhance_table[] = {
+#include "luma_enhance_table.h"
+};
+
+/*
+ * preview_config_luma_enhancement - Configure the Luminance Enhancement table
+ */
+static void
+preview_config_luma_enhancement(struct isp_prev_device *prev,
+				const struct prev_params *params)
+{
+	struct isp_device *isp = to_isp_device(prev);
+	const struct omap3isp_prev_luma *yt = &params->luma;
+	unsigned int i;
+
+	isp_reg_writel(isp, ISPPRV_YENH_TABLE_ADDR,
+		       OMAP3_ISP_IOMEM_PREV, ISPPRV_SET_TBL_ADDR);
+	for (i = 0; i < OMAP3ISP_PREV_YENH_TBL_SIZE; i++) {
+		isp_reg_writel(isp, yt->table[i],
+			       OMAP3_ISP_IOMEM_PREV, ISPPRV_SET_TBL_DATA);
+	}
+}
+
+/*
+ * preview_enable_luma_enhancement - Enable/disable Luminance Enhancement
+ */
+static void
+preview_enable_luma_enhancement(struct isp_prev_device *prev, bool enable)
+{
+	struct isp_device *isp = to_isp_device(prev);
+
+	if (enable)
+		isp_reg_set(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
+			    ISPPRV_PCR_YNENHEN);
+	else
+		isp_reg_clr(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
+			    ISPPRV_PCR_YNENHEN);
+}
+
+/*
+ * preview_enable_invalaw - Enable/disable Inverse A-Law decompression
+ */
+static void preview_enable_invalaw(struct isp_prev_device *prev, bool enable)
+{
+	struct isp_device *isp = to_isp_device(prev);
+
+	if (enable)
+		isp_reg_set(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
+			    ISPPRV_PCR_INVALAW);
+	else
+		isp_reg_clr(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
+			    ISPPRV_PCR_INVALAW);
+}
+
+/*
+ * preview_config_hmed - Configure the Horizontal Median Filter
+ */
+static void preview_config_hmed(struct isp_prev_device *prev,
+				const struct prev_params *params)
+{
+	struct isp_device *isp = to_isp_device(prev);
+	const struct omap3isp_prev_hmed *hmed = &params->hmed;
+
+	isp_reg_writel(isp, (hmed->odddist == 1 ? 0 : ISPPRV_HMED_ODDDIST) |
+		       (hmed->evendist == 1 ? 0 : ISPPRV_HMED_EVENDIST) |
+		       (hmed->thres << ISPPRV_HMED_THRESHOLD_SHIFT),
+		       OMAP3_ISP_IOMEM_PREV, ISPPRV_HMED);
+}
+
+/*
+ * preview_enable_hmed - Enable/disable the Horizontal Median Filter
+ */
+static void preview_enable_hmed(struct isp_prev_device *prev, bool enable)
+{
+	struct isp_device *isp = to_isp_device(prev);
+
+	if (enable)
+		isp_reg_set(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
+			    ISPPRV_PCR_HMEDEN);
+	else
+		isp_reg_clr(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
+			    ISPPRV_PCR_HMEDEN);
+}
+
+/*
+ * preview_config_cfa - Configure CFA Interpolation for Bayer formats
+ *
+ * The CFA table is organised in four blocks, one per Bayer component. The
+ * hardware expects blocks to follow the Bayer order of the input data, while
+ * the driver stores the table in GRBG order in memory. The blocks need to be
+ * reordered to support non-GRBG Bayer patterns.
+ */
+static void preview_config_cfa(struct isp_prev_device *prev,
+			       const struct prev_params *params)
+{
+	static const unsigned int cfa_coef_order[4][4] = {
+		{ 0, 1, 2, 3 }, /* GRBG */
+		{ 1, 0, 3, 2 }, /* RGGB */
+		{ 2, 3, 0, 1 }, /* BGGR */
+		{ 3, 2, 1, 0 }, /* GBRG */
+	};
+	const unsigned int *order = cfa_coef_order[prev->params.cfa_order];
+	const struct omap3isp_prev_cfa *cfa = &params->cfa;
+	struct isp_device *isp = to_isp_device(prev);
+	unsigned int i;
+	unsigned int j;
+
+	isp_reg_writel(isp,
+		(cfa->gradthrs_vert << ISPPRV_CFA_GRADTH_VER_SHIFT) |
+		(cfa->gradthrs_horz << ISPPRV_CFA_GRADTH_HOR_SHIFT),
+		OMAP3_ISP_IOMEM_PREV, ISPPRV_CFA);
+
+	isp_reg_writel(isp, ISPPRV_CFA_TABLE_ADDR,
+		       OMAP3_ISP_IOMEM_PREV, ISPPRV_SET_TBL_ADDR);
+
+	for (i = 0; i < 4; ++i) {
+		const __u32 *block = cfa->table[order[i]];
+
+		for (j = 0; j < OMAP3ISP_PREV_CFA_BLK_SIZE; ++j)
+			isp_reg_writel(isp, block[j], OMAP3_ISP_IOMEM_PREV,
+				       ISPPRV_SET_TBL_DATA);
+	}
+}
+
+/*
+ * preview_config_chroma_suppression - Configure Chroma Suppression
+ */
+static void
+preview_config_chroma_suppression(struct isp_prev_device *prev,
+				  const struct prev_params *params)
+{
+	struct isp_device *isp = to_isp_device(prev);
+	const struct omap3isp_prev_csup *cs = &params->csup;
+
+	isp_reg_writel(isp,
+		       cs->gain | (cs->thres << ISPPRV_CSUP_THRES_SHIFT) |
+		       (cs->hypf_en << ISPPRV_CSUP_HPYF_SHIFT),
+		       OMAP3_ISP_IOMEM_PREV, ISPPRV_CSUP);
+}
+
+/*
+ * preview_enable_chroma_suppression - Enable/disable Chrominance Suppression
+ */
+static void
+preview_enable_chroma_suppression(struct isp_prev_device *prev, bool enable)
+{
+	struct isp_device *isp = to_isp_device(prev);
+
+	if (enable)
+		isp_reg_set(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
+			    ISPPRV_PCR_SUPEN);
+	else
+		isp_reg_clr(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
+			    ISPPRV_PCR_SUPEN);
+}
+
+/*
+ * preview_config_whitebalance - Configure White Balance parameters
+ *
+ * Coefficient matrix always with default values.
+ */
+static void
+preview_config_whitebalance(struct isp_prev_device *prev,
+			    const struct prev_params *params)
+{
+	struct isp_device *isp = to_isp_device(prev);
+	const struct omap3isp_prev_wbal *wbal = &params->wbal;
+	u32 val;
+
+	isp_reg_writel(isp, wbal->dgain, OMAP3_ISP_IOMEM_PREV, ISPPRV_WB_DGAIN);
+
+	val = wbal->coef0 << ISPPRV_WBGAIN_COEF0_SHIFT;
+	val |= wbal->coef1 << ISPPRV_WBGAIN_COEF1_SHIFT;
+	val |= wbal->coef2 << ISPPRV_WBGAIN_COEF2_SHIFT;
+	val |= wbal->coef3 << ISPPRV_WBGAIN_COEF3_SHIFT;
+	isp_reg_writel(isp, val, OMAP3_ISP_IOMEM_PREV, ISPPRV_WBGAIN);
+
+	isp_reg_writel(isp,
+		       ISPPRV_WBSEL_COEF0 << ISPPRV_WBSEL_N0_0_SHIFT |
+		       ISPPRV_WBSEL_COEF1 << ISPPRV_WBSEL_N0_1_SHIFT |
+		       ISPPRV_WBSEL_COEF0 << ISPPRV_WBSEL_N0_2_SHIFT |
+		       ISPPRV_WBSEL_COEF1 << ISPPRV_WBSEL_N0_3_SHIFT |
+		       ISPPRV_WBSEL_COEF2 << ISPPRV_WBSEL_N1_0_SHIFT |
+		       ISPPRV_WBSEL_COEF3 << ISPPRV_WBSEL_N1_1_SHIFT |
+		       ISPPRV_WBSEL_COEF2 << ISPPRV_WBSEL_N1_2_SHIFT |
+		       ISPPRV_WBSEL_COEF3 << ISPPRV_WBSEL_N1_3_SHIFT |
+		       ISPPRV_WBSEL_COEF0 << ISPPRV_WBSEL_N2_0_SHIFT |
+		       ISPPRV_WBSEL_COEF1 << ISPPRV_WBSEL_N2_1_SHIFT |
+		       ISPPRV_WBSEL_COEF0 << ISPPRV_WBSEL_N2_2_SHIFT |
+		       ISPPRV_WBSEL_COEF1 << ISPPRV_WBSEL_N2_3_SHIFT |
+		       ISPPRV_WBSEL_COEF2 << ISPPRV_WBSEL_N3_0_SHIFT |
+		       ISPPRV_WBSEL_COEF3 << ISPPRV_WBSEL_N3_1_SHIFT |
+		       ISPPRV_WBSEL_COEF2 << ISPPRV_WBSEL_N3_2_SHIFT |
+		       ISPPRV_WBSEL_COEF3 << ISPPRV_WBSEL_N3_3_SHIFT,
+		       OMAP3_ISP_IOMEM_PREV, ISPPRV_WBSEL);
+}
+
+/*
+ * preview_config_blkadj - Configure Black Adjustment
+ */
+static void
+preview_config_blkadj(struct isp_prev_device *prev,
+		      const struct prev_params *params)
+{
+	struct isp_device *isp = to_isp_device(prev);
+	const struct omap3isp_prev_blkadj *blkadj = &params->blkadj;
+
+	isp_reg_writel(isp, (blkadj->blue << ISPPRV_BLKADJOFF_B_SHIFT) |
+		       (blkadj->green << ISPPRV_BLKADJOFF_G_SHIFT) |
+		       (blkadj->red << ISPPRV_BLKADJOFF_R_SHIFT),
+		       OMAP3_ISP_IOMEM_PREV, ISPPRV_BLKADJOFF);
+}
+
+/*
+ * preview_config_rgb_blending - Configure RGB-RGB Blending
+ */
+static void
+preview_config_rgb_blending(struct isp_prev_device *prev,
+			    const struct prev_params *params)
+{
+	struct isp_device *isp = to_isp_device(prev);
+	const struct omap3isp_prev_rgbtorgb *rgbrgb = &params->rgb2rgb;
+	u32 val;
+
+	val = (rgbrgb->matrix[0][0] & 0xfff) << ISPPRV_RGB_MAT1_MTX_RR_SHIFT;
+	val |= (rgbrgb->matrix[0][1] & 0xfff) << ISPPRV_RGB_MAT1_MTX_GR_SHIFT;
+	isp_reg_writel(isp, val, OMAP3_ISP_IOMEM_PREV, ISPPRV_RGB_MAT1);
+
+	val = (rgbrgb->matrix[0][2] & 0xfff) << ISPPRV_RGB_MAT2_MTX_BR_SHIFT;
+	val |= (rgbrgb->matrix[1][0] & 0xfff) << ISPPRV_RGB_MAT2_MTX_RG_SHIFT;
+	isp_reg_writel(isp, val, OMAP3_ISP_IOMEM_PREV, ISPPRV_RGB_MAT2);
+
+	val = (rgbrgb->matrix[1][1] & 0xfff) << ISPPRV_RGB_MAT3_MTX_GG_SHIFT;
+	val |= (rgbrgb->matrix[1][2] & 0xfff) << ISPPRV_RGB_MAT3_MTX_BG_SHIFT;
+	isp_reg_writel(isp, val, OMAP3_ISP_IOMEM_PREV, ISPPRV_RGB_MAT3);
+
+	val = (rgbrgb->matrix[2][0] & 0xfff) << ISPPRV_RGB_MAT4_MTX_RB_SHIFT;
+	val |= (rgbrgb->matrix[2][1] & 0xfff) << ISPPRV_RGB_MAT4_MTX_GB_SHIFT;
+	isp_reg_writel(isp, val, OMAP3_ISP_IOMEM_PREV, ISPPRV_RGB_MAT4);
+
+	val = (rgbrgb->matrix[2][2] & 0xfff) << ISPPRV_RGB_MAT5_MTX_BB_SHIFT;
+	isp_reg_writel(isp, val, OMAP3_ISP_IOMEM_PREV, ISPPRV_RGB_MAT5);
+
+	val = (rgbrgb->offset[0] & 0x3ff) << ISPPRV_RGB_OFF1_MTX_OFFR_SHIFT;
+	val |= (rgbrgb->offset[1] & 0x3ff) << ISPPRV_RGB_OFF1_MTX_OFFG_SHIFT;
+	isp_reg_writel(isp, val, OMAP3_ISP_IOMEM_PREV, ISPPRV_RGB_OFF1);
+
+	val = (rgbrgb->offset[2] & 0x3ff) << ISPPRV_RGB_OFF2_MTX_OFFB_SHIFT;
+	isp_reg_writel(isp, val, OMAP3_ISP_IOMEM_PREV, ISPPRV_RGB_OFF2);
+}
+
+/*
+ * preview_config_csc - Configure Color Space Conversion (RGB to YCbYCr)
+ */
+static void
+preview_config_csc(struct isp_prev_device *prev,
+		   const struct prev_params *params)
+{
+	struct isp_device *isp = to_isp_device(prev);
+	const struct omap3isp_prev_csc *csc = &params->csc;
+	u32 val;
+
+	val = (csc->matrix[0][0] & 0x3ff) << ISPPRV_CSC0_RY_SHIFT;
+	val |= (csc->matrix[0][1] & 0x3ff) << ISPPRV_CSC0_GY_SHIFT;
+	val |= (csc->matrix[0][2] & 0x3ff) << ISPPRV_CSC0_BY_SHIFT;
+	isp_reg_writel(isp, val, OMAP3_ISP_IOMEM_PREV, ISPPRV_CSC0);
+
+	val = (csc->matrix[1][0] & 0x3ff) << ISPPRV_CSC1_RCB_SHIFT;
+	val |= (csc->matrix[1][1] & 0x3ff) << ISPPRV_CSC1_GCB_SHIFT;
+	val |= (csc->matrix[1][2] & 0x3ff) << ISPPRV_CSC1_BCB_SHIFT;
+	isp_reg_writel(isp, val, OMAP3_ISP_IOMEM_PREV, ISPPRV_CSC1);
+
+	val = (csc->matrix[2][0] & 0x3ff) << ISPPRV_CSC2_RCR_SHIFT;
+	val |= (csc->matrix[2][1] & 0x3ff) << ISPPRV_CSC2_GCR_SHIFT;
+	val |= (csc->matrix[2][2] & 0x3ff) << ISPPRV_CSC2_BCR_SHIFT;
+	isp_reg_writel(isp, val, OMAP3_ISP_IOMEM_PREV, ISPPRV_CSC2);
+
+	val = (csc->offset[0] & 0xff) << ISPPRV_CSC_OFFSET_Y_SHIFT;
+	val |= (csc->offset[1] & 0xff) << ISPPRV_CSC_OFFSET_CB_SHIFT;
+	val |= (csc->offset[2] & 0xff) << ISPPRV_CSC_OFFSET_CR_SHIFT;
+	isp_reg_writel(isp, val, OMAP3_ISP_IOMEM_PREV, ISPPRV_CSC_OFFSET);
+}
+
+/*
+ * preview_config_yc_range - Configure the max and min Y and C values
+ */
+static void
+preview_config_yc_range(struct isp_prev_device *prev,
+			const struct prev_params *params)
+{
+	struct isp_device *isp = to_isp_device(prev);
+	const struct omap3isp_prev_yclimit *yc = &params->yclimit;
+
+	isp_reg_writel(isp,
+		       yc->maxC << ISPPRV_SETUP_YC_MAXC_SHIFT |
+		       yc->maxY << ISPPRV_SETUP_YC_MAXY_SHIFT |
+		       yc->minC << ISPPRV_SETUP_YC_MINC_SHIFT |
+		       yc->minY << ISPPRV_SETUP_YC_MINY_SHIFT,
+		       OMAP3_ISP_IOMEM_PREV, ISPPRV_SETUP_YC);
+}
+
+/*
+ * preview_config_dcor - Configure Couplet Defect Correction
+ */
+static void
+preview_config_dcor(struct isp_prev_device *prev,
+		    const struct prev_params *params)
+{
+	struct isp_device *isp = to_isp_device(prev);
+	const struct omap3isp_prev_dcor *dcor = &params->dcor;
+
+	isp_reg_writel(isp, dcor->detect_correct[0],
+		       OMAP3_ISP_IOMEM_PREV, ISPPRV_CDC_THR0);
+	isp_reg_writel(isp, dcor->detect_correct[1],
+		       OMAP3_ISP_IOMEM_PREV, ISPPRV_CDC_THR1);
+	isp_reg_writel(isp, dcor->detect_correct[2],
+		       OMAP3_ISP_IOMEM_PREV, ISPPRV_CDC_THR2);
+	isp_reg_writel(isp, dcor->detect_correct[3],
+		       OMAP3_ISP_IOMEM_PREV, ISPPRV_CDC_THR3);
+	isp_reg_clr_set(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
+			ISPPRV_PCR_DCCOUP,
+			dcor->couplet_mode_en ? ISPPRV_PCR_DCCOUP : 0);
+}
+
+/*
+ * preview_enable_dcor - Enable/disable Couplet Defect Correction
+ */
+static void preview_enable_dcor(struct isp_prev_device *prev, bool enable)
+{
+	struct isp_device *isp = to_isp_device(prev);
+
+	if (enable)
+		isp_reg_set(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
+			    ISPPRV_PCR_DCOREN);
+	else
+		isp_reg_clr(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
+			    ISPPRV_PCR_DCOREN);
+}
+
+/*
+ * preview_enable_drkframe_capture - Enable/disable Dark Frame Capture
+ */
+static void
+preview_enable_drkframe_capture(struct isp_prev_device *prev, bool enable)
+{
+	struct isp_device *isp = to_isp_device(prev);
+
+	if (enable)
+		isp_reg_set(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
+			    ISPPRV_PCR_DRKFCAP);
+	else
+		isp_reg_clr(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
+			    ISPPRV_PCR_DRKFCAP);
+}
+
+/*
+ * preview_enable_drkframe - Enable/disable Dark Frame Subtraction
+ */
+static void preview_enable_drkframe(struct isp_prev_device *prev, bool enable)
+{
+	struct isp_device *isp = to_isp_device(prev);
+
+	if (enable)
+		isp_reg_set(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
+			    ISPPRV_PCR_DRKFEN);
+	else
+		isp_reg_clr(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
+			    ISPPRV_PCR_DRKFEN);
+}
+
+/*
+ * preview_config_noisefilter - Configure the Noise Filter
+ */
+static void
+preview_config_noisefilter(struct isp_prev_device *prev,
+			   const struct prev_params *params)
+{
+	struct isp_device *isp = to_isp_device(prev);
+	const struct omap3isp_prev_nf *nf = &params->nf;
+	unsigned int i;
+
+	isp_reg_writel(isp, nf->spread, OMAP3_ISP_IOMEM_PREV, ISPPRV_NF);
+	isp_reg_writel(isp, ISPPRV_NF_TABLE_ADDR,
+		       OMAP3_ISP_IOMEM_PREV, ISPPRV_SET_TBL_ADDR);
+	for (i = 0; i < OMAP3ISP_PREV_NF_TBL_SIZE; i++) {
+		isp_reg_writel(isp, nf->table[i],
+			       OMAP3_ISP_IOMEM_PREV, ISPPRV_SET_TBL_DATA);
+	}
+}
+
+/*
+ * preview_enable_noisefilter - Enable/disable the Noise Filter
+ */
+static void
+preview_enable_noisefilter(struct isp_prev_device *prev, bool enable)
+{
+	struct isp_device *isp = to_isp_device(prev);
+
+	if (enable)
+		isp_reg_set(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
+			    ISPPRV_PCR_NFEN);
+	else
+		isp_reg_clr(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
+			    ISPPRV_PCR_NFEN);
+}
+
+/*
+ * preview_config_gammacorrn - Configure the Gamma Correction tables
+ */
+static void
+preview_config_gammacorrn(struct isp_prev_device *prev,
+			  const struct prev_params *params)
+{
+	struct isp_device *isp = to_isp_device(prev);
+	const struct omap3isp_prev_gtables *gt = &params->gamma;
+	unsigned int i;
+
+	isp_reg_writel(isp, ISPPRV_REDGAMMA_TABLE_ADDR,
+		       OMAP3_ISP_IOMEM_PREV, ISPPRV_SET_TBL_ADDR);
+	for (i = 0; i < OMAP3ISP_PREV_GAMMA_TBL_SIZE; i++)
+		isp_reg_writel(isp, gt->red[i], OMAP3_ISP_IOMEM_PREV,
+			       ISPPRV_SET_TBL_DATA);
+
+	isp_reg_writel(isp, ISPPRV_GREENGAMMA_TABLE_ADDR,
+		       OMAP3_ISP_IOMEM_PREV, ISPPRV_SET_TBL_ADDR);
+	for (i = 0; i < OMAP3ISP_PREV_GAMMA_TBL_SIZE; i++)
+		isp_reg_writel(isp, gt->green[i], OMAP3_ISP_IOMEM_PREV,
+			       ISPPRV_SET_TBL_DATA);
+
+	isp_reg_writel(isp, ISPPRV_BLUEGAMMA_TABLE_ADDR,
+		       OMAP3_ISP_IOMEM_PREV, ISPPRV_SET_TBL_ADDR);
+	for (i = 0; i < OMAP3ISP_PREV_GAMMA_TBL_SIZE; i++)
+		isp_reg_writel(isp, gt->blue[i], OMAP3_ISP_IOMEM_PREV,
+			       ISPPRV_SET_TBL_DATA);
+}
+
+/*
+ * preview_enable_gammacorrn - Enable/disable Gamma Correction
+ *
+ * When gamma correction is disabled, the module is bypassed and its output is
+ * the 8 MSB of the 10-bit input .
+ */
+static void
+preview_enable_gammacorrn(struct isp_prev_device *prev, bool enable)
+{
+	struct isp_device *isp = to_isp_device(prev);
+
+	if (enable)
+		isp_reg_clr(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
+			    ISPPRV_PCR_GAMMA_BYPASS);
+	else
+		isp_reg_set(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
+			    ISPPRV_PCR_GAMMA_BYPASS);
+}
+
+/*
+ * preview_config_contrast - Configure the Contrast
+ *
+ * Value should be programmed before enabling the module.
+ */
+static void
+preview_config_contrast(struct isp_prev_device *prev,
+			const struct prev_params *params)
+{
+	struct isp_device *isp = to_isp_device(prev);
+
+	isp_reg_clr_set(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_CNT_BRT,
+			0xff << ISPPRV_CNT_BRT_CNT_SHIFT,
+			params->contrast << ISPPRV_CNT_BRT_CNT_SHIFT);
+}
+
+/*
+ * preview_config_brightness - Configure the Brightness
+ */
+static void
+preview_config_brightness(struct isp_prev_device *prev,
+			  const struct prev_params *params)
+{
+	struct isp_device *isp = to_isp_device(prev);
+
+	isp_reg_clr_set(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_CNT_BRT,
+			0xff << ISPPRV_CNT_BRT_BRT_SHIFT,
+			params->brightness << ISPPRV_CNT_BRT_BRT_SHIFT);
+}
+
+/*
+ * preview_update_contrast - Updates the contrast.
+ * @contrast: Pointer to hold the current programmed contrast value.
+ *
+ * Value should be programmed before enabling the module.
+ */
+static void
+preview_update_contrast(struct isp_prev_device *prev, u8 contrast)
+{
+	struct prev_params *params;
+	unsigned long flags;
+
+	spin_lock_irqsave(&prev->params.lock, flags);
+	params = (prev->params.active & OMAP3ISP_PREV_CONTRAST)
+	       ? &prev->params.params[0] : &prev->params.params[1];
+
+	if (params->contrast != (contrast * ISPPRV_CONTRAST_UNITS)) {
+		params->contrast = contrast * ISPPRV_CONTRAST_UNITS;
+		params->update |= OMAP3ISP_PREV_CONTRAST;
+	}
+	spin_unlock_irqrestore(&prev->params.lock, flags);
+}
+
+/*
+ * preview_update_brightness - Updates the brightness in preview module.
+ * @brightness: Pointer to hold the current programmed brightness value.
+ *
+ */
+static void
+preview_update_brightness(struct isp_prev_device *prev, u8 brightness)
+{
+	struct prev_params *params;
+	unsigned long flags;
+
+	spin_lock_irqsave(&prev->params.lock, flags);
+	params = (prev->params.active & OMAP3ISP_PREV_BRIGHTNESS)
+	       ? &prev->params.params[0] : &prev->params.params[1];
+
+	if (params->brightness != (brightness * ISPPRV_BRIGHT_UNITS)) {
+		params->brightness = brightness * ISPPRV_BRIGHT_UNITS;
+		params->update |= OMAP3ISP_PREV_BRIGHTNESS;
+	}
+	spin_unlock_irqrestore(&prev->params.lock, flags);
+}
+
+static u32
+preview_params_lock(struct isp_prev_device *prev, u32 update, bool shadow)
+{
+	u32 active = prev->params.active;
+
+	if (shadow) {
+		/* Mark all shadow parameters we are going to touch as busy. */
+		prev->params.params[0].busy |= ~active & update;
+		prev->params.params[1].busy |= active & update;
+	} else {
+		/* Mark all active parameters we are going to touch as busy. */
+		update = (prev->params.params[0].update & active)
+		       | (prev->params.params[1].update & ~active);
+
+		prev->params.params[0].busy |= active & update;
+		prev->params.params[1].busy |= ~active & update;
+	}
+
+	return update;
+}
+
+static void
+preview_params_unlock(struct isp_prev_device *prev, u32 update, bool shadow)
+{
+	u32 active = prev->params.active;
+
+	if (shadow) {
+		/* Set the update flag for shadow parameters that have been
+		 * updated and clear the busy flag for all shadow parameters.
+		 */
+		prev->params.params[0].update |= (~active & update);
+		prev->params.params[1].update |= (active & update);
+		prev->params.params[0].busy &= active;
+		prev->params.params[1].busy &= ~active;
+	} else {
+		/* Clear the update flag for active parameters that have been
+		 * applied and the busy flag for all active parameters.
+		 */
+		prev->params.params[0].update &= ~(active & update);
+		prev->params.params[1].update &= ~(~active & update);
+		prev->params.params[0].busy &= ~active;
+		prev->params.params[1].busy &= active;
+	}
+}
+
+static void preview_params_switch(struct isp_prev_device *prev)
+{
+	u32 to_switch;
+
+	/* Switch active parameters with updated shadow parameters when the
+	 * shadow parameter has been updated and neither the active not the
+	 * shadow parameter is busy.
+	 */
+	to_switch = (prev->params.params[0].update & ~prev->params.active)
+		  | (prev->params.params[1].update & prev->params.active);
+	to_switch &= ~(prev->params.params[0].busy |
+		       prev->params.params[1].busy);
+	if (to_switch == 0)
+		return;
+
+	prev->params.active ^= to_switch;
+
+	/* Remove the update flag for the shadow copy of parameters we have
+	 * switched.
+	 */
+	prev->params.params[0].update &= ~(~prev->params.active & to_switch);
+	prev->params.params[1].update &= ~(prev->params.active & to_switch);
+}
+
+/* preview parameters update structure */
+struct preview_update {
+	void (*config)(struct isp_prev_device *, const struct prev_params *);
+	void (*enable)(struct isp_prev_device *, bool);
+	unsigned int param_offset;
+	unsigned int param_size;
+	unsigned int config_offset;
+	bool skip;
+};
+
+/* Keep the array indexed by the OMAP3ISP_PREV_* bit number. */
+static const struct preview_update update_attrs[] = {
+	/* OMAP3ISP_PREV_LUMAENH */ {
+		preview_config_luma_enhancement,
+		preview_enable_luma_enhancement,
+		offsetof(struct prev_params, luma),
+		FIELD_SIZEOF(struct prev_params, luma),
+		offsetof(struct omap3isp_prev_update_config, luma),
+	}, /* OMAP3ISP_PREV_INVALAW */ {
+		NULL,
+		preview_enable_invalaw,
+	}, /* OMAP3ISP_PREV_HRZ_MED */ {
+		preview_config_hmed,
+		preview_enable_hmed,
+		offsetof(struct prev_params, hmed),
+		FIELD_SIZEOF(struct prev_params, hmed),
+		offsetof(struct omap3isp_prev_update_config, hmed),
+	}, /* OMAP3ISP_PREV_CFA */ {
+		preview_config_cfa,
+		NULL,
+		offsetof(struct prev_params, cfa),
+		FIELD_SIZEOF(struct prev_params, cfa),
+		offsetof(struct omap3isp_prev_update_config, cfa),
+	}, /* OMAP3ISP_PREV_CHROMA_SUPP */ {
+		preview_config_chroma_suppression,
+		preview_enable_chroma_suppression,
+		offsetof(struct prev_params, csup),
+		FIELD_SIZEOF(struct prev_params, csup),
+		offsetof(struct omap3isp_prev_update_config, csup),
+	}, /* OMAP3ISP_PREV_WB */ {
+		preview_config_whitebalance,
+		NULL,
+		offsetof(struct prev_params, wbal),
+		FIELD_SIZEOF(struct prev_params, wbal),
+		offsetof(struct omap3isp_prev_update_config, wbal),
+	}, /* OMAP3ISP_PREV_BLKADJ */ {
+		preview_config_blkadj,
+		NULL,
+		offsetof(struct prev_params, blkadj),
+		FIELD_SIZEOF(struct prev_params, blkadj),
+		offsetof(struct omap3isp_prev_update_config, blkadj),
+	}, /* OMAP3ISP_PREV_RGB2RGB */ {
+		preview_config_rgb_blending,
+		NULL,
+		offsetof(struct prev_params, rgb2rgb),
+		FIELD_SIZEOF(struct prev_params, rgb2rgb),
+		offsetof(struct omap3isp_prev_update_config, rgb2rgb),
+	}, /* OMAP3ISP_PREV_COLOR_CONV */ {
+		preview_config_csc,
+		NULL,
+		offsetof(struct prev_params, csc),
+		FIELD_SIZEOF(struct prev_params, csc),
+		offsetof(struct omap3isp_prev_update_config, csc),
+	}, /* OMAP3ISP_PREV_YC_LIMIT */ {
+		preview_config_yc_range,
+		NULL,
+		offsetof(struct prev_params, yclimit),
+		FIELD_SIZEOF(struct prev_params, yclimit),
+		offsetof(struct omap3isp_prev_update_config, yclimit),
+	}, /* OMAP3ISP_PREV_DEFECT_COR */ {
+		preview_config_dcor,
+		preview_enable_dcor,
+		offsetof(struct prev_params, dcor),
+		FIELD_SIZEOF(struct prev_params, dcor),
+		offsetof(struct omap3isp_prev_update_config, dcor),
+	}, /* Previously OMAP3ISP_PREV_GAMMABYPASS, not used anymore */ {
+		NULL,
+		NULL,
+	}, /* OMAP3ISP_PREV_DRK_FRM_CAPTURE */ {
+		NULL,
+		preview_enable_drkframe_capture,
+	}, /* OMAP3ISP_PREV_DRK_FRM_SUBTRACT */ {
+		NULL,
+		preview_enable_drkframe,
+	}, /* OMAP3ISP_PREV_LENS_SHADING */ {
+		NULL,
+		preview_enable_drkframe,
+	}, /* OMAP3ISP_PREV_NF */ {
+		preview_config_noisefilter,
+		preview_enable_noisefilter,
+		offsetof(struct prev_params, nf),
+		FIELD_SIZEOF(struct prev_params, nf),
+		offsetof(struct omap3isp_prev_update_config, nf),
+	}, /* OMAP3ISP_PREV_GAMMA */ {
+		preview_config_gammacorrn,
+		preview_enable_gammacorrn,
+		offsetof(struct prev_params, gamma),
+		FIELD_SIZEOF(struct prev_params, gamma),
+		offsetof(struct omap3isp_prev_update_config, gamma),
+	}, /* OMAP3ISP_PREV_CONTRAST */ {
+		preview_config_contrast,
+		NULL,
+		0, 0, 0, true,
+	}, /* OMAP3ISP_PREV_BRIGHTNESS */ {
+		preview_config_brightness,
+		NULL,
+		0, 0, 0, true,
+	},
+};
+
+/*
+ * preview_config - Copy and update local structure with userspace preview
+ *                  configuration.
+ * @prev: ISP preview engine
+ * @cfg: Configuration
+ *
+ * Return zero if success or -EFAULT if the configuration can't be copied from
+ * userspace.
+ */
+static int preview_config(struct isp_prev_device *prev,
+			  struct omap3isp_prev_update_config *cfg)
+{
+	unsigned long flags;
+	unsigned int i;
+	int rval = 0;
+	u32 update;
+	u32 active;
+
+	if (cfg->update == 0)
+		return 0;
+
+	/* Mark the shadow parameters we're going to update as busy. */
+	spin_lock_irqsave(&prev->params.lock, flags);
+	preview_params_lock(prev, cfg->update, true);
+	active = prev->params.active;
+	spin_unlock_irqrestore(&prev->params.lock, flags);
+
+	update = 0;
+
+	for (i = 0; i < ARRAY_SIZE(update_attrs); i++) {
+		const struct preview_update *attr = &update_attrs[i];
+		struct prev_params *params;
+		unsigned int bit = 1 << i;
+
+		if (attr->skip || !(cfg->update & bit))
+			continue;
+
+		params = &prev->params.params[!!(active & bit)];
+
+		if (cfg->flag & bit) {
+			void __user *from = *(void * __user *)
+				((void *)cfg + attr->config_offset);
+			void *to = (void *)params + attr->param_offset;
+			size_t size = attr->param_size;
+
+			if (to && from && size) {
+				if (copy_from_user(to, from, size)) {
+					rval = -EFAULT;
+					break;
+				}
+			}
+			params->features |= bit;
+		} else {
+			params->features &= ~bit;
+		}
+
+		update |= bit;
+	}
+
+	spin_lock_irqsave(&prev->params.lock, flags);
+	preview_params_unlock(prev, update, true);
+	preview_params_switch(prev);
+	spin_unlock_irqrestore(&prev->params.lock, flags);
+
+	return rval;
+}
+
+/*
+ * preview_setup_hw - Setup preview registers and/or internal memory
+ * @prev: pointer to preview private structure
+ * @update: Bitmask of parameters to setup
+ * @active: Bitmask of parameters active in set 0
+ * Note: can be called from interrupt context
+ * Return none
+ */
+static void preview_setup_hw(struct isp_prev_device *prev, u32 update,
+			     u32 active)
+{
+	unsigned int i;
+
+	if (update == 0)
+		return;
+
+	for (i = 0; i < ARRAY_SIZE(update_attrs); i++) {
+		const struct preview_update *attr = &update_attrs[i];
+		struct prev_params *params;
+		unsigned int bit = 1 << i;
+
+		if (!(update & bit))
+			continue;
+
+		params = &prev->params.params[!(active & bit)];
+
+		if (params->features & bit) {
+			if (attr->config)
+				attr->config(prev, params);
+			if (attr->enable)
+				attr->enable(prev, true);
+		} else {
+			if (attr->enable)
+				attr->enable(prev, false);
+		}
+	}
+}
+
+/*
+ * preview_config_ycpos - Configure byte layout of YUV image.
+ * @prev: pointer to previewer private structure
+ * @pixelcode: pixel code
+ */
+static void preview_config_ycpos(struct isp_prev_device *prev, u32 pixelcode)
+{
+	struct isp_device *isp = to_isp_device(prev);
+	enum preview_ycpos_mode mode;
+
+	switch (pixelcode) {
+	case MEDIA_BUS_FMT_YUYV8_1X16:
+		mode = YCPOS_CrYCbY;
+		break;
+	case MEDIA_BUS_FMT_UYVY8_1X16:
+		mode = YCPOS_YCrYCb;
+		break;
+	default:
+		return;
+	}
+
+	isp_reg_clr_set(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
+			ISPPRV_PCR_YCPOS_CrYCbY,
+			mode << ISPPRV_PCR_YCPOS_SHIFT);
+}
+
+/*
+ * preview_config_averager - Enable / disable / configure averager
+ * @average: Average value to be configured.
+ */
+static void preview_config_averager(struct isp_prev_device *prev, u8 average)
+{
+	struct isp_device *isp = to_isp_device(prev);
+
+	isp_reg_writel(isp, ISPPRV_AVE_EVENDIST_2 << ISPPRV_AVE_EVENDIST_SHIFT |
+		       ISPPRV_AVE_ODDDIST_2 << ISPPRV_AVE_ODDDIST_SHIFT |
+		       average, OMAP3_ISP_IOMEM_PREV, ISPPRV_AVE);
+}
+
+
+/*
+ * preview_config_input_format - Configure the input format
+ * @prev: The preview engine
+ * @info: Sink pad format information
+ *
+ * Enable and configure CFA interpolation for Bayer formats and disable it for
+ * greyscale formats.
+ *
+ * The CFA table is organised in four blocks, one per Bayer component. The
+ * hardware expects blocks to follow the Bayer order of the input data, while
+ * the driver stores the table in GRBG order in memory. The blocks need to be
+ * reordered to support non-GRBG Bayer patterns.
+ */
+static void preview_config_input_format(struct isp_prev_device *prev,
+					const struct isp_format_info *info)
+{
+	struct isp_device *isp = to_isp_device(prev);
+	struct prev_params *params;
+
+	if (info->width == 8)
+		isp_reg_set(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
+			    ISPPRV_PCR_WIDTH);
+	else
+		isp_reg_clr(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
+			    ISPPRV_PCR_WIDTH);
+
+	switch (info->flavor) {
+	case MEDIA_BUS_FMT_SGRBG8_1X8:
+		prev->params.cfa_order = 0;
+		break;
+	case MEDIA_BUS_FMT_SRGGB8_1X8:
+		prev->params.cfa_order = 1;
+		break;
+	case MEDIA_BUS_FMT_SBGGR8_1X8:
+		prev->params.cfa_order = 2;
+		break;
+	case MEDIA_BUS_FMT_SGBRG8_1X8:
+		prev->params.cfa_order = 3;
+		break;
+	default:
+		/* Disable CFA for non-Bayer formats. */
+		isp_reg_clr(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
+			    ISPPRV_PCR_CFAEN);
+		return;
+	}
+
+	isp_reg_set(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR, ISPPRV_PCR_CFAEN);
+	isp_reg_clr_set(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
+			ISPPRV_PCR_CFAFMT_MASK, ISPPRV_PCR_CFAFMT_BAYER);
+
+	params = (prev->params.active & OMAP3ISP_PREV_CFA)
+	       ? &prev->params.params[0] : &prev->params.params[1];
+
+	preview_config_cfa(prev, params);
+}
+
+/*
+ * preview_config_input_size - Configure the input frame size
+ *
+ * The preview engine crops several rows and columns internally depending on
+ * which processing blocks are enabled. The driver assumes all those blocks are
+ * enabled when reporting source pad formats to userspace. If this assumption is
+ * not true, rows and columns must be manually cropped at the preview engine
+ * input to avoid overflows at the end of lines and frames.
+ *
+ * See the explanation at the PREV_MARGIN_* definitions for more details.
+ */
+static void preview_config_input_size(struct isp_prev_device *prev, u32 active)
+{
+	const struct v4l2_mbus_framefmt *format = &prev->formats[PREV_PAD_SINK];
+	struct isp_device *isp = to_isp_device(prev);
+	unsigned int sph = prev->crop.left;
+	unsigned int eph = prev->crop.left + prev->crop.width - 1;
+	unsigned int slv = prev->crop.top;
+	unsigned int elv = prev->crop.top + prev->crop.height - 1;
+	u32 features;
+
+	if (format->code != MEDIA_BUS_FMT_Y8_1X8 &&
+	    format->code != MEDIA_BUS_FMT_Y10_1X10) {
+		sph -= 2;
+		eph += 2;
+		slv -= 2;
+		elv += 2;
+	}
+
+	features = (prev->params.params[0].features & active)
+		 | (prev->params.params[1].features & ~active);
+
+	if (features & (OMAP3ISP_PREV_DEFECT_COR | OMAP3ISP_PREV_NF)) {
+		sph -= 2;
+		eph += 2;
+		slv -= 2;
+		elv += 2;
+	}
+	if (features & OMAP3ISP_PREV_HRZ_MED) {
+		sph -= 2;
+		eph += 2;
+	}
+	if (features & (OMAP3ISP_PREV_CHROMA_SUPP | OMAP3ISP_PREV_LUMAENH))
+		sph -= 2;
+
+	isp_reg_writel(isp, (sph << ISPPRV_HORZ_INFO_SPH_SHIFT) | eph,
+		       OMAP3_ISP_IOMEM_PREV, ISPPRV_HORZ_INFO);
+	isp_reg_writel(isp, (slv << ISPPRV_VERT_INFO_SLV_SHIFT) | elv,
+		       OMAP3_ISP_IOMEM_PREV, ISPPRV_VERT_INFO);
+}
+
+/*
+ * preview_config_inlineoffset - Configures the Read address line offset.
+ * @prev: Preview module
+ * @offset: Line offset
+ *
+ * According to the TRM, the line offset must be aligned on a 32 bytes boundary.
+ * However, a hardware bug requires the memory start address to be aligned on a
+ * 64 bytes boundary, so the offset probably should be aligned on 64 bytes as
+ * well.
+ */
+static void
+preview_config_inlineoffset(struct isp_prev_device *prev, u32 offset)
+{
+	struct isp_device *isp = to_isp_device(prev);
+
+	isp_reg_writel(isp, offset & 0xffff, OMAP3_ISP_IOMEM_PREV,
+		       ISPPRV_RADR_OFFSET);
+}
+
+/*
+ * preview_set_inaddr - Sets memory address of input frame.
+ * @addr: 32bit memory address aligned on 32byte boundary.
+ *
+ * Configures the memory address from which the input frame is to be read.
+ */
+static void preview_set_inaddr(struct isp_prev_device *prev, u32 addr)
+{
+	struct isp_device *isp = to_isp_device(prev);
+
+	isp_reg_writel(isp, addr, OMAP3_ISP_IOMEM_PREV, ISPPRV_RSDR_ADDR);
+}
+
+/*
+ * preview_config_outlineoffset - Configures the Write address line offset.
+ * @offset: Line Offset for the preview output.
+ *
+ * The offset must be a multiple of 32 bytes.
+ */
+static void preview_config_outlineoffset(struct isp_prev_device *prev,
+				    u32 offset)
+{
+	struct isp_device *isp = to_isp_device(prev);
+
+	isp_reg_writel(isp, offset & 0xffff, OMAP3_ISP_IOMEM_PREV,
+		       ISPPRV_WADD_OFFSET);
+}
+
+/*
+ * preview_set_outaddr - Sets the memory address to store output frame
+ * @addr: 32bit memory address aligned on 32byte boundary.
+ *
+ * Configures the memory address to which the output frame is written.
+ */
+static void preview_set_outaddr(struct isp_prev_device *prev, u32 addr)
+{
+	struct isp_device *isp = to_isp_device(prev);
+
+	isp_reg_writel(isp, addr, OMAP3_ISP_IOMEM_PREV, ISPPRV_WSDR_ADDR);
+}
+
+static void preview_adjust_bandwidth(struct isp_prev_device *prev)
+{
+	struct isp_pipeline *pipe = to_isp_pipeline(&prev->subdev.entity);
+	struct isp_device *isp = to_isp_device(prev);
+	const struct v4l2_mbus_framefmt *ifmt = &prev->formats[PREV_PAD_SINK];
+	unsigned long l3_ick = pipe->l3_ick;
+	struct v4l2_fract *timeperframe;
+	unsigned int cycles_per_frame;
+	unsigned int requests_per_frame;
+	unsigned int cycles_per_request;
+	unsigned int minimum;
+	unsigned int maximum;
+	unsigned int value;
+
+	if (prev->input != PREVIEW_INPUT_MEMORY) {
+		isp_reg_clr(isp, OMAP3_ISP_IOMEM_SBL, ISPSBL_SDR_REQ_EXP,
+			    ISPSBL_SDR_REQ_PRV_EXP_MASK);
+		return;
+	}
+
+	/* Compute the minimum number of cycles per request, based on the
+	 * pipeline maximum data rate. This is an absolute lower bound if we
+	 * don't want SBL overflows, so round the value up.
+	 */
+	cycles_per_request = div_u64((u64)l3_ick / 2 * 256 + pipe->max_rate - 1,
+				     pipe->max_rate);
+	minimum = DIV_ROUND_UP(cycles_per_request, 32);
+
+	/* Compute the maximum number of cycles per request, based on the
+	 * requested frame rate. This is a soft upper bound to achieve a frame
+	 * rate equal or higher than the requested value, so round the value
+	 * down.
+	 */
+	timeperframe = &pipe->max_timeperframe;
+
+	requests_per_frame = DIV_ROUND_UP(ifmt->width * 2, 256) * ifmt->height;
+	cycles_per_frame = div_u64((u64)l3_ick * timeperframe->numerator,
+				   timeperframe->denominator);
+	cycles_per_request = cycles_per_frame / requests_per_frame;
+
+	maximum = cycles_per_request / 32;
+
+	value = max(minimum, maximum);
+
+	dev_dbg(isp->dev, "%s: cycles per request = %u\n", __func__, value);
+	isp_reg_clr_set(isp, OMAP3_ISP_IOMEM_SBL, ISPSBL_SDR_REQ_EXP,
+			ISPSBL_SDR_REQ_PRV_EXP_MASK,
+			value << ISPSBL_SDR_REQ_PRV_EXP_SHIFT);
+}
+
+/*
+ * omap3isp_preview_busy - Gets busy state of preview module.
+ */
+int omap3isp_preview_busy(struct isp_prev_device *prev)
+{
+	struct isp_device *isp = to_isp_device(prev);
+
+	return isp_reg_readl(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR)
+		& ISPPRV_PCR_BUSY;
+}
+
+/*
+ * omap3isp_preview_restore_context - Restores the values of preview registers
+ */
+void omap3isp_preview_restore_context(struct isp_device *isp)
+{
+	struct isp_prev_device *prev = &isp->isp_prev;
+	const u32 update = OMAP3ISP_PREV_FEATURES_END - 1;
+
+	prev->params.params[0].update = prev->params.active & update;
+	prev->params.params[1].update = ~prev->params.active & update;
+
+	preview_setup_hw(prev, update, prev->params.active);
+
+	prev->params.params[0].update = 0;
+	prev->params.params[1].update = 0;
+}
+
+/*
+ * preview_print_status - Dump preview module registers to the kernel log
+ */
+#define PREV_PRINT_REGISTER(isp, name)\
+	dev_dbg(isp->dev, "###PRV " #name "=0x%08x\n", \
+		isp_reg_readl(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_##name))
+
+static void preview_print_status(struct isp_prev_device *prev)
+{
+	struct isp_device *isp = to_isp_device(prev);
+
+	dev_dbg(isp->dev, "-------------Preview Register dump----------\n");
+
+	PREV_PRINT_REGISTER(isp, PCR);
+	PREV_PRINT_REGISTER(isp, HORZ_INFO);
+	PREV_PRINT_REGISTER(isp, VERT_INFO);
+	PREV_PRINT_REGISTER(isp, RSDR_ADDR);
+	PREV_PRINT_REGISTER(isp, RADR_OFFSET);
+	PREV_PRINT_REGISTER(isp, DSDR_ADDR);
+	PREV_PRINT_REGISTER(isp, DRKF_OFFSET);
+	PREV_PRINT_REGISTER(isp, WSDR_ADDR);
+	PREV_PRINT_REGISTER(isp, WADD_OFFSET);
+	PREV_PRINT_REGISTER(isp, AVE);
+	PREV_PRINT_REGISTER(isp, HMED);
+	PREV_PRINT_REGISTER(isp, NF);
+	PREV_PRINT_REGISTER(isp, WB_DGAIN);
+	PREV_PRINT_REGISTER(isp, WBGAIN);
+	PREV_PRINT_REGISTER(isp, WBSEL);
+	PREV_PRINT_REGISTER(isp, CFA);
+	PREV_PRINT_REGISTER(isp, BLKADJOFF);
+	PREV_PRINT_REGISTER(isp, RGB_MAT1);
+	PREV_PRINT_REGISTER(isp, RGB_MAT2);
+	PREV_PRINT_REGISTER(isp, RGB_MAT3);
+	PREV_PRINT_REGISTER(isp, RGB_MAT4);
+	PREV_PRINT_REGISTER(isp, RGB_MAT5);
+	PREV_PRINT_REGISTER(isp, RGB_OFF1);
+	PREV_PRINT_REGISTER(isp, RGB_OFF2);
+	PREV_PRINT_REGISTER(isp, CSC0);
+	PREV_PRINT_REGISTER(isp, CSC1);
+	PREV_PRINT_REGISTER(isp, CSC2);
+	PREV_PRINT_REGISTER(isp, CSC_OFFSET);
+	PREV_PRINT_REGISTER(isp, CNT_BRT);
+	PREV_PRINT_REGISTER(isp, CSUP);
+	PREV_PRINT_REGISTER(isp, SETUP_YC);
+	PREV_PRINT_REGISTER(isp, SET_TBL_ADDR);
+	PREV_PRINT_REGISTER(isp, CDC_THR0);
+	PREV_PRINT_REGISTER(isp, CDC_THR1);
+	PREV_PRINT_REGISTER(isp, CDC_THR2);
+	PREV_PRINT_REGISTER(isp, CDC_THR3);
+
+	dev_dbg(isp->dev, "--------------------------------------------\n");
+}
+
+/*
+ * preview_init_params - init image processing parameters.
+ * @prev: pointer to previewer private structure
+ */
+static void preview_init_params(struct isp_prev_device *prev)
+{
+	struct prev_params *params;
+	unsigned int i;
+
+	spin_lock_init(&prev->params.lock);
+
+	prev->params.active = ~0;
+	prev->params.params[0].busy = 0;
+	prev->params.params[0].update = OMAP3ISP_PREV_FEATURES_END - 1;
+	prev->params.params[1].busy = 0;
+	prev->params.params[1].update = 0;
+
+	params = &prev->params.params[0];
+
+	/* Init values */
+	params->contrast = ISPPRV_CONTRAST_DEF * ISPPRV_CONTRAST_UNITS;
+	params->brightness = ISPPRV_BRIGHT_DEF * ISPPRV_BRIGHT_UNITS;
+	params->cfa.format = OMAP3ISP_CFAFMT_BAYER;
+	memcpy(params->cfa.table, cfa_coef_table,
+	       sizeof(params->cfa.table));
+	params->cfa.gradthrs_horz = FLR_CFA_GRADTHRS_HORZ;
+	params->cfa.gradthrs_vert = FLR_CFA_GRADTHRS_VERT;
+	params->csup.gain = FLR_CSUP_GAIN;
+	params->csup.thres = FLR_CSUP_THRES;
+	params->csup.hypf_en = 0;
+	memcpy(params->luma.table, luma_enhance_table,
+	       sizeof(params->luma.table));
+	params->nf.spread = FLR_NF_STRGTH;
+	memcpy(params->nf.table, noise_filter_table, sizeof(params->nf.table));
+	params->dcor.couplet_mode_en = 1;
+	for (i = 0; i < OMAP3ISP_PREV_DETECT_CORRECT_CHANNELS; i++)
+		params->dcor.detect_correct[i] = DEF_DETECT_CORRECT_VAL;
+	memcpy(params->gamma.blue, gamma_table, sizeof(params->gamma.blue));
+	memcpy(params->gamma.green, gamma_table, sizeof(params->gamma.green));
+	memcpy(params->gamma.red, gamma_table, sizeof(params->gamma.red));
+	params->wbal.dgain = FLR_WBAL_DGAIN;
+	params->wbal.coef0 = FLR_WBAL_COEF;
+	params->wbal.coef1 = FLR_WBAL_COEF;
+	params->wbal.coef2 = FLR_WBAL_COEF;
+	params->wbal.coef3 = FLR_WBAL_COEF;
+	params->blkadj.red = FLR_BLKADJ_RED;
+	params->blkadj.green = FLR_BLKADJ_GREEN;
+	params->blkadj.blue = FLR_BLKADJ_BLUE;
+	params->rgb2rgb = flr_rgb2rgb;
+	params->csc = flr_prev_csc;
+	params->yclimit.minC = ISPPRV_YC_MIN;
+	params->yclimit.maxC = ISPPRV_YC_MAX;
+	params->yclimit.minY = ISPPRV_YC_MIN;
+	params->yclimit.maxY = ISPPRV_YC_MAX;
+
+	params->features = OMAP3ISP_PREV_CFA | OMAP3ISP_PREV_DEFECT_COR
+			 | OMAP3ISP_PREV_NF | OMAP3ISP_PREV_GAMMA
+			 | OMAP3ISP_PREV_BLKADJ | OMAP3ISP_PREV_YC_LIMIT
+			 | OMAP3ISP_PREV_RGB2RGB | OMAP3ISP_PREV_COLOR_CONV
+			 | OMAP3ISP_PREV_WB | OMAP3ISP_PREV_BRIGHTNESS
+			 | OMAP3ISP_PREV_CONTRAST;
+}
+
+/*
+ * preview_max_out_width - Handle previewer hardware output limitations
+ * @prev: pointer to previewer private structure
+ * returns maximum width output for current isp revision
+ */
+static unsigned int preview_max_out_width(struct isp_prev_device *prev)
+{
+	struct isp_device *isp = to_isp_device(prev);
+
+	switch (isp->revision) {
+	case ISP_REVISION_1_0:
+		return PREV_MAX_OUT_WIDTH_REV_1;
+
+	case ISP_REVISION_2_0:
+	default:
+		return PREV_MAX_OUT_WIDTH_REV_2;
+
+	case ISP_REVISION_15_0:
+		return PREV_MAX_OUT_WIDTH_REV_15;
+	}
+}
+
+static void preview_configure(struct isp_prev_device *prev)
+{
+	struct isp_device *isp = to_isp_device(prev);
+	const struct isp_format_info *info;
+	struct v4l2_mbus_framefmt *format;
+	unsigned long flags;
+	u32 update;
+	u32 active;
+
+	spin_lock_irqsave(&prev->params.lock, flags);
+	/* Mark all active parameters we are going to touch as busy. */
+	update = preview_params_lock(prev, 0, false);
+	active = prev->params.active;
+	spin_unlock_irqrestore(&prev->params.lock, flags);
+
+	/* PREV_PAD_SINK */
+	format = &prev->formats[PREV_PAD_SINK];
+	info = omap3isp_video_format_info(format->code);
+
+	preview_adjust_bandwidth(prev);
+
+	preview_config_input_format(prev, info);
+	preview_config_input_size(prev, active);
+
+	if (prev->input == PREVIEW_INPUT_CCDC)
+		preview_config_inlineoffset(prev, 0);
+	else
+		preview_config_inlineoffset(prev, ALIGN(format->width, 0x20) *
+					    info->bpp);
+
+	preview_setup_hw(prev, update, active);
+
+	/* PREV_PAD_SOURCE */
+	format = &prev->formats[PREV_PAD_SOURCE];
+
+	if (prev->output & PREVIEW_OUTPUT_MEMORY)
+		isp_reg_set(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
+			    ISPPRV_PCR_SDRPORT);
+	else
+		isp_reg_clr(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
+			    ISPPRV_PCR_SDRPORT);
+
+	if (prev->output & PREVIEW_OUTPUT_RESIZER)
+		isp_reg_set(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
+			    ISPPRV_PCR_RSZPORT);
+	else
+		isp_reg_clr(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
+			    ISPPRV_PCR_RSZPORT);
+
+	if (prev->output & PREVIEW_OUTPUT_MEMORY)
+		preview_config_outlineoffset(prev,
+				ALIGN(format->width, 0x10) * 2);
+
+	preview_config_averager(prev, 0);
+	preview_config_ycpos(prev, format->code);
+
+	spin_lock_irqsave(&prev->params.lock, flags);
+	preview_params_unlock(prev, update, false);
+	spin_unlock_irqrestore(&prev->params.lock, flags);
+}
+
+/* -----------------------------------------------------------------------------
+ * Interrupt handling
+ */
+
+static void preview_enable_oneshot(struct isp_prev_device *prev)
+{
+	struct isp_device *isp = to_isp_device(prev);
+
+	/* The PCR.SOURCE bit is automatically reset to 0 when the PCR.ENABLE
+	 * bit is set. As the preview engine is used in single-shot mode, we
+	 * need to set PCR.SOURCE before enabling the preview engine.
+	 */
+	if (prev->input == PREVIEW_INPUT_MEMORY)
+		isp_reg_set(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
+			    ISPPRV_PCR_SOURCE);
+
+	isp_reg_set(isp, OMAP3_ISP_IOMEM_PREV, ISPPRV_PCR,
+		    ISPPRV_PCR_EN | ISPPRV_PCR_ONESHOT);
+}
+
+void omap3isp_preview_isr_frame_sync(struct isp_prev_device *prev)
+{
+	/*
+	 * If ISP_VIDEO_DMAQUEUE_QUEUED is set, DMA queue had an underrun
+	 * condition, the module was paused and now we have a buffer queued
+	 * on the output again. Restart the pipeline if running in continuous
+	 * mode.
+	 */
+	if (prev->state == ISP_PIPELINE_STREAM_CONTINUOUS &&
+	    prev->video_out.dmaqueue_flags & ISP_VIDEO_DMAQUEUE_QUEUED) {
+		preview_enable_oneshot(prev);
+		isp_video_dmaqueue_flags_clr(&prev->video_out);
+	}
+}
+
+static void preview_isr_buffer(struct isp_prev_device *prev)
+{
+	struct isp_pipeline *pipe = to_isp_pipeline(&prev->subdev.entity);
+	struct isp_buffer *buffer;
+	int restart = 0;
+
+	if (prev->input == PREVIEW_INPUT_MEMORY) {
+		buffer = omap3isp_video_buffer_next(&prev->video_in);
+		if (buffer != NULL)
+			preview_set_inaddr(prev, buffer->dma);
+		pipe->state |= ISP_PIPELINE_IDLE_INPUT;
+	}
+
+	if (prev->output & PREVIEW_OUTPUT_MEMORY) {
+		buffer = omap3isp_video_buffer_next(&prev->video_out);
+		if (buffer != NULL) {
+			preview_set_outaddr(prev, buffer->dma);
+			restart = 1;
+		}
+		pipe->state |= ISP_PIPELINE_IDLE_OUTPUT;
+	}
+
+	switch (prev->state) {
+	case ISP_PIPELINE_STREAM_SINGLESHOT:
+		if (isp_pipeline_ready(pipe))
+			omap3isp_pipeline_set_stream(pipe,
+						ISP_PIPELINE_STREAM_SINGLESHOT);
+		break;
+
+	case ISP_PIPELINE_STREAM_CONTINUOUS:
+		/* If an underrun occurs, the video queue operation handler will
+		 * restart the preview engine. Otherwise restart it immediately.
+		 */
+		if (restart)
+			preview_enable_oneshot(prev);
+		break;
+
+	case ISP_PIPELINE_STREAM_STOPPED:
+	default:
+		return;
+	}
+}
+
+/*
+ * omap3isp_preview_isr - ISP preview engine interrupt handler
+ *
+ * Manage the preview engine video buffers and configure shadowed registers.
+ */
+void omap3isp_preview_isr(struct isp_prev_device *prev)
+{
+	unsigned long flags;
+	u32 update;
+	u32 active;
+
+	if (omap3isp_module_sync_is_stopping(&prev->wait, &prev->stopping))
+		return;
+
+	spin_lock_irqsave(&prev->params.lock, flags);
+	preview_params_switch(prev);
+	update = preview_params_lock(prev, 0, false);
+	active = prev->params.active;
+	spin_unlock_irqrestore(&prev->params.lock, flags);
+
+	preview_setup_hw(prev, update, active);
+	preview_config_input_size(prev, active);
+
+	if (prev->input == PREVIEW_INPUT_MEMORY ||
+	    prev->output & PREVIEW_OUTPUT_MEMORY)
+		preview_isr_buffer(prev);
+	else if (prev->state == ISP_PIPELINE_STREAM_CONTINUOUS)
+		preview_enable_oneshot(prev);
+
+	spin_lock_irqsave(&prev->params.lock, flags);
+	preview_params_unlock(prev, update, false);
+	spin_unlock_irqrestore(&prev->params.lock, flags);
+}
+
+/* -----------------------------------------------------------------------------
+ * ISP video operations
+ */
+
+static int preview_video_queue(struct isp_video *video,
+			       struct isp_buffer *buffer)
+{
+	struct isp_prev_device *prev = &video->isp->isp_prev;
+
+	if (video->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		preview_set_inaddr(prev, buffer->dma);
+
+	if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		preview_set_outaddr(prev, buffer->dma);
+
+	return 0;
+}
+
+static const struct isp_video_operations preview_video_ops = {
+	.queue = preview_video_queue,
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev operations
+ */
+
+/*
+ * preview_s_ctrl - Handle set control subdev method
+ * @ctrl: pointer to v4l2 control structure
+ */
+static int preview_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct isp_prev_device *prev =
+		container_of(ctrl->handler, struct isp_prev_device, ctrls);
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		preview_update_brightness(prev, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		preview_update_contrast(prev, ctrl->val);
+		break;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops preview_ctrl_ops = {
+	.s_ctrl = preview_s_ctrl,
+};
+
+/*
+ * preview_ioctl - Handle preview module private ioctl's
+ * @sd: pointer to v4l2 subdev structure
+ * @cmd: configuration command
+ * @arg: configuration argument
+ * return -EINVAL or zero on success
+ */
+static long preview_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
+{
+	struct isp_prev_device *prev = v4l2_get_subdevdata(sd);
+
+	switch (cmd) {
+	case VIDIOC_OMAP3ISP_PRV_CFG:
+		return preview_config(prev, arg);
+
+	default:
+		return -ENOIOCTLCMD;
+	}
+}
+
+/*
+ * preview_set_stream - Enable/Disable streaming on preview subdev
+ * @sd    : pointer to v4l2 subdev structure
+ * @enable: 1 == Enable, 0 == Disable
+ * return -EINVAL or zero on success
+ */
+static int preview_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct isp_prev_device *prev = v4l2_get_subdevdata(sd);
+	struct isp_video *video_out = &prev->video_out;
+	struct isp_device *isp = to_isp_device(prev);
+	struct device *dev = to_device(prev);
+
+	if (prev->state == ISP_PIPELINE_STREAM_STOPPED) {
+		if (enable == ISP_PIPELINE_STREAM_STOPPED)
+			return 0;
+
+		omap3isp_subclk_enable(isp, OMAP3_ISP_SUBCLK_PREVIEW);
+		preview_configure(prev);
+		atomic_set(&prev->stopping, 0);
+		preview_print_status(prev);
+	}
+
+	switch (enable) {
+	case ISP_PIPELINE_STREAM_CONTINUOUS:
+		if (prev->output & PREVIEW_OUTPUT_MEMORY)
+			omap3isp_sbl_enable(isp, OMAP3_ISP_SBL_PREVIEW_WRITE);
+
+		if (video_out->dmaqueue_flags & ISP_VIDEO_DMAQUEUE_QUEUED ||
+		    !(prev->output & PREVIEW_OUTPUT_MEMORY))
+			preview_enable_oneshot(prev);
+
+		isp_video_dmaqueue_flags_clr(video_out);
+		break;
+
+	case ISP_PIPELINE_STREAM_SINGLESHOT:
+		if (prev->input == PREVIEW_INPUT_MEMORY)
+			omap3isp_sbl_enable(isp, OMAP3_ISP_SBL_PREVIEW_READ);
+		if (prev->output & PREVIEW_OUTPUT_MEMORY)
+			omap3isp_sbl_enable(isp, OMAP3_ISP_SBL_PREVIEW_WRITE);
+
+		preview_enable_oneshot(prev);
+		break;
+
+	case ISP_PIPELINE_STREAM_STOPPED:
+		if (omap3isp_module_sync_idle(&sd->entity, &prev->wait,
+					      &prev->stopping))
+			dev_dbg(dev, "%s: stop timeout.\n", sd->name);
+		omap3isp_sbl_disable(isp, OMAP3_ISP_SBL_PREVIEW_READ);
+		omap3isp_sbl_disable(isp, OMAP3_ISP_SBL_PREVIEW_WRITE);
+		omap3isp_subclk_disable(isp, OMAP3_ISP_SUBCLK_PREVIEW);
+		isp_video_dmaqueue_flags_clr(video_out);
+		break;
+	}
+
+	prev->state = enable;
+	return 0;
+}
+
+static struct v4l2_mbus_framefmt *
+__preview_get_format(struct isp_prev_device *prev, struct v4l2_subdev_pad_config *cfg,
+		     unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(&prev->subdev, cfg, pad);
+	else
+		return &prev->formats[pad];
+}
+
+static struct v4l2_rect *
+__preview_get_crop(struct isp_prev_device *prev, struct v4l2_subdev_pad_config *cfg,
+		   enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_crop(&prev->subdev, cfg, PREV_PAD_SINK);
+	else
+		return &prev->crop;
+}
+
+/* previewer format descriptions */
+static const unsigned int preview_input_fmts[] = {
+	MEDIA_BUS_FMT_Y8_1X8,
+	MEDIA_BUS_FMT_SGRBG8_1X8,
+	MEDIA_BUS_FMT_SRGGB8_1X8,
+	MEDIA_BUS_FMT_SBGGR8_1X8,
+	MEDIA_BUS_FMT_SGBRG8_1X8,
+	MEDIA_BUS_FMT_Y10_1X10,
+	MEDIA_BUS_FMT_SGRBG10_1X10,
+	MEDIA_BUS_FMT_SRGGB10_1X10,
+	MEDIA_BUS_FMT_SBGGR10_1X10,
+	MEDIA_BUS_FMT_SGBRG10_1X10,
+};
+
+static const unsigned int preview_output_fmts[] = {
+	MEDIA_BUS_FMT_UYVY8_1X16,
+	MEDIA_BUS_FMT_YUYV8_1X16,
+};
+
+/*
+ * preview_try_format - Validate a format
+ * @prev: ISP preview engine
+ * @cfg: V4L2 subdev pad configuration
+ * @pad: pad number
+ * @fmt: format to be validated
+ * @which: try/active format selector
+ *
+ * Validate and adjust the given format for the given pad based on the preview
+ * engine limits and the format and crop rectangles on other pads.
+ */
+static void preview_try_format(struct isp_prev_device *prev,
+			       struct v4l2_subdev_pad_config *cfg, unsigned int pad,
+			       struct v4l2_mbus_framefmt *fmt,
+			       enum v4l2_subdev_format_whence which)
+{
+	u32 pixelcode;
+	struct v4l2_rect *crop;
+	unsigned int i;
+
+	switch (pad) {
+	case PREV_PAD_SINK:
+		/* When reading data from the CCDC, the input size has already
+		 * been mangled by the CCDC output pad so it can be accepted
+		 * as-is.
+		 *
+		 * When reading data from memory, clamp the requested width and
+		 * height. The TRM doesn't specify a minimum input height, make
+		 * sure we got enough lines to enable the noise filter and color
+		 * filter array interpolation.
+		 */
+		if (prev->input == PREVIEW_INPUT_MEMORY) {
+			fmt->width = clamp_t(u32, fmt->width, PREV_MIN_IN_WIDTH,
+					     preview_max_out_width(prev));
+			fmt->height = clamp_t(u32, fmt->height,
+					      PREV_MIN_IN_HEIGHT,
+					      PREV_MAX_IN_HEIGHT);
+		}
+
+		fmt->colorspace = V4L2_COLORSPACE_SRGB;
+
+		for (i = 0; i < ARRAY_SIZE(preview_input_fmts); i++) {
+			if (fmt->code == preview_input_fmts[i])
+				break;
+		}
+
+		/* If not found, use SGRBG10 as default */
+		if (i >= ARRAY_SIZE(preview_input_fmts))
+			fmt->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+		break;
+
+	case PREV_PAD_SOURCE:
+		pixelcode = fmt->code;
+		*fmt = *__preview_get_format(prev, cfg, PREV_PAD_SINK, which);
+
+		switch (pixelcode) {
+		case MEDIA_BUS_FMT_YUYV8_1X16:
+		case MEDIA_BUS_FMT_UYVY8_1X16:
+			fmt->code = pixelcode;
+			break;
+
+		default:
+			fmt->code = MEDIA_BUS_FMT_YUYV8_1X16;
+			break;
+		}
+
+		/* The preview module output size is configurable through the
+		 * averager (horizontal scaling by 1/1, 1/2, 1/4 or 1/8). This
+		 * is not supported yet, hardcode the output size to the crop
+		 * rectangle size.
+		 */
+		crop = __preview_get_crop(prev, cfg, which);
+		fmt->width = crop->width;
+		fmt->height = crop->height;
+
+		fmt->colorspace = V4L2_COLORSPACE_JPEG;
+		break;
+	}
+
+	fmt->field = V4L2_FIELD_NONE;
+}
+
+/*
+ * preview_try_crop - Validate a crop rectangle
+ * @prev: ISP preview engine
+ * @sink: format on the sink pad
+ * @crop: crop rectangle to be validated
+ *
+ * The preview engine crops lines and columns for its internal operation,
+ * depending on which filters are enabled. Enforce minimum crop margins to
+ * handle that transparently for userspace.
+ *
+ * See the explanation at the PREV_MARGIN_* definitions for more details.
+ */
+static void preview_try_crop(struct isp_prev_device *prev,
+			     const struct v4l2_mbus_framefmt *sink,
+			     struct v4l2_rect *crop)
+{
+	unsigned int left = PREV_MARGIN_LEFT;
+	unsigned int right = sink->width - PREV_MARGIN_RIGHT;
+	unsigned int top = PREV_MARGIN_TOP;
+	unsigned int bottom = sink->height - PREV_MARGIN_BOTTOM;
+
+	/* When processing data on-the-fly from the CCDC, at least 2 pixels must
+	 * be cropped from the left and right sides of the image. As we don't
+	 * know which filters will be enabled, increase the left and right
+	 * margins by two.
+	 */
+	if (prev->input == PREVIEW_INPUT_CCDC) {
+		left += 2;
+		right -= 2;
+	}
+
+	/* The CFA filter crops 4 lines and 4 columns in Bayer mode, and 2 lines
+	 * and no columns in other modes. Increase the margins based on the sink
+	 * format.
+	 */
+	if (sink->code != MEDIA_BUS_FMT_Y8_1X8 &&
+	    sink->code != MEDIA_BUS_FMT_Y10_1X10) {
+		left += 2;
+		right -= 2;
+		top += 2;
+		bottom -= 2;
+	}
+
+	/* Restrict left/top to even values to keep the Bayer pattern. */
+	crop->left &= ~1;
+	crop->top &= ~1;
+
+	crop->left = clamp_t(u32, crop->left, left, right - PREV_MIN_OUT_WIDTH);
+	crop->top = clamp_t(u32, crop->top, top, bottom - PREV_MIN_OUT_HEIGHT);
+	crop->width = clamp_t(u32, crop->width, PREV_MIN_OUT_WIDTH,
+			      right - crop->left);
+	crop->height = clamp_t(u32, crop->height, PREV_MIN_OUT_HEIGHT,
+			       bottom - crop->top);
+}
+
+/*
+ * preview_enum_mbus_code - Handle pixel format enumeration
+ * @sd     : pointer to v4l2 subdev structure
+ * @cfg: V4L2 subdev pad configuration
+ * @code   : pointer to v4l2_subdev_mbus_code_enum structure
+ * return -EINVAL or zero on success
+ */
+static int preview_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_pad_config *cfg,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	switch (code->pad) {
+	case PREV_PAD_SINK:
+		if (code->index >= ARRAY_SIZE(preview_input_fmts))
+			return -EINVAL;
+
+		code->code = preview_input_fmts[code->index];
+		break;
+	case PREV_PAD_SOURCE:
+		if (code->index >= ARRAY_SIZE(preview_output_fmts))
+			return -EINVAL;
+
+		code->code = preview_output_fmts[code->index];
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int preview_enum_frame_size(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_pad_config *cfg,
+				   struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct isp_prev_device *prev = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt format;
+
+	if (fse->index != 0)
+		return -EINVAL;
+
+	format.code = fse->code;
+	format.width = 1;
+	format.height = 1;
+	preview_try_format(prev, cfg, fse->pad, &format, fse->which);
+	fse->min_width = format.width;
+	fse->min_height = format.height;
+
+	if (format.code != fse->code)
+		return -EINVAL;
+
+	format.code = fse->code;
+	format.width = -1;
+	format.height = -1;
+	preview_try_format(prev, cfg, fse->pad, &format, fse->which);
+	fse->max_width = format.width;
+	fse->max_height = format.height;
+
+	return 0;
+}
+
+/*
+ * preview_get_selection - Retrieve a selection rectangle on a pad
+ * @sd: ISP preview V4L2 subdevice
+ * @cfg: V4L2 subdev pad configuration
+ * @sel: Selection rectangle
+ *
+ * The only supported rectangles are the crop rectangles on the sink pad.
+ *
+ * Return 0 on success or a negative error code otherwise.
+ */
+static int preview_get_selection(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_pad_config *cfg,
+				 struct v4l2_subdev_selection *sel)
+{
+	struct isp_prev_device *prev = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	if (sel->pad != PREV_PAD_SINK)
+		return -EINVAL;
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = INT_MAX;
+		sel->r.height = INT_MAX;
+
+		format = __preview_get_format(prev, cfg, PREV_PAD_SINK,
+					      sel->which);
+		preview_try_crop(prev, format, &sel->r);
+		break;
+
+	case V4L2_SEL_TGT_CROP:
+		sel->r = *__preview_get_crop(prev, cfg, sel->which);
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/*
+ * preview_set_selection - Set a selection rectangle on a pad
+ * @sd: ISP preview V4L2 subdevice
+ * @cfg: V4L2 subdev pad configuration
+ * @sel: Selection rectangle
+ *
+ * The only supported rectangle is the actual crop rectangle on the sink pad.
+ *
+ * Return 0 on success or a negative error code otherwise.
+ */
+static int preview_set_selection(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_pad_config *cfg,
+				 struct v4l2_subdev_selection *sel)
+{
+	struct isp_prev_device *prev = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	if (sel->target != V4L2_SEL_TGT_CROP ||
+	    sel->pad != PREV_PAD_SINK)
+		return -EINVAL;
+
+	/* The crop rectangle can't be changed while streaming. */
+	if (prev->state != ISP_PIPELINE_STREAM_STOPPED)
+		return -EBUSY;
+
+	/* Modifying the crop rectangle always changes the format on the source
+	 * pad. If the KEEP_CONFIG flag is set, just return the current crop
+	 * rectangle.
+	 */
+	if (sel->flags & V4L2_SEL_FLAG_KEEP_CONFIG) {
+		sel->r = *__preview_get_crop(prev, cfg, sel->which);
+		return 0;
+	}
+
+	format = __preview_get_format(prev, cfg, PREV_PAD_SINK, sel->which);
+	preview_try_crop(prev, format, &sel->r);
+	*__preview_get_crop(prev, cfg, sel->which) = sel->r;
+
+	/* Update the source format. */
+	format = __preview_get_format(prev, cfg, PREV_PAD_SOURCE, sel->which);
+	preview_try_format(prev, cfg, PREV_PAD_SOURCE, format, sel->which);
+
+	return 0;
+}
+
+/*
+ * preview_get_format - Handle get format by pads subdev method
+ * @sd : pointer to v4l2 subdev structure
+ * @cfg: V4L2 subdev pad configuration
+ * @fmt: pointer to v4l2 subdev format structure
+ * return -EINVAL or zero on success
+ */
+static int preview_get_format(struct v4l2_subdev *sd, struct v4l2_subdev_pad_config *cfg,
+			      struct v4l2_subdev_format *fmt)
+{
+	struct isp_prev_device *prev = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __preview_get_format(prev, cfg, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
+
+	fmt->format = *format;
+	return 0;
+}
+
+/*
+ * preview_set_format - Handle set format by pads subdev method
+ * @sd : pointer to v4l2 subdev structure
+ * @cfg: V4L2 subdev pad configuration
+ * @fmt: pointer to v4l2 subdev format structure
+ * return -EINVAL or zero on success
+ */
+static int preview_set_format(struct v4l2_subdev *sd, struct v4l2_subdev_pad_config *cfg,
+			      struct v4l2_subdev_format *fmt)
+{
+	struct isp_prev_device *prev = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+	struct v4l2_rect *crop;
+
+	format = __preview_get_format(prev, cfg, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
+
+	preview_try_format(prev, cfg, fmt->pad, &fmt->format, fmt->which);
+	*format = fmt->format;
+
+	/* Propagate the format from sink to source */
+	if (fmt->pad == PREV_PAD_SINK) {
+		/* Reset the crop rectangle. */
+		crop = __preview_get_crop(prev, cfg, fmt->which);
+		crop->left = 0;
+		crop->top = 0;
+		crop->width = fmt->format.width;
+		crop->height = fmt->format.height;
+
+		preview_try_crop(prev, &fmt->format, crop);
+
+		/* Update the source format. */
+		format = __preview_get_format(prev, cfg, PREV_PAD_SOURCE,
+					      fmt->which);
+		preview_try_format(prev, cfg, PREV_PAD_SOURCE, format,
+				   fmt->which);
+	}
+
+	return 0;
+}
+
+/*
+ * preview_init_formats - Initialize formats on all pads
+ * @sd: ISP preview V4L2 subdevice
+ * @fh: V4L2 subdev file handle
+ *
+ * Initialize all pad formats with default values. If fh is not NULL, try
+ * formats are initialized on the file handle. Otherwise active formats are
+ * initialized on the device.
+ */
+static int preview_init_formats(struct v4l2_subdev *sd,
+				struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_subdev_format format;
+
+	memset(&format, 0, sizeof(format));
+	format.pad = PREV_PAD_SINK;
+	format.which = fh ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE;
+	format.format.code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	format.format.width = 4096;
+	format.format.height = 4096;
+	preview_set_format(sd, fh ? fh->pad : NULL, &format);
+
+	return 0;
+}
+
+/* subdev core operations */
+static const struct v4l2_subdev_core_ops preview_v4l2_core_ops = {
+	.ioctl = preview_ioctl,
+};
+
+/* subdev video operations */
+static const struct v4l2_subdev_video_ops preview_v4l2_video_ops = {
+	.s_stream = preview_set_stream,
+};
+
+/* subdev pad operations */
+static const struct v4l2_subdev_pad_ops preview_v4l2_pad_ops = {
+	.enum_mbus_code = preview_enum_mbus_code,
+	.enum_frame_size = preview_enum_frame_size,
+	.get_fmt = preview_get_format,
+	.set_fmt = preview_set_format,
+	.get_selection = preview_get_selection,
+	.set_selection = preview_set_selection,
+};
+
+/* subdev operations */
+static const struct v4l2_subdev_ops preview_v4l2_ops = {
+	.core = &preview_v4l2_core_ops,
+	.video = &preview_v4l2_video_ops,
+	.pad = &preview_v4l2_pad_ops,
+};
+
+/* subdev internal operations */
+static const struct v4l2_subdev_internal_ops preview_v4l2_internal_ops = {
+	.open = preview_init_formats,
+};
+
+/* -----------------------------------------------------------------------------
+ * Media entity operations
+ */
+
+/*
+ * preview_link_setup - Setup previewer connections.
+ * @entity : Pointer to media entity structure
+ * @local  : Pointer to local pad array
+ * @remote : Pointer to remote pad array
+ * @flags  : Link flags
+ * return -EINVAL or zero on success
+ */
+static int preview_link_setup(struct media_entity *entity,
+			      const struct media_pad *local,
+			      const struct media_pad *remote, u32 flags)
+{
+	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+	struct isp_prev_device *prev = v4l2_get_subdevdata(sd);
+
+	switch (local->index | media_entity_type(remote->entity)) {
+	case PREV_PAD_SINK | MEDIA_ENT_T_DEVNODE:
+		/* read from memory */
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (prev->input == PREVIEW_INPUT_CCDC)
+				return -EBUSY;
+			prev->input = PREVIEW_INPUT_MEMORY;
+		} else {
+			if (prev->input == PREVIEW_INPUT_MEMORY)
+				prev->input = PREVIEW_INPUT_NONE;
+		}
+		break;
+
+	case PREV_PAD_SINK | MEDIA_ENT_T_V4L2_SUBDEV:
+		/* read from ccdc */
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (prev->input == PREVIEW_INPUT_MEMORY)
+				return -EBUSY;
+			prev->input = PREVIEW_INPUT_CCDC;
+		} else {
+			if (prev->input == PREVIEW_INPUT_CCDC)
+				prev->input = PREVIEW_INPUT_NONE;
+		}
+		break;
+
+	/*
+	 * The ISP core doesn't support pipelines with multiple video outputs.
+	 * Revisit this when it will be implemented, and return -EBUSY for now.
+	 */
+
+	case PREV_PAD_SOURCE | MEDIA_ENT_T_DEVNODE:
+		/* write to memory */
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (prev->output & ~PREVIEW_OUTPUT_MEMORY)
+				return -EBUSY;
+			prev->output |= PREVIEW_OUTPUT_MEMORY;
+		} else {
+			prev->output &= ~PREVIEW_OUTPUT_MEMORY;
+		}
+		break;
+
+	case PREV_PAD_SOURCE | MEDIA_ENT_T_V4L2_SUBDEV:
+		/* write to resizer */
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (prev->output & ~PREVIEW_OUTPUT_RESIZER)
+				return -EBUSY;
+			prev->output |= PREVIEW_OUTPUT_RESIZER;
+		} else {
+			prev->output &= ~PREVIEW_OUTPUT_RESIZER;
+		}
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* media operations */
+static const struct media_entity_operations preview_media_ops = {
+	.link_setup = preview_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+void omap3isp_preview_unregister_entities(struct isp_prev_device *prev)
+{
+	v4l2_device_unregister_subdev(&prev->subdev);
+	omap3isp_video_unregister(&prev->video_in);
+	omap3isp_video_unregister(&prev->video_out);
+}
+
+int omap3isp_preview_register_entities(struct isp_prev_device *prev,
+	struct v4l2_device *vdev)
+{
+	int ret;
+
+	/* Register the subdev and video nodes. */
+	ret = v4l2_device_register_subdev(vdev, &prev->subdev);
+	if (ret < 0)
+		goto error;
+
+	ret = omap3isp_video_register(&prev->video_in, vdev);
+	if (ret < 0)
+		goto error;
+
+	ret = omap3isp_video_register(&prev->video_out, vdev);
+	if (ret < 0)
+		goto error;
+
+	return 0;
+
+error:
+	omap3isp_preview_unregister_entities(prev);
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * ISP previewer initialisation and cleanup
+ */
+
+/*
+ * preview_init_entities - Initialize subdev and media entity.
+ * @prev : Pointer to preview structure
+ * return -ENOMEM or zero on success
+ */
+static int preview_init_entities(struct isp_prev_device *prev)
+{
+	struct v4l2_subdev *sd = &prev->subdev;
+	struct media_pad *pads = prev->pads;
+	struct media_entity *me = &sd->entity;
+	int ret;
+
+	prev->input = PREVIEW_INPUT_NONE;
+
+	v4l2_subdev_init(sd, &preview_v4l2_ops);
+	sd->internal_ops = &preview_v4l2_internal_ops;
+	strlcpy(sd->name, "OMAP3 ISP preview", sizeof(sd->name));
+	sd->grp_id = 1 << 16;	/* group ID for isp subdevs */
+	v4l2_set_subdevdata(sd, prev);
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	v4l2_ctrl_handler_init(&prev->ctrls, 2);
+	v4l2_ctrl_new_std(&prev->ctrls, &preview_ctrl_ops, V4L2_CID_BRIGHTNESS,
+			  ISPPRV_BRIGHT_LOW, ISPPRV_BRIGHT_HIGH,
+			  ISPPRV_BRIGHT_STEP, ISPPRV_BRIGHT_DEF);
+	v4l2_ctrl_new_std(&prev->ctrls, &preview_ctrl_ops, V4L2_CID_CONTRAST,
+			  ISPPRV_CONTRAST_LOW, ISPPRV_CONTRAST_HIGH,
+			  ISPPRV_CONTRAST_STEP, ISPPRV_CONTRAST_DEF);
+	v4l2_ctrl_handler_setup(&prev->ctrls);
+	sd->ctrl_handler = &prev->ctrls;
+
+	pads[PREV_PAD_SINK].flags = MEDIA_PAD_FL_SINK
+				    | MEDIA_PAD_FL_MUST_CONNECT;
+	pads[PREV_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
+
+	me->ops = &preview_media_ops;
+	ret = media_entity_init(me, PREV_PADS_NUM, pads, 0);
+	if (ret < 0)
+		return ret;
+
+	preview_init_formats(sd, NULL);
+
+	/* According to the OMAP34xx TRM, video buffers need to be aligned on a
+	 * 32 bytes boundary. However, an undocumented hardware bug requires a
+	 * 64 bytes boundary at the preview engine input.
+	 */
+	prev->video_in.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	prev->video_in.ops = &preview_video_ops;
+	prev->video_in.isp = to_isp_device(prev);
+	prev->video_in.capture_mem = PAGE_ALIGN(4096 * 4096) * 2 * 3;
+	prev->video_in.bpl_alignment = 64;
+	prev->video_out.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	prev->video_out.ops = &preview_video_ops;
+	prev->video_out.isp = to_isp_device(prev);
+	prev->video_out.capture_mem = PAGE_ALIGN(4096 * 4096) * 2 * 3;
+	prev->video_out.bpl_alignment = 32;
+
+	ret = omap3isp_video_init(&prev->video_in, "preview");
+	if (ret < 0)
+		goto error_video_in;
+
+	ret = omap3isp_video_init(&prev->video_out, "preview");
+	if (ret < 0)
+		goto error_video_out;
+
+	/* Connect the video nodes to the previewer subdev. */
+	ret = media_entity_create_link(&prev->video_in.video.entity, 0,
+			&prev->subdev.entity, PREV_PAD_SINK, 0);
+	if (ret < 0)
+		goto error_link;
+
+	ret = media_entity_create_link(&prev->subdev.entity, PREV_PAD_SOURCE,
+			&prev->video_out.video.entity, 0, 0);
+	if (ret < 0)
+		goto error_link;
+
+	return 0;
+
+error_link:
+	omap3isp_video_cleanup(&prev->video_out);
+error_video_out:
+	omap3isp_video_cleanup(&prev->video_in);
+error_video_in:
+	media_entity_cleanup(&prev->subdev.entity);
+	return ret;
+}
+
+/*
+ * omap3isp_preview_init - Previewer initialization.
+ * @isp : Pointer to ISP device
+ * return -ENOMEM or zero on success
+ */
+int omap3isp_preview_init(struct isp_device *isp)
+{
+	struct isp_prev_device *prev = &isp->isp_prev;
+
+	init_waitqueue_head(&prev->wait);
+
+	preview_init_params(prev);
+
+	return preview_init_entities(prev);
+}
+
+void omap3isp_preview_cleanup(struct isp_device *isp)
+{
+	struct isp_prev_device *prev = &isp->isp_prev;
+
+	v4l2_ctrl_handler_free(&prev->ctrls);
+	omap3isp_video_cleanup(&prev->video_in);
+	omap3isp_video_cleanup(&prev->video_out);
+	media_entity_cleanup(&prev->subdev.entity);
+}
diff --git a/drivers/media/platform/omap3isp/isppreview.h b/drivers/media/platform/omap3isp/isppreview.h
new file mode 100644
index 0000000..16fdc03
--- /dev/null
+++ b/drivers/media/platform/omap3isp/isppreview.h
@@ -0,0 +1,164 @@
+/*
+ * isppreview.h
+ *
+ * TI OMAP3 ISP - Preview module
+ *
+ * Copyright (C) 2010 Nokia Corporation
+ * Copyright (C) 2009 Texas Instruments, Inc.
+ *
+ * Contacts: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *	     Sakari Ailus <sakari.ailus@iki.fi>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef OMAP3_ISP_PREVIEW_H
+#define OMAP3_ISP_PREVIEW_H
+
+#include <linux/omap3isp.h>
+#include <linux/types.h>
+#include <media/v4l2-ctrls.h>
+
+#include "ispvideo.h"
+
+#define ISPPRV_BRIGHT_STEP		0x1
+#define ISPPRV_BRIGHT_DEF		0x0
+#define ISPPRV_BRIGHT_LOW		0x0
+#define ISPPRV_BRIGHT_HIGH		0xFF
+#define ISPPRV_BRIGHT_UNITS		0x1
+
+#define ISPPRV_CONTRAST_STEP		0x1
+#define ISPPRV_CONTRAST_DEF		0x10
+#define ISPPRV_CONTRAST_LOW		0x0
+#define ISPPRV_CONTRAST_HIGH		0xFF
+#define ISPPRV_CONTRAST_UNITS		0x1
+
+/* Additional features not listed in linux/omap3isp.h */
+#define OMAP3ISP_PREV_CONTRAST		(1 << 17)
+#define OMAP3ISP_PREV_BRIGHTNESS	(1 << 18)
+#define OMAP3ISP_PREV_FEATURES_END	(1 << 19)
+
+enum preview_input_entity {
+	PREVIEW_INPUT_NONE,
+	PREVIEW_INPUT_CCDC,
+	PREVIEW_INPUT_MEMORY,
+};
+
+#define PREVIEW_OUTPUT_RESIZER		(1 << 1)
+#define PREVIEW_OUTPUT_MEMORY		(1 << 2)
+
+/* Configure byte layout of YUV image */
+enum preview_ycpos_mode {
+	YCPOS_YCrYCb = 0,
+	YCPOS_YCbYCr = 1,
+	YCPOS_CbYCrY = 2,
+	YCPOS_CrYCbY = 3
+};
+
+/*
+ * struct prev_params - Structure for all configuration
+ * @busy: Bitmask of busy parameters (being updated or used)
+ * @update: Bitmask of the parameters to be updated
+ * @features: Set of features enabled.
+ * @cfa: CFA coefficients.
+ * @csup: Chroma suppression coefficients.
+ * @luma: Luma enhancement coefficients.
+ * @nf: Noise filter coefficients.
+ * @dcor: Noise filter coefficients.
+ * @gamma: Gamma coefficients.
+ * @wbal: White Balance parameters.
+ * @blkadj: Black adjustment parameters.
+ * @rgb2rgb: RGB blending parameters.
+ * @csc: Color space conversion (RGB to YCbCr) parameters.
+ * @hmed: Horizontal median filter.
+ * @yclimit: YC limits parameters.
+ * @contrast: Contrast.
+ * @brightness: Brightness.
+ */
+struct prev_params {
+	u32 busy;
+	u32 update;
+	u32 features;
+	struct omap3isp_prev_cfa cfa;
+	struct omap3isp_prev_csup csup;
+	struct omap3isp_prev_luma luma;
+	struct omap3isp_prev_nf nf;
+	struct omap3isp_prev_dcor dcor;
+	struct omap3isp_prev_gtables gamma;
+	struct omap3isp_prev_wbal wbal;
+	struct omap3isp_prev_blkadj blkadj;
+	struct omap3isp_prev_rgbtorgb rgb2rgb;
+	struct omap3isp_prev_csc csc;
+	struct omap3isp_prev_hmed hmed;
+	struct omap3isp_prev_yclimit yclimit;
+	u8 contrast;
+	u8 brightness;
+};
+
+/* Sink and source previewer pads */
+#define PREV_PAD_SINK			0
+#define PREV_PAD_SOURCE			1
+#define PREV_PADS_NUM			2
+
+/*
+ * struct isp_prev_device - Structure for storing ISP Preview module information
+ * @subdev: V4L2 subdevice
+ * @pads: Media entity pads
+ * @formats: Active formats at the subdev pad
+ * @crop: Active crop rectangle
+ * @input: Module currently connected to the input pad
+ * @output: Bitmask of the active output
+ * @video_in: Input video entity
+ * @video_out: Output video entity
+ * @params.params : Active and shadow parameters sets
+ * @params.active: Bitmask of parameters active in set 0
+ * @params.lock: Parameters lock, protects params.active and params.shadow
+ * @underrun: Whether the preview entity has queued buffers on the output
+ * @state: Current preview pipeline state
+ *
+ * This structure is used to store the OMAP ISP Preview module Information.
+ */
+struct isp_prev_device {
+	struct v4l2_subdev subdev;
+	struct media_pad pads[PREV_PADS_NUM];
+	struct v4l2_mbus_framefmt formats[PREV_PADS_NUM];
+	struct v4l2_rect crop;
+
+	struct v4l2_ctrl_handler ctrls;
+
+	enum preview_input_entity input;
+	unsigned int output;
+	struct isp_video video_in;
+	struct isp_video video_out;
+
+	struct {
+		unsigned int cfa_order;
+		struct prev_params params[2];
+		u32 active;
+		spinlock_t lock;
+	} params;
+
+	enum isp_pipeline_stream_state state;
+	wait_queue_head_t wait;
+	atomic_t stopping;
+};
+
+struct isp_device;
+
+int omap3isp_preview_init(struct isp_device *isp);
+void omap3isp_preview_cleanup(struct isp_device *isp);
+
+int omap3isp_preview_register_entities(struct isp_prev_device *prv,
+				       struct v4l2_device *vdev);
+void omap3isp_preview_unregister_entities(struct isp_prev_device *prv);
+
+void omap3isp_preview_isr_frame_sync(struct isp_prev_device *prev);
+void omap3isp_preview_isr(struct isp_prev_device *prev);
+
+int omap3isp_preview_busy(struct isp_prev_device *isp_prev);
+
+void omap3isp_preview_restore_context(struct isp_device *isp);
+
+#endif	/* OMAP3_ISP_PREVIEW_H */
diff --git a/drivers/media/platform/omap3isp/ispreg.h b/drivers/media/platform/omap3isp/ispreg.h
new file mode 100644
index 0000000..b5ea8da
--- /dev/null
+++ b/drivers/media/platform/omap3isp/ispreg.h
@@ -0,0 +1,1517 @@
+/*
+ * ispreg.h
+ *
+ * TI OMAP3 ISP - Registers definitions
+ *
+ * Copyright (C) 2010 Nokia Corporation
+ * Copyright (C) 2009 Texas Instruments, Inc
+ *
+ * Contacts: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *	     Sakari Ailus <sakari.ailus@iki.fi>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef OMAP3_ISP_REG_H
+#define OMAP3_ISP_REG_H
+
+#define CM_CAM_MCLK_HZ			172800000	/* Hz */
+
+/* ISP module register offset */
+
+#define ISP_REVISION			(0x000)
+#define ISP_SYSCONFIG			(0x004)
+#define ISP_SYSSTATUS			(0x008)
+#define ISP_IRQ0ENABLE			(0x00C)
+#define ISP_IRQ0STATUS			(0x010)
+#define ISP_IRQ1ENABLE			(0x014)
+#define ISP_IRQ1STATUS			(0x018)
+#define ISP_TCTRL_GRESET_LENGTH		(0x030)
+#define ISP_TCTRL_PSTRB_REPLAY		(0x034)
+#define ISP_CTRL			(0x040)
+#define ISP_SECURE			(0x044)
+#define ISP_TCTRL_CTRL			(0x050)
+#define ISP_TCTRL_FRAME			(0x054)
+#define ISP_TCTRL_PSTRB_DELAY		(0x058)
+#define ISP_TCTRL_STRB_DELAY		(0x05C)
+#define ISP_TCTRL_SHUT_DELAY		(0x060)
+#define ISP_TCTRL_PSTRB_LENGTH		(0x064)
+#define ISP_TCTRL_STRB_LENGTH		(0x068)
+#define ISP_TCTRL_SHUT_LENGTH		(0x06C)
+#define ISP_PING_PONG_ADDR		(0x070)
+#define ISP_PING_PONG_MEM_RANGE		(0x074)
+#define ISP_PING_PONG_BUF_SIZE		(0x078)
+
+/* CCP2 receiver registers */
+
+#define ISPCCP2_REVISION		(0x000)
+#define ISPCCP2_SYSCONFIG		(0x004)
+#define ISPCCP2_SYSCONFIG_SOFT_RESET	(1 << 1)
+#define ISPCCP2_SYSCONFIG_AUTO_IDLE		0x1
+#define ISPCCP2_SYSCONFIG_MSTANDBY_MODE_SHIFT	12
+#define ISPCCP2_SYSCONFIG_MSTANDBY_MODE_FORCE	\
+	(0x0 << ISPCCP2_SYSCONFIG_MSTANDBY_MODE_SHIFT)
+#define ISPCCP2_SYSCONFIG_MSTANDBY_MODE_NO	\
+	(0x1 << ISPCCP2_SYSCONFIG_MSTANDBY_MODE_SHIFT)
+#define ISPCCP2_SYSCONFIG_MSTANDBY_MODE_SMART	\
+	(0x2 << ISPCCP2_SYSCONFIG_MSTANDBY_MODE_SHIFT)
+#define ISPCCP2_SYSSTATUS		(0x008)
+#define ISPCCP2_SYSSTATUS_RESET_DONE	(1 << 0)
+#define ISPCCP2_LC01_IRQENABLE		(0x00C)
+#define ISPCCP2_LC01_IRQSTATUS		(0x010)
+#define ISPCCP2_LC01_IRQSTATUS_LC0_FS_IRQ	(1 << 11)
+#define ISPCCP2_LC01_IRQSTATUS_LC0_LE_IRQ	(1 << 10)
+#define ISPCCP2_LC01_IRQSTATUS_LC0_LS_IRQ	(1 << 9)
+#define ISPCCP2_LC01_IRQSTATUS_LC0_FE_IRQ	(1 << 8)
+#define ISPCCP2_LC01_IRQSTATUS_LC0_COUNT_IRQ	(1 << 7)
+#define ISPCCP2_LC01_IRQSTATUS_LC0_FIFO_OVF_IRQ	(1 << 5)
+#define ISPCCP2_LC01_IRQSTATUS_LC0_CRC_IRQ	(1 << 4)
+#define ISPCCP2_LC01_IRQSTATUS_LC0_FSP_IRQ	(1 << 3)
+#define ISPCCP2_LC01_IRQSTATUS_LC0_FW_IRQ	(1 << 2)
+#define ISPCCP2_LC01_IRQSTATUS_LC0_FSC_IRQ	(1 << 1)
+#define ISPCCP2_LC01_IRQSTATUS_LC0_SSC_IRQ	(1 << 0)
+
+#define ISPCCP2_LC23_IRQENABLE		(0x014)
+#define ISPCCP2_LC23_IRQSTATUS		(0x018)
+#define ISPCCP2_LCM_IRQENABLE		(0x02C)
+#define ISPCCP2_LCM_IRQSTATUS_EOF_IRQ		(1 << 0)
+#define ISPCCP2_LCM_IRQSTATUS_OCPERROR_IRQ	(1 << 1)
+#define ISPCCP2_LCM_IRQSTATUS		(0x030)
+#define ISPCCP2_CTRL			(0x040)
+#define ISPCCP2_CTRL_IF_EN		(1 << 0)
+#define ISPCCP2_CTRL_PHY_SEL		(1 << 1)
+#define ISPCCP2_CTRL_PHY_SEL_CLOCK	(0 << 1)
+#define ISPCCP2_CTRL_PHY_SEL_STROBE	(1 << 1)
+#define ISPCCP2_CTRL_PHY_SEL_MASK	0x1
+#define ISPCCP2_CTRL_PHY_SEL_SHIFT	1
+#define ISPCCP2_CTRL_IO_OUT_SEL		(1 << 2)
+#define ISPCCP2_CTRL_MODE		(1 << 4)
+#define ISPCCP2_CTRL_VP_CLK_FORCE_ON	(1 << 9)
+#define ISPCCP2_CTRL_INV		(1 << 10)
+#define ISPCCP2_CTRL_INV_MASK		0x1
+#define ISPCCP2_CTRL_INV_SHIFT		10
+#define ISPCCP2_CTRL_VP_ONLY_EN		(1 << 11)
+#define ISPCCP2_CTRL_VP_CLK_POL		(1 << 12)
+#define ISPCCP2_CTRL_VPCLK_DIV_SHIFT	15
+#define ISPCCP2_CTRL_VPCLK_DIV_MASK	0x1ffff /* [31:15] */
+#define ISPCCP2_CTRL_VP_OUT_CTRL_SHIFT	8 /* 3430 bits */
+#define ISPCCP2_CTRL_VP_OUT_CTRL_MASK	0x3 /* 3430 bits */
+#define ISPCCP2_DBG			(0x044)
+#define ISPCCP2_GNQ			(0x048)
+#define ISPCCP2_LCx_CTRL(x)			((0x050)+0x30*(x))
+#define ISPCCP2_LCx_CTRL_CHAN_EN		(1 << 0)
+#define ISPCCP2_LCx_CTRL_CRC_EN			(1 << 19)
+#define ISPCCP2_LCx_CTRL_CRC_MASK		0x1
+#define ISPCCP2_LCx_CTRL_CRC_SHIFT		2
+#define ISPCCP2_LCx_CTRL_CRC_SHIFT_15_0		19
+#define ISPCCP2_LCx_CTRL_REGION_EN		(1 << 1)
+#define ISPCCP2_LCx_CTRL_REGION_MASK		0x1
+#define ISPCCP2_LCx_CTRL_REGION_SHIFT		1
+#define ISPCCP2_LCx_CTRL_FORMAT_MASK_15_0	0x3f
+#define ISPCCP2_LCx_CTRL_FORMAT_SHIFT_15_0	0x2
+#define ISPCCP2_LCx_CTRL_FORMAT_MASK		0x1f
+#define ISPCCP2_LCx_CTRL_FORMAT_SHIFT		0x3
+#define ISPCCP2_LCx_CODE(x)		((0x054)+0x30*(x))
+#define ISPCCP2_LCx_STAT_START(x)	((0x058)+0x30*(x))
+#define ISPCCP2_LCx_STAT_SIZE(x)	((0x05C)+0x30*(x))
+#define ISPCCP2_LCx_SOF_ADDR(x)		((0x060)+0x30*(x))
+#define ISPCCP2_LCx_EOF_ADDR(x)		((0x064)+0x30*(x))
+#define ISPCCP2_LCx_DAT_START(x)	((0x068)+0x30*(x))
+#define ISPCCP2_LCx_DAT_SIZE(x)		((0x06C)+0x30*(x))
+#define ISPCCP2_LCx_DAT_MASK		0xFFF
+#define ISPCCP2_LCx_DAT_SHIFT		16
+#define ISPCCP2_LCx_DAT_PING_ADDR(x)	((0x070)+0x30*(x))
+#define ISPCCP2_LCx_DAT_PONG_ADDR(x)	((0x074)+0x30*(x))
+#define ISPCCP2_LCx_DAT_OFST(x)		((0x078)+0x30*(x))
+#define ISPCCP2_LCM_CTRL		(0x1D0)
+#define ISPCCP2_LCM_CTRL_CHAN_EN               (1 << 0)
+#define ISPCCP2_LCM_CTRL_DST_PORT              (1 << 2)
+#define ISPCCP2_LCM_CTRL_DST_PORT_SHIFT		2
+#define ISPCCP2_LCM_CTRL_READ_THROTTLE_SHIFT	3
+#define ISPCCP2_LCM_CTRL_READ_THROTTLE_MASK	0x11
+#define ISPCCP2_LCM_CTRL_BURST_SIZE_SHIFT	5
+#define ISPCCP2_LCM_CTRL_BURST_SIZE_MASK	0x7
+#define ISPCCP2_LCM_CTRL_SRC_FORMAT_SHIFT	16
+#define ISPCCP2_LCM_CTRL_SRC_FORMAT_MASK	0x7
+#define ISPCCP2_LCM_CTRL_SRC_DECOMPR_SHIFT	20
+#define ISPCCP2_LCM_CTRL_SRC_DECOMPR_MASK	0x3
+#define ISPCCP2_LCM_CTRL_SRC_DPCM_PRED		(1 << 22)
+#define ISPCCP2_LCM_CTRL_SRC_PACK		(1 << 23)
+#define ISPCCP2_LCM_CTRL_DST_FORMAT_SHIFT	24
+#define ISPCCP2_LCM_CTRL_DST_FORMAT_MASK	0x7
+#define ISPCCP2_LCM_VSIZE		(0x1D4)
+#define ISPCCP2_LCM_VSIZE_SHIFT		16
+#define ISPCCP2_LCM_HSIZE		(0x1D8)
+#define ISPCCP2_LCM_HSIZE_SHIFT		16
+#define ISPCCP2_LCM_PREFETCH		(0x1DC)
+#define ISPCCP2_LCM_PREFETCH_SHIFT	3
+#define ISPCCP2_LCM_SRC_ADDR		(0x1E0)
+#define ISPCCP2_LCM_SRC_OFST		(0x1E4)
+#define ISPCCP2_LCM_DST_ADDR		(0x1E8)
+#define ISPCCP2_LCM_DST_OFST		(0x1EC)
+
+/* CCDC module register offset */
+
+#define ISPCCDC_PID			(0x000)
+#define ISPCCDC_PCR			(0x004)
+#define ISPCCDC_SYN_MODE		(0x008)
+#define ISPCCDC_HD_VD_WID		(0x00C)
+#define ISPCCDC_PIX_LINES		(0x010)
+#define ISPCCDC_HORZ_INFO		(0x014)
+#define ISPCCDC_VERT_START		(0x018)
+#define ISPCCDC_VERT_LINES		(0x01C)
+#define ISPCCDC_CULLING			(0x020)
+#define ISPCCDC_HSIZE_OFF		(0x024)
+#define ISPCCDC_SDOFST			(0x028)
+#define ISPCCDC_SDR_ADDR		(0x02C)
+#define ISPCCDC_CLAMP			(0x030)
+#define ISPCCDC_DCSUB			(0x034)
+#define ISPCCDC_COLPTN			(0x038)
+#define ISPCCDC_BLKCMP			(0x03C)
+#define ISPCCDC_FPC			(0x040)
+#define ISPCCDC_FPC_ADDR		(0x044)
+#define ISPCCDC_VDINT			(0x048)
+#define ISPCCDC_ALAW			(0x04C)
+#define ISPCCDC_REC656IF		(0x050)
+#define ISPCCDC_CFG			(0x054)
+#define ISPCCDC_FMTCFG			(0x058)
+#define ISPCCDC_FMT_HORZ		(0x05C)
+#define ISPCCDC_FMT_VERT		(0x060)
+#define ISPCCDC_FMT_ADDR0		(0x064)
+#define ISPCCDC_FMT_ADDR1		(0x068)
+#define ISPCCDC_FMT_ADDR2		(0x06C)
+#define ISPCCDC_FMT_ADDR3		(0x070)
+#define ISPCCDC_FMT_ADDR4		(0x074)
+#define ISPCCDC_FMT_ADDR5		(0x078)
+#define ISPCCDC_FMT_ADDR6		(0x07C)
+#define ISPCCDC_FMT_ADDR7		(0x080)
+#define ISPCCDC_PRGEVEN0		(0x084)
+#define ISPCCDC_PRGEVEN1		(0x088)
+#define ISPCCDC_PRGODD0			(0x08C)
+#define ISPCCDC_PRGODD1			(0x090)
+#define ISPCCDC_VP_OUT			(0x094)
+
+#define ISPCCDC_LSC_CONFIG		(0x098)
+#define ISPCCDC_LSC_INITIAL		(0x09C)
+#define ISPCCDC_LSC_TABLE_BASE		(0x0A0)
+#define ISPCCDC_LSC_TABLE_OFFSET	(0x0A4)
+
+/* SBL */
+#define ISPSBL_PCR			0x4
+#define ISPSBL_PCR_H3A_AEAWB_WBL_OVF	(1 << 16)
+#define ISPSBL_PCR_H3A_AF_WBL_OVF	(1 << 17)
+#define ISPSBL_PCR_RSZ4_WBL_OVF		(1 << 18)
+#define ISPSBL_PCR_RSZ3_WBL_OVF		(1 << 19)
+#define ISPSBL_PCR_RSZ2_WBL_OVF		(1 << 20)
+#define ISPSBL_PCR_RSZ1_WBL_OVF		(1 << 21)
+#define ISPSBL_PCR_PRV_WBL_OVF		(1 << 22)
+#define ISPSBL_PCR_CCDC_WBL_OVF		(1 << 23)
+#define ISPSBL_PCR_CCDCPRV_2_RSZ_OVF	(1 << 24)
+#define ISPSBL_PCR_CSIA_WBL_OVF		(1 << 25)
+#define ISPSBL_PCR_CSIB_WBL_OVF		(1 << 26)
+#define ISPSBL_CCDC_WR_0		(0x028)
+#define ISPSBL_CCDC_WR_0_DATA_READY	(1 << 21)
+#define ISPSBL_CCDC_WR_1		(0x02C)
+#define ISPSBL_CCDC_WR_2		(0x030)
+#define ISPSBL_CCDC_WR_3		(0x034)
+
+#define ISPSBL_SDR_REQ_EXP		0xF8
+#define ISPSBL_SDR_REQ_HIST_EXP_SHIFT	0
+#define ISPSBL_SDR_REQ_HIST_EXP_MASK	(0x3FF)
+#define ISPSBL_SDR_REQ_RSZ_EXP_SHIFT	10
+#define ISPSBL_SDR_REQ_RSZ_EXP_MASK	(0x3FF << ISPSBL_SDR_REQ_RSZ_EXP_SHIFT)
+#define ISPSBL_SDR_REQ_PRV_EXP_SHIFT	20
+#define ISPSBL_SDR_REQ_PRV_EXP_MASK	(0x3FF << ISPSBL_SDR_REQ_PRV_EXP_SHIFT)
+
+/* Histogram registers */
+#define ISPHIST_PID			(0x000)
+#define ISPHIST_PCR			(0x004)
+#define ISPHIST_CNT			(0x008)
+#define ISPHIST_WB_GAIN			(0x00C)
+#define ISPHIST_R0_HORZ			(0x010)
+#define ISPHIST_R0_VERT			(0x014)
+#define ISPHIST_R1_HORZ			(0x018)
+#define ISPHIST_R1_VERT			(0x01C)
+#define ISPHIST_R2_HORZ			(0x020)
+#define ISPHIST_R2_VERT			(0x024)
+#define ISPHIST_R3_HORZ			(0x028)
+#define ISPHIST_R3_VERT			(0x02C)
+#define ISPHIST_ADDR			(0x030)
+#define ISPHIST_DATA			(0x034)
+#define ISPHIST_RADD			(0x038)
+#define ISPHIST_RADD_OFF		(0x03C)
+#define ISPHIST_H_V_INFO		(0x040)
+
+/* H3A module registers */
+#define ISPH3A_PID			(0x000)
+#define ISPH3A_PCR			(0x004)
+#define ISPH3A_AEWWIN1			(0x04C)
+#define ISPH3A_AEWINSTART		(0x050)
+#define ISPH3A_AEWINBLK			(0x054)
+#define ISPH3A_AEWSUBWIN		(0x058)
+#define ISPH3A_AEWBUFST			(0x05C)
+#define ISPH3A_AFPAX1			(0x008)
+#define ISPH3A_AFPAX2			(0x00C)
+#define ISPH3A_AFPAXSTART		(0x010)
+#define ISPH3A_AFIIRSH			(0x014)
+#define ISPH3A_AFBUFST			(0x018)
+#define ISPH3A_AFCOEF010		(0x01C)
+#define ISPH3A_AFCOEF032		(0x020)
+#define ISPH3A_AFCOEF054		(0x024)
+#define ISPH3A_AFCOEF076		(0x028)
+#define ISPH3A_AFCOEF098		(0x02C)
+#define ISPH3A_AFCOEF0010		(0x030)
+#define ISPH3A_AFCOEF110		(0x034)
+#define ISPH3A_AFCOEF132		(0x038)
+#define ISPH3A_AFCOEF154		(0x03C)
+#define ISPH3A_AFCOEF176		(0x040)
+#define ISPH3A_AFCOEF198		(0x044)
+#define ISPH3A_AFCOEF1010		(0x048)
+
+#define ISPPRV_PCR			(0x004)
+#define ISPPRV_HORZ_INFO		(0x008)
+#define ISPPRV_VERT_INFO		(0x00C)
+#define ISPPRV_RSDR_ADDR		(0x010)
+#define ISPPRV_RADR_OFFSET		(0x014)
+#define ISPPRV_DSDR_ADDR		(0x018)
+#define ISPPRV_DRKF_OFFSET		(0x01C)
+#define ISPPRV_WSDR_ADDR		(0x020)
+#define ISPPRV_WADD_OFFSET		(0x024)
+#define ISPPRV_AVE			(0x028)
+#define ISPPRV_HMED			(0x02C)
+#define ISPPRV_NF			(0x030)
+#define ISPPRV_WB_DGAIN			(0x034)
+#define ISPPRV_WBGAIN			(0x038)
+#define ISPPRV_WBSEL			(0x03C)
+#define ISPPRV_CFA			(0x040)
+#define ISPPRV_BLKADJOFF		(0x044)
+#define ISPPRV_RGB_MAT1			(0x048)
+#define ISPPRV_RGB_MAT2			(0x04C)
+#define ISPPRV_RGB_MAT3			(0x050)
+#define ISPPRV_RGB_MAT4			(0x054)
+#define ISPPRV_RGB_MAT5			(0x058)
+#define ISPPRV_RGB_OFF1			(0x05C)
+#define ISPPRV_RGB_OFF2			(0x060)
+#define ISPPRV_CSC0			(0x064)
+#define ISPPRV_CSC1			(0x068)
+#define ISPPRV_CSC2			(0x06C)
+#define ISPPRV_CSC_OFFSET		(0x070)
+#define ISPPRV_CNT_BRT			(0x074)
+#define ISPPRV_CSUP			(0x078)
+#define ISPPRV_SETUP_YC			(0x07C)
+#define ISPPRV_SET_TBL_ADDR		(0x080)
+#define ISPPRV_SET_TBL_DATA		(0x084)
+#define ISPPRV_CDC_THR0			(0x090)
+#define ISPPRV_CDC_THR1			(ISPPRV_CDC_THR0 + (0x4))
+#define ISPPRV_CDC_THR2			(ISPPRV_CDC_THR0 + (0x4) * 2)
+#define ISPPRV_CDC_THR3			(ISPPRV_CDC_THR0 + (0x4) * 3)
+
+#define ISPPRV_REDGAMMA_TABLE_ADDR	0x0000
+#define ISPPRV_GREENGAMMA_TABLE_ADDR	0x0400
+#define ISPPRV_BLUEGAMMA_TABLE_ADDR	0x0800
+#define ISPPRV_NF_TABLE_ADDR		0x0C00
+#define ISPPRV_YENH_TABLE_ADDR		0x1000
+#define ISPPRV_CFA_TABLE_ADDR		0x1400
+
+#define ISPRSZ_MIN_OUTPUT		64
+#define ISPRSZ_MAX_OUTPUT		3312
+
+/* Resizer module register offset */
+#define ISPRSZ_PID			(0x000)
+#define ISPRSZ_PCR			(0x004)
+#define ISPRSZ_CNT			(0x008)
+#define ISPRSZ_OUT_SIZE			(0x00C)
+#define ISPRSZ_IN_START			(0x010)
+#define ISPRSZ_IN_SIZE			(0x014)
+#define ISPRSZ_SDR_INADD		(0x018)
+#define ISPRSZ_SDR_INOFF		(0x01C)
+#define ISPRSZ_SDR_OUTADD		(0x020)
+#define ISPRSZ_SDR_OUTOFF		(0x024)
+#define ISPRSZ_HFILT10			(0x028)
+#define ISPRSZ_HFILT32			(0x02C)
+#define ISPRSZ_HFILT54			(0x030)
+#define ISPRSZ_HFILT76			(0x034)
+#define ISPRSZ_HFILT98			(0x038)
+#define ISPRSZ_HFILT1110		(0x03C)
+#define ISPRSZ_HFILT1312		(0x040)
+#define ISPRSZ_HFILT1514		(0x044)
+#define ISPRSZ_HFILT1716		(0x048)
+#define ISPRSZ_HFILT1918		(0x04C)
+#define ISPRSZ_HFILT2120		(0x050)
+#define ISPRSZ_HFILT2322		(0x054)
+#define ISPRSZ_HFILT2524		(0x058)
+#define ISPRSZ_HFILT2726		(0x05C)
+#define ISPRSZ_HFILT2928		(0x060)
+#define ISPRSZ_HFILT3130		(0x064)
+#define ISPRSZ_VFILT10			(0x068)
+#define ISPRSZ_VFILT32			(0x06C)
+#define ISPRSZ_VFILT54			(0x070)
+#define ISPRSZ_VFILT76			(0x074)
+#define ISPRSZ_VFILT98			(0x078)
+#define ISPRSZ_VFILT1110		(0x07C)
+#define ISPRSZ_VFILT1312		(0x080)
+#define ISPRSZ_VFILT1514		(0x084)
+#define ISPRSZ_VFILT1716		(0x088)
+#define ISPRSZ_VFILT1918		(0x08C)
+#define ISPRSZ_VFILT2120		(0x090)
+#define ISPRSZ_VFILT2322		(0x094)
+#define ISPRSZ_VFILT2524		(0x098)
+#define ISPRSZ_VFILT2726		(0x09C)
+#define ISPRSZ_VFILT2928		(0x0A0)
+#define ISPRSZ_VFILT3130		(0x0A4)
+#define ISPRSZ_YENH			(0x0A8)
+
+#define ISP_INT_CLR			0xFF113F11
+#define ISPPRV_PCR_EN			1
+#define ISPPRV_PCR_BUSY			(1 << 1)
+#define ISPPRV_PCR_SOURCE		(1 << 2)
+#define ISPPRV_PCR_ONESHOT		(1 << 3)
+#define ISPPRV_PCR_WIDTH		(1 << 4)
+#define ISPPRV_PCR_INVALAW		(1 << 5)
+#define ISPPRV_PCR_DRKFEN		(1 << 6)
+#define ISPPRV_PCR_DRKFCAP		(1 << 7)
+#define ISPPRV_PCR_HMEDEN		(1 << 8)
+#define ISPPRV_PCR_NFEN			(1 << 9)
+#define ISPPRV_PCR_CFAEN		(1 << 10)
+#define ISPPRV_PCR_CFAFMT_SHIFT		11
+#define ISPPRV_PCR_CFAFMT_MASK		0x7800
+#define ISPPRV_PCR_CFAFMT_BAYER		(0 << 11)
+#define ISPPRV_PCR_CFAFMT_SONYVGA	(1 << 11)
+#define ISPPRV_PCR_CFAFMT_RGBFOVEON	(2 << 11)
+#define ISPPRV_PCR_CFAFMT_DNSPL		(3 << 11)
+#define ISPPRV_PCR_CFAFMT_HONEYCOMB	(4 << 11)
+#define ISPPRV_PCR_CFAFMT_RRGGBBFOVEON	(5 << 11)
+#define ISPPRV_PCR_YNENHEN		(1 << 15)
+#define ISPPRV_PCR_SUPEN		(1 << 16)
+#define ISPPRV_PCR_YCPOS_SHIFT		17
+#define ISPPRV_PCR_YCPOS_YCrYCb		(0 << 17)
+#define ISPPRV_PCR_YCPOS_YCbYCr		(1 << 17)
+#define ISPPRV_PCR_YCPOS_CbYCrY		(2 << 17)
+#define ISPPRV_PCR_YCPOS_CrYCbY		(3 << 17)
+#define ISPPRV_PCR_RSZPORT		(1 << 19)
+#define ISPPRV_PCR_SDRPORT		(1 << 20)
+#define ISPPRV_PCR_SCOMP_EN		(1 << 21)
+#define ISPPRV_PCR_SCOMP_SFT_SHIFT	(22)
+#define ISPPRV_PCR_SCOMP_SFT_MASK	(7 << 22)
+#define ISPPRV_PCR_GAMMA_BYPASS		(1 << 26)
+#define ISPPRV_PCR_DCOREN		(1 << 27)
+#define ISPPRV_PCR_DCCOUP		(1 << 28)
+#define ISPPRV_PCR_DRK_FAIL		(1 << 31)
+
+#define ISPPRV_HORZ_INFO_EPH_SHIFT	0
+#define ISPPRV_HORZ_INFO_EPH_MASK	0x3fff
+#define ISPPRV_HORZ_INFO_SPH_SHIFT	16
+#define ISPPRV_HORZ_INFO_SPH_MASK	0x3fff0
+
+#define ISPPRV_VERT_INFO_ELV_SHIFT	0
+#define ISPPRV_VERT_INFO_ELV_MASK	0x3fff
+#define ISPPRV_VERT_INFO_SLV_SHIFT	16
+#define ISPPRV_VERT_INFO_SLV_MASK	0x3fff0
+
+#define ISPPRV_AVE_EVENDIST_SHIFT	2
+#define ISPPRV_AVE_EVENDIST_1		0x0
+#define ISPPRV_AVE_EVENDIST_2		0x1
+#define ISPPRV_AVE_EVENDIST_3		0x2
+#define ISPPRV_AVE_EVENDIST_4		0x3
+#define ISPPRV_AVE_ODDDIST_SHIFT	4
+#define ISPPRV_AVE_ODDDIST_1		0x0
+#define ISPPRV_AVE_ODDDIST_2		0x1
+#define ISPPRV_AVE_ODDDIST_3		0x2
+#define ISPPRV_AVE_ODDDIST_4		0x3
+
+#define ISPPRV_HMED_THRESHOLD_SHIFT	0
+#define ISPPRV_HMED_EVENDIST		(1 << 8)
+#define ISPPRV_HMED_ODDDIST		(1 << 9)
+
+#define ISPPRV_WBGAIN_COEF0_SHIFT	0
+#define ISPPRV_WBGAIN_COEF1_SHIFT	8
+#define ISPPRV_WBGAIN_COEF2_SHIFT	16
+#define ISPPRV_WBGAIN_COEF3_SHIFT	24
+
+#define ISPPRV_WBSEL_COEF0		0x0
+#define ISPPRV_WBSEL_COEF1		0x1
+#define ISPPRV_WBSEL_COEF2		0x2
+#define ISPPRV_WBSEL_COEF3		0x3
+
+#define ISPPRV_WBSEL_N0_0_SHIFT		0
+#define ISPPRV_WBSEL_N0_1_SHIFT		2
+#define ISPPRV_WBSEL_N0_2_SHIFT		4
+#define ISPPRV_WBSEL_N0_3_SHIFT		6
+#define ISPPRV_WBSEL_N1_0_SHIFT		8
+#define ISPPRV_WBSEL_N1_1_SHIFT		10
+#define ISPPRV_WBSEL_N1_2_SHIFT		12
+#define ISPPRV_WBSEL_N1_3_SHIFT		14
+#define ISPPRV_WBSEL_N2_0_SHIFT		16
+#define ISPPRV_WBSEL_N2_1_SHIFT		18
+#define ISPPRV_WBSEL_N2_2_SHIFT		20
+#define ISPPRV_WBSEL_N2_3_SHIFT		22
+#define ISPPRV_WBSEL_N3_0_SHIFT		24
+#define ISPPRV_WBSEL_N3_1_SHIFT		26
+#define ISPPRV_WBSEL_N3_2_SHIFT		28
+#define ISPPRV_WBSEL_N3_3_SHIFT		30
+
+#define ISPPRV_CFA_GRADTH_HOR_SHIFT	0
+#define ISPPRV_CFA_GRADTH_VER_SHIFT	8
+
+#define ISPPRV_BLKADJOFF_B_SHIFT	0
+#define ISPPRV_BLKADJOFF_G_SHIFT	8
+#define ISPPRV_BLKADJOFF_R_SHIFT	16
+
+#define ISPPRV_RGB_MAT1_MTX_RR_SHIFT	0
+#define ISPPRV_RGB_MAT1_MTX_GR_SHIFT	16
+
+#define ISPPRV_RGB_MAT2_MTX_BR_SHIFT	0
+#define ISPPRV_RGB_MAT2_MTX_RG_SHIFT	16
+
+#define ISPPRV_RGB_MAT3_MTX_GG_SHIFT	0
+#define ISPPRV_RGB_MAT3_MTX_BG_SHIFT	16
+
+#define ISPPRV_RGB_MAT4_MTX_RB_SHIFT	0
+#define ISPPRV_RGB_MAT4_MTX_GB_SHIFT	16
+
+#define ISPPRV_RGB_MAT5_MTX_BB_SHIFT	0
+
+#define ISPPRV_RGB_OFF1_MTX_OFFG_SHIFT	0
+#define ISPPRV_RGB_OFF1_MTX_OFFR_SHIFT	16
+
+#define ISPPRV_RGB_OFF2_MTX_OFFB_SHIFT	0
+
+#define ISPPRV_CSC0_RY_SHIFT		0
+#define ISPPRV_CSC0_GY_SHIFT		10
+#define ISPPRV_CSC0_BY_SHIFT		20
+
+#define ISPPRV_CSC1_RCB_SHIFT		0
+#define ISPPRV_CSC1_GCB_SHIFT		10
+#define ISPPRV_CSC1_BCB_SHIFT		20
+
+#define ISPPRV_CSC2_RCR_SHIFT		0
+#define ISPPRV_CSC2_GCR_SHIFT		10
+#define ISPPRV_CSC2_BCR_SHIFT		20
+
+#define ISPPRV_CSC_OFFSET_CR_SHIFT	0
+#define ISPPRV_CSC_OFFSET_CB_SHIFT	8
+#define ISPPRV_CSC_OFFSET_Y_SHIFT	16
+
+#define ISPPRV_CNT_BRT_BRT_SHIFT	0
+#define ISPPRV_CNT_BRT_CNT_SHIFT	8
+
+#define ISPPRV_CONTRAST_MAX		0x10
+#define ISPPRV_CONTRAST_MIN		0xFF
+#define ISPPRV_BRIGHT_MIN		0x00
+#define ISPPRV_BRIGHT_MAX		0xFF
+
+#define ISPPRV_CSUP_CSUPG_SHIFT		0
+#define ISPPRV_CSUP_THRES_SHIFT		8
+#define ISPPRV_CSUP_HPYF_SHIFT		16
+
+#define ISPPRV_SETUP_YC_MINC_SHIFT	0
+#define ISPPRV_SETUP_YC_MAXC_SHIFT	8
+#define ISPPRV_SETUP_YC_MINY_SHIFT	16
+#define ISPPRV_SETUP_YC_MAXY_SHIFT	24
+#define ISPPRV_YC_MAX			0xFF
+#define ISPPRV_YC_MIN			0x0
+
+/* Define bit fields within selected registers */
+#define ISP_REVISION_SHIFT			0
+
+#define ISP_SYSCONFIG_AUTOIDLE			(1 << 0)
+#define ISP_SYSCONFIG_SOFTRESET			(1 << 1)
+#define ISP_SYSCONFIG_MIDLEMODE_SHIFT		12
+#define ISP_SYSCONFIG_MIDLEMODE_FORCESTANDBY	0x0
+#define ISP_SYSCONFIG_MIDLEMODE_NOSTANBY	0x1
+#define ISP_SYSCONFIG_MIDLEMODE_SMARTSTANDBY	0x2
+
+#define ISP_SYSSTATUS_RESETDONE			0
+
+#define IRQ0ENABLE_CSIA_IRQ			(1 << 0)
+#define IRQ0ENABLE_CSIC_IRQ			(1 << 1)
+#define IRQ0ENABLE_CCP2_LCM_IRQ			(1 << 3)
+#define IRQ0ENABLE_CCP2_LC0_IRQ			(1 << 4)
+#define IRQ0ENABLE_CCP2_LC1_IRQ			(1 << 5)
+#define IRQ0ENABLE_CCP2_LC2_IRQ			(1 << 6)
+#define IRQ0ENABLE_CCP2_LC3_IRQ			(1 << 7)
+#define IRQ0ENABLE_CSIB_IRQ			(IRQ0ENABLE_CCP2_LCM_IRQ | \
+						IRQ0ENABLE_CCP2_LC0_IRQ | \
+						IRQ0ENABLE_CCP2_LC1_IRQ | \
+						IRQ0ENABLE_CCP2_LC2_IRQ | \
+						IRQ0ENABLE_CCP2_LC3_IRQ)
+
+#define IRQ0ENABLE_CCDC_VD0_IRQ			(1 << 8)
+#define IRQ0ENABLE_CCDC_VD1_IRQ			(1 << 9)
+#define IRQ0ENABLE_CCDC_VD2_IRQ			(1 << 10)
+#define IRQ0ENABLE_CCDC_ERR_IRQ			(1 << 11)
+#define IRQ0ENABLE_H3A_AF_DONE_IRQ		(1 << 12)
+#define IRQ0ENABLE_H3A_AWB_DONE_IRQ		(1 << 13)
+#define IRQ0ENABLE_HIST_DONE_IRQ		(1 << 16)
+#define IRQ0ENABLE_CCDC_LSC_DONE_IRQ		(1 << 17)
+#define IRQ0ENABLE_CCDC_LSC_PREF_COMP_IRQ	(1 << 18)
+#define IRQ0ENABLE_CCDC_LSC_PREF_ERR_IRQ	(1 << 19)
+#define IRQ0ENABLE_PRV_DONE_IRQ			(1 << 20)
+#define IRQ0ENABLE_RSZ_DONE_IRQ			(1 << 24)
+#define IRQ0ENABLE_OVF_IRQ			(1 << 25)
+#define IRQ0ENABLE_PING_IRQ			(1 << 26)
+#define IRQ0ENABLE_PONG_IRQ			(1 << 27)
+#define IRQ0ENABLE_MMU_ERR_IRQ			(1 << 28)
+#define IRQ0ENABLE_OCP_ERR_IRQ			(1 << 29)
+#define IRQ0ENABLE_SEC_ERR_IRQ			(1 << 30)
+#define IRQ0ENABLE_HS_VS_IRQ			(1 << 31)
+
+#define IRQ0STATUS_CSIA_IRQ			(1 << 0)
+#define IRQ0STATUS_CSI2C_IRQ			(1 << 1)
+#define IRQ0STATUS_CCP2_LCM_IRQ			(1 << 3)
+#define IRQ0STATUS_CCP2_LC0_IRQ			(1 << 4)
+#define IRQ0STATUS_CSIB_IRQ			(IRQ0STATUS_CCP2_LCM_IRQ | \
+						IRQ0STATUS_CCP2_LC0_IRQ)
+
+#define IRQ0STATUS_CSIB_LC1_IRQ			(1 << 5)
+#define IRQ0STATUS_CSIB_LC2_IRQ			(1 << 6)
+#define IRQ0STATUS_CSIB_LC3_IRQ			(1 << 7)
+#define IRQ0STATUS_CCDC_VD0_IRQ			(1 << 8)
+#define IRQ0STATUS_CCDC_VD1_IRQ			(1 << 9)
+#define IRQ0STATUS_CCDC_VD2_IRQ			(1 << 10)
+#define IRQ0STATUS_CCDC_ERR_IRQ			(1 << 11)
+#define IRQ0STATUS_H3A_AF_DONE_IRQ		(1 << 12)
+#define IRQ0STATUS_H3A_AWB_DONE_IRQ		(1 << 13)
+#define IRQ0STATUS_HIST_DONE_IRQ		(1 << 16)
+#define IRQ0STATUS_CCDC_LSC_DONE_IRQ		(1 << 17)
+#define IRQ0STATUS_CCDC_LSC_PREF_COMP_IRQ	(1 << 18)
+#define IRQ0STATUS_CCDC_LSC_PREF_ERR_IRQ	(1 << 19)
+#define IRQ0STATUS_PRV_DONE_IRQ			(1 << 20)
+#define IRQ0STATUS_RSZ_DONE_IRQ			(1 << 24)
+#define IRQ0STATUS_OVF_IRQ			(1 << 25)
+#define IRQ0STATUS_PING_IRQ			(1 << 26)
+#define IRQ0STATUS_PONG_IRQ			(1 << 27)
+#define IRQ0STATUS_MMU_ERR_IRQ			(1 << 28)
+#define IRQ0STATUS_OCP_ERR_IRQ			(1 << 29)
+#define IRQ0STATUS_SEC_ERR_IRQ			(1 << 30)
+#define IRQ0STATUS_HS_VS_IRQ			(1 << 31)
+
+#define TCTRL_GRESET_LEN			0
+
+#define TCTRL_PSTRB_REPLAY_DELAY		0
+#define TCTRL_PSTRB_REPLAY_COUNTER_SHIFT	25
+
+#define ISPCTRL_PAR_SER_CLK_SEL_PARALLEL	0x0
+#define ISPCTRL_PAR_SER_CLK_SEL_CSIA		0x1
+#define ISPCTRL_PAR_SER_CLK_SEL_CSIB		0x2
+#define ISPCTRL_PAR_SER_CLK_SEL_CSIC		0x3
+#define ISPCTRL_PAR_SER_CLK_SEL_MASK		0x3
+
+#define ISPCTRL_PAR_BRIDGE_SHIFT		2
+#define ISPCTRL_PAR_BRIDGE_DISABLE		(0x0 << 2)
+#define ISPCTRL_PAR_BRIDGE_LENDIAN		(0x2 << 2)
+#define ISPCTRL_PAR_BRIDGE_BENDIAN		(0x3 << 2)
+#define ISPCTRL_PAR_BRIDGE_MASK			(0x3 << 2)
+
+#define ISPCTRL_PAR_CLK_POL_SHIFT		4
+#define ISPCTRL_PAR_CLK_POL_INV			(1 << 4)
+#define ISPCTRL_PING_PONG_EN			(1 << 5)
+#define ISPCTRL_SHIFT_SHIFT			6
+#define ISPCTRL_SHIFT_0				(0x0 << 6)
+#define ISPCTRL_SHIFT_2				(0x1 << 6)
+#define ISPCTRL_SHIFT_4				(0x2 << 6)
+#define ISPCTRL_SHIFT_MASK			(0x3 << 6)
+
+#define ISPCTRL_CCDC_CLK_EN			(1 << 8)
+#define ISPCTRL_SCMP_CLK_EN			(1 << 9)
+#define ISPCTRL_H3A_CLK_EN			(1 << 10)
+#define ISPCTRL_HIST_CLK_EN			(1 << 11)
+#define ISPCTRL_PREV_CLK_EN			(1 << 12)
+#define ISPCTRL_RSZ_CLK_EN			(1 << 13)
+#define ISPCTRL_SYNC_DETECT_SHIFT		14
+#define ISPCTRL_SYNC_DETECT_HSFALL	(0x0 << ISPCTRL_SYNC_DETECT_SHIFT)
+#define ISPCTRL_SYNC_DETECT_HSRISE	(0x1 << ISPCTRL_SYNC_DETECT_SHIFT)
+#define ISPCTRL_SYNC_DETECT_VSFALL	(0x2 << ISPCTRL_SYNC_DETECT_SHIFT)
+#define ISPCTRL_SYNC_DETECT_VSRISE	(0x3 << ISPCTRL_SYNC_DETECT_SHIFT)
+#define ISPCTRL_SYNC_DETECT_MASK	(0x3 << ISPCTRL_SYNC_DETECT_SHIFT)
+
+#define ISPCTRL_CCDC_RAM_EN		(1 << 16)
+#define ISPCTRL_PREV_RAM_EN		(1 << 17)
+#define ISPCTRL_SBL_RD_RAM_EN		(1 << 18)
+#define ISPCTRL_SBL_WR1_RAM_EN		(1 << 19)
+#define ISPCTRL_SBL_WR0_RAM_EN		(1 << 20)
+#define ISPCTRL_SBL_AUTOIDLE		(1 << 21)
+#define ISPCTRL_SBL_SHARED_WPORTC	(1 << 26)
+#define ISPCTRL_SBL_SHARED_RPORTA	(1 << 27)
+#define ISPCTRL_SBL_SHARED_RPORTB	(1 << 28)
+#define ISPCTRL_JPEG_FLUSH		(1 << 30)
+#define ISPCTRL_CCDC_FLUSH		(1 << 31)
+
+#define ISPSECURE_SECUREMODE		0
+
+#define ISPTCTRL_CTRL_DIV_LOW		0x0
+#define ISPTCTRL_CTRL_DIV_HIGH		0x1
+#define ISPTCTRL_CTRL_DIV_BYPASS	0x1F
+
+#define ISPTCTRL_CTRL_DIVA_SHIFT	0
+#define ISPTCTRL_CTRL_DIVA_MASK		(0x1F << ISPTCTRL_CTRL_DIVA_SHIFT)
+
+#define ISPTCTRL_CTRL_DIVB_SHIFT	5
+#define ISPTCTRL_CTRL_DIVB_MASK		(0x1F << ISPTCTRL_CTRL_DIVB_SHIFT)
+
+#define ISPTCTRL_CTRL_DIVC_SHIFT	10
+#define ISPTCTRL_CTRL_DIVC_NOCLOCK	(0x0 << 10)
+
+#define ISPTCTRL_CTRL_SHUTEN		(1 << 21)
+#define ISPTCTRL_CTRL_PSTRBEN		(1 << 22)
+#define ISPTCTRL_CTRL_STRBEN		(1 << 23)
+#define ISPTCTRL_CTRL_SHUTPOL		(1 << 24)
+#define ISPTCTRL_CTRL_STRBPSTRBPOL	(1 << 26)
+
+#define ISPTCTRL_CTRL_INSEL_SHIFT	27
+#define ISPTCTRL_CTRL_INSEL_PARALLEL	(0x0 << 27)
+#define ISPTCTRL_CTRL_INSEL_CSIA	(0x1 << 27)
+#define ISPTCTRL_CTRL_INSEL_CSIB	(0x2 << 27)
+
+#define ISPTCTRL_CTRL_GRESETEn		(1 << 29)
+#define ISPTCTRL_CTRL_GRESETPOL		(1 << 30)
+#define ISPTCTRL_CTRL_GRESETDIR		(1 << 31)
+
+#define ISPTCTRL_FRAME_SHUT_SHIFT		0
+#define ISPTCTRL_FRAME_PSTRB_SHIFT		6
+#define ISPTCTRL_FRAME_STRB_SHIFT		12
+
+#define ISPCCDC_PID_PREV_SHIFT			0
+#define ISPCCDC_PID_CID_SHIFT			8
+#define ISPCCDC_PID_TID_SHIFT			16
+
+#define ISPCCDC_PCR_EN				1
+#define ISPCCDC_PCR_BUSY			(1 << 1)
+
+#define ISPCCDC_SYN_MODE_VDHDOUT		0x1
+#define ISPCCDC_SYN_MODE_FLDOUT			(1 << 1)
+#define ISPCCDC_SYN_MODE_VDPOL			(1 << 2)
+#define ISPCCDC_SYN_MODE_HDPOL			(1 << 3)
+#define ISPCCDC_SYN_MODE_FLDPOL			(1 << 4)
+#define ISPCCDC_SYN_MODE_EXWEN			(1 << 5)
+#define ISPCCDC_SYN_MODE_DATAPOL		(1 << 6)
+#define ISPCCDC_SYN_MODE_FLDMODE		(1 << 7)
+#define ISPCCDC_SYN_MODE_DATSIZ_MASK		(0x7 << 8)
+#define ISPCCDC_SYN_MODE_DATSIZ_8_16		(0x0 << 8)
+#define ISPCCDC_SYN_MODE_DATSIZ_12		(0x4 << 8)
+#define ISPCCDC_SYN_MODE_DATSIZ_11		(0x5 << 8)
+#define ISPCCDC_SYN_MODE_DATSIZ_10		(0x6 << 8)
+#define ISPCCDC_SYN_MODE_DATSIZ_8		(0x7 << 8)
+#define ISPCCDC_SYN_MODE_PACK8			(1 << 11)
+#define ISPCCDC_SYN_MODE_INPMOD_MASK		(3 << 12)
+#define ISPCCDC_SYN_MODE_INPMOD_RAW		(0 << 12)
+#define ISPCCDC_SYN_MODE_INPMOD_YCBCR16		(1 << 12)
+#define ISPCCDC_SYN_MODE_INPMOD_YCBCR8		(2 << 12)
+#define ISPCCDC_SYN_MODE_LPF			(1 << 14)
+#define ISPCCDC_SYN_MODE_FLDSTAT		(1 << 15)
+#define ISPCCDC_SYN_MODE_VDHDEN			(1 << 16)
+#define ISPCCDC_SYN_MODE_WEN			(1 << 17)
+#define ISPCCDC_SYN_MODE_VP2SDR			(1 << 18)
+#define ISPCCDC_SYN_MODE_SDR2RSZ		(1 << 19)
+
+#define ISPCCDC_HD_VD_WID_VDW_SHIFT		0
+#define ISPCCDC_HD_VD_WID_HDW_SHIFT		16
+
+#define ISPCCDC_PIX_LINES_HLPRF_SHIFT		0
+#define ISPCCDC_PIX_LINES_PPLN_SHIFT		16
+
+#define ISPCCDC_HORZ_INFO_NPH_SHIFT		0
+#define ISPCCDC_HORZ_INFO_NPH_MASK		0x00007fff
+#define ISPCCDC_HORZ_INFO_SPH_SHIFT		16
+#define ISPCCDC_HORZ_INFO_SPH_MASK		0x7fff0000
+
+#define ISPCCDC_VERT_START_SLV1_SHIFT		0
+#define ISPCCDC_VERT_START_SLV0_SHIFT		16
+#define ISPCCDC_VERT_START_SLV0_MASK		0x7fff0000
+
+#define ISPCCDC_VERT_LINES_NLV_SHIFT		0
+#define ISPCCDC_VERT_LINES_NLV_MASK		0x00007fff
+
+#define ISPCCDC_CULLING_CULV_SHIFT		0
+#define ISPCCDC_CULLING_CULHODD_SHIFT		16
+#define ISPCCDC_CULLING_CULHEVN_SHIFT		24
+
+#define ISPCCDC_HSIZE_OFF_SHIFT			0
+
+#define ISPCCDC_SDOFST_FIINV			(1 << 14)
+#define ISPCCDC_SDOFST_FOFST_SHIFT		12
+#define ISPCCDC_SDOFST_FOFST_MASK		(3 << 12)
+#define ISPCCDC_SDOFST_LOFST3_SHIFT		0
+#define ISPCCDC_SDOFST_LOFST2_SHIFT		3
+#define ISPCCDC_SDOFST_LOFST1_SHIFT		6
+#define ISPCCDC_SDOFST_LOFST0_SHIFT		9
+
+#define ISPCCDC_CLAMP_OBGAIN_SHIFT		0
+#define ISPCCDC_CLAMP_OBST_SHIFT		10
+#define ISPCCDC_CLAMP_OBSLN_SHIFT		25
+#define ISPCCDC_CLAMP_OBSLEN_SHIFT		28
+#define ISPCCDC_CLAMP_CLAMPEN			(1 << 31)
+
+#define ISPCCDC_COLPTN_R_Ye			0x0
+#define ISPCCDC_COLPTN_Gr_Cy			0x1
+#define ISPCCDC_COLPTN_Gb_G			0x2
+#define ISPCCDC_COLPTN_B_Mg			0x3
+#define ISPCCDC_COLPTN_CP0PLC0_SHIFT		0
+#define ISPCCDC_COLPTN_CP0PLC1_SHIFT		2
+#define ISPCCDC_COLPTN_CP0PLC2_SHIFT		4
+#define ISPCCDC_COLPTN_CP0PLC3_SHIFT		6
+#define ISPCCDC_COLPTN_CP1PLC0_SHIFT		8
+#define ISPCCDC_COLPTN_CP1PLC1_SHIFT		10
+#define ISPCCDC_COLPTN_CP1PLC2_SHIFT		12
+#define ISPCCDC_COLPTN_CP1PLC3_SHIFT		14
+#define ISPCCDC_COLPTN_CP2PLC0_SHIFT		16
+#define ISPCCDC_COLPTN_CP2PLC1_SHIFT		18
+#define ISPCCDC_COLPTN_CP2PLC2_SHIFT		20
+#define ISPCCDC_COLPTN_CP2PLC3_SHIFT		22
+#define ISPCCDC_COLPTN_CP3PLC0_SHIFT		24
+#define ISPCCDC_COLPTN_CP3PLC1_SHIFT		26
+#define ISPCCDC_COLPTN_CP3PLC2_SHIFT		28
+#define ISPCCDC_COLPTN_CP3PLC3_SHIFT		30
+
+#define ISPCCDC_BLKCMP_B_MG_SHIFT		0
+#define ISPCCDC_BLKCMP_GB_G_SHIFT		8
+#define ISPCCDC_BLKCMP_GR_CY_SHIFT		16
+#define ISPCCDC_BLKCMP_R_YE_SHIFT		24
+
+#define ISPCCDC_FPC_FPNUM_SHIFT			0
+#define ISPCCDC_FPC_FPCEN			(1 << 15)
+#define ISPCCDC_FPC_FPERR			(1 << 16)
+
+#define ISPCCDC_VDINT_1_SHIFT			0
+#define ISPCCDC_VDINT_1_MASK			0x00007fff
+#define ISPCCDC_VDINT_0_SHIFT			16
+#define ISPCCDC_VDINT_0_MASK			0x7fff0000
+
+#define ISPCCDC_ALAW_GWDI_12_3			(0x3 << 0)
+#define ISPCCDC_ALAW_GWDI_11_2			(0x4 << 0)
+#define ISPCCDC_ALAW_GWDI_10_1			(0x5 << 0)
+#define ISPCCDC_ALAW_GWDI_9_0			(0x6 << 0)
+#define ISPCCDC_ALAW_CCDTBL			(1 << 3)
+
+#define ISPCCDC_REC656IF_R656ON			1
+#define ISPCCDC_REC656IF_ECCFVH			(1 << 1)
+
+#define ISPCCDC_CFG_BW656			(1 << 5)
+#define ISPCCDC_CFG_FIDMD_SHIFT			6
+#define ISPCCDC_CFG_WENLOG			(1 << 8)
+#define ISPCCDC_CFG_WENLOG_AND			(0 << 8)
+#define ISPCCDC_CFG_WENLOG_OR			(1 << 8)
+#define ISPCCDC_CFG_Y8POS			(1 << 11)
+#define ISPCCDC_CFG_BSWD			(1 << 12)
+#define ISPCCDC_CFG_MSBINVI			(1 << 13)
+#define ISPCCDC_CFG_VDLC			(1 << 15)
+
+#define ISPCCDC_FMTCFG_FMTEN			0x1
+#define ISPCCDC_FMTCFG_LNALT			(1 << 1)
+#define ISPCCDC_FMTCFG_LNUM_SHIFT		2
+#define ISPCCDC_FMTCFG_PLEN_ODD_SHIFT		4
+#define ISPCCDC_FMTCFG_PLEN_EVEN_SHIFT		8
+#define ISPCCDC_FMTCFG_VPIN_MASK		0x00007000
+#define ISPCCDC_FMTCFG_VPIN_12_3		(0x3 << 12)
+#define ISPCCDC_FMTCFG_VPIN_11_2		(0x4 << 12)
+#define ISPCCDC_FMTCFG_VPIN_10_1		(0x5 << 12)
+#define ISPCCDC_FMTCFG_VPIN_9_0			(0x6 << 12)
+#define ISPCCDC_FMTCFG_VPEN			(1 << 15)
+
+#define ISPCCDC_FMTCFG_VPIF_FRQ_MASK		0x003f0000
+#define ISPCCDC_FMTCFG_VPIF_FRQ_SHIFT		16
+#define ISPCCDC_FMTCFG_VPIF_FRQ_BY2		(0x0 << 16)
+#define ISPCCDC_FMTCFG_VPIF_FRQ_BY3		(0x1 << 16)
+#define ISPCCDC_FMTCFG_VPIF_FRQ_BY4		(0x2 << 16)
+#define ISPCCDC_FMTCFG_VPIF_FRQ_BY5		(0x3 << 16)
+#define ISPCCDC_FMTCFG_VPIF_FRQ_BY6		(0x4 << 16)
+
+#define ISPCCDC_FMT_HORZ_FMTLNH_SHIFT		0
+#define ISPCCDC_FMT_HORZ_FMTSPH_SHIFT		16
+
+#define ISPCCDC_FMT_VERT_FMTLNV_SHIFT		0
+#define ISPCCDC_FMT_VERT_FMTSLV_SHIFT		16
+
+#define ISPCCDC_FMT_HORZ_FMTSPH_MASK		0x1fff0000
+#define ISPCCDC_FMT_HORZ_FMTLNH_MASK		0x00001fff
+
+#define ISPCCDC_FMT_VERT_FMTSLV_MASK		0x1fff0000
+#define ISPCCDC_FMT_VERT_FMTLNV_MASK		0x00001fff
+
+#define ISPCCDC_VP_OUT_HORZ_ST_SHIFT		0
+#define ISPCCDC_VP_OUT_HORZ_NUM_SHIFT		4
+#define ISPCCDC_VP_OUT_VERT_NUM_SHIFT		17
+
+#define ISPRSZ_PID_PREV_SHIFT			0
+#define ISPRSZ_PID_CID_SHIFT			8
+#define ISPRSZ_PID_TID_SHIFT			16
+
+#define ISPRSZ_PCR_ENABLE			(1 << 0)
+#define ISPRSZ_PCR_BUSY				(1 << 1)
+#define ISPRSZ_PCR_ONESHOT			(1 << 2)
+
+#define ISPRSZ_CNT_HRSZ_SHIFT			0
+#define ISPRSZ_CNT_HRSZ_MASK			\
+	(0x3FF << ISPRSZ_CNT_HRSZ_SHIFT)
+#define ISPRSZ_CNT_VRSZ_SHIFT			10
+#define ISPRSZ_CNT_VRSZ_MASK			\
+	(0x3FF << ISPRSZ_CNT_VRSZ_SHIFT)
+#define ISPRSZ_CNT_HSTPH_SHIFT			20
+#define ISPRSZ_CNT_HSTPH_MASK			(0x7 << ISPRSZ_CNT_HSTPH_SHIFT)
+#define ISPRSZ_CNT_VSTPH_SHIFT			23
+#define ISPRSZ_CNT_VSTPH_MASK			(0x7 << ISPRSZ_CNT_VSTPH_SHIFT)
+#define ISPRSZ_CNT_YCPOS			(1 << 26)
+#define ISPRSZ_CNT_INPTYP			(1 << 27)
+#define ISPRSZ_CNT_INPSRC			(1 << 28)
+#define ISPRSZ_CNT_CBILIN			(1 << 29)
+
+#define ISPRSZ_OUT_SIZE_HORZ_SHIFT		0
+#define ISPRSZ_OUT_SIZE_HORZ_MASK		\
+	(0xFFF << ISPRSZ_OUT_SIZE_HORZ_SHIFT)
+#define ISPRSZ_OUT_SIZE_VERT_SHIFT		16
+#define ISPRSZ_OUT_SIZE_VERT_MASK		\
+	(0xFFF << ISPRSZ_OUT_SIZE_VERT_SHIFT)
+
+#define ISPRSZ_IN_START_HORZ_ST_SHIFT		0
+#define ISPRSZ_IN_START_HORZ_ST_MASK		\
+	(0x1FFF << ISPRSZ_IN_START_HORZ_ST_SHIFT)
+#define ISPRSZ_IN_START_VERT_ST_SHIFT		16
+#define ISPRSZ_IN_START_VERT_ST_MASK		\
+	(0x1FFF << ISPRSZ_IN_START_VERT_ST_SHIFT)
+
+#define ISPRSZ_IN_SIZE_HORZ_SHIFT		0
+#define ISPRSZ_IN_SIZE_HORZ_MASK		\
+	(0x1FFF << ISPRSZ_IN_SIZE_HORZ_SHIFT)
+#define ISPRSZ_IN_SIZE_VERT_SHIFT		16
+#define ISPRSZ_IN_SIZE_VERT_MASK		\
+	(0x1FFF << ISPRSZ_IN_SIZE_VERT_SHIFT)
+
+#define ISPRSZ_SDR_INADD_ADDR_SHIFT		0
+#define ISPRSZ_SDR_INADD_ADDR_MASK		0xFFFFFFFF
+
+#define ISPRSZ_SDR_INOFF_OFFSET_SHIFT		0
+#define ISPRSZ_SDR_INOFF_OFFSET_MASK		\
+	(0xFFFF << ISPRSZ_SDR_INOFF_OFFSET_SHIFT)
+
+#define ISPRSZ_SDR_OUTADD_ADDR_SHIFT		0
+#define ISPRSZ_SDR_OUTADD_ADDR_MASK		0xFFFFFFFF
+
+
+#define ISPRSZ_SDR_OUTOFF_OFFSET_SHIFT		0
+#define ISPRSZ_SDR_OUTOFF_OFFSET_MASK		\
+	(0xFFFF << ISPRSZ_SDR_OUTOFF_OFFSET_SHIFT)
+
+#define ISPRSZ_HFILT_COEF0_SHIFT		0
+#define ISPRSZ_HFILT_COEF0_MASK			\
+	(0x3FF << ISPRSZ_HFILT_COEF0_SHIFT)
+#define ISPRSZ_HFILT_COEF1_SHIFT		16
+#define ISPRSZ_HFILT_COEF1_MASK			\
+	(0x3FF << ISPRSZ_HFILT_COEF1_SHIFT)
+
+#define ISPRSZ_HFILT32_COEF2_SHIFT		0
+#define ISPRSZ_HFILT32_COEF2_MASK		0x3FF
+#define ISPRSZ_HFILT32_COEF3_SHIFT		16
+#define ISPRSZ_HFILT32_COEF3_MASK		0x3FF0000
+
+#define ISPRSZ_HFILT54_COEF4_SHIFT		0
+#define ISPRSZ_HFILT54_COEF4_MASK		0x3FF
+#define ISPRSZ_HFILT54_COEF5_SHIFT		16
+#define ISPRSZ_HFILT54_COEF5_MASK		0x3FF0000
+
+#define ISPRSZ_HFILT76_COEFF6_SHIFT		0
+#define ISPRSZ_HFILT76_COEFF6_MASK		0x3FF
+#define ISPRSZ_HFILT76_COEFF7_SHIFT		16
+#define ISPRSZ_HFILT76_COEFF7_MASK		0x3FF0000
+
+#define ISPRSZ_HFILT98_COEFF8_SHIFT		0
+#define ISPRSZ_HFILT98_COEFF8_MASK		0x3FF
+#define ISPRSZ_HFILT98_COEFF9_SHIFT		16
+#define ISPRSZ_HFILT98_COEFF9_MASK		0x3FF0000
+
+#define ISPRSZ_HFILT1110_COEF10_SHIFT		0
+#define ISPRSZ_HFILT1110_COEF10_MASK		0x3FF
+#define ISPRSZ_HFILT1110_COEF11_SHIFT		16
+#define ISPRSZ_HFILT1110_COEF11_MASK		0x3FF0000
+
+#define ISPRSZ_HFILT1312_COEFF12_SHIFT		0
+#define ISPRSZ_HFILT1312_COEFF12_MASK		0x3FF
+#define ISPRSZ_HFILT1312_COEFF13_SHIFT		16
+#define ISPRSZ_HFILT1312_COEFF13_MASK		0x3FF0000
+
+#define ISPRSZ_HFILT1514_COEFF14_SHIFT		0
+#define ISPRSZ_HFILT1514_COEFF14_MASK		0x3FF
+#define ISPRSZ_HFILT1514_COEFF15_SHIFT		16
+#define ISPRSZ_HFILT1514_COEFF15_MASK		0x3FF0000
+
+#define ISPRSZ_HFILT1716_COEF16_SHIFT		0
+#define ISPRSZ_HFILT1716_COEF16_MASK		0x3FF
+#define ISPRSZ_HFILT1716_COEF17_SHIFT		16
+#define ISPRSZ_HFILT1716_COEF17_MASK		0x3FF0000
+
+#define ISPRSZ_HFILT1918_COEF18_SHIFT		0
+#define ISPRSZ_HFILT1918_COEF18_MASK		0x3FF
+#define ISPRSZ_HFILT1918_COEF19_SHIFT		16
+#define ISPRSZ_HFILT1918_COEF19_MASK		0x3FF0000
+
+#define ISPRSZ_HFILT2120_COEF20_SHIFT		0
+#define ISPRSZ_HFILT2120_COEF20_MASK		0x3FF
+#define ISPRSZ_HFILT2120_COEF21_SHIFT		16
+#define ISPRSZ_HFILT2120_COEF21_MASK		0x3FF0000
+
+#define ISPRSZ_HFILT2322_COEF22_SHIFT		0
+#define ISPRSZ_HFILT2322_COEF22_MASK		0x3FF
+#define ISPRSZ_HFILT2322_COEF23_SHIFT		16
+#define ISPRSZ_HFILT2322_COEF23_MASK		0x3FF0000
+
+#define ISPRSZ_HFILT2524_COEF24_SHIFT		0
+#define ISPRSZ_HFILT2524_COEF24_MASK		0x3FF
+#define ISPRSZ_HFILT2524_COEF25_SHIFT		16
+#define ISPRSZ_HFILT2524_COEF25_MASK		0x3FF0000
+
+#define ISPRSZ_HFILT2726_COEF26_SHIFT		0
+#define ISPRSZ_HFILT2726_COEF26_MASK		0x3FF
+#define ISPRSZ_HFILT2726_COEF27_SHIFT		16
+#define ISPRSZ_HFILT2726_COEF27_MASK		0x3FF0000
+
+#define ISPRSZ_HFILT2928_COEF28_SHIFT		0
+#define ISPRSZ_HFILT2928_COEF28_MASK		0x3FF
+#define ISPRSZ_HFILT2928_COEF29_SHIFT		16
+#define ISPRSZ_HFILT2928_COEF29_MASK		0x3FF0000
+
+#define ISPRSZ_HFILT3130_COEF30_SHIFT		0
+#define ISPRSZ_HFILT3130_COEF30_MASK		0x3FF
+#define ISPRSZ_HFILT3130_COEF31_SHIFT		16
+#define ISPRSZ_HFILT3130_COEF31_MASK		0x3FF0000
+
+#define ISPRSZ_VFILT_COEF0_SHIFT		0
+#define ISPRSZ_VFILT_COEF0_MASK			\
+	(0x3FF << ISPRSZ_VFILT_COEF0_SHIFT)
+#define ISPRSZ_VFILT_COEF1_SHIFT		16
+#define ISPRSZ_VFILT_COEF1_MASK			\
+	(0x3FF << ISPRSZ_VFILT_COEF1_SHIFT)
+
+#define ISPRSZ_VFILT10_COEF0_SHIFT		0
+#define ISPRSZ_VFILT10_COEF0_MASK		0x3FF
+#define ISPRSZ_VFILT10_COEF1_SHIFT		16
+#define ISPRSZ_VFILT10_COEF1_MASK		0x3FF0000
+
+#define ISPRSZ_VFILT32_COEF2_SHIFT		0
+#define ISPRSZ_VFILT32_COEF2_MASK		0x3FF
+#define ISPRSZ_VFILT32_COEF3_SHIFT		16
+#define ISPRSZ_VFILT32_COEF3_MASK		0x3FF0000
+
+#define ISPRSZ_VFILT54_COEF4_SHIFT		0
+#define ISPRSZ_VFILT54_COEF4_MASK		0x3FF
+#define ISPRSZ_VFILT54_COEF5_SHIFT		16
+#define ISPRSZ_VFILT54_COEF5_MASK		0x3FF0000
+
+#define ISPRSZ_VFILT76_COEFF6_SHIFT		0
+#define ISPRSZ_VFILT76_COEFF6_MASK		0x3FF
+#define ISPRSZ_VFILT76_COEFF7_SHIFT		16
+#define ISPRSZ_VFILT76_COEFF7_MASK		0x3FF0000
+
+#define ISPRSZ_VFILT98_COEFF8_SHIFT		0
+#define ISPRSZ_VFILT98_COEFF8_MASK		0x3FF
+#define ISPRSZ_VFILT98_COEFF9_SHIFT		16
+#define ISPRSZ_VFILT98_COEFF9_MASK		0x3FF0000
+
+#define ISPRSZ_VFILT1110_COEF10_SHIFT		0
+#define ISPRSZ_VFILT1110_COEF10_MASK		0x3FF
+#define ISPRSZ_VFILT1110_COEF11_SHIFT		16
+#define ISPRSZ_VFILT1110_COEF11_MASK		0x3FF0000
+
+#define ISPRSZ_VFILT1312_COEFF12_SHIFT		0
+#define ISPRSZ_VFILT1312_COEFF12_MASK		0x3FF
+#define ISPRSZ_VFILT1312_COEFF13_SHIFT		16
+#define ISPRSZ_VFILT1312_COEFF13_MASK		0x3FF0000
+
+#define ISPRSZ_VFILT1514_COEFF14_SHIFT		0
+#define ISPRSZ_VFILT1514_COEFF14_MASK		0x3FF
+#define ISPRSZ_VFILT1514_COEFF15_SHIFT		16
+#define ISPRSZ_VFILT1514_COEFF15_MASK		0x3FF0000
+
+#define ISPRSZ_VFILT1716_COEF16_SHIFT		0
+#define ISPRSZ_VFILT1716_COEF16_MASK		0x3FF
+#define ISPRSZ_VFILT1716_COEF17_SHIFT		16
+#define ISPRSZ_VFILT1716_COEF17_MASK		0x3FF0000
+
+#define ISPRSZ_VFILT1918_COEF18_SHIFT		0
+#define ISPRSZ_VFILT1918_COEF18_MASK		0x3FF
+#define ISPRSZ_VFILT1918_COEF19_SHIFT		16
+#define ISPRSZ_VFILT1918_COEF19_MASK		0x3FF0000
+
+#define ISPRSZ_VFILT2120_COEF20_SHIFT		0
+#define ISPRSZ_VFILT2120_COEF20_MASK		0x3FF
+#define ISPRSZ_VFILT2120_COEF21_SHIFT		16
+#define ISPRSZ_VFILT2120_COEF21_MASK		0x3FF0000
+
+#define ISPRSZ_VFILT2322_COEF22_SHIFT		0
+#define ISPRSZ_VFILT2322_COEF22_MASK		0x3FF
+#define ISPRSZ_VFILT2322_COEF23_SHIFT		16
+#define ISPRSZ_VFILT2322_COEF23_MASK		0x3FF0000
+
+#define ISPRSZ_VFILT2524_COEF24_SHIFT		0
+#define ISPRSZ_VFILT2524_COEF24_MASK		0x3FF
+#define ISPRSZ_VFILT2524_COEF25_SHIFT		16
+#define ISPRSZ_VFILT2524_COEF25_MASK		0x3FF0000
+
+#define ISPRSZ_VFILT2726_COEF26_SHIFT		0
+#define ISPRSZ_VFILT2726_COEF26_MASK		0x3FF
+#define ISPRSZ_VFILT2726_COEF27_SHIFT		16
+#define ISPRSZ_VFILT2726_COEF27_MASK		0x3FF0000
+
+#define ISPRSZ_VFILT2928_COEF28_SHIFT		0
+#define ISPRSZ_VFILT2928_COEF28_MASK		0x3FF
+#define ISPRSZ_VFILT2928_COEF29_SHIFT		16
+#define ISPRSZ_VFILT2928_COEF29_MASK		0x3FF0000
+
+#define ISPRSZ_VFILT3130_COEF30_SHIFT		0
+#define ISPRSZ_VFILT3130_COEF30_MASK		0x3FF
+#define ISPRSZ_VFILT3130_COEF31_SHIFT		16
+#define ISPRSZ_VFILT3130_COEF31_MASK		0x3FF0000
+
+#define ISPRSZ_YENH_CORE_SHIFT			0
+#define ISPRSZ_YENH_CORE_MASK			\
+	(0xFF << ISPRSZ_YENH_CORE_SHIFT)
+#define ISPRSZ_YENH_SLOP_SHIFT			8
+#define ISPRSZ_YENH_SLOP_MASK			\
+	(0xF << ISPRSZ_YENH_SLOP_SHIFT)
+#define ISPRSZ_YENH_GAIN_SHIFT			12
+#define ISPRSZ_YENH_GAIN_MASK			\
+	(0xF << ISPRSZ_YENH_GAIN_SHIFT)
+#define ISPRSZ_YENH_ALGO_SHIFT			16
+#define ISPRSZ_YENH_ALGO_MASK			\
+	(0x3 << ISPRSZ_YENH_ALGO_SHIFT)
+
+#define ISPH3A_PCR_AEW_ALAW_EN_SHIFT		1
+#define ISPH3A_PCR_AF_MED_TH_SHIFT		3
+#define ISPH3A_PCR_AF_RGBPOS_SHIFT		11
+#define ISPH3A_PCR_AEW_AVE2LMT_SHIFT		22
+#define ISPH3A_PCR_AEW_AVE2LMT_MASK		0xFFC00000
+#define ISPH3A_PCR_BUSYAF			(1 << 15)
+#define ISPH3A_PCR_BUSYAEAWB			(1 << 18)
+
+#define ISPH3A_AEWWIN1_WINHC_SHIFT		0
+#define ISPH3A_AEWWIN1_WINHC_MASK		0x3F
+#define ISPH3A_AEWWIN1_WINVC_SHIFT		6
+#define ISPH3A_AEWWIN1_WINVC_MASK		0x1FC0
+#define ISPH3A_AEWWIN1_WINW_SHIFT		13
+#define ISPH3A_AEWWIN1_WINW_MASK		0xFE000
+#define ISPH3A_AEWWIN1_WINH_SHIFT		24
+#define ISPH3A_AEWWIN1_WINH_MASK		0x7F000000
+
+#define ISPH3A_AEWINSTART_WINSH_SHIFT		0
+#define ISPH3A_AEWINSTART_WINSH_MASK		0x0FFF
+#define ISPH3A_AEWINSTART_WINSV_SHIFT		16
+#define ISPH3A_AEWINSTART_WINSV_MASK		0x0FFF0000
+
+#define ISPH3A_AEWINBLK_WINH_SHIFT		0
+#define ISPH3A_AEWINBLK_WINH_MASK		0x7F
+#define ISPH3A_AEWINBLK_WINSV_SHIFT		16
+#define ISPH3A_AEWINBLK_WINSV_MASK		0x0FFF0000
+
+#define ISPH3A_AEWSUBWIN_AEWINCH_SHIFT		0
+#define ISPH3A_AEWSUBWIN_AEWINCH_MASK		0x0F
+#define ISPH3A_AEWSUBWIN_AEWINCV_SHIFT		8
+#define ISPH3A_AEWSUBWIN_AEWINCV_MASK		0x0F00
+
+#define ISPHIST_PCR_ENABLE_SHIFT	0
+#define ISPHIST_PCR_ENABLE_MASK		0x01
+#define ISPHIST_PCR_ENABLE		(1 << ISPHIST_PCR_ENABLE_SHIFT)
+#define ISPHIST_PCR_BUSY		0x02
+
+#define ISPHIST_CNT_DATASIZE_SHIFT	8
+#define ISPHIST_CNT_DATASIZE_MASK	0x0100
+#define ISPHIST_CNT_CLEAR_SHIFT		7
+#define ISPHIST_CNT_CLEAR_MASK		0x080
+#define ISPHIST_CNT_CLEAR		(1 << ISPHIST_CNT_CLEAR_SHIFT)
+#define ISPHIST_CNT_CFA_SHIFT		6
+#define ISPHIST_CNT_CFA_MASK		0x040
+#define ISPHIST_CNT_BINS_SHIFT		4
+#define ISPHIST_CNT_BINS_MASK		0x030
+#define ISPHIST_CNT_SOURCE_SHIFT	3
+#define ISPHIST_CNT_SOURCE_MASK		0x08
+#define ISPHIST_CNT_SHIFT_SHIFT		0
+#define ISPHIST_CNT_SHIFT_MASK		0x07
+
+#define ISPHIST_WB_GAIN_WG00_SHIFT	24
+#define ISPHIST_WB_GAIN_WG00_MASK	0xFF000000
+#define ISPHIST_WB_GAIN_WG01_SHIFT	16
+#define ISPHIST_WB_GAIN_WG01_MASK	0xFF0000
+#define ISPHIST_WB_GAIN_WG02_SHIFT	8
+#define ISPHIST_WB_GAIN_WG02_MASK	0xFF00
+#define ISPHIST_WB_GAIN_WG03_SHIFT	0
+#define ISPHIST_WB_GAIN_WG03_MASK	0xFF
+
+#define ISPHIST_REG_START_END_MASK		0x3FFF
+#define ISPHIST_REG_START_SHIFT			16
+#define ISPHIST_REG_END_SHIFT			0
+#define ISPHIST_REG_START_MASK			(ISPHIST_REG_START_END_MASK << \
+						 ISPHIST_REG_START_SHIFT)
+#define ISPHIST_REG_END_MASK			(ISPHIST_REG_START_END_MASK << \
+						 ISPHIST_REG_END_SHIFT)
+
+#define ISPHIST_REG_MASK			(ISPHIST_REG_START_MASK | \
+						 ISPHIST_REG_END_MASK)
+
+#define ISPHIST_ADDR_SHIFT			0
+#define ISPHIST_ADDR_MASK			0x3FF
+
+#define ISPHIST_DATA_SHIFT			0
+#define ISPHIST_DATA_MASK			0xFFFFF
+
+#define ISPHIST_RADD_SHIFT			0
+#define ISPHIST_RADD_MASK			0xFFFFFFFF
+
+#define ISPHIST_RADD_OFF_SHIFT			0
+#define ISPHIST_RADD_OFF_MASK			0xFFFF
+
+#define ISPHIST_HV_INFO_HSIZE_SHIFT		16
+#define ISPHIST_HV_INFO_HSIZE_MASK		0x3FFF0000
+#define ISPHIST_HV_INFO_VSIZE_SHIFT		0
+#define ISPHIST_HV_INFO_VSIZE_MASK		0x3FFF
+
+#define ISPHIST_HV_INFO_MASK			0x3FFF3FFF
+
+#define ISPCCDC_LSC_ENABLE			1
+#define ISPCCDC_LSC_BUSY			(1 << 7)
+#define ISPCCDC_LSC_GAIN_MODE_N_MASK		0x700
+#define ISPCCDC_LSC_GAIN_MODE_N_SHIFT		8
+#define ISPCCDC_LSC_GAIN_MODE_M_MASK		0x3800
+#define ISPCCDC_LSC_GAIN_MODE_M_SHIFT		12
+#define ISPCCDC_LSC_GAIN_FORMAT_MASK		0xE
+#define ISPCCDC_LSC_GAIN_FORMAT_SHIFT		1
+#define ISPCCDC_LSC_AFTER_REFORMATTER_MASK	(1<<6)
+
+#define ISPCCDC_LSC_INITIAL_X_MASK		0x3F
+#define ISPCCDC_LSC_INITIAL_X_SHIFT		0
+#define ISPCCDC_LSC_INITIAL_Y_MASK		0x3F0000
+#define ISPCCDC_LSC_INITIAL_Y_SHIFT		16
+
+/* -----------------------------------------------------------------------------
+ * CSI2 receiver registers (ES2.0)
+ */
+
+#define ISPCSI2_REVISION			(0x000)
+#define ISPCSI2_SYSCONFIG			(0x010)
+#define ISPCSI2_SYSCONFIG_MSTANDBY_MODE_SHIFT	12
+#define ISPCSI2_SYSCONFIG_MSTANDBY_MODE_MASK	\
+	(0x3 << ISPCSI2_SYSCONFIG_MSTANDBY_MODE_SHIFT)
+#define ISPCSI2_SYSCONFIG_MSTANDBY_MODE_FORCE	\
+	(0x0 << ISPCSI2_SYSCONFIG_MSTANDBY_MODE_SHIFT)
+#define ISPCSI2_SYSCONFIG_MSTANDBY_MODE_NO	\
+	(0x1 << ISPCSI2_SYSCONFIG_MSTANDBY_MODE_SHIFT)
+#define ISPCSI2_SYSCONFIG_MSTANDBY_MODE_SMART	\
+	(0x2 << ISPCSI2_SYSCONFIG_MSTANDBY_MODE_SHIFT)
+#define ISPCSI2_SYSCONFIG_SOFT_RESET		(1 << 1)
+#define ISPCSI2_SYSCONFIG_AUTO_IDLE		(1 << 0)
+
+#define ISPCSI2_SYSSTATUS			(0x014)
+#define ISPCSI2_SYSSTATUS_RESET_DONE		(1 << 0)
+
+#define ISPCSI2_IRQSTATUS			(0x018)
+#define ISPCSI2_IRQSTATUS_OCP_ERR_IRQ		(1 << 14)
+#define ISPCSI2_IRQSTATUS_SHORT_PACKET_IRQ	(1 << 13)
+#define ISPCSI2_IRQSTATUS_ECC_CORRECTION_IRQ	(1 << 12)
+#define ISPCSI2_IRQSTATUS_ECC_NO_CORRECTION_IRQ	(1 << 11)
+#define ISPCSI2_IRQSTATUS_COMPLEXIO2_ERR_IRQ	(1 << 10)
+#define ISPCSI2_IRQSTATUS_COMPLEXIO1_ERR_IRQ	(1 << 9)
+#define ISPCSI2_IRQSTATUS_FIFO_OVF_IRQ		(1 << 8)
+#define ISPCSI2_IRQSTATUS_CONTEXT(n)		(1 << (n))
+
+#define ISPCSI2_IRQENABLE			(0x01c)
+#define ISPCSI2_CTRL				(0x040)
+#define ISPCSI2_CTRL_VP_CLK_EN			(1 << 15)
+#define ISPCSI2_CTRL_VP_ONLY_EN			(1 << 11)
+#define ISPCSI2_CTRL_VP_OUT_CTRL_SHIFT		8
+#define ISPCSI2_CTRL_VP_OUT_CTRL_MASK		\
+	(3 << ISPCSI2_CTRL_VP_OUT_CTRL_SHIFT)
+#define ISPCSI2_CTRL_DBG_EN			(1 << 7)
+#define ISPCSI2_CTRL_BURST_SIZE_SHIFT		5
+#define ISPCSI2_CTRL_BURST_SIZE_MASK		\
+	(3 << ISPCSI2_CTRL_BURST_SIZE_SHIFT)
+#define ISPCSI2_CTRL_FRAME			(1 << 3)
+#define ISPCSI2_CTRL_ECC_EN			(1 << 2)
+#define ISPCSI2_CTRL_SECURE			(1 << 1)
+#define ISPCSI2_CTRL_IF_EN			(1 << 0)
+
+#define ISPCSI2_DBG_H				(0x044)
+#define ISPCSI2_GNQ				(0x048)
+#define ISPCSI2_PHY_CFG				(0x050)
+#define ISPCSI2_PHY_CFG_RESET_CTRL		(1 << 30)
+#define ISPCSI2_PHY_CFG_RESET_DONE		(1 << 29)
+#define ISPCSI2_PHY_CFG_PWR_CMD_SHIFT		27
+#define ISPCSI2_PHY_CFG_PWR_CMD_MASK		\
+	(0x3 << ISPCSI2_PHY_CFG_PWR_CMD_SHIFT)
+#define ISPCSI2_PHY_CFG_PWR_CMD_OFF		\
+	(0x0 << ISPCSI2_PHY_CFG_PWR_CMD_SHIFT)
+#define ISPCSI2_PHY_CFG_PWR_CMD_ON		\
+	(0x1 << ISPCSI2_PHY_CFG_PWR_CMD_SHIFT)
+#define ISPCSI2_PHY_CFG_PWR_CMD_ULPW		\
+	(0x2 << ISPCSI2_PHY_CFG_PWR_CMD_SHIFT)
+#define ISPCSI2_PHY_CFG_PWR_STATUS_SHIFT	25
+#define ISPCSI2_PHY_CFG_PWR_STATUS_MASK		\
+	(0x3 << ISPCSI2_PHY_CFG_PWR_STATUS_SHIFT)
+#define ISPCSI2_PHY_CFG_PWR_STATUS_OFF		\
+	(0x0 << ISPCSI2_PHY_CFG_PWR_STATUS_SHIFT)
+#define ISPCSI2_PHY_CFG_PWR_STATUS_ON		\
+	(0x1 << ISPCSI2_PHY_CFG_PWR_STATUS_SHIFT)
+#define ISPCSI2_PHY_CFG_PWR_STATUS_ULPW		\
+	(0x2 << ISPCSI2_PHY_CFG_PWR_STATUS_SHIFT)
+#define ISPCSI2_PHY_CFG_PWR_AUTO		(1 << 24)
+
+#define ISPCSI2_PHY_CFG_DATA_POL_SHIFT(n)	(3 + ((n) * 4))
+#define ISPCSI2_PHY_CFG_DATA_POL_MASK(n)	\
+	(0x1 << ISPCSI2_PHY_CFG_DATA_POL_SHIFT(n))
+#define ISPCSI2_PHY_CFG_DATA_POL_PN(n)		\
+	(0x0 << ISPCSI2_PHY_CFG_DATA_POL_SHIFT(n))
+#define ISPCSI2_PHY_CFG_DATA_POL_NP(n)		\
+	(0x1 << ISPCSI2_PHY_CFG_DATA_POL_SHIFT(n))
+
+#define ISPCSI2_PHY_CFG_DATA_POSITION_SHIFT(n)	((n) * 4)
+#define ISPCSI2_PHY_CFG_DATA_POSITION_MASK(n)	\
+	(0x7 << ISPCSI2_PHY_CFG_DATA_POSITION_SHIFT(n))
+#define ISPCSI2_PHY_CFG_DATA_POSITION_NC(n)	\
+	(0x0 << ISPCSI2_PHY_CFG_DATA_POSITION_SHIFT(n))
+#define ISPCSI2_PHY_CFG_DATA_POSITION_1(n)	\
+	(0x1 << ISPCSI2_PHY_CFG_DATA_POSITION_SHIFT(n))
+#define ISPCSI2_PHY_CFG_DATA_POSITION_2(n)	\
+	(0x2 << ISPCSI2_PHY_CFG_DATA_POSITION_SHIFT(n))
+#define ISPCSI2_PHY_CFG_DATA_POSITION_3(n)	\
+	(0x3 << ISPCSI2_PHY_CFG_DATA_POSITION_SHIFT(n))
+#define ISPCSI2_PHY_CFG_DATA_POSITION_4(n)	\
+	(0x4 << ISPCSI2_PHY_CFG_DATA_POSITION_SHIFT(n))
+#define ISPCSI2_PHY_CFG_DATA_POSITION_5(n)	\
+	(0x5 << ISPCSI2_PHY_CFG_DATA_POSITION_SHIFT(n))
+
+#define ISPCSI2_PHY_CFG_CLOCK_POL_SHIFT		3
+#define ISPCSI2_PHY_CFG_CLOCK_POL_MASK		\
+	(0x1 << ISPCSI2_PHY_CFG_CLOCK_POL_SHIFT)
+#define ISPCSI2_PHY_CFG_CLOCK_POL_PN		\
+	(0x0 << ISPCSI2_PHY_CFG_CLOCK_POL_SHIFT)
+#define ISPCSI2_PHY_CFG_CLOCK_POL_NP		\
+	(0x1 << ISPCSI2_PHY_CFG_CLOCK_POL_SHIFT)
+
+#define ISPCSI2_PHY_CFG_CLOCK_POSITION_SHIFT	0
+#define ISPCSI2_PHY_CFG_CLOCK_POSITION_MASK	\
+	(0x7 << ISPCSI2_PHY_CFG_CLOCK_POSITION_SHIFT)
+#define ISPCSI2_PHY_CFG_CLOCK_POSITION_1	\
+	(0x1 << ISPCSI2_PHY_CFG_CLOCK_POSITION_SHIFT)
+#define ISPCSI2_PHY_CFG_CLOCK_POSITION_2	\
+	(0x2 << ISPCSI2_PHY_CFG_CLOCK_POSITION_SHIFT)
+#define ISPCSI2_PHY_CFG_CLOCK_POSITION_3	\
+	(0x3 << ISPCSI2_PHY_CFG_CLOCK_POSITION_SHIFT)
+#define ISPCSI2_PHY_CFG_CLOCK_POSITION_4	\
+	(0x4 << ISPCSI2_PHY_CFG_CLOCK_POSITION_SHIFT)
+#define ISPCSI2_PHY_CFG_CLOCK_POSITION_5	\
+	(0x5 << ISPCSI2_PHY_CFG_CLOCK_POSITION_SHIFT)
+
+#define ISPCSI2_PHY_IRQSTATUS			(0x054)
+#define ISPCSI2_PHY_IRQSTATUS_STATEALLULPMEXIT	(1 << 26)
+#define ISPCSI2_PHY_IRQSTATUS_STATEALLULPMENTER	(1 << 25)
+#define ISPCSI2_PHY_IRQSTATUS_STATEULPM5	(1 << 24)
+#define ISPCSI2_PHY_IRQSTATUS_STATEULPM4	(1 << 23)
+#define ISPCSI2_PHY_IRQSTATUS_STATEULPM3	(1 << 22)
+#define ISPCSI2_PHY_IRQSTATUS_STATEULPM2	(1 << 21)
+#define ISPCSI2_PHY_IRQSTATUS_STATEULPM1	(1 << 20)
+#define ISPCSI2_PHY_IRQSTATUS_ERRCONTROL5	(1 << 19)
+#define ISPCSI2_PHY_IRQSTATUS_ERRCONTROL4	(1 << 18)
+#define ISPCSI2_PHY_IRQSTATUS_ERRCONTROL3	(1 << 17)
+#define ISPCSI2_PHY_IRQSTATUS_ERRCONTROL2	(1 << 16)
+#define ISPCSI2_PHY_IRQSTATUS_ERRCONTROL1	(1 << 15)
+#define ISPCSI2_PHY_IRQSTATUS_ERRESC5		(1 << 14)
+#define ISPCSI2_PHY_IRQSTATUS_ERRESC4		(1 << 13)
+#define ISPCSI2_PHY_IRQSTATUS_ERRESC3		(1 << 12)
+#define ISPCSI2_PHY_IRQSTATUS_ERRESC2		(1 << 11)
+#define ISPCSI2_PHY_IRQSTATUS_ERRESC1		(1 << 10)
+#define ISPCSI2_PHY_IRQSTATUS_ERRSOTSYNCHS5	(1 << 9)
+#define ISPCSI2_PHY_IRQSTATUS_ERRSOTSYNCHS4	(1 << 8)
+#define ISPCSI2_PHY_IRQSTATUS_ERRSOTSYNCHS3	(1 << 7)
+#define ISPCSI2_PHY_IRQSTATUS_ERRSOTSYNCHS2	(1 << 6)
+#define ISPCSI2_PHY_IRQSTATUS_ERRSOTSYNCHS1	(1 << 5)
+#define ISPCSI2_PHY_IRQSTATUS_ERRSOTHS5		(1 << 4)
+#define ISPCSI2_PHY_IRQSTATUS_ERRSOTHS4		(1 << 3)
+#define ISPCSI2_PHY_IRQSTATUS_ERRSOTHS3		(1 << 2)
+#define ISPCSI2_PHY_IRQSTATUS_ERRSOTHS2		(1 << 1)
+#define ISPCSI2_PHY_IRQSTATUS_ERRSOTHS1		1
+
+#define ISPCSI2_SHORT_PACKET			(0x05c)
+#define ISPCSI2_PHY_IRQENABLE			(0x060)
+#define ISPCSI2_PHY_IRQENABLE_STATEALLULPMEXIT	(1 << 26)
+#define ISPCSI2_PHY_IRQENABLE_STATEALLULPMENTER	(1 << 25)
+#define ISPCSI2_PHY_IRQENABLE_STATEULPM5	(1 << 24)
+#define ISPCSI2_PHY_IRQENABLE_STATEULPM4	(1 << 23)
+#define ISPCSI2_PHY_IRQENABLE_STATEULPM3	(1 << 22)
+#define ISPCSI2_PHY_IRQENABLE_STATEULPM2	(1 << 21)
+#define ISPCSI2_PHY_IRQENABLE_STATEULPM1	(1 << 20)
+#define ISPCSI2_PHY_IRQENABLE_ERRCONTROL5	(1 << 19)
+#define ISPCSI2_PHY_IRQENABLE_ERRCONTROL4	(1 << 18)
+#define ISPCSI2_PHY_IRQENABLE_ERRCONTROL3	(1 << 17)
+#define ISPCSI2_PHY_IRQENABLE_ERRCONTROL2	(1 << 16)
+#define ISPCSI2_PHY_IRQENABLE_ERRCONTROL1	(1 << 15)
+#define ISPCSI2_PHY_IRQENABLE_ERRESC5		(1 << 14)
+#define ISPCSI2_PHY_IRQENABLE_ERRESC4		(1 << 13)
+#define ISPCSI2_PHY_IRQENABLE_ERRESC3		(1 << 12)
+#define ISPCSI2_PHY_IRQENABLE_ERRESC2		(1 << 11)
+#define ISPCSI2_PHY_IRQENABLE_ERRESC1		(1 << 10)
+#define ISPCSI2_PHY_IRQENABLE_ERRSOTSYNCHS5	(1 << 9)
+#define ISPCSI2_PHY_IRQENABLE_ERRSOTSYNCHS4	(1 << 8)
+#define ISPCSI2_PHY_IRQENABLE_ERRSOTSYNCHS3	(1 << 7)
+#define ISPCSI2_PHY_IRQENABLE_ERRSOTSYNCHS2	(1 << 6)
+#define ISPCSI2_PHY_IRQENABLE_ERRSOTSYNCHS1	(1 << 5)
+#define ISPCSI2_PHY_IRQENABLE_ERRSOTHS5		(1 << 4)
+#define ISPCSI2_PHY_IRQENABLE_ERRSOTHS4		(1 << 3)
+#define ISPCSI2_PHY_IRQENABLE_ERRSOTHS3		(1 << 2)
+#define ISPCSI2_PHY_IRQENABLE_ERRSOTHS2		(1 << 1)
+#define ISPCSI2_PHY_IRQENABLE_ERRSOTHS1		(1 << 0)
+
+#define ISPCSI2_DBG_P				(0x068)
+#define ISPCSI2_TIMING				(0x06c)
+#define ISPCSI2_TIMING_FORCE_RX_MODE_IO(n)	(1 << ((16 * ((n) - 1)) + 15))
+#define ISPCSI2_TIMING_STOP_STATE_X16_IO(n)	(1 << ((16 * ((n) - 1)) + 14))
+#define ISPCSI2_TIMING_STOP_STATE_X4_IO(n)	(1 << ((16 * ((n) - 1)) + 13))
+#define ISPCSI2_TIMING_STOP_STATE_COUNTER_IO_SHIFT(n)	(16 * ((n) - 1))
+#define ISPCSI2_TIMING_STOP_STATE_COUNTER_IO_MASK(n)	\
+	(0x1fff << ISPCSI2_TIMING_STOP_STATE_COUNTER_IO_SHIFT(n))
+
+#define ISPCSI2_CTX_CTRL1(n)			((0x070) + 0x20 * (n))
+#define ISPCSI2_CTX_CTRL1_COUNT_SHIFT		8
+#define ISPCSI2_CTX_CTRL1_COUNT_MASK		\
+	(0xff << ISPCSI2_CTX_CTRL1_COUNT_SHIFT)
+#define ISPCSI2_CTX_CTRL1_EOF_EN		(1 << 7)
+#define ISPCSI2_CTX_CTRL1_EOL_EN		(1 << 6)
+#define ISPCSI2_CTX_CTRL1_CS_EN			(1 << 5)
+#define ISPCSI2_CTX_CTRL1_COUNT_UNLOCK		(1 << 4)
+#define ISPCSI2_CTX_CTRL1_PING_PONG		(1 << 3)
+#define ISPCSI2_CTX_CTRL1_CTX_EN		(1 << 0)
+
+#define ISPCSI2_CTX_CTRL2(n)			((0x074) + 0x20 * (n))
+#define ISPCSI2_CTX_CTRL2_USER_DEF_MAP_SHIFT	13
+#define ISPCSI2_CTX_CTRL2_USER_DEF_MAP_MASK	\
+	(0x3 << ISPCSI2_CTX_CTRL2_USER_DEF_MAP_SHIFT)
+#define ISPCSI2_CTX_CTRL2_VIRTUAL_ID_SHIFT	11
+#define ISPCSI2_CTX_CTRL2_VIRTUAL_ID_MASK	\
+	(0x3 <<	ISPCSI2_CTX_CTRL2_VIRTUAL_ID_SHIFT)
+#define ISPCSI2_CTX_CTRL2_DPCM_PRED		(1 << 10)
+#define ISPCSI2_CTX_CTRL2_FORMAT_SHIFT		0
+#define ISPCSI2_CTX_CTRL2_FORMAT_MASK		\
+	(0x3ff << ISPCSI2_CTX_CTRL2_FORMAT_SHIFT)
+#define ISPCSI2_CTX_CTRL2_FRAME_SHIFT		16
+#define ISPCSI2_CTX_CTRL2_FRAME_MASK		\
+	(0xffff << ISPCSI2_CTX_CTRL2_FRAME_SHIFT)
+
+#define ISPCSI2_CTX_DAT_OFST(n)			((0x078) + 0x20 * (n))
+#define ISPCSI2_CTX_DAT_OFST_OFST_SHIFT		0
+#define ISPCSI2_CTX_DAT_OFST_OFST_MASK		\
+	(0x1ffe0 << ISPCSI2_CTX_DAT_OFST_OFST_SHIFT)
+
+#define ISPCSI2_CTX_DAT_PING_ADDR(n)		((0x07c) + 0x20 * (n))
+#define ISPCSI2_CTX_DAT_PONG_ADDR(n)		((0x080) + 0x20 * (n))
+#define ISPCSI2_CTX_IRQENABLE(n)		((0x084) + 0x20 * (n))
+#define ISPCSI2_CTX_IRQENABLE_ECC_CORRECTION_IRQ	(1 << 8)
+#define ISPCSI2_CTX_IRQENABLE_LINE_NUMBER_IRQ	(1 << 7)
+#define ISPCSI2_CTX_IRQENABLE_FRAME_NUMBER_IRQ	(1 << 6)
+#define ISPCSI2_CTX_IRQENABLE_CS_IRQ		(1 << 5)
+#define ISPCSI2_CTX_IRQENABLE_LE_IRQ		(1 << 3)
+#define ISPCSI2_CTX_IRQENABLE_LS_IRQ		(1 << 2)
+#define ISPCSI2_CTX_IRQENABLE_FE_IRQ		(1 << 1)
+#define ISPCSI2_CTX_IRQENABLE_FS_IRQ		(1 << 0)
+
+#define ISPCSI2_CTX_IRQSTATUS(n)		((0x088) + 0x20 * (n))
+#define ISPCSI2_CTX_IRQSTATUS_ECC_CORRECTION_IRQ	(1 << 8)
+#define ISPCSI2_CTX_IRQSTATUS_LINE_NUMBER_IRQ	(1 << 7)
+#define ISPCSI2_CTX_IRQSTATUS_FRAME_NUMBER_IRQ	(1 << 6)
+#define ISPCSI2_CTX_IRQSTATUS_CS_IRQ		(1 << 5)
+#define ISPCSI2_CTX_IRQSTATUS_LE_IRQ		(1 << 3)
+#define ISPCSI2_CTX_IRQSTATUS_LS_IRQ		(1 << 2)
+#define ISPCSI2_CTX_IRQSTATUS_FE_IRQ		(1 << 1)
+#define ISPCSI2_CTX_IRQSTATUS_FS_IRQ		(1 << 0)
+
+#define ISPCSI2_CTX_CTRL3(n)			((0x08c) + 0x20 * (n))
+#define ISPCSI2_CTX_CTRL3_ALPHA_SHIFT		5
+#define ISPCSI2_CTX_CTRL3_ALPHA_MASK		\
+	(0x3fff << ISPCSI2_CTX_CTRL3_ALPHA_SHIFT)
+
+/* This instance is for OMAP3630 only */
+#define ISPCSI2_CTX_TRANSCODEH(n)		(0x000 + 0x8 * (n))
+#define ISPCSI2_CTX_TRANSCODEH_HCOUNT_SHIFT	16
+#define ISPCSI2_CTX_TRANSCODEH_HCOUNT_MASK	\
+	(0x1fff << ISPCSI2_CTX_TRANSCODEH_HCOUNT_SHIFT)
+#define ISPCSI2_CTX_TRANSCODEH_HSKIP_SHIFT	0
+#define ISPCSI2_CTX_TRANSCODEH_HSKIP_MASK	\
+	(0x1fff << ISPCSI2_CTX_TRANSCODEH_HCOUNT_SHIFT)
+#define ISPCSI2_CTX_TRANSCODEV(n)		(0x004 + 0x8 * (n))
+#define ISPCSI2_CTX_TRANSCODEV_VCOUNT_SHIFT	16
+#define ISPCSI2_CTX_TRANSCODEV_VCOUNT_MASK	\
+	(0x1fff << ISPCSI2_CTX_TRANSCODEV_VCOUNT_SHIFT)
+#define ISPCSI2_CTX_TRANSCODEV_VSKIP_SHIFT	0
+#define ISPCSI2_CTX_TRANSCODEV_VSKIP_MASK	\
+	(0x1fff << ISPCSI2_CTX_TRANSCODEV_VCOUNT_SHIFT)
+
+/* -----------------------------------------------------------------------------
+ * CSI PHY registers
+ */
+
+#define ISPCSIPHY_REG0				(0x000)
+#define ISPCSIPHY_REG0_THS_TERM_SHIFT		8
+#define ISPCSIPHY_REG0_THS_TERM_MASK		\
+	(0xff << ISPCSIPHY_REG0_THS_TERM_SHIFT)
+#define ISPCSIPHY_REG0_THS_SETTLE_SHIFT		0
+#define ISPCSIPHY_REG0_THS_SETTLE_MASK		\
+	(0xff << ISPCSIPHY_REG0_THS_SETTLE_SHIFT)
+
+#define ISPCSIPHY_REG1					(0x004)
+#define ISPCSIPHY_REG1_RESET_DONE_CTRLCLK		(1 << 29)
+/* This field is for OMAP3630 only */
+#define ISPCSIPHY_REG1_CLOCK_MISS_DETECTOR_STATUS	(1 << 25)
+#define ISPCSIPHY_REG1_TCLK_TERM_SHIFT			18
+#define ISPCSIPHY_REG1_TCLK_TERM_MASK			\
+	(0x7f << ISPCSIPHY_REG1_TCLK_TERM_SHIFT)
+#define ISPCSIPHY_REG1_DPHY_HS_SYNC_PATTERN_SHIFT	10
+#define ISPCSIPHY_REG1_DPHY_HS_SYNC_PATTERN_MASK	\
+	(0xff << ISPCSIPHY_REG1_DPHY_HS_SYNC_PATTERN)
+/* This field is for OMAP3430 only */
+#define ISPCSIPHY_REG1_TCLK_MISS_SHIFT			8
+#define ISPCSIPHY_REG1_TCLK_MISS_MASK			\
+	(0x3 << ISPCSIPHY_REG1_TCLK_MISS_SHIFT)
+/* This field is for OMAP3630 only */
+#define ISPCSIPHY_REG1_CTRLCLK_DIV_FACTOR_SHIFT		8
+#define ISPCSIPHY_REG1_CTRLCLK_DIV_FACTOR_MASK		\
+	(0x3 << ISPCSIPHY_REG1_CTRLCLK_DIV_FACTOR_SHIFT)
+#define ISPCSIPHY_REG1_TCLK_SETTLE_SHIFT		0
+#define ISPCSIPHY_REG1_TCLK_SETTLE_MASK			\
+	(0xff << ISPCSIPHY_REG1_TCLK_SETTLE_SHIFT)
+
+/* This register is for OMAP3630 only */
+#define ISPCSIPHY_REG2					(0x008)
+#define ISPCSIPHY_REG2_TRIGGER_CMD_RXTRIGESC0_SHIFT	30
+#define ISPCSIPHY_REG2_TRIGGER_CMD_RXTRIGESC0_MASK	\
+	(0x3 << ISPCSIPHY_REG2_TRIGGER_CMD_RXTRIGESC0_SHIFT)
+#define ISPCSIPHY_REG2_TRIGGER_CMD_RXTRIGESC1_SHIFT	28
+#define ISPCSIPHY_REG2_TRIGGER_CMD_RXTRIGESC1_MASK	\
+	(0x3 << ISPCSIPHY_REG2_TRIGGER_CMD_RXTRIGESC1_SHIFT)
+#define ISPCSIPHY_REG2_TRIGGER_CMD_RXTRIGESC2_SHIFT	26
+#define ISPCSIPHY_REG2_TRIGGER_CMD_RXTRIGESC2_MASK	\
+	(0x3 << ISPCSIPHY_REG2_TRIGGER_CMD_RXTRIGESC2_SHIFT)
+#define ISPCSIPHY_REG2_TRIGGER_CMD_RXTRIGESC3_SHIFT	24
+#define ISPCSIPHY_REG2_TRIGGER_CMD_RXTRIGESC3_MASK	\
+	(0x3 << ISPCSIPHY_REG2_TRIGGER_CMD_RXTRIGESC3_SHIFT)
+#define ISPCSIPHY_REG2_CCP2_SYNC_PATTERN_SHIFT		0
+#define ISPCSIPHY_REG2_CCP2_SYNC_PATTERN_MASK		\
+	(0x7fffff << ISPCSIPHY_REG2_CCP2_SYNC_PATTERN_SHIFT)
+
+/* -----------------------------------------------------------------------------
+ * CONTROL registers for CSI-2 phy routing
+ */
+
+/* OMAP343X_CONTROL_CSIRXFE */
+#define OMAP343X_CONTROL_CSIRXFE_CSIB_INV	(1 << 7)
+#define OMAP343X_CONTROL_CSIRXFE_RESENABLE	(1 << 8)
+#define OMAP343X_CONTROL_CSIRXFE_SELFORM	(1 << 10)
+#define OMAP343X_CONTROL_CSIRXFE_PWRDNZ		(1 << 12)
+#define OMAP343X_CONTROL_CSIRXFE_RESET		(1 << 13)
+
+/* OMAP3630_CONTROL_CAMERA_PHY_CTRL */
+#define OMAP3630_CONTROL_CAMERA_PHY_CTRL_CAMMODE_PHY1_SHIFT	2
+#define OMAP3630_CONTROL_CAMERA_PHY_CTRL_CAMMODE_PHY2_SHIFT	0
+#define OMAP3630_CONTROL_CAMERA_PHY_CTRL_CAMMODE_DPHY		0x0
+#define OMAP3630_CONTROL_CAMERA_PHY_CTRL_CAMMODE_CCP2_DATA_STROBE 0x1
+#define OMAP3630_CONTROL_CAMERA_PHY_CTRL_CAMMODE_CCP2_DATA_CLOCK 0x2
+#define OMAP3630_CONTROL_CAMERA_PHY_CTRL_CAMMODE_GPI		0x3
+#define OMAP3630_CONTROL_CAMERA_PHY_CTRL_CAMMODE_MASK		0x3
+/* CCP2B: set to receive data from PHY2 instead of PHY1 */
+#define OMAP3630_CONTROL_CAMERA_PHY_CTRL_CSI1_RX_SEL_PHY2	(1 << 4)
+
+#endif	/* OMAP3_ISP_REG_H */
diff --git a/drivers/media/platform/omap3isp/ispresizer.c b/drivers/media/platform/omap3isp/ispresizer.c
new file mode 100644
index 0000000..7cfb43d
--- /dev/null
+++ b/drivers/media/platform/omap3isp/ispresizer.c
@@ -0,0 +1,1803 @@
+/*
+ * ispresizer.c
+ *
+ * TI OMAP3 ISP - Resizer module
+ *
+ * Copyright (C) 2010 Nokia Corporation
+ * Copyright (C) 2009 Texas Instruments, Inc
+ *
+ * Contacts: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *	     Sakari Ailus <sakari.ailus@iki.fi>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/device.h>
+#include <linux/mm.h>
+#include <linux/module.h>
+
+#include "isp.h"
+#include "ispreg.h"
+#include "ispresizer.h"
+
+/*
+ * Resizer Constants
+ */
+#define MIN_RESIZE_VALUE		64
+#define MID_RESIZE_VALUE		512
+#define MAX_RESIZE_VALUE		1024
+
+#define MIN_IN_WIDTH			32
+#define MIN_IN_HEIGHT			32
+#define MAX_IN_WIDTH_MEMORY_MODE	4095
+#define MAX_IN_WIDTH_ONTHEFLY_MODE_ES1	1280
+#define MAX_IN_WIDTH_ONTHEFLY_MODE_ES2	4095
+#define MAX_IN_HEIGHT			4095
+
+#define MIN_OUT_WIDTH			16
+#define MIN_OUT_HEIGHT			2
+#define MAX_OUT_HEIGHT			4095
+
+/*
+ * Resizer Use Constraints
+ * "TRM ES3.1, table 12-46"
+ */
+#define MAX_4TAP_OUT_WIDTH_ES1		1280
+#define MAX_7TAP_OUT_WIDTH_ES1		640
+#define MAX_4TAP_OUT_WIDTH_ES2		3312
+#define MAX_7TAP_OUT_WIDTH_ES2		1650
+#define MAX_4TAP_OUT_WIDTH_3630		4096
+#define MAX_7TAP_OUT_WIDTH_3630		2048
+
+/*
+ * Constants for ratio calculation
+ */
+#define RESIZE_DIVISOR			256
+#define DEFAULT_PHASE			1
+
+/*
+ * Default (and only) configuration of filter coefficients.
+ * 7-tap mode is for scale factors 0.25x to 0.5x.
+ * 4-tap mode is for scale factors 0.5x to 4.0x.
+ * There shouldn't be any reason to recalculate these, EVER.
+ */
+static const struct isprsz_coef filter_coefs = {
+	/* For 8-phase 4-tap horizontal filter: */
+	{
+		0x0000, 0x0100, 0x0000, 0x0000,
+		0x03FA, 0x00F6, 0x0010, 0x0000,
+		0x03F9, 0x00DB, 0x002C, 0x0000,
+		0x03FB, 0x00B3, 0x0053, 0x03FF,
+		0x03FD, 0x0082, 0x0084, 0x03FD,
+		0x03FF, 0x0053, 0x00B3, 0x03FB,
+		0x0000, 0x002C, 0x00DB, 0x03F9,
+		0x0000, 0x0010, 0x00F6, 0x03FA
+	},
+	/* For 8-phase 4-tap vertical filter: */
+	{
+		0x0000, 0x0100, 0x0000, 0x0000,
+		0x03FA, 0x00F6, 0x0010, 0x0000,
+		0x03F9, 0x00DB, 0x002C, 0x0000,
+		0x03FB, 0x00B3, 0x0053, 0x03FF,
+		0x03FD, 0x0082, 0x0084, 0x03FD,
+		0x03FF, 0x0053, 0x00B3, 0x03FB,
+		0x0000, 0x002C, 0x00DB, 0x03F9,
+		0x0000, 0x0010, 0x00F6, 0x03FA
+	},
+	/* For 4-phase 7-tap horizontal filter: */
+	#define DUMMY 0
+	{
+		0x0004, 0x0023, 0x005A, 0x0058, 0x0023, 0x0004, 0x0000, DUMMY,
+		0x0002, 0x0018, 0x004d, 0x0060, 0x0031, 0x0008, 0x0000, DUMMY,
+		0x0001, 0x000f, 0x003f, 0x0062, 0x003f, 0x000f, 0x0001, DUMMY,
+		0x0000, 0x0008, 0x0031, 0x0060, 0x004d, 0x0018, 0x0002, DUMMY
+	},
+	/* For 4-phase 7-tap vertical filter: */
+	{
+		0x0004, 0x0023, 0x005A, 0x0058, 0x0023, 0x0004, 0x0000, DUMMY,
+		0x0002, 0x0018, 0x004d, 0x0060, 0x0031, 0x0008, 0x0000, DUMMY,
+		0x0001, 0x000f, 0x003f, 0x0062, 0x003f, 0x000f, 0x0001, DUMMY,
+		0x0000, 0x0008, 0x0031, 0x0060, 0x004d, 0x0018, 0x0002, DUMMY
+	}
+	/*
+	 * The dummy padding is required in 7-tap mode because of how the
+	 * registers are arranged physically.
+	 */
+	#undef DUMMY
+};
+
+/*
+ * __resizer_get_format - helper function for getting resizer format
+ * @res   : pointer to resizer private structure
+ * @pad   : pad number
+ * @cfg: V4L2 subdev pad configuration
+ * @which : wanted subdev format
+ * return zero
+ */
+static struct v4l2_mbus_framefmt *
+__resizer_get_format(struct isp_res_device *res, struct v4l2_subdev_pad_config *cfg,
+		     unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(&res->subdev, cfg, pad);
+	else
+		return &res->formats[pad];
+}
+
+/*
+ * __resizer_get_crop - helper function for getting resizer crop rectangle
+ * @res   : pointer to resizer private structure
+ * @cfg: V4L2 subdev pad configuration
+ * @which : wanted subdev crop rectangle
+ */
+static struct v4l2_rect *
+__resizer_get_crop(struct isp_res_device *res, struct v4l2_subdev_pad_config *cfg,
+		   enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_crop(&res->subdev, cfg, RESZ_PAD_SINK);
+	else
+		return &res->crop.request;
+}
+
+/*
+ * resizer_set_filters - Set resizer filters
+ * @res: Device context.
+ * @h_coeff: horizontal coefficient
+ * @v_coeff: vertical coefficient
+ * Return none
+ */
+static void resizer_set_filters(struct isp_res_device *res, const u16 *h_coeff,
+				const u16 *v_coeff)
+{
+	struct isp_device *isp = to_isp_device(res);
+	u32 startaddr_h, startaddr_v, tmp_h, tmp_v;
+	int i;
+
+	startaddr_h = ISPRSZ_HFILT10;
+	startaddr_v = ISPRSZ_VFILT10;
+
+	for (i = 0; i < COEFF_CNT; i += 2) {
+		tmp_h = h_coeff[i] |
+			(h_coeff[i + 1] << ISPRSZ_HFILT_COEF1_SHIFT);
+		tmp_v = v_coeff[i] |
+			(v_coeff[i + 1] << ISPRSZ_VFILT_COEF1_SHIFT);
+		isp_reg_writel(isp, tmp_h, OMAP3_ISP_IOMEM_RESZ, startaddr_h);
+		isp_reg_writel(isp, tmp_v, OMAP3_ISP_IOMEM_RESZ, startaddr_v);
+		startaddr_h += 4;
+		startaddr_v += 4;
+	}
+}
+
+/*
+ * resizer_set_bilinear - Chrominance horizontal algorithm select
+ * @res: Device context.
+ * @type: Filtering interpolation type.
+ *
+ * Filtering that is same as luminance processing is
+ * intended only for downsampling, and bilinear interpolation
+ * is intended only for upsampling.
+ */
+static void resizer_set_bilinear(struct isp_res_device *res,
+				 enum resizer_chroma_algo type)
+{
+	struct isp_device *isp = to_isp_device(res);
+
+	if (type == RSZ_BILINEAR)
+		isp_reg_set(isp, OMAP3_ISP_IOMEM_RESZ, ISPRSZ_CNT,
+			    ISPRSZ_CNT_CBILIN);
+	else
+		isp_reg_clr(isp, OMAP3_ISP_IOMEM_RESZ, ISPRSZ_CNT,
+			    ISPRSZ_CNT_CBILIN);
+}
+
+/*
+ * resizer_set_ycpos - Luminance and chrominance order
+ * @res: Device context.
+ * @pixelcode: pixel code.
+ */
+static void resizer_set_ycpos(struct isp_res_device *res, u32 pixelcode)
+{
+	struct isp_device *isp = to_isp_device(res);
+
+	switch (pixelcode) {
+	case MEDIA_BUS_FMT_YUYV8_1X16:
+		isp_reg_set(isp, OMAP3_ISP_IOMEM_RESZ, ISPRSZ_CNT,
+			    ISPRSZ_CNT_YCPOS);
+		break;
+	case MEDIA_BUS_FMT_UYVY8_1X16:
+		isp_reg_clr(isp, OMAP3_ISP_IOMEM_RESZ, ISPRSZ_CNT,
+			    ISPRSZ_CNT_YCPOS);
+		break;
+	default:
+		return;
+	}
+}
+
+/*
+ * resizer_set_phase - Setup horizontal and vertical starting phase
+ * @res: Device context.
+ * @h_phase: horizontal phase parameters.
+ * @v_phase: vertical phase parameters.
+ *
+ * Horizontal and vertical phase range is 0 to 7
+ */
+static void resizer_set_phase(struct isp_res_device *res, u32 h_phase,
+			      u32 v_phase)
+{
+	struct isp_device *isp = to_isp_device(res);
+	u32 rgval;
+
+	rgval = isp_reg_readl(isp, OMAP3_ISP_IOMEM_RESZ, ISPRSZ_CNT) &
+	      ~(ISPRSZ_CNT_HSTPH_MASK | ISPRSZ_CNT_VSTPH_MASK);
+	rgval |= (h_phase << ISPRSZ_CNT_HSTPH_SHIFT) & ISPRSZ_CNT_HSTPH_MASK;
+	rgval |= (v_phase << ISPRSZ_CNT_VSTPH_SHIFT) & ISPRSZ_CNT_VSTPH_MASK;
+
+	isp_reg_writel(isp, rgval, OMAP3_ISP_IOMEM_RESZ, ISPRSZ_CNT);
+}
+
+/*
+ * resizer_set_luma - Setup luminance enhancer parameters
+ * @res: Device context.
+ * @luma: Structure for luminance enhancer parameters.
+ *
+ * Algorithm select:
+ *  0x0: Disable
+ *  0x1: [-1  2 -1]/2 high-pass filter
+ *  0x2: [-1 -2  6 -2 -1]/4 high-pass filter
+ *
+ * Maximum gain:
+ *  The data is coded in U4Q4 representation.
+ *
+ * Slope:
+ *  The data is coded in U4Q4 representation.
+ *
+ * Coring offset:
+ *  The data is coded in U8Q0 representation.
+ *
+ * The new luminance value is computed as:
+ *  Y += HPF(Y) x max(GAIN, (HPF(Y) - CORE) x SLOP + 8) >> 4.
+ */
+static void resizer_set_luma(struct isp_res_device *res,
+			     struct resizer_luma_yenh *luma)
+{
+	struct isp_device *isp = to_isp_device(res);
+	u32 rgval;
+
+	rgval  = (luma->algo << ISPRSZ_YENH_ALGO_SHIFT)
+		  & ISPRSZ_YENH_ALGO_MASK;
+	rgval |= (luma->gain << ISPRSZ_YENH_GAIN_SHIFT)
+		  & ISPRSZ_YENH_GAIN_MASK;
+	rgval |= (luma->slope << ISPRSZ_YENH_SLOP_SHIFT)
+		  & ISPRSZ_YENH_SLOP_MASK;
+	rgval |= (luma->core << ISPRSZ_YENH_CORE_SHIFT)
+		  & ISPRSZ_YENH_CORE_MASK;
+
+	isp_reg_writel(isp, rgval, OMAP3_ISP_IOMEM_RESZ, ISPRSZ_YENH);
+}
+
+/*
+ * resizer_set_source - Input source select
+ * @res: Device context.
+ * @source: Input source type
+ *
+ * If this field is set to RESIZER_INPUT_VP, the resizer input is fed from
+ * Preview/CCDC engine, otherwise from memory.
+ */
+static void resizer_set_source(struct isp_res_device *res,
+			       enum resizer_input_entity source)
+{
+	struct isp_device *isp = to_isp_device(res);
+
+	if (source == RESIZER_INPUT_MEMORY)
+		isp_reg_set(isp, OMAP3_ISP_IOMEM_RESZ, ISPRSZ_CNT,
+			    ISPRSZ_CNT_INPSRC);
+	else
+		isp_reg_clr(isp, OMAP3_ISP_IOMEM_RESZ, ISPRSZ_CNT,
+			    ISPRSZ_CNT_INPSRC);
+}
+
+/*
+ * resizer_set_ratio - Setup horizontal and vertical resizing value
+ * @res: Device context.
+ * @ratio: Structure for ratio parameters.
+ *
+ * Resizing range from 64 to 1024
+ */
+static void resizer_set_ratio(struct isp_res_device *res,
+			      const struct resizer_ratio *ratio)
+{
+	struct isp_device *isp = to_isp_device(res);
+	const u16 *h_filter, *v_filter;
+	u32 rgval;
+
+	rgval = isp_reg_readl(isp, OMAP3_ISP_IOMEM_RESZ, ISPRSZ_CNT) &
+			      ~(ISPRSZ_CNT_HRSZ_MASK | ISPRSZ_CNT_VRSZ_MASK);
+	rgval |= ((ratio->horz - 1) << ISPRSZ_CNT_HRSZ_SHIFT)
+		  & ISPRSZ_CNT_HRSZ_MASK;
+	rgval |= ((ratio->vert - 1) << ISPRSZ_CNT_VRSZ_SHIFT)
+		  & ISPRSZ_CNT_VRSZ_MASK;
+	isp_reg_writel(isp, rgval, OMAP3_ISP_IOMEM_RESZ, ISPRSZ_CNT);
+
+	/* prepare horizontal filter coefficients */
+	if (ratio->horz > MID_RESIZE_VALUE)
+		h_filter = &filter_coefs.h_filter_coef_7tap[0];
+	else
+		h_filter = &filter_coefs.h_filter_coef_4tap[0];
+
+	/* prepare vertical filter coefficients */
+	if (ratio->vert > MID_RESIZE_VALUE)
+		v_filter = &filter_coefs.v_filter_coef_7tap[0];
+	else
+		v_filter = &filter_coefs.v_filter_coef_4tap[0];
+
+	resizer_set_filters(res, h_filter, v_filter);
+}
+
+/*
+ * resizer_set_dst_size - Setup the output height and width
+ * @res: Device context.
+ * @width: Output width.
+ * @height: Output height.
+ *
+ * Width :
+ *  The value must be EVEN.
+ *
+ * Height:
+ *  The number of bytes written to SDRAM must be
+ *  a multiple of 16-bytes if the vertical resizing factor
+ *  is greater than 1x (upsizing)
+ */
+static void resizer_set_output_size(struct isp_res_device *res,
+				    u32 width, u32 height)
+{
+	struct isp_device *isp = to_isp_device(res);
+	u32 rgval;
+
+	rgval  = (width << ISPRSZ_OUT_SIZE_HORZ_SHIFT)
+		 & ISPRSZ_OUT_SIZE_HORZ_MASK;
+	rgval |= (height << ISPRSZ_OUT_SIZE_VERT_SHIFT)
+		 & ISPRSZ_OUT_SIZE_VERT_MASK;
+	isp_reg_writel(isp, rgval, OMAP3_ISP_IOMEM_RESZ, ISPRSZ_OUT_SIZE);
+}
+
+/*
+ * resizer_set_output_offset - Setup memory offset for the output lines.
+ * @res: Device context.
+ * @offset: Memory offset.
+ *
+ * The 5 LSBs are forced to be zeros by the hardware to align on a 32-byte
+ * boundary; the 5 LSBs are read-only. For optimal use of SDRAM bandwidth,
+ * the SDRAM line offset must be set on a 256-byte boundary
+ */
+static void resizer_set_output_offset(struct isp_res_device *res, u32 offset)
+{
+	struct isp_device *isp = to_isp_device(res);
+
+	isp_reg_writel(isp, offset, OMAP3_ISP_IOMEM_RESZ, ISPRSZ_SDR_OUTOFF);
+}
+
+/*
+ * resizer_set_start - Setup vertical and horizontal start position
+ * @res: Device context.
+ * @left: Horizontal start position.
+ * @top: Vertical start position.
+ *
+ * Vertical start line:
+ *  This field makes sense only when the resizer obtains its input
+ *  from the preview engine/CCDC
+ *
+ * Horizontal start pixel:
+ *  Pixels are coded on 16 bits for YUV and 8 bits for color separate data.
+ *  When the resizer gets its input from SDRAM, this field must be set
+ *  to <= 15 for YUV 16-bit data and <= 31 for 8-bit color separate data
+ */
+static void resizer_set_start(struct isp_res_device *res, u32 left, u32 top)
+{
+	struct isp_device *isp = to_isp_device(res);
+	u32 rgval;
+
+	rgval = (left << ISPRSZ_IN_START_HORZ_ST_SHIFT)
+		& ISPRSZ_IN_START_HORZ_ST_MASK;
+	rgval |= (top << ISPRSZ_IN_START_VERT_ST_SHIFT)
+		 & ISPRSZ_IN_START_VERT_ST_MASK;
+
+	isp_reg_writel(isp, rgval, OMAP3_ISP_IOMEM_RESZ, ISPRSZ_IN_START);
+}
+
+/*
+ * resizer_set_input_size - Setup the input size
+ * @res: Device context.
+ * @width: The range is 0 to 4095 pixels
+ * @height: The range is 0 to 4095 lines
+ */
+static void resizer_set_input_size(struct isp_res_device *res,
+				   u32 width, u32 height)
+{
+	struct isp_device *isp = to_isp_device(res);
+	u32 rgval;
+
+	rgval = (width << ISPRSZ_IN_SIZE_HORZ_SHIFT)
+		& ISPRSZ_IN_SIZE_HORZ_MASK;
+	rgval |= (height << ISPRSZ_IN_SIZE_VERT_SHIFT)
+		 & ISPRSZ_IN_SIZE_VERT_MASK;
+
+	isp_reg_writel(isp, rgval, OMAP3_ISP_IOMEM_RESZ, ISPRSZ_IN_SIZE);
+}
+
+/*
+ * resizer_set_src_offs - Setup the memory offset for the input lines
+ * @res: Device context.
+ * @offset: Memory offset.
+ *
+ * The 5 LSBs are forced to be zeros by the hardware to align on a 32-byte
+ * boundary; the 5 LSBs are read-only. This field must be programmed to be
+ * 0x0 if the resizer input is from preview engine/CCDC.
+ */
+static void resizer_set_input_offset(struct isp_res_device *res, u32 offset)
+{
+	struct isp_device *isp = to_isp_device(res);
+
+	isp_reg_writel(isp, offset, OMAP3_ISP_IOMEM_RESZ, ISPRSZ_SDR_INOFF);
+}
+
+/*
+ * resizer_set_intype - Input type select
+ * @res: Device context.
+ * @type: Pixel format type.
+ */
+static void resizer_set_intype(struct isp_res_device *res,
+			       enum resizer_colors_type type)
+{
+	struct isp_device *isp = to_isp_device(res);
+
+	if (type == RSZ_COLOR8)
+		isp_reg_set(isp, OMAP3_ISP_IOMEM_RESZ, ISPRSZ_CNT,
+			    ISPRSZ_CNT_INPTYP);
+	else
+		isp_reg_clr(isp, OMAP3_ISP_IOMEM_RESZ, ISPRSZ_CNT,
+			    ISPRSZ_CNT_INPTYP);
+}
+
+/*
+ * __resizer_set_inaddr - Helper function for set input address
+ * @res : pointer to resizer private data structure
+ * @addr: input address
+ * return none
+ */
+static void __resizer_set_inaddr(struct isp_res_device *res, u32 addr)
+{
+	struct isp_device *isp = to_isp_device(res);
+
+	isp_reg_writel(isp, addr, OMAP3_ISP_IOMEM_RESZ, ISPRSZ_SDR_INADD);
+}
+
+/*
+ * The data rate at the horizontal resizer output must not exceed half the
+ * functional clock or 100 MP/s, whichever is lower. According to the TRM
+ * there's no similar requirement for the vertical resizer output. However
+ * experience showed that vertical upscaling by 4 leads to SBL overflows (with
+ * data rates at the resizer output exceeding 300 MP/s). Limiting the resizer
+ * output data rate to the functional clock or 200 MP/s, whichever is lower,
+ * seems to get rid of SBL overflows.
+ *
+ * The maximum data rate at the output of the horizontal resizer can thus be
+ * computed with
+ *
+ * max intermediate rate <= L3 clock * input height / output height
+ * max intermediate rate <= L3 clock / 2
+ *
+ * The maximum data rate at the resizer input is then
+ *
+ * max input rate <= max intermediate rate * input width / output width
+ *
+ * where the input width and height are the resizer input crop rectangle size.
+ * The TRM doesn't clearly explain if that's a maximum instant data rate or a
+ * maximum average data rate.
+ */
+void omap3isp_resizer_max_rate(struct isp_res_device *res,
+			       unsigned int *max_rate)
+{
+	struct isp_pipeline *pipe = to_isp_pipeline(&res->subdev.entity);
+	const struct v4l2_mbus_framefmt *ofmt = &res->formats[RESZ_PAD_SOURCE];
+	unsigned long limit = min(pipe->l3_ick, 200000000UL);
+	unsigned long clock;
+
+	clock = div_u64((u64)limit * res->crop.active.height, ofmt->height);
+	clock = min(clock, limit / 2);
+	*max_rate = div_u64((u64)clock * res->crop.active.width, ofmt->width);
+}
+
+/*
+ * When the resizer processes images from memory, the driver must slow down read
+ * requests on the input to at least comply with the internal data rate
+ * requirements. If the application real-time requirements can cope with slower
+ * processing, the resizer can be slowed down even more to put less pressure on
+ * the overall system.
+ *
+ * When the resizer processes images on the fly (either from the CCDC or the
+ * preview module), the same data rate requirements apply but they can't be
+ * enforced at the resizer level. The image input module (sensor, CCP2 or
+ * preview module) must not provide image data faster than the resizer can
+ * process.
+ *
+ * For live image pipelines, the data rate is set by the frame format, size and
+ * rate. The sensor output frame rate must not exceed the maximum resizer data
+ * rate.
+ *
+ * The resizer slows down read requests by inserting wait cycles in the SBL
+ * requests. The maximum number of 256-byte requests per second can be computed
+ * as (the data rate is multiplied by 2 to convert from pixels per second to
+ * bytes per second)
+ *
+ * request per second = data rate * 2 / 256
+ * cycles per request = cycles per second / requests per second
+ *
+ * The number of cycles per second is controlled by the L3 clock, leading to
+ *
+ * cycles per request = L3 frequency / 2 * 256 / data rate
+ */
+static void resizer_adjust_bandwidth(struct isp_res_device *res)
+{
+	struct isp_pipeline *pipe = to_isp_pipeline(&res->subdev.entity);
+	struct isp_device *isp = to_isp_device(res);
+	unsigned long l3_ick = pipe->l3_ick;
+	struct v4l2_fract *timeperframe;
+	unsigned int cycles_per_frame;
+	unsigned int requests_per_frame;
+	unsigned int cycles_per_request;
+	unsigned int granularity;
+	unsigned int minimum;
+	unsigned int maximum;
+	unsigned int value;
+
+	if (res->input != RESIZER_INPUT_MEMORY) {
+		isp_reg_clr(isp, OMAP3_ISP_IOMEM_SBL, ISPSBL_SDR_REQ_EXP,
+			    ISPSBL_SDR_REQ_RSZ_EXP_MASK);
+		return;
+	}
+
+	switch (isp->revision) {
+	case ISP_REVISION_1_0:
+	case ISP_REVISION_2_0:
+	default:
+		granularity = 1024;
+		break;
+
+	case ISP_REVISION_15_0:
+		granularity = 32;
+		break;
+	}
+
+	/* Compute the minimum number of cycles per request, based on the
+	 * pipeline maximum data rate. This is an absolute lower bound if we
+	 * don't want SBL overflows, so round the value up.
+	 */
+	cycles_per_request = div_u64((u64)l3_ick / 2 * 256 + pipe->max_rate - 1,
+				     pipe->max_rate);
+	minimum = DIV_ROUND_UP(cycles_per_request, granularity);
+
+	/* Compute the maximum number of cycles per request, based on the
+	 * requested frame rate. This is a soft upper bound to achieve a frame
+	 * rate equal or higher than the requested value, so round the value
+	 * down.
+	 */
+	timeperframe = &pipe->max_timeperframe;
+
+	requests_per_frame = DIV_ROUND_UP(res->crop.active.width * 2, 256)
+			   * res->crop.active.height;
+	cycles_per_frame = div_u64((u64)l3_ick * timeperframe->numerator,
+				   timeperframe->denominator);
+	cycles_per_request = cycles_per_frame / requests_per_frame;
+
+	maximum = cycles_per_request / granularity;
+
+	value = max(minimum, maximum);
+
+	dev_dbg(isp->dev, "%s: cycles per request = %u\n", __func__, value);
+	isp_reg_clr_set(isp, OMAP3_ISP_IOMEM_SBL, ISPSBL_SDR_REQ_EXP,
+			ISPSBL_SDR_REQ_RSZ_EXP_MASK,
+			value << ISPSBL_SDR_REQ_RSZ_EXP_SHIFT);
+}
+
+/*
+ * omap3isp_resizer_busy - Checks if ISP resizer is busy.
+ *
+ * Returns busy field from ISPRSZ_PCR register.
+ */
+int omap3isp_resizer_busy(struct isp_res_device *res)
+{
+	struct isp_device *isp = to_isp_device(res);
+
+	return isp_reg_readl(isp, OMAP3_ISP_IOMEM_RESZ, ISPRSZ_PCR) &
+			     ISPRSZ_PCR_BUSY;
+}
+
+/*
+ * resizer_set_inaddr - Sets the memory address of the input frame.
+ * @addr: 32bit memory address aligned on 32byte boundary.
+ */
+static void resizer_set_inaddr(struct isp_res_device *res, u32 addr)
+{
+	res->addr_base = addr;
+
+	/* This will handle crop settings in stream off state */
+	if (res->crop_offset)
+		addr += res->crop_offset & ~0x1f;
+
+	__resizer_set_inaddr(res, addr);
+}
+
+/*
+ * Configures the memory address to which the output frame is written.
+ * @addr: 32bit memory address aligned on 32byte boundary.
+ * Note: For SBL efficiency reasons the address should be on a 256-byte
+ * boundary.
+ */
+static void resizer_set_outaddr(struct isp_res_device *res, u32 addr)
+{
+	struct isp_device *isp = to_isp_device(res);
+
+	/*
+	 * Set output address. This needs to be in its own function
+	 * because it changes often.
+	 */
+	isp_reg_writel(isp, addr << ISPRSZ_SDR_OUTADD_ADDR_SHIFT,
+		       OMAP3_ISP_IOMEM_RESZ, ISPRSZ_SDR_OUTADD);
+}
+
+/*
+ * resizer_print_status - Prints the values of the resizer module registers.
+ */
+#define RSZ_PRINT_REGISTER(isp, name)\
+	dev_dbg(isp->dev, "###RSZ " #name "=0x%08x\n", \
+		isp_reg_readl(isp, OMAP3_ISP_IOMEM_RESZ, ISPRSZ_##name))
+
+static void resizer_print_status(struct isp_res_device *res)
+{
+	struct isp_device *isp = to_isp_device(res);
+
+	dev_dbg(isp->dev, "-------------Resizer Register dump----------\n");
+
+	RSZ_PRINT_REGISTER(isp, PCR);
+	RSZ_PRINT_REGISTER(isp, CNT);
+	RSZ_PRINT_REGISTER(isp, OUT_SIZE);
+	RSZ_PRINT_REGISTER(isp, IN_START);
+	RSZ_PRINT_REGISTER(isp, IN_SIZE);
+	RSZ_PRINT_REGISTER(isp, SDR_INADD);
+	RSZ_PRINT_REGISTER(isp, SDR_INOFF);
+	RSZ_PRINT_REGISTER(isp, SDR_OUTADD);
+	RSZ_PRINT_REGISTER(isp, SDR_OUTOFF);
+	RSZ_PRINT_REGISTER(isp, YENH);
+
+	dev_dbg(isp->dev, "--------------------------------------------\n");
+}
+
+/*
+ * resizer_calc_ratios - Helper function for calculating resizer ratios
+ * @res: pointer to resizer private data structure
+ * @input: input frame size
+ * @output: output frame size
+ * @ratio : return calculated ratios
+ * return none
+ *
+ * The resizer uses a polyphase sample rate converter. The upsampling filter
+ * has a fixed number of phases that depend on the resizing ratio. As the ratio
+ * computation depends on the number of phases, we need to compute a first
+ * approximation and then refine it.
+ *
+ * The input/output/ratio relationship is given by the OMAP34xx TRM:
+ *
+ * - 8-phase, 4-tap mode (RSZ = 64 ~ 512)
+ *	iw = (32 * sph + (ow - 1) * hrsz + 16) >> 8 + 7
+ *	ih = (32 * spv + (oh - 1) * vrsz + 16) >> 8 + 4
+ * - 4-phase, 7-tap mode (RSZ = 513 ~ 1024)
+ *	iw = (64 * sph + (ow - 1) * hrsz + 32) >> 8 + 7
+ *	ih = (64 * spv + (oh - 1) * vrsz + 32) >> 8 + 7
+ *
+ * iw and ih are the input width and height after cropping. Those equations need
+ * to be satisfied exactly for the resizer to work correctly.
+ *
+ * The equations can't be easily reverted, as the >> 8 operation is not linear.
+ * In addition, not all input sizes can be achieved for a given output size. To
+ * get the highest input size lower than or equal to the requested input size,
+ * we need to compute the highest resizing ratio that satisfies the following
+ * inequality (taking the 4-tap mode width equation as an example)
+ *
+ *	iw >= (32 * sph + (ow - 1) * hrsz + 16) >> 8 - 7
+ *
+ * (where iw is the requested input width) which can be rewritten as
+ *
+ *	  iw - 7            >= (32 * sph + (ow - 1) * hrsz + 16) >> 8
+ *	 (iw - 7) << 8      >=  32 * sph + (ow - 1) * hrsz + 16 - b
+ *	((iw - 7) << 8) + b >=  32 * sph + (ow - 1) * hrsz + 16
+ *
+ * where b is the value of the 8 least significant bits of the right hand side
+ * expression of the last inequality. The highest resizing ratio value will be
+ * achieved when b is equal to its maximum value of 255. That resizing ratio
+ * value will still satisfy the original inequality, as b will disappear when
+ * the expression will be shifted right by 8.
+ *
+ * The reverted equations thus become
+ *
+ * - 8-phase, 4-tap mode
+ *	hrsz = ((iw - 7) * 256 + 255 - 16 - 32 * sph) / (ow - 1)
+ *	vrsz = ((ih - 4) * 256 + 255 - 16 - 32 * spv) / (oh - 1)
+ * - 4-phase, 7-tap mode
+ *	hrsz = ((iw - 7) * 256 + 255 - 32 - 64 * sph) / (ow - 1)
+ *	vrsz = ((ih - 7) * 256 + 255 - 32 - 64 * spv) / (oh - 1)
+ *
+ * The ratios are integer values, and are rounded down to ensure that the
+ * cropped input size is not bigger than the uncropped input size.
+ *
+ * As the number of phases/taps, used to select the correct equations to compute
+ * the ratio, depends on the ratio, we start with the 4-tap mode equations to
+ * compute an approximation of the ratio, and switch to the 7-tap mode equations
+ * if the approximation is higher than the ratio threshold.
+ *
+ * As the 7-tap mode equations will return a ratio smaller than or equal to the
+ * 4-tap mode equations, the resulting ratio could become lower than or equal to
+ * the ratio threshold. This 'equations loop' isn't an issue as long as the
+ * correct equations are used to compute the final input size. Starting with the
+ * 4-tap mode equations ensure that, in case of values resulting in a 'ratio
+ * loop', the smallest of the ratio values will be used, never exceeding the
+ * requested input size.
+ *
+ * We first clamp the output size according to the hardware capability to avoid
+ * auto-cropping the input more than required to satisfy the TRM equations. The
+ * minimum output size is achieved with a scaling factor of 1024. It is thus
+ * computed using the 7-tap equations.
+ *
+ *	min ow = ((iw - 7) * 256 - 32 - 64 * sph) / 1024 + 1
+ *	min oh = ((ih - 7) * 256 - 32 - 64 * spv) / 1024 + 1
+ *
+ * Similarly, the maximum output size is achieved with a scaling factor of 64
+ * and computed using the 4-tap equations.
+ *
+ *	max ow = ((iw - 7) * 256 + 255 - 16 - 32 * sph) / 64 + 1
+ *	max oh = ((ih - 4) * 256 + 255 - 16 - 32 * spv) / 64 + 1
+ *
+ * The additional +255 term compensates for the round down operation performed
+ * by the TRM equations when shifting the value right by 8 bits.
+ *
+ * We then compute and clamp the ratios (x1/4 ~ x4). Clamping the output size to
+ * the maximum value guarantees that the ratio value will never be smaller than
+ * the minimum, but it could still slightly exceed the maximum. Clamping the
+ * ratio will thus result in a resizing factor slightly larger than the
+ * requested value.
+ *
+ * To accommodate that, and make sure the TRM equations are satisfied exactly, we
+ * compute the input crop rectangle as the last step.
+ *
+ * As if the situation wasn't complex enough, the maximum output width depends
+ * on the vertical resizing ratio.  Fortunately, the output height doesn't
+ * depend on the horizontal resizing ratio. We can then start by computing the
+ * output height and the vertical ratio, and then move to computing the output
+ * width and the horizontal ratio.
+ */
+static void resizer_calc_ratios(struct isp_res_device *res,
+				struct v4l2_rect *input,
+				struct v4l2_mbus_framefmt *output,
+				struct resizer_ratio *ratio)
+{
+	struct isp_device *isp = to_isp_device(res);
+	const unsigned int spv = DEFAULT_PHASE;
+	const unsigned int sph = DEFAULT_PHASE;
+	unsigned int upscaled_width;
+	unsigned int upscaled_height;
+	unsigned int min_width;
+	unsigned int min_height;
+	unsigned int max_width;
+	unsigned int max_height;
+	unsigned int width_alignment;
+	unsigned int width;
+	unsigned int height;
+
+	/*
+	 * Clamp the output height based on the hardware capabilities and
+	 * compute the vertical resizing ratio.
+	 */
+	min_height = ((input->height - 7) * 256 - 32 - 64 * spv) / 1024 + 1;
+	min_height = max_t(unsigned int, min_height, MIN_OUT_HEIGHT);
+	max_height = ((input->height - 4) * 256 + 255 - 16 - 32 * spv) / 64 + 1;
+	max_height = min_t(unsigned int, max_height, MAX_OUT_HEIGHT);
+	output->height = clamp(output->height, min_height, max_height);
+
+	ratio->vert = ((input->height - 4) * 256 + 255 - 16 - 32 * spv)
+		    / (output->height - 1);
+	if (ratio->vert > MID_RESIZE_VALUE)
+		ratio->vert = ((input->height - 7) * 256 + 255 - 32 - 64 * spv)
+			    / (output->height - 1);
+	ratio->vert = clamp_t(unsigned int, ratio->vert,
+			      MIN_RESIZE_VALUE, MAX_RESIZE_VALUE);
+
+	if (ratio->vert <= MID_RESIZE_VALUE) {
+		upscaled_height = (output->height - 1) * ratio->vert
+				+ 32 * spv + 16;
+		height = (upscaled_height >> 8) + 4;
+	} else {
+		upscaled_height = (output->height - 1) * ratio->vert
+				+ 64 * spv + 32;
+		height = (upscaled_height >> 8) + 7;
+	}
+
+	/*
+	 * Compute the minimum and maximum output widths based on the hardware
+	 * capabilities. The maximum depends on the vertical resizing ratio.
+	 */
+	min_width = ((input->width - 7) * 256 - 32 - 64 * sph) / 1024 + 1;
+	min_width = max_t(unsigned int, min_width, MIN_OUT_WIDTH);
+
+	if (ratio->vert <= MID_RESIZE_VALUE) {
+		switch (isp->revision) {
+		case ISP_REVISION_1_0:
+			max_width = MAX_4TAP_OUT_WIDTH_ES1;
+			break;
+
+		case ISP_REVISION_2_0:
+		default:
+			max_width = MAX_4TAP_OUT_WIDTH_ES2;
+			break;
+
+		case ISP_REVISION_15_0:
+			max_width = MAX_4TAP_OUT_WIDTH_3630;
+			break;
+		}
+	} else {
+		switch (isp->revision) {
+		case ISP_REVISION_1_0:
+			max_width = MAX_7TAP_OUT_WIDTH_ES1;
+			break;
+
+		case ISP_REVISION_2_0:
+		default:
+			max_width = MAX_7TAP_OUT_WIDTH_ES2;
+			break;
+
+		case ISP_REVISION_15_0:
+			max_width = MAX_7TAP_OUT_WIDTH_3630;
+			break;
+		}
+	}
+	max_width = min(((input->width - 7) * 256 + 255 - 16 - 32 * sph) / 64
+			+ 1, max_width);
+
+	/*
+	 * The output width must be even, and must be a multiple of 16 bytes
+	 * when upscaling vertically. Clamp the output width to the valid range.
+	 * Take the alignment into account (the maximum width in 7-tap mode on
+	 * ES2 isn't a multiple of 8) and align the result up to make sure it
+	 * won't be smaller than the minimum.
+	 */
+	width_alignment = ratio->vert < 256 ? 8 : 2;
+	output->width = clamp(output->width, min_width,
+			      max_width & ~(width_alignment - 1));
+	output->width = ALIGN(output->width, width_alignment);
+
+	ratio->horz = ((input->width - 7) * 256 + 255 - 16 - 32 * sph)
+		    / (output->width - 1);
+	if (ratio->horz > MID_RESIZE_VALUE)
+		ratio->horz = ((input->width - 7) * 256 + 255 - 32 - 64 * sph)
+			    / (output->width - 1);
+	ratio->horz = clamp_t(unsigned int, ratio->horz,
+			      MIN_RESIZE_VALUE, MAX_RESIZE_VALUE);
+
+	if (ratio->horz <= MID_RESIZE_VALUE) {
+		upscaled_width = (output->width - 1) * ratio->horz
+			       + 32 * sph + 16;
+		width = (upscaled_width >> 8) + 7;
+	} else {
+		upscaled_width = (output->width - 1) * ratio->horz
+			       + 64 * sph + 32;
+		width = (upscaled_width >> 8) + 7;
+	}
+
+	/* Center the new crop rectangle. */
+	input->left += (input->width - width) / 2;
+	input->top += (input->height - height) / 2;
+	input->width = width;
+	input->height = height;
+}
+
+/*
+ * resizer_set_crop_params - Setup hardware with cropping parameters
+ * @res : resizer private structure
+ * @input : format on sink pad
+ * @output : format on source pad
+ * return none
+ */
+static void resizer_set_crop_params(struct isp_res_device *res,
+				    const struct v4l2_mbus_framefmt *input,
+				    const struct v4l2_mbus_framefmt *output)
+{
+	resizer_set_ratio(res, &res->ratio);
+
+	/* Set chrominance horizontal algorithm */
+	if (res->ratio.horz >= RESIZE_DIVISOR)
+		resizer_set_bilinear(res, RSZ_THE_SAME);
+	else
+		resizer_set_bilinear(res, RSZ_BILINEAR);
+
+	resizer_adjust_bandwidth(res);
+
+	if (res->input == RESIZER_INPUT_MEMORY) {
+		/* Calculate additional offset for crop */
+		res->crop_offset = (res->crop.active.top * input->width +
+				    res->crop.active.left) * 2;
+		/*
+		 * Write lowest 4 bits of horizontal pixel offset (in pixels),
+		 * vertical start must be 0.
+		 */
+		resizer_set_start(res, (res->crop_offset / 2) & 0xf, 0);
+
+		/*
+		 * Set start (read) address for cropping, in bytes.
+		 * Lowest 5 bits must be zero.
+		 */
+		__resizer_set_inaddr(res,
+				res->addr_base + (res->crop_offset & ~0x1f));
+	} else {
+		/*
+		 * Set vertical start line and horizontal starting pixel.
+		 * If the input is from CCDC/PREV, horizontal start field is
+		 * in bytes (twice number of pixels).
+		 */
+		resizer_set_start(res, res->crop.active.left * 2,
+				  res->crop.active.top);
+		/* Input address and offset must be 0 for preview/ccdc input */
+		__resizer_set_inaddr(res, 0);
+		resizer_set_input_offset(res, 0);
+	}
+
+	/* Set the input size */
+	resizer_set_input_size(res, res->crop.active.width,
+			       res->crop.active.height);
+}
+
+static void resizer_configure(struct isp_res_device *res)
+{
+	struct v4l2_mbus_framefmt *informat, *outformat;
+	struct resizer_luma_yenh luma = {0, 0, 0, 0};
+
+	resizer_set_source(res, res->input);
+
+	informat = &res->formats[RESZ_PAD_SINK];
+	outformat = &res->formats[RESZ_PAD_SOURCE];
+
+	/* RESZ_PAD_SINK */
+	if (res->input == RESIZER_INPUT_VP)
+		resizer_set_input_offset(res, 0);
+	else
+		resizer_set_input_offset(res, ALIGN(informat->width, 0x10) * 2);
+
+	/* YUV422 interleaved, default phase, no luma enhancement */
+	resizer_set_intype(res, RSZ_YUV422);
+	resizer_set_ycpos(res, informat->code);
+	resizer_set_phase(res, DEFAULT_PHASE, DEFAULT_PHASE);
+	resizer_set_luma(res, &luma);
+
+	/* RESZ_PAD_SOURCE */
+	resizer_set_output_offset(res, ALIGN(outformat->width * 2, 32));
+	resizer_set_output_size(res, outformat->width, outformat->height);
+
+	resizer_set_crop_params(res, informat, outformat);
+}
+
+/* -----------------------------------------------------------------------------
+ * Interrupt handling
+ */
+
+static void resizer_enable_oneshot(struct isp_res_device *res)
+{
+	struct isp_device *isp = to_isp_device(res);
+
+	isp_reg_set(isp, OMAP3_ISP_IOMEM_RESZ, ISPRSZ_PCR,
+		    ISPRSZ_PCR_ENABLE | ISPRSZ_PCR_ONESHOT);
+}
+
+void omap3isp_resizer_isr_frame_sync(struct isp_res_device *res)
+{
+	/*
+	 * If ISP_VIDEO_DMAQUEUE_QUEUED is set, DMA queue had an underrun
+	 * condition, the module was paused and now we have a buffer queued
+	 * on the output again. Restart the pipeline if running in continuous
+	 * mode.
+	 */
+	if (res->state == ISP_PIPELINE_STREAM_CONTINUOUS &&
+	    res->video_out.dmaqueue_flags & ISP_VIDEO_DMAQUEUE_QUEUED) {
+		resizer_enable_oneshot(res);
+		isp_video_dmaqueue_flags_clr(&res->video_out);
+	}
+}
+
+static void resizer_isr_buffer(struct isp_res_device *res)
+{
+	struct isp_pipeline *pipe = to_isp_pipeline(&res->subdev.entity);
+	struct isp_buffer *buffer;
+	int restart = 0;
+
+	if (res->state == ISP_PIPELINE_STREAM_STOPPED)
+		return;
+
+	/* Complete the output buffer and, if reading from memory, the input
+	 * buffer.
+	 */
+	buffer = omap3isp_video_buffer_next(&res->video_out);
+	if (buffer != NULL) {
+		resizer_set_outaddr(res, buffer->dma);
+		restart = 1;
+	}
+
+	pipe->state |= ISP_PIPELINE_IDLE_OUTPUT;
+
+	if (res->input == RESIZER_INPUT_MEMORY) {
+		buffer = omap3isp_video_buffer_next(&res->video_in);
+		if (buffer != NULL)
+			resizer_set_inaddr(res, buffer->dma);
+		pipe->state |= ISP_PIPELINE_IDLE_INPUT;
+	}
+
+	if (res->state == ISP_PIPELINE_STREAM_SINGLESHOT) {
+		if (isp_pipeline_ready(pipe))
+			omap3isp_pipeline_set_stream(pipe,
+						ISP_PIPELINE_STREAM_SINGLESHOT);
+	} else {
+		/* If an underrun occurs, the video queue operation handler will
+		 * restart the resizer. Otherwise restart it immediately.
+		 */
+		if (restart)
+			resizer_enable_oneshot(res);
+	}
+}
+
+/*
+ * omap3isp_resizer_isr - ISP resizer interrupt handler
+ *
+ * Manage the resizer video buffers and configure shadowed and busy-locked
+ * registers.
+ */
+void omap3isp_resizer_isr(struct isp_res_device *res)
+{
+	struct v4l2_mbus_framefmt *informat, *outformat;
+	unsigned long flags;
+
+	if (omap3isp_module_sync_is_stopping(&res->wait, &res->stopping))
+		return;
+
+	spin_lock_irqsave(&res->lock, flags);
+
+	if (res->applycrop) {
+		outformat = __resizer_get_format(res, NULL, RESZ_PAD_SOURCE,
+					      V4L2_SUBDEV_FORMAT_ACTIVE);
+		informat = __resizer_get_format(res, NULL, RESZ_PAD_SINK,
+					      V4L2_SUBDEV_FORMAT_ACTIVE);
+		resizer_set_crop_params(res, informat, outformat);
+		res->applycrop = 0;
+	}
+
+	spin_unlock_irqrestore(&res->lock, flags);
+
+	resizer_isr_buffer(res);
+}
+
+/* -----------------------------------------------------------------------------
+ * ISP video operations
+ */
+
+static int resizer_video_queue(struct isp_video *video,
+			       struct isp_buffer *buffer)
+{
+	struct isp_res_device *res = &video->isp->isp_res;
+
+	if (video->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		resizer_set_inaddr(res, buffer->dma);
+
+	/*
+	 * We now have a buffer queued on the output. Despite what the
+	 * TRM says, the resizer can't be restarted immediately.
+	 * Enabling it in one shot mode in the middle of a frame (or at
+	 * least asynchronously to the frame) results in the output
+	 * being shifted randomly left/right and up/down, as if the
+	 * hardware didn't synchronize itself to the beginning of the
+	 * frame correctly.
+	 *
+	 * Restart the resizer on the next sync interrupt if running in
+	 * continuous mode or when starting the stream.
+	 */
+	if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		resizer_set_outaddr(res, buffer->dma);
+
+	return 0;
+}
+
+static const struct isp_video_operations resizer_video_ops = {
+	.queue = resizer_video_queue,
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev operations
+ */
+
+/*
+ * resizer_set_stream - Enable/Disable streaming on resizer subdev
+ * @sd: ISP resizer V4L2 subdev
+ * @enable: 1 == Enable, 0 == Disable
+ *
+ * The resizer hardware can't be enabled without a memory buffer to write to.
+ * As the s_stream operation is called in response to a STREAMON call without
+ * any buffer queued yet, just update the state field and return immediately.
+ * The resizer will be enabled in resizer_video_queue().
+ */
+static int resizer_set_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct isp_res_device *res = v4l2_get_subdevdata(sd);
+	struct isp_video *video_out = &res->video_out;
+	struct isp_device *isp = to_isp_device(res);
+	struct device *dev = to_device(res);
+
+	if (res->state == ISP_PIPELINE_STREAM_STOPPED) {
+		if (enable == ISP_PIPELINE_STREAM_STOPPED)
+			return 0;
+
+		omap3isp_subclk_enable(isp, OMAP3_ISP_SUBCLK_RESIZER);
+		resizer_configure(res);
+		resizer_print_status(res);
+	}
+
+	switch (enable) {
+	case ISP_PIPELINE_STREAM_CONTINUOUS:
+		omap3isp_sbl_enable(isp, OMAP3_ISP_SBL_RESIZER_WRITE);
+		if (video_out->dmaqueue_flags & ISP_VIDEO_DMAQUEUE_QUEUED) {
+			resizer_enable_oneshot(res);
+			isp_video_dmaqueue_flags_clr(video_out);
+		}
+		break;
+
+	case ISP_PIPELINE_STREAM_SINGLESHOT:
+		if (res->input == RESIZER_INPUT_MEMORY)
+			omap3isp_sbl_enable(isp, OMAP3_ISP_SBL_RESIZER_READ);
+		omap3isp_sbl_enable(isp, OMAP3_ISP_SBL_RESIZER_WRITE);
+
+		resizer_enable_oneshot(res);
+		break;
+
+	case ISP_PIPELINE_STREAM_STOPPED:
+		if (omap3isp_module_sync_idle(&sd->entity, &res->wait,
+					      &res->stopping))
+			dev_dbg(dev, "%s: module stop timeout.\n", sd->name);
+		omap3isp_sbl_disable(isp, OMAP3_ISP_SBL_RESIZER_READ |
+				OMAP3_ISP_SBL_RESIZER_WRITE);
+		omap3isp_subclk_disable(isp, OMAP3_ISP_SUBCLK_RESIZER);
+		isp_video_dmaqueue_flags_clr(video_out);
+		break;
+	}
+
+	res->state = enable;
+	return 0;
+}
+
+/*
+ * resizer_try_crop - mangles crop parameters.
+ */
+static void resizer_try_crop(const struct v4l2_mbus_framefmt *sink,
+			     const struct v4l2_mbus_framefmt *source,
+			     struct v4l2_rect *crop)
+{
+	const unsigned int spv = DEFAULT_PHASE;
+	const unsigned int sph = DEFAULT_PHASE;
+
+	/* Crop rectangle is constrained by the output size so that zoom ratio
+	 * cannot exceed +/-4.0.
+	 */
+	unsigned int min_width =
+		((32 * sph + (source->width - 1) * 64 + 16) >> 8) + 7;
+	unsigned int min_height =
+		((32 * spv + (source->height - 1) * 64 + 16) >> 8) + 4;
+	unsigned int max_width =
+		((64 * sph + (source->width - 1) * 1024 + 32) >> 8) + 7;
+	unsigned int max_height =
+		((64 * spv + (source->height - 1) * 1024 + 32) >> 8) + 7;
+
+	crop->width = clamp_t(u32, crop->width, min_width, max_width);
+	crop->height = clamp_t(u32, crop->height, min_height, max_height);
+
+	/* Crop can not go beyond of the input rectangle */
+	crop->left = clamp_t(u32, crop->left, 0, sink->width - MIN_IN_WIDTH);
+	crop->width = clamp_t(u32, crop->width, MIN_IN_WIDTH,
+			      sink->width - crop->left);
+	crop->top = clamp_t(u32, crop->top, 0, sink->height - MIN_IN_HEIGHT);
+	crop->height = clamp_t(u32, crop->height, MIN_IN_HEIGHT,
+			       sink->height - crop->top);
+}
+
+/*
+ * resizer_get_selection - Retrieve a selection rectangle on a pad
+ * @sd: ISP resizer V4L2 subdevice
+ * @cfg: V4L2 subdev pad configuration
+ * @sel: Selection rectangle
+ *
+ * The only supported rectangles are the crop rectangles on the sink pad.
+ *
+ * Return 0 on success or a negative error code otherwise.
+ */
+static int resizer_get_selection(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_pad_config *cfg,
+				 struct v4l2_subdev_selection *sel)
+{
+	struct isp_res_device *res = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format_source;
+	struct v4l2_mbus_framefmt *format_sink;
+	struct resizer_ratio ratio;
+
+	if (sel->pad != RESZ_PAD_SINK)
+		return -EINVAL;
+
+	format_sink = __resizer_get_format(res, cfg, RESZ_PAD_SINK,
+					   sel->which);
+	format_source = __resizer_get_format(res, cfg, RESZ_PAD_SOURCE,
+					     sel->which);
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = INT_MAX;
+		sel->r.height = INT_MAX;
+
+		resizer_try_crop(format_sink, format_source, &sel->r);
+		resizer_calc_ratios(res, &sel->r, format_source, &ratio);
+		break;
+
+	case V4L2_SEL_TGT_CROP:
+		sel->r = *__resizer_get_crop(res, cfg, sel->which);
+		resizer_calc_ratios(res, &sel->r, format_source, &ratio);
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/*
+ * resizer_set_selection - Set a selection rectangle on a pad
+ * @sd: ISP resizer V4L2 subdevice
+ * @cfg: V4L2 subdev pad configuration
+ * @sel: Selection rectangle
+ *
+ * The only supported rectangle is the actual crop rectangle on the sink pad.
+ *
+ * FIXME: This function currently behaves as if the KEEP_CONFIG selection flag
+ * was always set.
+ *
+ * Return 0 on success or a negative error code otherwise.
+ */
+static int resizer_set_selection(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_pad_config *cfg,
+				 struct v4l2_subdev_selection *sel)
+{
+	struct isp_res_device *res = v4l2_get_subdevdata(sd);
+	struct isp_device *isp = to_isp_device(res);
+	const struct v4l2_mbus_framefmt *format_sink;
+	struct v4l2_mbus_framefmt format_source;
+	struct resizer_ratio ratio;
+	unsigned long flags;
+
+	if (sel->target != V4L2_SEL_TGT_CROP ||
+	    sel->pad != RESZ_PAD_SINK)
+		return -EINVAL;
+
+	format_sink = __resizer_get_format(res, cfg, RESZ_PAD_SINK,
+					   sel->which);
+	format_source = *__resizer_get_format(res, cfg, RESZ_PAD_SOURCE,
+					      sel->which);
+
+	dev_dbg(isp->dev, "%s(%s): req %ux%u -> (%d,%d)/%ux%u -> %ux%u\n",
+		__func__, sel->which == V4L2_SUBDEV_FORMAT_TRY ? "try" : "act",
+		format_sink->width, format_sink->height,
+		sel->r.left, sel->r.top, sel->r.width, sel->r.height,
+		format_source.width, format_source.height);
+
+	/* Clamp the crop rectangle to the bounds, and then mangle it further to
+	 * fulfill the TRM equations. Store the clamped but otherwise unmangled
+	 * rectangle to avoid cropping the input multiple times: when an
+	 * application sets the output format, the current crop rectangle is
+	 * mangled during crop rectangle computation, which would lead to a new,
+	 * smaller input crop rectangle every time the output size is set if we
+	 * stored the mangled rectangle.
+	 */
+	resizer_try_crop(format_sink, &format_source, &sel->r);
+	*__resizer_get_crop(res, cfg, sel->which) = sel->r;
+	resizer_calc_ratios(res, &sel->r, &format_source, &ratio);
+
+	dev_dbg(isp->dev, "%s(%s): got %ux%u -> (%d,%d)/%ux%u -> %ux%u\n",
+		__func__, sel->which == V4L2_SUBDEV_FORMAT_TRY ? "try" : "act",
+		format_sink->width, format_sink->height,
+		sel->r.left, sel->r.top, sel->r.width, sel->r.height,
+		format_source.width, format_source.height);
+
+	if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
+		*__resizer_get_format(res, cfg, RESZ_PAD_SOURCE, sel->which) =
+			format_source;
+		return 0;
+	}
+
+	/* Update the source format, resizing ratios and crop rectangle. If
+	 * streaming is on the IRQ handler will reprogram the resizer after the
+	 * current frame. We thus we need to protect against race conditions.
+	 */
+	spin_lock_irqsave(&res->lock, flags);
+
+	*__resizer_get_format(res, cfg, RESZ_PAD_SOURCE, sel->which) =
+		format_source;
+
+	res->ratio = ratio;
+	res->crop.active = sel->r;
+
+	if (res->state != ISP_PIPELINE_STREAM_STOPPED)
+		res->applycrop = 1;
+
+	spin_unlock_irqrestore(&res->lock, flags);
+
+	return 0;
+}
+
+/* resizer pixel formats */
+static const unsigned int resizer_formats[] = {
+	MEDIA_BUS_FMT_UYVY8_1X16,
+	MEDIA_BUS_FMT_YUYV8_1X16,
+};
+
+static unsigned int resizer_max_in_width(struct isp_res_device *res)
+{
+	struct isp_device *isp = to_isp_device(res);
+
+	if (res->input == RESIZER_INPUT_MEMORY) {
+		return MAX_IN_WIDTH_MEMORY_MODE;
+	} else {
+		if (isp->revision == ISP_REVISION_1_0)
+			return MAX_IN_WIDTH_ONTHEFLY_MODE_ES1;
+		else
+			return MAX_IN_WIDTH_ONTHEFLY_MODE_ES2;
+	}
+}
+
+/*
+ * resizer_try_format - Handle try format by pad subdev method
+ * @res   : ISP resizer device
+ * @cfg: V4L2 subdev pad configuration
+ * @pad   : pad num
+ * @fmt   : pointer to v4l2 format structure
+ * @which : wanted subdev format
+ */
+static void resizer_try_format(struct isp_res_device *res,
+			       struct v4l2_subdev_pad_config *cfg, unsigned int pad,
+			       struct v4l2_mbus_framefmt *fmt,
+			       enum v4l2_subdev_format_whence which)
+{
+	struct v4l2_mbus_framefmt *format;
+	struct resizer_ratio ratio;
+	struct v4l2_rect crop;
+
+	switch (pad) {
+	case RESZ_PAD_SINK:
+		if (fmt->code != MEDIA_BUS_FMT_YUYV8_1X16 &&
+		    fmt->code != MEDIA_BUS_FMT_UYVY8_1X16)
+			fmt->code = MEDIA_BUS_FMT_YUYV8_1X16;
+
+		fmt->width = clamp_t(u32, fmt->width, MIN_IN_WIDTH,
+				     resizer_max_in_width(res));
+		fmt->height = clamp_t(u32, fmt->height, MIN_IN_HEIGHT,
+				      MAX_IN_HEIGHT);
+		break;
+
+	case RESZ_PAD_SOURCE:
+		format = __resizer_get_format(res, cfg, RESZ_PAD_SINK, which);
+		fmt->code = format->code;
+
+		crop = *__resizer_get_crop(res, cfg, which);
+		resizer_calc_ratios(res, &crop, fmt, &ratio);
+		break;
+	}
+
+	fmt->colorspace = V4L2_COLORSPACE_JPEG;
+	fmt->field = V4L2_FIELD_NONE;
+}
+
+/*
+ * resizer_enum_mbus_code - Handle pixel format enumeration
+ * @sd     : pointer to v4l2 subdev structure
+ * @cfg: V4L2 subdev pad configuration
+ * @code   : pointer to v4l2_subdev_mbus_code_enum structure
+ * return -EINVAL or zero on success
+ */
+static int resizer_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_pad_config *cfg,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct isp_res_device *res = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	if (code->pad == RESZ_PAD_SINK) {
+		if (code->index >= ARRAY_SIZE(resizer_formats))
+			return -EINVAL;
+
+		code->code = resizer_formats[code->index];
+	} else {
+		if (code->index != 0)
+			return -EINVAL;
+
+		format = __resizer_get_format(res, cfg, RESZ_PAD_SINK,
+					      code->which);
+		code->code = format->code;
+	}
+
+	return 0;
+}
+
+static int resizer_enum_frame_size(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_pad_config *cfg,
+				   struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct isp_res_device *res = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt format;
+
+	if (fse->index != 0)
+		return -EINVAL;
+
+	format.code = fse->code;
+	format.width = 1;
+	format.height = 1;
+	resizer_try_format(res, cfg, fse->pad, &format, fse->which);
+	fse->min_width = format.width;
+	fse->min_height = format.height;
+
+	if (format.code != fse->code)
+		return -EINVAL;
+
+	format.code = fse->code;
+	format.width = -1;
+	format.height = -1;
+	resizer_try_format(res, cfg, fse->pad, &format, fse->which);
+	fse->max_width = format.width;
+	fse->max_height = format.height;
+
+	return 0;
+}
+
+/*
+ * resizer_get_format - Handle get format by pads subdev method
+ * @sd    : pointer to v4l2 subdev structure
+ * @cfg: V4L2 subdev pad configuration
+ * @fmt   : pointer to v4l2 subdev format structure
+ * return -EINVAL or zero on success
+ */
+static int resizer_get_format(struct v4l2_subdev *sd, struct v4l2_subdev_pad_config *cfg,
+			      struct v4l2_subdev_format *fmt)
+{
+	struct isp_res_device *res = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+
+	format = __resizer_get_format(res, cfg, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
+
+	fmt->format = *format;
+	return 0;
+}
+
+/*
+ * resizer_set_format - Handle set format by pads subdev method
+ * @sd    : pointer to v4l2 subdev structure
+ * @cfg: V4L2 subdev pad configuration
+ * @fmt   : pointer to v4l2 subdev format structure
+ * return -EINVAL or zero on success
+ */
+static int resizer_set_format(struct v4l2_subdev *sd, struct v4l2_subdev_pad_config *cfg,
+			      struct v4l2_subdev_format *fmt)
+{
+	struct isp_res_device *res = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *format;
+	struct v4l2_rect *crop;
+
+	format = __resizer_get_format(res, cfg, fmt->pad, fmt->which);
+	if (format == NULL)
+		return -EINVAL;
+
+	resizer_try_format(res, cfg, fmt->pad, &fmt->format, fmt->which);
+	*format = fmt->format;
+
+	if (fmt->pad == RESZ_PAD_SINK) {
+		/* reset crop rectangle */
+		crop = __resizer_get_crop(res, cfg, fmt->which);
+		crop->left = 0;
+		crop->top = 0;
+		crop->width = fmt->format.width;
+		crop->height = fmt->format.height;
+
+		/* Propagate the format from sink to source */
+		format = __resizer_get_format(res, cfg, RESZ_PAD_SOURCE,
+					      fmt->which);
+		*format = fmt->format;
+		resizer_try_format(res, cfg, RESZ_PAD_SOURCE, format,
+				   fmt->which);
+	}
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		/* Compute and store the active crop rectangle and resizer
+		 * ratios. format already points to the source pad active
+		 * format.
+		 */
+		res->crop.active = res->crop.request;
+		resizer_calc_ratios(res, &res->crop.active, format,
+				       &res->ratio);
+	}
+
+	return 0;
+}
+
+static int resizer_link_validate(struct v4l2_subdev *sd,
+				 struct media_link *link,
+				 struct v4l2_subdev_format *source_fmt,
+				 struct v4l2_subdev_format *sink_fmt)
+{
+	struct isp_res_device *res = v4l2_get_subdevdata(sd);
+	struct isp_pipeline *pipe = to_isp_pipeline(&sd->entity);
+
+	omap3isp_resizer_max_rate(res, &pipe->max_rate);
+
+	return v4l2_subdev_link_validate_default(sd, link,
+						 source_fmt, sink_fmt);
+}
+
+/*
+ * resizer_init_formats - Initialize formats on all pads
+ * @sd: ISP resizer V4L2 subdevice
+ * @fh: V4L2 subdev file handle
+ *
+ * Initialize all pad formats with default values. If fh is not NULL, try
+ * formats are initialized on the file handle. Otherwise active formats are
+ * initialized on the device.
+ */
+static int resizer_init_formats(struct v4l2_subdev *sd,
+				struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_subdev_format format;
+
+	memset(&format, 0, sizeof(format));
+	format.pad = RESZ_PAD_SINK;
+	format.which = fh ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE;
+	format.format.code = MEDIA_BUS_FMT_YUYV8_1X16;
+	format.format.width = 4096;
+	format.format.height = 4096;
+	resizer_set_format(sd, fh ? fh->pad : NULL, &format);
+
+	return 0;
+}
+
+/* subdev video operations */
+static const struct v4l2_subdev_video_ops resizer_v4l2_video_ops = {
+	.s_stream = resizer_set_stream,
+};
+
+/* subdev pad operations */
+static const struct v4l2_subdev_pad_ops resizer_v4l2_pad_ops = {
+	.enum_mbus_code = resizer_enum_mbus_code,
+	.enum_frame_size = resizer_enum_frame_size,
+	.get_fmt = resizer_get_format,
+	.set_fmt = resizer_set_format,
+	.get_selection = resizer_get_selection,
+	.set_selection = resizer_set_selection,
+	.link_validate = resizer_link_validate,
+};
+
+/* subdev operations */
+static const struct v4l2_subdev_ops resizer_v4l2_ops = {
+	.video = &resizer_v4l2_video_ops,
+	.pad = &resizer_v4l2_pad_ops,
+};
+
+/* subdev internal operations */
+static const struct v4l2_subdev_internal_ops resizer_v4l2_internal_ops = {
+	.open = resizer_init_formats,
+};
+
+/* -----------------------------------------------------------------------------
+ * Media entity operations
+ */
+
+/*
+ * resizer_link_setup - Setup resizer connections.
+ * @entity : Pointer to media entity structure
+ * @local  : Pointer to local pad array
+ * @remote : Pointer to remote pad array
+ * @flags  : Link flags
+ * return -EINVAL or zero on success
+ */
+static int resizer_link_setup(struct media_entity *entity,
+			      const struct media_pad *local,
+			      const struct media_pad *remote, u32 flags)
+{
+	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+	struct isp_res_device *res = v4l2_get_subdevdata(sd);
+
+	switch (local->index | media_entity_type(remote->entity)) {
+	case RESZ_PAD_SINK | MEDIA_ENT_T_DEVNODE:
+		/* read from memory */
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (res->input == RESIZER_INPUT_VP)
+				return -EBUSY;
+			res->input = RESIZER_INPUT_MEMORY;
+		} else {
+			if (res->input == RESIZER_INPUT_MEMORY)
+				res->input = RESIZER_INPUT_NONE;
+		}
+		break;
+
+	case RESZ_PAD_SINK | MEDIA_ENT_T_V4L2_SUBDEV:
+		/* read from ccdc or previewer */
+		if (flags & MEDIA_LNK_FL_ENABLED) {
+			if (res->input == RESIZER_INPUT_MEMORY)
+				return -EBUSY;
+			res->input = RESIZER_INPUT_VP;
+		} else {
+			if (res->input == RESIZER_INPUT_VP)
+				res->input = RESIZER_INPUT_NONE;
+		}
+		break;
+
+	case RESZ_PAD_SOURCE | MEDIA_ENT_T_DEVNODE:
+		/* resizer always write to memory */
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* media operations */
+static const struct media_entity_operations resizer_media_ops = {
+	.link_setup = resizer_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+void omap3isp_resizer_unregister_entities(struct isp_res_device *res)
+{
+	v4l2_device_unregister_subdev(&res->subdev);
+	omap3isp_video_unregister(&res->video_in);
+	omap3isp_video_unregister(&res->video_out);
+}
+
+int omap3isp_resizer_register_entities(struct isp_res_device *res,
+				       struct v4l2_device *vdev)
+{
+	int ret;
+
+	/* Register the subdev and video nodes. */
+	ret = v4l2_device_register_subdev(vdev, &res->subdev);
+	if (ret < 0)
+		goto error;
+
+	ret = omap3isp_video_register(&res->video_in, vdev);
+	if (ret < 0)
+		goto error;
+
+	ret = omap3isp_video_register(&res->video_out, vdev);
+	if (ret < 0)
+		goto error;
+
+	return 0;
+
+error:
+	omap3isp_resizer_unregister_entities(res);
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * ISP resizer initialization and cleanup
+ */
+
+/*
+ * resizer_init_entities - Initialize resizer subdev and media entity.
+ * @res : Pointer to resizer device structure
+ * return -ENOMEM or zero on success
+ */
+static int resizer_init_entities(struct isp_res_device *res)
+{
+	struct v4l2_subdev *sd = &res->subdev;
+	struct media_pad *pads = res->pads;
+	struct media_entity *me = &sd->entity;
+	int ret;
+
+	res->input = RESIZER_INPUT_NONE;
+
+	v4l2_subdev_init(sd, &resizer_v4l2_ops);
+	sd->internal_ops = &resizer_v4l2_internal_ops;
+	strlcpy(sd->name, "OMAP3 ISP resizer", sizeof(sd->name));
+	sd->grp_id = 1 << 16;	/* group ID for isp subdevs */
+	v4l2_set_subdevdata(sd, res);
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	pads[RESZ_PAD_SINK].flags = MEDIA_PAD_FL_SINK
+				    | MEDIA_PAD_FL_MUST_CONNECT;
+	pads[RESZ_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
+
+	me->ops = &resizer_media_ops;
+	ret = media_entity_init(me, RESZ_PADS_NUM, pads, 0);
+	if (ret < 0)
+		return ret;
+
+	resizer_init_formats(sd, NULL);
+
+	res->video_in.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	res->video_in.ops = &resizer_video_ops;
+	res->video_in.isp = to_isp_device(res);
+	res->video_in.capture_mem = PAGE_ALIGN(4096 * 4096) * 2 * 3;
+	res->video_in.bpl_alignment = 32;
+	res->video_out.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	res->video_out.ops = &resizer_video_ops;
+	res->video_out.isp = to_isp_device(res);
+	res->video_out.capture_mem = PAGE_ALIGN(4096 * 4096) * 2 * 3;
+	res->video_out.bpl_alignment = 32;
+
+	ret = omap3isp_video_init(&res->video_in, "resizer");
+	if (ret < 0)
+		goto error_video_in;
+
+	ret = omap3isp_video_init(&res->video_out, "resizer");
+	if (ret < 0)
+		goto error_video_out;
+
+	res->video_out.video.entity.flags |= MEDIA_ENT_FL_DEFAULT;
+
+	/* Connect the video nodes to the resizer subdev. */
+	ret = media_entity_create_link(&res->video_in.video.entity, 0,
+			&res->subdev.entity, RESZ_PAD_SINK, 0);
+	if (ret < 0)
+		goto error_link;
+
+	ret = media_entity_create_link(&res->subdev.entity, RESZ_PAD_SOURCE,
+			&res->video_out.video.entity, 0, 0);
+	if (ret < 0)
+		goto error_link;
+
+	return 0;
+
+error_link:
+	omap3isp_video_cleanup(&res->video_out);
+error_video_out:
+	omap3isp_video_cleanup(&res->video_in);
+error_video_in:
+	media_entity_cleanup(&res->subdev.entity);
+	return ret;
+}
+
+/*
+ * isp_resizer_init - Resizer initialization.
+ * @isp : Pointer to ISP device
+ * return -ENOMEM or zero on success
+ */
+int omap3isp_resizer_init(struct isp_device *isp)
+{
+	struct isp_res_device *res = &isp->isp_res;
+
+	init_waitqueue_head(&res->wait);
+	atomic_set(&res->stopping, 0);
+	spin_lock_init(&res->lock);
+
+	return resizer_init_entities(res);
+}
+
+void omap3isp_resizer_cleanup(struct isp_device *isp)
+{
+	struct isp_res_device *res = &isp->isp_res;
+
+	omap3isp_video_cleanup(&res->video_in);
+	omap3isp_video_cleanup(&res->video_out);
+	media_entity_cleanup(&res->subdev.entity);
+}
diff --git a/drivers/media/platform/omap3isp/ispresizer.h b/drivers/media/platform/omap3isp/ispresizer.h
new file mode 100644
index 0000000..5414542
--- /dev/null
+++ b/drivers/media/platform/omap3isp/ispresizer.h
@@ -0,0 +1,139 @@
+/*
+ * ispresizer.h
+ *
+ * TI OMAP3 ISP - Resizer module
+ *
+ * Copyright (C) 2010 Nokia Corporation
+ * Copyright (C) 2009 Texas Instruments, Inc
+ *
+ * Contacts: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *	     Sakari Ailus <sakari.ailus@iki.fi>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef OMAP3_ISP_RESIZER_H
+#define OMAP3_ISP_RESIZER_H
+
+#include <linux/spinlock.h>
+#include <linux/types.h>
+
+/*
+ * Constants for filter coefficients count
+ */
+#define COEFF_CNT		32
+
+/*
+ * struct isprsz_coef - Structure for resizer filter coefficients.
+ * @h_filter_coef_4tap: Horizontal filter coefficients for 8-phase/4-tap
+ *			mode (.5x-4x)
+ * @v_filter_coef_4tap: Vertical filter coefficients for 8-phase/4-tap
+ *			mode (.5x-4x)
+ * @h_filter_coef_7tap: Horizontal filter coefficients for 4-phase/7-tap
+ *			mode (.25x-.5x)
+ * @v_filter_coef_7tap: Vertical filter coefficients for 4-phase/7-tap
+ *			mode (.25x-.5x)
+ */
+struct isprsz_coef {
+	u16 h_filter_coef_4tap[32];
+	u16 v_filter_coef_4tap[32];
+	/* Every 8th value is a dummy value in the following arrays: */
+	u16 h_filter_coef_7tap[32];
+	u16 v_filter_coef_7tap[32];
+};
+
+/* Chrominance horizontal algorithm */
+enum resizer_chroma_algo {
+	RSZ_THE_SAME = 0,	/* Chrominance the same as Luminance */
+	RSZ_BILINEAR = 1,	/* Chrominance uses bilinear interpolation */
+};
+
+/* Resizer input type select */
+enum resizer_colors_type {
+	RSZ_YUV422 = 0,		/* YUV422 color is interleaved */
+	RSZ_COLOR8 = 1,		/* Color separate data on 8 bits */
+};
+
+/*
+ * Structure for horizontal and vertical resizing value
+ */
+struct resizer_ratio {
+	u32 horz;
+	u32 vert;
+};
+
+/*
+ * Structure for luminance enhancer parameters.
+ */
+struct resizer_luma_yenh {
+	u8 algo;		/* algorithm select. */
+	u8 gain;		/* maximum gain. */
+	u8 slope;		/* slope. */
+	u8 core;		/* core offset. */
+};
+
+enum resizer_input_entity {
+	RESIZER_INPUT_NONE,
+	RESIZER_INPUT_VP,	/* input video port - prev or ccdc */
+	RESIZER_INPUT_MEMORY,
+};
+
+/* Sink and source resizer pads */
+#define RESZ_PAD_SINK			0
+#define RESZ_PAD_SOURCE			1
+#define RESZ_PADS_NUM			2
+
+/*
+ * struct isp_res_device - OMAP3 ISP resizer module
+ * @lock: Protects formats and crop rectangles between set_selection and IRQ
+ * @crop.request: Crop rectangle requested by the user
+ * @crop.active: Active crop rectangle (based on hardware requirements)
+ */
+struct isp_res_device {
+	struct v4l2_subdev subdev;
+	struct media_pad pads[RESZ_PADS_NUM];
+	struct v4l2_mbus_framefmt formats[RESZ_PADS_NUM];
+
+	enum resizer_input_entity input;
+	struct isp_video video_in;
+	struct isp_video video_out;
+
+	u32 addr_base;   /* stored source buffer address in memory mode */
+	u32 crop_offset; /* additional offset for crop in memory mode */
+	struct resizer_ratio ratio;
+	int pm_state;
+	unsigned int applycrop:1;
+	enum isp_pipeline_stream_state state;
+	wait_queue_head_t wait;
+	atomic_t stopping;
+	spinlock_t lock;
+
+	struct {
+		struct v4l2_rect request;
+		struct v4l2_rect active;
+	} crop;
+};
+
+struct isp_device;
+
+int omap3isp_resizer_init(struct isp_device *isp);
+void omap3isp_resizer_cleanup(struct isp_device *isp);
+
+int omap3isp_resizer_register_entities(struct isp_res_device *res,
+				       struct v4l2_device *vdev);
+void omap3isp_resizer_unregister_entities(struct isp_res_device *res);
+void omap3isp_resizer_isr_frame_sync(struct isp_res_device *res);
+void omap3isp_resizer_isr(struct isp_res_device *isp_res);
+
+void omap3isp_resizer_max_rate(struct isp_res_device *res,
+			       unsigned int *max_rate);
+
+void omap3isp_resizer_suspend(struct isp_res_device *isp_res);
+
+void omap3isp_resizer_resume(struct isp_res_device *isp_res);
+
+int omap3isp_resizer_busy(struct isp_res_device *isp_res);
+
+#endif	/* OMAP3_ISP_RESIZER_H */
diff --git a/drivers/media/platform/omap3isp/ispstat.c b/drivers/media/platform/omap3isp/ispstat.c
new file mode 100644
index 0000000..94d4c29
--- /dev/null
+++ b/drivers/media/platform/omap3isp/ispstat.c
@@ -0,0 +1,1062 @@
+/*
+ * ispstat.c
+ *
+ * TI OMAP3 ISP - Statistics core
+ *
+ * Copyright (C) 2010 Nokia Corporation
+ * Copyright (C) 2009 Texas Instruments, Inc
+ *
+ * Contacts: David Cohen <dacohen@gmail.com>
+ *	     Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *	     Sakari Ailus <sakari.ailus@iki.fi>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/dma-mapping.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+
+#include "isp.h"
+
+#define ISP_STAT_USES_DMAENGINE(stat)	((stat)->dma_ch != NULL)
+
+/*
+ * MAGIC_SIZE must always be the greatest common divisor of
+ * AEWB_PACKET_SIZE and AF_PAXEL_SIZE.
+ */
+#define MAGIC_SIZE		16
+#define MAGIC_NUM		0x55
+
+/* HACK: AF module seems to be writing one more paxel data than it should. */
+#define AF_EXTRA_DATA		OMAP3ISP_AF_PAXEL_SIZE
+
+/*
+ * HACK: H3A modules go to an invalid state after have a SBL overflow. It makes
+ * the next buffer to start to be written in the same point where the overflow
+ * occurred instead of the configured address. The only known way to make it to
+ * go back to a valid state is having a valid buffer processing. Of course it
+ * requires at least a doubled buffer size to avoid an access to invalid memory
+ * region. But it does not fix everything. It may happen more than one
+ * consecutive SBL overflows. In that case, it might be unpredictable how many
+ * buffers the allocated memory should fit. For that case, a recover
+ * configuration was created. It produces the minimum buffer size for each H3A
+ * module and decrease the change for more SBL overflows. This recover state
+ * will be enabled every time a SBL overflow occur. As the output buffer size
+ * isn't big, it's possible to have an extra size able to fit many recover
+ * buffers making it extreamily unlikely to have an access to invalid memory
+ * region.
+ */
+#define NUM_H3A_RECOVER_BUFS	10
+
+/*
+ * HACK: Because of HW issues the generic layer sometimes need to have
+ * different behaviour for different statistic modules.
+ */
+#define IS_H3A_AF(stat)		((stat) == &(stat)->isp->isp_af)
+#define IS_H3A_AEWB(stat)	((stat) == &(stat)->isp->isp_aewb)
+#define IS_H3A(stat)		(IS_H3A_AF(stat) || IS_H3A_AEWB(stat))
+
+static void __isp_stat_buf_sync_magic(struct ispstat *stat,
+				      struct ispstat_buffer *buf,
+				      u32 buf_size, enum dma_data_direction dir,
+				      void (*dma_sync)(struct device *,
+					dma_addr_t, unsigned long, size_t,
+					enum dma_data_direction))
+{
+	/* Sync the initial and final magic words. */
+	dma_sync(stat->isp->dev, buf->dma_addr, 0, MAGIC_SIZE, dir);
+	dma_sync(stat->isp->dev, buf->dma_addr + (buf_size & PAGE_MASK),
+		 buf_size & ~PAGE_MASK, MAGIC_SIZE, dir);
+}
+
+static void isp_stat_buf_sync_magic_for_device(struct ispstat *stat,
+					       struct ispstat_buffer *buf,
+					       u32 buf_size,
+					       enum dma_data_direction dir)
+{
+	if (ISP_STAT_USES_DMAENGINE(stat))
+		return;
+
+	__isp_stat_buf_sync_magic(stat, buf, buf_size, dir,
+				  dma_sync_single_range_for_device);
+}
+
+static void isp_stat_buf_sync_magic_for_cpu(struct ispstat *stat,
+					    struct ispstat_buffer *buf,
+					    u32 buf_size,
+					    enum dma_data_direction dir)
+{
+	if (ISP_STAT_USES_DMAENGINE(stat))
+		return;
+
+	__isp_stat_buf_sync_magic(stat, buf, buf_size, dir,
+				  dma_sync_single_range_for_cpu);
+}
+
+static int isp_stat_buf_check_magic(struct ispstat *stat,
+				    struct ispstat_buffer *buf)
+{
+	const u32 buf_size = IS_H3A_AF(stat) ?
+			     buf->buf_size + AF_EXTRA_DATA : buf->buf_size;
+	u8 *w;
+	u8 *end;
+	int ret = -EINVAL;
+
+	isp_stat_buf_sync_magic_for_cpu(stat, buf, buf_size, DMA_FROM_DEVICE);
+
+	/* Checking initial magic numbers. They shouldn't be here anymore. */
+	for (w = buf->virt_addr, end = w + MAGIC_SIZE; w < end; w++)
+		if (likely(*w != MAGIC_NUM))
+			ret = 0;
+
+	if (ret) {
+		dev_dbg(stat->isp->dev, "%s: beginning magic check does not "
+					"match.\n", stat->subdev.name);
+		return ret;
+	}
+
+	/* Checking magic numbers at the end. They must be still here. */
+	for (w = buf->virt_addr + buf_size, end = w + MAGIC_SIZE;
+	     w < end; w++) {
+		if (unlikely(*w != MAGIC_NUM)) {
+			dev_dbg(stat->isp->dev, "%s: ending magic check does "
+				"not match.\n", stat->subdev.name);
+			return -EINVAL;
+		}
+	}
+
+	isp_stat_buf_sync_magic_for_device(stat, buf, buf_size,
+					   DMA_FROM_DEVICE);
+
+	return 0;
+}
+
+static void isp_stat_buf_insert_magic(struct ispstat *stat,
+				      struct ispstat_buffer *buf)
+{
+	const u32 buf_size = IS_H3A_AF(stat) ?
+			     stat->buf_size + AF_EXTRA_DATA : stat->buf_size;
+
+	isp_stat_buf_sync_magic_for_cpu(stat, buf, buf_size, DMA_FROM_DEVICE);
+
+	/*
+	 * Inserting MAGIC_NUM at the beginning and end of the buffer.
+	 * buf->buf_size is set only after the buffer is queued. For now the
+	 * right buf_size for the current configuration is pointed by
+	 * stat->buf_size.
+	 */
+	memset(buf->virt_addr, MAGIC_NUM, MAGIC_SIZE);
+	memset(buf->virt_addr + buf_size, MAGIC_NUM, MAGIC_SIZE);
+
+	isp_stat_buf_sync_magic_for_device(stat, buf, buf_size,
+					   DMA_BIDIRECTIONAL);
+}
+
+static void isp_stat_buf_sync_for_device(struct ispstat *stat,
+					 struct ispstat_buffer *buf)
+{
+	if (ISP_STAT_USES_DMAENGINE(stat))
+		return;
+
+	dma_sync_sg_for_device(stat->isp->dev, buf->sgt.sgl,
+			       buf->sgt.nents, DMA_FROM_DEVICE);
+}
+
+static void isp_stat_buf_sync_for_cpu(struct ispstat *stat,
+				      struct ispstat_buffer *buf)
+{
+	if (ISP_STAT_USES_DMAENGINE(stat))
+		return;
+
+	dma_sync_sg_for_cpu(stat->isp->dev, buf->sgt.sgl,
+			    buf->sgt.nents, DMA_FROM_DEVICE);
+}
+
+static void isp_stat_buf_clear(struct ispstat *stat)
+{
+	int i;
+
+	for (i = 0; i < STAT_MAX_BUFS; i++)
+		stat->buf[i].empty = 1;
+}
+
+static struct ispstat_buffer *
+__isp_stat_buf_find(struct ispstat *stat, int look_empty)
+{
+	struct ispstat_buffer *found = NULL;
+	int i;
+
+	for (i = 0; i < STAT_MAX_BUFS; i++) {
+		struct ispstat_buffer *curr = &stat->buf[i];
+
+		/*
+		 * Don't select the buffer which is being copied to
+		 * userspace or used by the module.
+		 */
+		if (curr == stat->locked_buf || curr == stat->active_buf)
+			continue;
+
+		/* Don't select uninitialised buffers if it's not required */
+		if (!look_empty && curr->empty)
+			continue;
+
+		/* Pick uninitialised buffer over anything else if look_empty */
+		if (curr->empty) {
+			found = curr;
+			break;
+		}
+
+		/* Choose the oldest buffer */
+		if (!found ||
+		    (s32)curr->frame_number - (s32)found->frame_number < 0)
+			found = curr;
+	}
+
+	return found;
+}
+
+static inline struct ispstat_buffer *
+isp_stat_buf_find_oldest(struct ispstat *stat)
+{
+	return __isp_stat_buf_find(stat, 0);
+}
+
+static inline struct ispstat_buffer *
+isp_stat_buf_find_oldest_or_empty(struct ispstat *stat)
+{
+	return __isp_stat_buf_find(stat, 1);
+}
+
+static int isp_stat_buf_queue(struct ispstat *stat)
+{
+	if (!stat->active_buf)
+		return STAT_NO_BUF;
+
+	v4l2_get_timestamp(&stat->active_buf->ts);
+
+	stat->active_buf->buf_size = stat->buf_size;
+	if (isp_stat_buf_check_magic(stat, stat->active_buf)) {
+		dev_dbg(stat->isp->dev, "%s: data wasn't properly written.\n",
+			stat->subdev.name);
+		return STAT_NO_BUF;
+	}
+	stat->active_buf->config_counter = stat->config_counter;
+	stat->active_buf->frame_number = stat->frame_number;
+	stat->active_buf->empty = 0;
+	stat->active_buf = NULL;
+
+	return STAT_BUF_DONE;
+}
+
+/* Get next free buffer to write the statistics to and mark it active. */
+static void isp_stat_buf_next(struct ispstat *stat)
+{
+	if (unlikely(stat->active_buf))
+		/* Overwriting unused active buffer */
+		dev_dbg(stat->isp->dev, "%s: new buffer requested without "
+					"queuing active one.\n",
+					stat->subdev.name);
+	else
+		stat->active_buf = isp_stat_buf_find_oldest_or_empty(stat);
+}
+
+static void isp_stat_buf_release(struct ispstat *stat)
+{
+	unsigned long flags;
+
+	isp_stat_buf_sync_for_device(stat, stat->locked_buf);
+	spin_lock_irqsave(&stat->isp->stat_lock, flags);
+	stat->locked_buf = NULL;
+	spin_unlock_irqrestore(&stat->isp->stat_lock, flags);
+}
+
+/* Get buffer to userspace. */
+static struct ispstat_buffer *isp_stat_buf_get(struct ispstat *stat,
+					       struct omap3isp_stat_data *data)
+{
+	int rval = 0;
+	unsigned long flags;
+	struct ispstat_buffer *buf;
+
+	spin_lock_irqsave(&stat->isp->stat_lock, flags);
+
+	while (1) {
+		buf = isp_stat_buf_find_oldest(stat);
+		if (!buf) {
+			spin_unlock_irqrestore(&stat->isp->stat_lock, flags);
+			dev_dbg(stat->isp->dev, "%s: cannot find a buffer.\n",
+				stat->subdev.name);
+			return ERR_PTR(-EBUSY);
+		}
+		if (isp_stat_buf_check_magic(stat, buf)) {
+			dev_dbg(stat->isp->dev, "%s: current buffer has "
+				"corrupted data\n.", stat->subdev.name);
+			/* Mark empty because it doesn't have valid data. */
+			buf->empty = 1;
+		} else {
+			/* Buffer isn't corrupted. */
+			break;
+		}
+	}
+
+	stat->locked_buf = buf;
+
+	spin_unlock_irqrestore(&stat->isp->stat_lock, flags);
+
+	if (buf->buf_size > data->buf_size) {
+		dev_warn(stat->isp->dev, "%s: userspace's buffer size is "
+					 "not enough.\n", stat->subdev.name);
+		isp_stat_buf_release(stat);
+		return ERR_PTR(-EINVAL);
+	}
+
+	isp_stat_buf_sync_for_cpu(stat, buf);
+
+	rval = copy_to_user(data->buf,
+			    buf->virt_addr,
+			    buf->buf_size);
+
+	if (rval) {
+		dev_info(stat->isp->dev,
+			 "%s: failed copying %d bytes of stat data\n",
+			 stat->subdev.name, rval);
+		buf = ERR_PTR(-EFAULT);
+		isp_stat_buf_release(stat);
+	}
+
+	return buf;
+}
+
+static void isp_stat_bufs_free(struct ispstat *stat)
+{
+	struct device *dev = ISP_STAT_USES_DMAENGINE(stat)
+			   ? NULL : stat->isp->dev;
+	unsigned int i;
+
+	for (i = 0; i < STAT_MAX_BUFS; i++) {
+		struct ispstat_buffer *buf = &stat->buf[i];
+
+		if (!buf->virt_addr)
+			continue;
+
+		sg_free_table(&buf->sgt);
+
+		dma_free_coherent(dev, stat->buf_alloc_size, buf->virt_addr,
+				  buf->dma_addr);
+
+		buf->dma_addr = 0;
+		buf->virt_addr = NULL;
+		buf->empty = 1;
+	}
+
+	dev_dbg(stat->isp->dev, "%s: all buffers were freed.\n",
+		stat->subdev.name);
+
+	stat->buf_alloc_size = 0;
+	stat->active_buf = NULL;
+}
+
+static int isp_stat_bufs_alloc_one(struct device *dev,
+				   struct ispstat_buffer *buf,
+				   unsigned int size)
+{
+	int ret;
+
+	buf->virt_addr = dma_alloc_coherent(dev, size, &buf->dma_addr,
+					    GFP_KERNEL | GFP_DMA);
+	if (!buf->virt_addr)
+		return -ENOMEM;
+
+	ret = dma_get_sgtable(dev, &buf->sgt, buf->virt_addr, buf->dma_addr,
+			      size);
+	if (ret < 0) {
+		dma_free_coherent(dev, size, buf->virt_addr, buf->dma_addr);
+		buf->virt_addr = NULL;
+		buf->dma_addr = 0;
+		return ret;
+	}
+
+	return 0;
+}
+
+/*
+ * The device passed to the DMA API depends on whether the statistics block uses
+ * ISP DMA, external DMA or PIO to transfer data.
+ *
+ * The first case (for the AEWB and AF engines) passes the ISP device, resulting
+ * in the DMA buffers being mapped through the ISP IOMMU.
+ *
+ * The second case (for the histogram engine) should pass the DMA engine device.
+ * As that device isn't accessible through the OMAP DMA engine API the driver
+ * passes NULL instead, resulting in the buffers being mapped directly as
+ * physical pages.
+ *
+ * The third case (for the histogram engine) doesn't require any mapping. The
+ * buffers could be allocated with kmalloc/vmalloc, but we still use
+ * dma_alloc_coherent() for consistency purpose.
+ */
+static int isp_stat_bufs_alloc(struct ispstat *stat, u32 size)
+{
+	struct device *dev = ISP_STAT_USES_DMAENGINE(stat)
+			   ? NULL : stat->isp->dev;
+	unsigned long flags;
+	unsigned int i;
+
+	spin_lock_irqsave(&stat->isp->stat_lock, flags);
+
+	BUG_ON(stat->locked_buf != NULL);
+
+	/* Are the old buffers big enough? */
+	if (stat->buf_alloc_size >= size) {
+		spin_unlock_irqrestore(&stat->isp->stat_lock, flags);
+		return 0;
+	}
+
+	if (stat->state != ISPSTAT_DISABLED || stat->buf_processing) {
+		dev_info(stat->isp->dev,
+			 "%s: trying to allocate memory when busy\n",
+			 stat->subdev.name);
+		spin_unlock_irqrestore(&stat->isp->stat_lock, flags);
+		return -EBUSY;
+	}
+
+	spin_unlock_irqrestore(&stat->isp->stat_lock, flags);
+
+	isp_stat_bufs_free(stat);
+
+	stat->buf_alloc_size = size;
+
+	for (i = 0; i < STAT_MAX_BUFS; i++) {
+		struct ispstat_buffer *buf = &stat->buf[i];
+		int ret;
+
+		ret = isp_stat_bufs_alloc_one(dev, buf, size);
+		if (ret < 0) {
+			dev_err(stat->isp->dev,
+				"%s: Failed to allocate DMA buffer %u\n",
+				stat->subdev.name, i);
+			isp_stat_bufs_free(stat);
+			return ret;
+		}
+
+		buf->empty = 1;
+
+		dev_dbg(stat->isp->dev,
+			"%s: buffer[%u] allocated. dma=0x%08lx virt=0x%08lx",
+			stat->subdev.name, i,
+			(unsigned long)buf->dma_addr,
+			(unsigned long)buf->virt_addr);
+	}
+
+	return 0;
+}
+
+static void isp_stat_queue_event(struct ispstat *stat, int err)
+{
+	struct video_device *vdev = stat->subdev.devnode;
+	struct v4l2_event event;
+	struct omap3isp_stat_event_status *status = (void *)event.u.data;
+
+	memset(&event, 0, sizeof(event));
+	if (!err) {
+		status->frame_number = stat->frame_number;
+		status->config_counter = stat->config_counter;
+	} else {
+		status->buf_err = 1;
+	}
+	event.type = stat->event_type;
+	v4l2_event_queue(vdev, &event);
+}
+
+
+/*
+ * omap3isp_stat_request_statistics - Request statistics.
+ * @data: Pointer to return statistics data.
+ *
+ * Returns 0 if successful.
+ */
+int omap3isp_stat_request_statistics(struct ispstat *stat,
+				     struct omap3isp_stat_data *data)
+{
+	struct ispstat_buffer *buf;
+
+	if (stat->state != ISPSTAT_ENABLED) {
+		dev_dbg(stat->isp->dev, "%s: engine not enabled.\n",
+			stat->subdev.name);
+		return -EINVAL;
+	}
+
+	mutex_lock(&stat->ioctl_lock);
+	buf = isp_stat_buf_get(stat, data);
+	if (IS_ERR(buf)) {
+		mutex_unlock(&stat->ioctl_lock);
+		return PTR_ERR(buf);
+	}
+
+	data->ts = buf->ts;
+	data->config_counter = buf->config_counter;
+	data->frame_number = buf->frame_number;
+	data->buf_size = buf->buf_size;
+
+	buf->empty = 1;
+	isp_stat_buf_release(stat);
+	mutex_unlock(&stat->ioctl_lock);
+
+	return 0;
+}
+
+/*
+ * omap3isp_stat_config - Receives new statistic engine configuration.
+ * @new_conf: Pointer to config structure.
+ *
+ * Returns 0 if successful, -EINVAL if new_conf pointer is NULL, -ENOMEM if
+ * was unable to allocate memory for the buffer, or other errors if parameters
+ * are invalid.
+ */
+int omap3isp_stat_config(struct ispstat *stat, void *new_conf)
+{
+	int ret;
+	unsigned long irqflags;
+	struct ispstat_generic_config *user_cfg = new_conf;
+	u32 buf_size = user_cfg->buf_size;
+
+	if (!new_conf) {
+		dev_dbg(stat->isp->dev, "%s: configuration is NULL\n",
+			stat->subdev.name);
+		return -EINVAL;
+	}
+
+	mutex_lock(&stat->ioctl_lock);
+
+	dev_dbg(stat->isp->dev, "%s: configuring module with buffer "
+		"size=0x%08lx\n", stat->subdev.name, (unsigned long)buf_size);
+
+	ret = stat->ops->validate_params(stat, new_conf);
+	if (ret) {
+		mutex_unlock(&stat->ioctl_lock);
+		dev_dbg(stat->isp->dev, "%s: configuration values are "
+					"invalid.\n", stat->subdev.name);
+		return ret;
+	}
+
+	if (buf_size != user_cfg->buf_size)
+		dev_dbg(stat->isp->dev, "%s: driver has corrected buffer size "
+			"request to 0x%08lx\n", stat->subdev.name,
+			(unsigned long)user_cfg->buf_size);
+
+	/*
+	 * Hack: H3A modules may need a doubled buffer size to avoid access
+	 * to a invalid memory address after a SBL overflow.
+	 * The buffer size is always PAGE_ALIGNED.
+	 * Hack 2: MAGIC_SIZE is added to buf_size so a magic word can be
+	 * inserted at the end to data integrity check purpose.
+	 * Hack 3: AF module writes one paxel data more than it should, so
+	 * the buffer allocation must consider it to avoid invalid memory
+	 * access.
+	 * Hack 4: H3A need to allocate extra space for the recover state.
+	 */
+	if (IS_H3A(stat)) {
+		buf_size = user_cfg->buf_size * 2 + MAGIC_SIZE;
+		if (IS_H3A_AF(stat))
+			/*
+			 * Adding one extra paxel data size for each recover
+			 * buffer + 2 regular ones.
+			 */
+			buf_size += AF_EXTRA_DATA * (NUM_H3A_RECOVER_BUFS + 2);
+		if (stat->recover_priv) {
+			struct ispstat_generic_config *recover_cfg =
+				stat->recover_priv;
+			buf_size += recover_cfg->buf_size *
+				    NUM_H3A_RECOVER_BUFS;
+		}
+		buf_size = PAGE_ALIGN(buf_size);
+	} else { /* Histogram */
+		buf_size = PAGE_ALIGN(user_cfg->buf_size + MAGIC_SIZE);
+	}
+
+	ret = isp_stat_bufs_alloc(stat, buf_size);
+	if (ret) {
+		mutex_unlock(&stat->ioctl_lock);
+		return ret;
+	}
+
+	spin_lock_irqsave(&stat->isp->stat_lock, irqflags);
+	stat->ops->set_params(stat, new_conf);
+	spin_unlock_irqrestore(&stat->isp->stat_lock, irqflags);
+
+	/*
+	 * Returning the right future config_counter for this setup, so
+	 * userspace can *know* when it has been applied.
+	 */
+	user_cfg->config_counter = stat->config_counter + stat->inc_config;
+
+	/* Module has a valid configuration. */
+	stat->configured = 1;
+	dev_dbg(stat->isp->dev, "%s: module has been successfully "
+		"configured.\n", stat->subdev.name);
+
+	mutex_unlock(&stat->ioctl_lock);
+
+	return 0;
+}
+
+/*
+ * isp_stat_buf_process - Process statistic buffers.
+ * @buf_state: points out if buffer is ready to be processed. It's necessary
+ *	       because histogram needs to copy the data from internal memory
+ *	       before be able to process the buffer.
+ */
+static int isp_stat_buf_process(struct ispstat *stat, int buf_state)
+{
+	int ret = STAT_NO_BUF;
+
+	if (!atomic_add_unless(&stat->buf_err, -1, 0) &&
+	    buf_state == STAT_BUF_DONE && stat->state == ISPSTAT_ENABLED) {
+		ret = isp_stat_buf_queue(stat);
+		isp_stat_buf_next(stat);
+	}
+
+	return ret;
+}
+
+int omap3isp_stat_pcr_busy(struct ispstat *stat)
+{
+	return stat->ops->busy(stat);
+}
+
+int omap3isp_stat_busy(struct ispstat *stat)
+{
+	return omap3isp_stat_pcr_busy(stat) | stat->buf_processing |
+		(stat->state != ISPSTAT_DISABLED);
+}
+
+/*
+ * isp_stat_pcr_enable - Disables/Enables statistic engines.
+ * @pcr_enable: 0/1 - Disables/Enables the engine.
+ *
+ * Must be called from ISP driver when the module is idle and synchronized
+ * with CCDC.
+ */
+static void isp_stat_pcr_enable(struct ispstat *stat, u8 pcr_enable)
+{
+	if ((stat->state != ISPSTAT_ENABLING &&
+	     stat->state != ISPSTAT_ENABLED) && pcr_enable)
+		/* Userspace has disabled the module. Aborting. */
+		return;
+
+	stat->ops->enable(stat, pcr_enable);
+	if (stat->state == ISPSTAT_DISABLING && !pcr_enable)
+		stat->state = ISPSTAT_DISABLED;
+	else if (stat->state == ISPSTAT_ENABLING && pcr_enable)
+		stat->state = ISPSTAT_ENABLED;
+}
+
+void omap3isp_stat_suspend(struct ispstat *stat)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&stat->isp->stat_lock, flags);
+
+	if (stat->state != ISPSTAT_DISABLED)
+		stat->ops->enable(stat, 0);
+	if (stat->state == ISPSTAT_ENABLED)
+		stat->state = ISPSTAT_SUSPENDED;
+
+	spin_unlock_irqrestore(&stat->isp->stat_lock, flags);
+}
+
+void omap3isp_stat_resume(struct ispstat *stat)
+{
+	/* Module will be re-enabled with its pipeline */
+	if (stat->state == ISPSTAT_SUSPENDED)
+		stat->state = ISPSTAT_ENABLING;
+}
+
+static void isp_stat_try_enable(struct ispstat *stat)
+{
+	unsigned long irqflags;
+
+	if (stat->priv == NULL)
+		/* driver wasn't initialised */
+		return;
+
+	spin_lock_irqsave(&stat->isp->stat_lock, irqflags);
+	if (stat->state == ISPSTAT_ENABLING && !stat->buf_processing &&
+	    stat->buf_alloc_size) {
+		/*
+		 * Userspace's requested to enable the engine but it wasn't yet.
+		 * Let's do that now.
+		 */
+		stat->update = 1;
+		isp_stat_buf_next(stat);
+		stat->ops->setup_regs(stat, stat->priv);
+		isp_stat_buf_insert_magic(stat, stat->active_buf);
+
+		/*
+		 * H3A module has some hw issues which forces the driver to
+		 * ignore next buffers even if it was disabled in the meantime.
+		 * On the other hand, Histogram shouldn't ignore buffers anymore
+		 * if it's being enabled.
+		 */
+		if (!IS_H3A(stat))
+			atomic_set(&stat->buf_err, 0);
+
+		isp_stat_pcr_enable(stat, 1);
+		spin_unlock_irqrestore(&stat->isp->stat_lock, irqflags);
+		dev_dbg(stat->isp->dev, "%s: module is enabled.\n",
+			stat->subdev.name);
+	} else {
+		spin_unlock_irqrestore(&stat->isp->stat_lock, irqflags);
+	}
+}
+
+void omap3isp_stat_isr_frame_sync(struct ispstat *stat)
+{
+	isp_stat_try_enable(stat);
+}
+
+void omap3isp_stat_sbl_overflow(struct ispstat *stat)
+{
+	unsigned long irqflags;
+
+	spin_lock_irqsave(&stat->isp->stat_lock, irqflags);
+	/*
+	 * Due to a H3A hw issue which prevents the next buffer to start from
+	 * the correct memory address, 2 buffers must be ignored.
+	 */
+	atomic_set(&stat->buf_err, 2);
+
+	/*
+	 * If more than one SBL overflow happen in a row, H3A module may access
+	 * invalid memory region.
+	 * stat->sbl_ovl_recover is set to tell to the driver to temporarily use
+	 * a soft configuration which helps to avoid consecutive overflows.
+	 */
+	if (stat->recover_priv)
+		stat->sbl_ovl_recover = 1;
+	spin_unlock_irqrestore(&stat->isp->stat_lock, irqflags);
+}
+
+/*
+ * omap3isp_stat_enable - Disable/Enable statistic engine as soon as possible
+ * @enable: 0/1 - Disables/Enables the engine.
+ *
+ * Client should configure all the module registers before this.
+ * This function can be called from a userspace request.
+ */
+int omap3isp_stat_enable(struct ispstat *stat, u8 enable)
+{
+	unsigned long irqflags;
+
+	dev_dbg(stat->isp->dev, "%s: user wants to %s module.\n",
+		stat->subdev.name, enable ? "enable" : "disable");
+
+	/* Prevent enabling while configuring */
+	mutex_lock(&stat->ioctl_lock);
+
+	spin_lock_irqsave(&stat->isp->stat_lock, irqflags);
+
+	if (!stat->configured && enable) {
+		spin_unlock_irqrestore(&stat->isp->stat_lock, irqflags);
+		mutex_unlock(&stat->ioctl_lock);
+		dev_dbg(stat->isp->dev, "%s: cannot enable module as it's "
+			"never been successfully configured so far.\n",
+			stat->subdev.name);
+		return -EINVAL;
+	}
+
+	if (enable) {
+		if (stat->state == ISPSTAT_DISABLING)
+			/* Previous disabling request wasn't done yet */
+			stat->state = ISPSTAT_ENABLED;
+		else if (stat->state == ISPSTAT_DISABLED)
+			/* Module is now being enabled */
+			stat->state = ISPSTAT_ENABLING;
+	} else {
+		if (stat->state == ISPSTAT_ENABLING) {
+			/* Previous enabling request wasn't done yet */
+			stat->state = ISPSTAT_DISABLED;
+		} else if (stat->state == ISPSTAT_ENABLED) {
+			/* Module is now being disabled */
+			stat->state = ISPSTAT_DISABLING;
+			isp_stat_buf_clear(stat);
+		}
+	}
+
+	spin_unlock_irqrestore(&stat->isp->stat_lock, irqflags);
+	mutex_unlock(&stat->ioctl_lock);
+
+	return 0;
+}
+
+int omap3isp_stat_s_stream(struct v4l2_subdev *subdev, int enable)
+{
+	struct ispstat *stat = v4l2_get_subdevdata(subdev);
+
+	if (enable) {
+		/*
+		 * Only set enable PCR bit if the module was previously
+		 * enabled through ioctl.
+		 */
+		isp_stat_try_enable(stat);
+	} else {
+		unsigned long flags;
+		/* Disable PCR bit and config enable field */
+		omap3isp_stat_enable(stat, 0);
+		spin_lock_irqsave(&stat->isp->stat_lock, flags);
+		stat->ops->enable(stat, 0);
+		spin_unlock_irqrestore(&stat->isp->stat_lock, flags);
+
+		/*
+		 * If module isn't busy, a new interrupt may come or not to
+		 * set the state to DISABLED. As Histogram needs to read its
+		 * internal memory to clear it, let interrupt handler
+		 * responsible of changing state to DISABLED. If the last
+		 * interrupt is coming, it's still safe as the handler will
+		 * ignore the second time when state is already set to DISABLED.
+		 * It's necessary to synchronize Histogram with streamoff, once
+		 * the module may be considered idle before last SDMA transfer
+		 * starts if we return here.
+		 */
+		if (!omap3isp_stat_pcr_busy(stat))
+			omap3isp_stat_isr(stat);
+
+		dev_dbg(stat->isp->dev, "%s: module is being disabled\n",
+			stat->subdev.name);
+	}
+
+	return 0;
+}
+
+/*
+ * __stat_isr - Interrupt handler for statistic drivers
+ */
+static void __stat_isr(struct ispstat *stat, int from_dma)
+{
+	int ret = STAT_BUF_DONE;
+	int buf_processing;
+	unsigned long irqflags;
+	struct isp_pipeline *pipe;
+
+	/*
+	 * stat->buf_processing must be set before disable module. It's
+	 * necessary to not inform too early the buffers aren't busy in case
+	 * of SDMA is going to be used.
+	 */
+	spin_lock_irqsave(&stat->isp->stat_lock, irqflags);
+	if (stat->state == ISPSTAT_DISABLED) {
+		spin_unlock_irqrestore(&stat->isp->stat_lock, irqflags);
+		return;
+	}
+	buf_processing = stat->buf_processing;
+	stat->buf_processing = 1;
+	stat->ops->enable(stat, 0);
+
+	if (buf_processing && !from_dma) {
+		if (stat->state == ISPSTAT_ENABLED) {
+			spin_unlock_irqrestore(&stat->isp->stat_lock, irqflags);
+			dev_err(stat->isp->dev,
+				"%s: interrupt occurred when module was still "
+				"processing a buffer.\n", stat->subdev.name);
+			ret = STAT_NO_BUF;
+			goto out;
+		} else {
+			/*
+			 * Interrupt handler was called from streamoff when
+			 * the module wasn't busy anymore to ensure it is being
+			 * disabled after process last buffer. If such buffer
+			 * processing has already started, no need to do
+			 * anything else.
+			 */
+			spin_unlock_irqrestore(&stat->isp->stat_lock, irqflags);
+			return;
+		}
+	}
+	spin_unlock_irqrestore(&stat->isp->stat_lock, irqflags);
+
+	/* If it's busy we can't process this buffer anymore */
+	if (!omap3isp_stat_pcr_busy(stat)) {
+		if (!from_dma && stat->ops->buf_process)
+			/* Module still need to copy data to buffer. */
+			ret = stat->ops->buf_process(stat);
+		if (ret == STAT_BUF_WAITING_DMA)
+			/* Buffer is not ready yet */
+			return;
+
+		spin_lock_irqsave(&stat->isp->stat_lock, irqflags);
+
+		/*
+		 * Histogram needs to read its internal memory to clear it
+		 * before be disabled. For that reason, common statistic layer
+		 * can return only after call stat's buf_process() operator.
+		 */
+		if (stat->state == ISPSTAT_DISABLING) {
+			stat->state = ISPSTAT_DISABLED;
+			spin_unlock_irqrestore(&stat->isp->stat_lock, irqflags);
+			stat->buf_processing = 0;
+			return;
+		}
+		pipe = to_isp_pipeline(&stat->subdev.entity);
+		stat->frame_number = atomic_read(&pipe->frame_number);
+
+		/*
+		 * Before this point, 'ret' stores the buffer's status if it's
+		 * ready to be processed. Afterwards, it holds the status if
+		 * it was processed successfully.
+		 */
+		ret = isp_stat_buf_process(stat, ret);
+
+		if (likely(!stat->sbl_ovl_recover)) {
+			stat->ops->setup_regs(stat, stat->priv);
+		} else {
+			/*
+			 * Using recover config to increase the chance to have
+			 * a good buffer processing and make the H3A module to
+			 * go back to a valid state.
+			 */
+			stat->update = 1;
+			stat->ops->setup_regs(stat, stat->recover_priv);
+			stat->sbl_ovl_recover = 0;
+
+			/*
+			 * Set 'update' in case of the module needs to use
+			 * regular configuration after next buffer.
+			 */
+			stat->update = 1;
+		}
+
+		isp_stat_buf_insert_magic(stat, stat->active_buf);
+
+		/*
+		 * Hack: H3A modules may access invalid memory address or send
+		 * corrupted data to userspace if more than 1 SBL overflow
+		 * happens in a row without re-writing its buffer's start memory
+		 * address in the meantime. Such situation is avoided if the
+		 * module is not immediately re-enabled when the ISR misses the
+		 * timing to process the buffer and to setup the registers.
+		 * Because of that, pcr_enable(1) was moved to inside this 'if'
+		 * block. But the next interruption will still happen as during
+		 * pcr_enable(0) the module was busy.
+		 */
+		isp_stat_pcr_enable(stat, 1);
+		spin_unlock_irqrestore(&stat->isp->stat_lock, irqflags);
+	} else {
+		/*
+		 * If a SBL overflow occurs and the H3A driver misses the timing
+		 * to process the buffer, stat->buf_err is set and won't be
+		 * cleared now. So the next buffer will be correctly ignored.
+		 * It's necessary due to a hw issue which makes the next H3A
+		 * buffer to start from the memory address where the previous
+		 * one stopped, instead of start where it was configured to.
+		 * Do not "stat->buf_err = 0" here.
+		 */
+
+		if (stat->ops->buf_process)
+			/*
+			 * Driver may need to erase current data prior to
+			 * process a new buffer. If it misses the timing, the
+			 * next buffer might be wrong. So should be ignored.
+			 * It happens only for Histogram.
+			 */
+			atomic_set(&stat->buf_err, 1);
+
+		ret = STAT_NO_BUF;
+		dev_dbg(stat->isp->dev, "%s: cannot process buffer, "
+					"device is busy.\n", stat->subdev.name);
+	}
+
+out:
+	stat->buf_processing = 0;
+	isp_stat_queue_event(stat, ret != STAT_BUF_DONE);
+}
+
+void omap3isp_stat_isr(struct ispstat *stat)
+{
+	__stat_isr(stat, 0);
+}
+
+void omap3isp_stat_dma_isr(struct ispstat *stat)
+{
+	__stat_isr(stat, 1);
+}
+
+int omap3isp_stat_subscribe_event(struct v4l2_subdev *subdev,
+				  struct v4l2_fh *fh,
+				  struct v4l2_event_subscription *sub)
+{
+	struct ispstat *stat = v4l2_get_subdevdata(subdev);
+
+	if (sub->type != stat->event_type)
+		return -EINVAL;
+
+	return v4l2_event_subscribe(fh, sub, STAT_NEVENTS, NULL);
+}
+
+int omap3isp_stat_unsubscribe_event(struct v4l2_subdev *subdev,
+				    struct v4l2_fh *fh,
+				    struct v4l2_event_subscription *sub)
+{
+	return v4l2_event_unsubscribe(fh, sub);
+}
+
+void omap3isp_stat_unregister_entities(struct ispstat *stat)
+{
+	v4l2_device_unregister_subdev(&stat->subdev);
+}
+
+int omap3isp_stat_register_entities(struct ispstat *stat,
+				    struct v4l2_device *vdev)
+{
+	return v4l2_device_register_subdev(vdev, &stat->subdev);
+}
+
+static int isp_stat_init_entities(struct ispstat *stat, const char *name,
+				  const struct v4l2_subdev_ops *sd_ops)
+{
+	struct v4l2_subdev *subdev = &stat->subdev;
+	struct media_entity *me = &subdev->entity;
+
+	v4l2_subdev_init(subdev, sd_ops);
+	snprintf(subdev->name, V4L2_SUBDEV_NAME_SIZE, "OMAP3 ISP %s", name);
+	subdev->grp_id = 1 << 16;	/* group ID for isp subdevs */
+	subdev->flags |= V4L2_SUBDEV_FL_HAS_EVENTS | V4L2_SUBDEV_FL_HAS_DEVNODE;
+	v4l2_set_subdevdata(subdev, stat);
+
+	stat->pad.flags = MEDIA_PAD_FL_SINK | MEDIA_PAD_FL_MUST_CONNECT;
+	me->ops = NULL;
+
+	return media_entity_init(me, 1, &stat->pad, 0);
+}
+
+int omap3isp_stat_init(struct ispstat *stat, const char *name,
+		       const struct v4l2_subdev_ops *sd_ops)
+{
+	int ret;
+
+	stat->buf = kcalloc(STAT_MAX_BUFS, sizeof(*stat->buf), GFP_KERNEL);
+	if (!stat->buf)
+		return -ENOMEM;
+
+	isp_stat_buf_clear(stat);
+	mutex_init(&stat->ioctl_lock);
+	atomic_set(&stat->buf_err, 0);
+
+	ret = isp_stat_init_entities(stat, name, sd_ops);
+	if (ret < 0) {
+		mutex_destroy(&stat->ioctl_lock);
+		kfree(stat->buf);
+	}
+
+	return ret;
+}
+
+void omap3isp_stat_cleanup(struct ispstat *stat)
+{
+	media_entity_cleanup(&stat->subdev.entity);
+	mutex_destroy(&stat->ioctl_lock);
+	isp_stat_bufs_free(stat);
+	kfree(stat->buf);
+}
diff --git a/drivers/media/platform/omap3isp/ispstat.h b/drivers/media/platform/omap3isp/ispstat.h
new file mode 100644
index 0000000..6d9b024
--- /dev/null
+++ b/drivers/media/platform/omap3isp/ispstat.h
@@ -0,0 +1,157 @@
+/*
+ * ispstat.h
+ *
+ * TI OMAP3 ISP - Statistics core
+ *
+ * Copyright (C) 2010 Nokia Corporation
+ * Copyright (C) 2009 Texas Instruments, Inc
+ *
+ * Contacts: David Cohen <dacohen@gmail.com>
+ *	     Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *	     Sakari Ailus <sakari.ailus@iki.fi>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef OMAP3_ISP_STAT_H
+#define OMAP3_ISP_STAT_H
+
+#include <linux/types.h>
+#include <linux/omap3isp.h>
+#include <media/v4l2-event.h>
+
+#include "isp.h"
+#include "ispvideo.h"
+
+#define STAT_MAX_BUFS		5
+#define STAT_NEVENTS		8
+
+#define STAT_BUF_DONE		0	/* Buffer is ready */
+#define STAT_NO_BUF		1	/* An error has occurred */
+#define STAT_BUF_WAITING_DMA	2	/* Histogram only: DMA is running */
+
+struct dma_chan;
+struct ispstat;
+
+struct ispstat_buffer {
+	struct sg_table sgt;
+	void *virt_addr;
+	dma_addr_t dma_addr;
+	struct timeval ts;
+	u32 buf_size;
+	u32 frame_number;
+	u16 config_counter;
+	u8 empty;
+};
+
+struct ispstat_ops {
+	/*
+	 * Validate new params configuration.
+	 * new_conf->buf_size value must be changed to the exact buffer size
+	 * necessary for the new configuration if it's smaller.
+	 */
+	int (*validate_params)(struct ispstat *stat, void *new_conf);
+
+	/*
+	 * Save new params configuration.
+	 * stat->priv->buf_size value must be set to the exact buffer size for
+	 * the new configuration.
+	 * stat->update is set to 1 if new configuration is different than
+	 * current one.
+	 */
+	void (*set_params)(struct ispstat *stat, void *new_conf);
+
+	/* Apply stored configuration. */
+	void (*setup_regs)(struct ispstat *stat, void *priv);
+
+	/* Enable/Disable module. */
+	void (*enable)(struct ispstat *stat, int enable);
+
+	/* Verify is module is busy. */
+	int (*busy)(struct ispstat *stat);
+
+	/* Used for specific operations during generic buf process task. */
+	int (*buf_process)(struct ispstat *stat);
+};
+
+enum ispstat_state_t {
+	ISPSTAT_DISABLED = 0,
+	ISPSTAT_DISABLING,
+	ISPSTAT_ENABLED,
+	ISPSTAT_ENABLING,
+	ISPSTAT_SUSPENDED,
+};
+
+struct ispstat {
+	struct v4l2_subdev subdev;
+	struct media_pad pad;	/* sink pad */
+
+	/* Control */
+	unsigned configured:1;
+	unsigned update:1;
+	unsigned buf_processing:1;
+	unsigned sbl_ovl_recover:1;
+	u8 inc_config;
+	atomic_t buf_err;
+	enum ispstat_state_t state;	/* enabling/disabling state */
+	struct isp_device *isp;
+	void *priv;		/* pointer to priv config struct */
+	void *recover_priv;	/* pointer to recover priv configuration */
+	struct mutex ioctl_lock; /* serialize private ioctl */
+
+	const struct ispstat_ops *ops;
+
+	/* Buffer */
+	u8 wait_acc_frames;
+	u16 config_counter;
+	u32 frame_number;
+	u32 buf_size;
+	u32 buf_alloc_size;
+	struct dma_chan *dma_ch;
+	unsigned long event_type;
+	struct ispstat_buffer *buf;
+	struct ispstat_buffer *active_buf;
+	struct ispstat_buffer *locked_buf;
+};
+
+struct ispstat_generic_config {
+	/*
+	 * Fields must be in the same order as in:
+	 *  - omap3isp_h3a_aewb_config
+	 *  - omap3isp_h3a_af_config
+	 *  - omap3isp_hist_config
+	 */
+	u32 buf_size;
+	u16 config_counter;
+};
+
+int omap3isp_stat_config(struct ispstat *stat, void *new_conf);
+int omap3isp_stat_request_statistics(struct ispstat *stat,
+				     struct omap3isp_stat_data *data);
+int omap3isp_stat_init(struct ispstat *stat, const char *name,
+		       const struct v4l2_subdev_ops *sd_ops);
+void omap3isp_stat_cleanup(struct ispstat *stat);
+int omap3isp_stat_subscribe_event(struct v4l2_subdev *subdev,
+				  struct v4l2_fh *fh,
+				  struct v4l2_event_subscription *sub);
+int omap3isp_stat_unsubscribe_event(struct v4l2_subdev *subdev,
+				    struct v4l2_fh *fh,
+				    struct v4l2_event_subscription *sub);
+int omap3isp_stat_s_stream(struct v4l2_subdev *subdev, int enable);
+
+int omap3isp_stat_busy(struct ispstat *stat);
+int omap3isp_stat_pcr_busy(struct ispstat *stat);
+void omap3isp_stat_suspend(struct ispstat *stat);
+void omap3isp_stat_resume(struct ispstat *stat);
+int omap3isp_stat_enable(struct ispstat *stat, u8 enable);
+void omap3isp_stat_sbl_overflow(struct ispstat *stat);
+void omap3isp_stat_isr(struct ispstat *stat);
+void omap3isp_stat_isr_frame_sync(struct ispstat *stat);
+void omap3isp_stat_dma_isr(struct ispstat *stat);
+int omap3isp_stat_register_entities(struct ispstat *stat,
+				    struct v4l2_device *vdev);
+void omap3isp_stat_unregister_entities(struct ispstat *stat);
+
+#endif /* OMAP3_ISP_STAT_H */
diff --git a/drivers/media/platform/omap3isp/ispvideo.c b/drivers/media/platform/omap3isp/ispvideo.c
new file mode 100644
index 0000000..f4f5916
--- /dev/null
+++ b/drivers/media/platform/omap3isp/ispvideo.c
@@ -0,0 +1,1430 @@
+/*
+ * ispvideo.c
+ *
+ * TI OMAP3 ISP - Generic video node
+ *
+ * Copyright (C) 2009-2010 Nokia Corporation
+ *
+ * Contacts: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *	     Sakari Ailus <sakari.ailus@iki.fi>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <asm/cacheflush.h>
+#include <linux/clk.h>
+#include <linux/mm.h>
+#include <linux/module.h>
+#include <linux/pagemap.h>
+#include <linux/scatterlist.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/vmalloc.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "ispvideo.h"
+#include "isp.h"
+
+
+/* -----------------------------------------------------------------------------
+ * Helper functions
+ */
+
+/*
+ * NOTE: When adding new media bus codes, always remember to add
+ * corresponding in-memory formats to the table below!!!
+ */
+static struct isp_format_info formats[] = {
+	{ MEDIA_BUS_FMT_Y8_1X8, MEDIA_BUS_FMT_Y8_1X8,
+	  MEDIA_BUS_FMT_Y8_1X8, MEDIA_BUS_FMT_Y8_1X8,
+	  V4L2_PIX_FMT_GREY, 8, 1, },
+	{ MEDIA_BUS_FMT_Y10_1X10, MEDIA_BUS_FMT_Y10_1X10,
+	  MEDIA_BUS_FMT_Y10_1X10, MEDIA_BUS_FMT_Y8_1X8,
+	  V4L2_PIX_FMT_Y10, 10, 2, },
+	{ MEDIA_BUS_FMT_Y12_1X12, MEDIA_BUS_FMT_Y10_1X10,
+	  MEDIA_BUS_FMT_Y12_1X12, MEDIA_BUS_FMT_Y8_1X8,
+	  V4L2_PIX_FMT_Y12, 12, 2, },
+	{ MEDIA_BUS_FMT_SBGGR8_1X8, MEDIA_BUS_FMT_SBGGR8_1X8,
+	  MEDIA_BUS_FMT_SBGGR8_1X8, MEDIA_BUS_FMT_SBGGR8_1X8,
+	  V4L2_PIX_FMT_SBGGR8, 8, 1, },
+	{ MEDIA_BUS_FMT_SGBRG8_1X8, MEDIA_BUS_FMT_SGBRG8_1X8,
+	  MEDIA_BUS_FMT_SGBRG8_1X8, MEDIA_BUS_FMT_SGBRG8_1X8,
+	  V4L2_PIX_FMT_SGBRG8, 8, 1, },
+	{ MEDIA_BUS_FMT_SGRBG8_1X8, MEDIA_BUS_FMT_SGRBG8_1X8,
+	  MEDIA_BUS_FMT_SGRBG8_1X8, MEDIA_BUS_FMT_SGRBG8_1X8,
+	  V4L2_PIX_FMT_SGRBG8, 8, 1, },
+	{ MEDIA_BUS_FMT_SRGGB8_1X8, MEDIA_BUS_FMT_SRGGB8_1X8,
+	  MEDIA_BUS_FMT_SRGGB8_1X8, MEDIA_BUS_FMT_SRGGB8_1X8,
+	  V4L2_PIX_FMT_SRGGB8, 8, 1, },
+	{ MEDIA_BUS_FMT_SBGGR10_DPCM8_1X8, MEDIA_BUS_FMT_SBGGR10_DPCM8_1X8,
+	  MEDIA_BUS_FMT_SBGGR10_1X10, 0,
+	  V4L2_PIX_FMT_SBGGR10DPCM8, 8, 1, },
+	{ MEDIA_BUS_FMT_SGBRG10_DPCM8_1X8, MEDIA_BUS_FMT_SGBRG10_DPCM8_1X8,
+	  MEDIA_BUS_FMT_SGBRG10_1X10, 0,
+	  V4L2_PIX_FMT_SGBRG10DPCM8, 8, 1, },
+	{ MEDIA_BUS_FMT_SGRBG10_DPCM8_1X8, MEDIA_BUS_FMT_SGRBG10_DPCM8_1X8,
+	  MEDIA_BUS_FMT_SGRBG10_1X10, 0,
+	  V4L2_PIX_FMT_SGRBG10DPCM8, 8, 1, },
+	{ MEDIA_BUS_FMT_SRGGB10_DPCM8_1X8, MEDIA_BUS_FMT_SRGGB10_DPCM8_1X8,
+	  MEDIA_BUS_FMT_SRGGB10_1X10, 0,
+	  V4L2_PIX_FMT_SRGGB10DPCM8, 8, 1, },
+	{ MEDIA_BUS_FMT_SBGGR10_1X10, MEDIA_BUS_FMT_SBGGR10_1X10,
+	  MEDIA_BUS_FMT_SBGGR10_1X10, MEDIA_BUS_FMT_SBGGR8_1X8,
+	  V4L2_PIX_FMT_SBGGR10, 10, 2, },
+	{ MEDIA_BUS_FMT_SGBRG10_1X10, MEDIA_BUS_FMT_SGBRG10_1X10,
+	  MEDIA_BUS_FMT_SGBRG10_1X10, MEDIA_BUS_FMT_SGBRG8_1X8,
+	  V4L2_PIX_FMT_SGBRG10, 10, 2, },
+	{ MEDIA_BUS_FMT_SGRBG10_1X10, MEDIA_BUS_FMT_SGRBG10_1X10,
+	  MEDIA_BUS_FMT_SGRBG10_1X10, MEDIA_BUS_FMT_SGRBG8_1X8,
+	  V4L2_PIX_FMT_SGRBG10, 10, 2, },
+	{ MEDIA_BUS_FMT_SRGGB10_1X10, MEDIA_BUS_FMT_SRGGB10_1X10,
+	  MEDIA_BUS_FMT_SRGGB10_1X10, MEDIA_BUS_FMT_SRGGB8_1X8,
+	  V4L2_PIX_FMT_SRGGB10, 10, 2, },
+	{ MEDIA_BUS_FMT_SBGGR12_1X12, MEDIA_BUS_FMT_SBGGR10_1X10,
+	  MEDIA_BUS_FMT_SBGGR12_1X12, MEDIA_BUS_FMT_SBGGR8_1X8,
+	  V4L2_PIX_FMT_SBGGR12, 12, 2, },
+	{ MEDIA_BUS_FMT_SGBRG12_1X12, MEDIA_BUS_FMT_SGBRG10_1X10,
+	  MEDIA_BUS_FMT_SGBRG12_1X12, MEDIA_BUS_FMT_SGBRG8_1X8,
+	  V4L2_PIX_FMT_SGBRG12, 12, 2, },
+	{ MEDIA_BUS_FMT_SGRBG12_1X12, MEDIA_BUS_FMT_SGRBG10_1X10,
+	  MEDIA_BUS_FMT_SGRBG12_1X12, MEDIA_BUS_FMT_SGRBG8_1X8,
+	  V4L2_PIX_FMT_SGRBG12, 12, 2, },
+	{ MEDIA_BUS_FMT_SRGGB12_1X12, MEDIA_BUS_FMT_SRGGB10_1X10,
+	  MEDIA_BUS_FMT_SRGGB12_1X12, MEDIA_BUS_FMT_SRGGB8_1X8,
+	  V4L2_PIX_FMT_SRGGB12, 12, 2, },
+	{ MEDIA_BUS_FMT_UYVY8_1X16, MEDIA_BUS_FMT_UYVY8_1X16,
+	  MEDIA_BUS_FMT_UYVY8_1X16, 0,
+	  V4L2_PIX_FMT_UYVY, 16, 2, },
+	{ MEDIA_BUS_FMT_YUYV8_1X16, MEDIA_BUS_FMT_YUYV8_1X16,
+	  MEDIA_BUS_FMT_YUYV8_1X16, 0,
+	  V4L2_PIX_FMT_YUYV, 16, 2, },
+	{ MEDIA_BUS_FMT_UYVY8_2X8, MEDIA_BUS_FMT_UYVY8_2X8,
+	  MEDIA_BUS_FMT_UYVY8_2X8, 0,
+	  V4L2_PIX_FMT_UYVY, 8, 2, },
+	{ MEDIA_BUS_FMT_YUYV8_2X8, MEDIA_BUS_FMT_YUYV8_2X8,
+	  MEDIA_BUS_FMT_YUYV8_2X8, 0,
+	  V4L2_PIX_FMT_YUYV, 8, 2, },
+	/* Empty entry to catch the unsupported pixel code (0) used by the CCDC
+	 * module and avoid NULL pointer dereferences.
+	 */
+	{ 0, }
+};
+
+const struct isp_format_info *omap3isp_video_format_info(u32 code)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(formats); ++i) {
+		if (formats[i].code == code)
+			return &formats[i];
+	}
+
+	return NULL;
+}
+
+/*
+ * isp_video_mbus_to_pix - Convert v4l2_mbus_framefmt to v4l2_pix_format
+ * @video: ISP video instance
+ * @mbus: v4l2_mbus_framefmt format (input)
+ * @pix: v4l2_pix_format format (output)
+ *
+ * Fill the output pix structure with information from the input mbus format.
+ * The bytesperline and sizeimage fields are computed from the requested bytes
+ * per line value in the pix format and information from the video instance.
+ *
+ * Return the number of padding bytes at end of line.
+ */
+static unsigned int isp_video_mbus_to_pix(const struct isp_video *video,
+					  const struct v4l2_mbus_framefmt *mbus,
+					  struct v4l2_pix_format *pix)
+{
+	unsigned int bpl = pix->bytesperline;
+	unsigned int min_bpl;
+	unsigned int i;
+
+	memset(pix, 0, sizeof(*pix));
+	pix->width = mbus->width;
+	pix->height = mbus->height;
+
+	for (i = 0; i < ARRAY_SIZE(formats); ++i) {
+		if (formats[i].code == mbus->code)
+			break;
+	}
+
+	if (WARN_ON(i == ARRAY_SIZE(formats)))
+		return 0;
+
+	min_bpl = pix->width * formats[i].bpp;
+
+	/* Clamp the requested bytes per line value. If the maximum bytes per
+	 * line value is zero, the module doesn't support user configurable line
+	 * sizes. Override the requested value with the minimum in that case.
+	 */
+	if (video->bpl_max)
+		bpl = clamp(bpl, min_bpl, video->bpl_max);
+	else
+		bpl = min_bpl;
+
+	if (!video->bpl_zero_padding || bpl != min_bpl)
+		bpl = ALIGN(bpl, video->bpl_alignment);
+
+	pix->pixelformat = formats[i].pixelformat;
+	pix->bytesperline = bpl;
+	pix->sizeimage = pix->bytesperline * pix->height;
+	pix->colorspace = mbus->colorspace;
+	pix->field = mbus->field;
+
+	return bpl - min_bpl;
+}
+
+static void isp_video_pix_to_mbus(const struct v4l2_pix_format *pix,
+				  struct v4l2_mbus_framefmt *mbus)
+{
+	unsigned int i;
+
+	memset(mbus, 0, sizeof(*mbus));
+	mbus->width = pix->width;
+	mbus->height = pix->height;
+
+	/* Skip the last format in the loop so that it will be selected if no
+	 * match is found.
+	 */
+	for (i = 0; i < ARRAY_SIZE(formats) - 1; ++i) {
+		if (formats[i].pixelformat == pix->pixelformat)
+			break;
+	}
+
+	mbus->code = formats[i].code;
+	mbus->colorspace = pix->colorspace;
+	mbus->field = pix->field;
+}
+
+static struct v4l2_subdev *
+isp_video_remote_subdev(struct isp_video *video, u32 *pad)
+{
+	struct media_pad *remote;
+
+	remote = media_entity_remote_pad(&video->pad);
+
+	if (remote == NULL ||
+	    media_entity_type(remote->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+		return NULL;
+
+	if (pad)
+		*pad = remote->index;
+
+	return media_entity_to_v4l2_subdev(remote->entity);
+}
+
+/* Return a pointer to the ISP video instance at the far end of the pipeline. */
+static int isp_video_get_graph_data(struct isp_video *video,
+				    struct isp_pipeline *pipe)
+{
+	struct media_entity_graph graph;
+	struct media_entity *entity = &video->video.entity;
+	struct media_device *mdev = entity->parent;
+	struct isp_video *far_end = NULL;
+
+	mutex_lock(&mdev->graph_mutex);
+	media_entity_graph_walk_start(&graph, entity);
+
+	while ((entity = media_entity_graph_walk_next(&graph))) {
+		struct isp_video *__video;
+
+		pipe->entities |= 1 << entity->id;
+
+		if (far_end != NULL)
+			continue;
+
+		if (entity == &video->video.entity)
+			continue;
+
+		if (media_entity_type(entity) != MEDIA_ENT_T_DEVNODE)
+			continue;
+
+		__video = to_isp_video(media_entity_to_video_device(entity));
+		if (__video->type != video->type)
+			far_end = __video;
+	}
+
+	mutex_unlock(&mdev->graph_mutex);
+
+	if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+		pipe->input = far_end;
+		pipe->output = video;
+	} else {
+		if (far_end == NULL)
+			return -EPIPE;
+
+		pipe->input = video;
+		pipe->output = far_end;
+	}
+
+	return 0;
+}
+
+static int
+__isp_video_get_format(struct isp_video *video, struct v4l2_format *format)
+{
+	struct v4l2_subdev_format fmt;
+	struct v4l2_subdev *subdev;
+	u32 pad;
+	int ret;
+
+	subdev = isp_video_remote_subdev(video, &pad);
+	if (subdev == NULL)
+		return -EINVAL;
+
+	fmt.pad = pad;
+	fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+
+	mutex_lock(&video->mutex);
+	ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt);
+	mutex_unlock(&video->mutex);
+
+	if (ret)
+		return ret;
+
+	format->type = video->type;
+	return isp_video_mbus_to_pix(video, &fmt.format, &format->fmt.pix);
+}
+
+static int
+isp_video_check_format(struct isp_video *video, struct isp_video_fh *vfh)
+{
+	struct v4l2_format format;
+	int ret;
+
+	memcpy(&format, &vfh->format, sizeof(format));
+	ret = __isp_video_get_format(video, &format);
+	if (ret < 0)
+		return ret;
+
+	if (vfh->format.fmt.pix.pixelformat != format.fmt.pix.pixelformat ||
+	    vfh->format.fmt.pix.height != format.fmt.pix.height ||
+	    vfh->format.fmt.pix.width != format.fmt.pix.width ||
+	    vfh->format.fmt.pix.bytesperline != format.fmt.pix.bytesperline ||
+	    vfh->format.fmt.pix.sizeimage != format.fmt.pix.sizeimage ||
+	    vfh->format.fmt.pix.field != format.fmt.pix.field)
+		return -EINVAL;
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * Video queue operations
+ */
+
+static int isp_video_queue_setup(struct vb2_queue *queue,
+				 const void *parg,
+				 unsigned int *count, unsigned int *num_planes,
+				 unsigned int sizes[], void *alloc_ctxs[])
+{
+	struct isp_video_fh *vfh = vb2_get_drv_priv(queue);
+	struct isp_video *video = vfh->video;
+
+	*num_planes = 1;
+
+	sizes[0] = vfh->format.fmt.pix.sizeimage;
+	if (sizes[0] == 0)
+		return -EINVAL;
+
+	alloc_ctxs[0] = video->alloc_ctx;
+
+	*count = min(*count, video->capture_mem / PAGE_ALIGN(sizes[0]));
+
+	return 0;
+}
+
+static int isp_video_buffer_prepare(struct vb2_buffer *buf)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(buf);
+	struct isp_video_fh *vfh = vb2_get_drv_priv(buf->vb2_queue);
+	struct isp_buffer *buffer = to_isp_buffer(vbuf);
+	struct isp_video *video = vfh->video;
+	dma_addr_t addr;
+
+	/* Refuse to prepare the buffer is the video node has registered an
+	 * error. We don't need to take any lock here as the operation is
+	 * inherently racy. The authoritative check will be performed in the
+	 * queue handler, which can't return an error, this check is just a best
+	 * effort to notify userspace as early as possible.
+	 */
+	if (unlikely(video->error))
+		return -EIO;
+
+	addr = vb2_dma_contig_plane_dma_addr(buf, 0);
+	if (!IS_ALIGNED(addr, 32)) {
+		dev_dbg(video->isp->dev,
+			"Buffer address must be aligned to 32 bytes boundary.\n");
+		return -EINVAL;
+	}
+
+	vb2_set_plane_payload(&buffer->vb.vb2_buf, 0,
+			      vfh->format.fmt.pix.sizeimage);
+	buffer->dma = addr;
+
+	return 0;
+}
+
+/*
+ * isp_video_buffer_queue - Add buffer to streaming queue
+ * @buf: Video buffer
+ *
+ * In memory-to-memory mode, start streaming on the pipeline if buffers are
+ * queued on both the input and the output, if the pipeline isn't already busy.
+ * If the pipeline is busy, it will be restarted in the output module interrupt
+ * handler.
+ */
+static void isp_video_buffer_queue(struct vb2_buffer *buf)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(buf);
+	struct isp_video_fh *vfh = vb2_get_drv_priv(buf->vb2_queue);
+	struct isp_buffer *buffer = to_isp_buffer(vbuf);
+	struct isp_video *video = vfh->video;
+	struct isp_pipeline *pipe = to_isp_pipeline(&video->video.entity);
+	enum isp_pipeline_state state;
+	unsigned long flags;
+	unsigned int empty;
+	unsigned int start;
+
+	spin_lock_irqsave(&video->irqlock, flags);
+
+	if (unlikely(video->error)) {
+		vb2_buffer_done(&buffer->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+		spin_unlock_irqrestore(&video->irqlock, flags);
+		return;
+	}
+
+	empty = list_empty(&video->dmaqueue);
+	list_add_tail(&buffer->irqlist, &video->dmaqueue);
+
+	spin_unlock_irqrestore(&video->irqlock, flags);
+
+	if (empty) {
+		if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+			state = ISP_PIPELINE_QUEUE_OUTPUT;
+		else
+			state = ISP_PIPELINE_QUEUE_INPUT;
+
+		spin_lock_irqsave(&pipe->lock, flags);
+		pipe->state |= state;
+		video->ops->queue(video, buffer);
+		video->dmaqueue_flags |= ISP_VIDEO_DMAQUEUE_QUEUED;
+
+		start = isp_pipeline_ready(pipe);
+		if (start)
+			pipe->state |= ISP_PIPELINE_STREAM;
+		spin_unlock_irqrestore(&pipe->lock, flags);
+
+		if (start)
+			omap3isp_pipeline_set_stream(pipe,
+						ISP_PIPELINE_STREAM_SINGLESHOT);
+	}
+}
+
+static const struct vb2_ops isp_video_queue_ops = {
+	.queue_setup = isp_video_queue_setup,
+	.buf_prepare = isp_video_buffer_prepare,
+	.buf_queue = isp_video_buffer_queue,
+};
+
+/*
+ * omap3isp_video_buffer_next - Complete the current buffer and return the next
+ * @video: ISP video object
+ *
+ * Remove the current video buffer from the DMA queue and fill its timestamp and
+ * field count before handing it back to videobuf2.
+ *
+ * For capture video nodes the buffer state is set to VB2_BUF_STATE_DONE if no
+ * error has been flagged in the pipeline, or to VB2_BUF_STATE_ERROR otherwise.
+ * For video output nodes the buffer state is always set to VB2_BUF_STATE_DONE.
+ *
+ * The DMA queue is expected to contain at least one buffer.
+ *
+ * Return a pointer to the next buffer in the DMA queue, or NULL if the queue is
+ * empty.
+ */
+struct isp_buffer *omap3isp_video_buffer_next(struct isp_video *video)
+{
+	struct isp_pipeline *pipe = to_isp_pipeline(&video->video.entity);
+	enum isp_pipeline_state state;
+	struct isp_buffer *buf;
+	unsigned long flags;
+
+	spin_lock_irqsave(&video->irqlock, flags);
+	if (WARN_ON(list_empty(&video->dmaqueue))) {
+		spin_unlock_irqrestore(&video->irqlock, flags);
+		return NULL;
+	}
+
+	buf = list_first_entry(&video->dmaqueue, struct isp_buffer,
+			       irqlist);
+	list_del(&buf->irqlist);
+	spin_unlock_irqrestore(&video->irqlock, flags);
+
+	v4l2_get_timestamp(&buf->vb.timestamp);
+
+	/* Do frame number propagation only if this is the output video node.
+	 * Frame number either comes from the CSI receivers or it gets
+	 * incremented here if H3A is not active.
+	 * Note: There is no guarantee that the output buffer will finish
+	 * first, so the input number might lag behind by 1 in some cases.
+	 */
+	if (video == pipe->output && !pipe->do_propagation)
+		buf->vb.sequence =
+			atomic_inc_return(&pipe->frame_number);
+	else
+		buf->vb.sequence = atomic_read(&pipe->frame_number);
+
+	if (pipe->field != V4L2_FIELD_NONE)
+		buf->vb.sequence /= 2;
+
+	buf->vb.field = pipe->field;
+
+	/* Report pipeline errors to userspace on the capture device side. */
+	if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE && pipe->error) {
+		state = VB2_BUF_STATE_ERROR;
+		pipe->error = false;
+	} else {
+		state = VB2_BUF_STATE_DONE;
+	}
+
+	vb2_buffer_done(&buf->vb.vb2_buf, state);
+
+	spin_lock_irqsave(&video->irqlock, flags);
+
+	if (list_empty(&video->dmaqueue)) {
+		spin_unlock_irqrestore(&video->irqlock, flags);
+
+		if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+			state = ISP_PIPELINE_QUEUE_OUTPUT
+			      | ISP_PIPELINE_STREAM;
+		else
+			state = ISP_PIPELINE_QUEUE_INPUT
+			      | ISP_PIPELINE_STREAM;
+
+		spin_lock_irqsave(&pipe->lock, flags);
+		pipe->state &= ~state;
+		if (video->pipe.stream_state == ISP_PIPELINE_STREAM_CONTINUOUS)
+			video->dmaqueue_flags |= ISP_VIDEO_DMAQUEUE_UNDERRUN;
+		spin_unlock_irqrestore(&pipe->lock, flags);
+		return NULL;
+	}
+
+	if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE && pipe->input != NULL) {
+		spin_lock(&pipe->lock);
+		pipe->state &= ~ISP_PIPELINE_STREAM;
+		spin_unlock(&pipe->lock);
+	}
+
+	buf = list_first_entry(&video->dmaqueue, struct isp_buffer,
+			       irqlist);
+
+	spin_unlock_irqrestore(&video->irqlock, flags);
+
+	return buf;
+}
+
+/*
+ * omap3isp_video_cancel_stream - Cancel stream on a video node
+ * @video: ISP video object
+ *
+ * Cancelling a stream mark all buffers on the video node as erroneous and makes
+ * sure no new buffer can be queued.
+ */
+void omap3isp_video_cancel_stream(struct isp_video *video)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&video->irqlock, flags);
+
+	while (!list_empty(&video->dmaqueue)) {
+		struct isp_buffer *buf;
+
+		buf = list_first_entry(&video->dmaqueue,
+				       struct isp_buffer, irqlist);
+		list_del(&buf->irqlist);
+		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+	}
+
+	video->error = true;
+
+	spin_unlock_irqrestore(&video->irqlock, flags);
+}
+
+/*
+ * omap3isp_video_resume - Perform resume operation on the buffers
+ * @video: ISP video object
+ * @continuous: Pipeline is in single shot mode if 0 or continuous mode otherwise
+ *
+ * This function is intended to be used on suspend/resume scenario. It
+ * requests video queue layer to discard buffers marked as DONE if it's in
+ * continuous mode and requests ISP modules to queue again the ACTIVE buffer
+ * if there's any.
+ */
+void omap3isp_video_resume(struct isp_video *video, int continuous)
+{
+	struct isp_buffer *buf = NULL;
+
+	if (continuous && video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+		mutex_lock(&video->queue_lock);
+		vb2_discard_done(video->queue);
+		mutex_unlock(&video->queue_lock);
+	}
+
+	if (!list_empty(&video->dmaqueue)) {
+		buf = list_first_entry(&video->dmaqueue,
+				       struct isp_buffer, irqlist);
+		video->ops->queue(video, buf);
+		video->dmaqueue_flags |= ISP_VIDEO_DMAQUEUE_QUEUED;
+	} else {
+		if (continuous)
+			video->dmaqueue_flags |= ISP_VIDEO_DMAQUEUE_UNDERRUN;
+	}
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 ioctls
+ */
+
+static int
+isp_video_querycap(struct file *file, void *fh, struct v4l2_capability *cap)
+{
+	struct isp_video *video = video_drvdata(file);
+
+	strlcpy(cap->driver, ISP_VIDEO_DRIVER_NAME, sizeof(cap->driver));
+	strlcpy(cap->card, video->video.name, sizeof(cap->card));
+	strlcpy(cap->bus_info, "media", sizeof(cap->bus_info));
+
+	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OUTPUT
+		| V4L2_CAP_STREAMING | V4L2_CAP_DEVICE_CAPS;
+
+	if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+	else
+		cap->device_caps = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING;
+
+	return 0;
+}
+
+static int
+isp_video_get_format(struct file *file, void *fh, struct v4l2_format *format)
+{
+	struct isp_video_fh *vfh = to_isp_video_fh(fh);
+	struct isp_video *video = video_drvdata(file);
+
+	if (format->type != video->type)
+		return -EINVAL;
+
+	mutex_lock(&video->mutex);
+	*format = vfh->format;
+	mutex_unlock(&video->mutex);
+
+	return 0;
+}
+
+static int
+isp_video_set_format(struct file *file, void *fh, struct v4l2_format *format)
+{
+	struct isp_video_fh *vfh = to_isp_video_fh(fh);
+	struct isp_video *video = video_drvdata(file);
+	struct v4l2_mbus_framefmt fmt;
+
+	if (format->type != video->type)
+		return -EINVAL;
+
+	/* Replace unsupported field orders with sane defaults. */
+	switch (format->fmt.pix.field) {
+	case V4L2_FIELD_NONE:
+		/* Progressive is supported everywhere. */
+		break;
+	case V4L2_FIELD_ALTERNATE:
+		/* ALTERNATE is not supported on output nodes. */
+		if (video->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
+			format->fmt.pix.field = V4L2_FIELD_NONE;
+		break;
+	case V4L2_FIELD_INTERLACED:
+		/* The ISP has no concept of video standard, select the
+		 * top-bottom order when the unqualified interlaced order is
+		 * requested.
+		 */
+		format->fmt.pix.field = V4L2_FIELD_INTERLACED_TB;
+		/* Fall-through */
+	case V4L2_FIELD_INTERLACED_TB:
+	case V4L2_FIELD_INTERLACED_BT:
+		/* Interlaced orders are only supported at the CCDC output. */
+		if (video != &video->isp->isp_ccdc.video_out)
+			format->fmt.pix.field = V4L2_FIELD_NONE;
+		break;
+	case V4L2_FIELD_TOP:
+	case V4L2_FIELD_BOTTOM:
+	case V4L2_FIELD_SEQ_TB:
+	case V4L2_FIELD_SEQ_BT:
+	default:
+		/* All other field orders are currently unsupported, default to
+		 * progressive.
+		 */
+		format->fmt.pix.field = V4L2_FIELD_NONE;
+		break;
+	}
+
+	/* Fill the bytesperline and sizeimage fields by converting to media bus
+	 * format and back to pixel format.
+	 */
+	isp_video_pix_to_mbus(&format->fmt.pix, &fmt);
+	isp_video_mbus_to_pix(video, &fmt, &format->fmt.pix);
+
+	mutex_lock(&video->mutex);
+	vfh->format = *format;
+	mutex_unlock(&video->mutex);
+
+	return 0;
+}
+
+static int
+isp_video_try_format(struct file *file, void *fh, struct v4l2_format *format)
+{
+	struct isp_video *video = video_drvdata(file);
+	struct v4l2_subdev_format fmt;
+	struct v4l2_subdev *subdev;
+	u32 pad;
+	int ret;
+
+	if (format->type != video->type)
+		return -EINVAL;
+
+	subdev = isp_video_remote_subdev(video, &pad);
+	if (subdev == NULL)
+		return -EINVAL;
+
+	isp_video_pix_to_mbus(&format->fmt.pix, &fmt.format);
+
+	fmt.pad = pad;
+	fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt);
+	if (ret)
+		return ret == -ENOIOCTLCMD ? -ENOTTY : ret;
+
+	isp_video_mbus_to_pix(video, &fmt.format, &format->fmt.pix);
+	return 0;
+}
+
+static int
+isp_video_cropcap(struct file *file, void *fh, struct v4l2_cropcap *cropcap)
+{
+	struct isp_video *video = video_drvdata(file);
+	struct v4l2_subdev *subdev;
+	int ret;
+
+	subdev = isp_video_remote_subdev(video, NULL);
+	if (subdev == NULL)
+		return -EINVAL;
+
+	mutex_lock(&video->mutex);
+	ret = v4l2_subdev_call(subdev, video, cropcap, cropcap);
+	mutex_unlock(&video->mutex);
+
+	return ret == -ENOIOCTLCMD ? -ENOTTY : ret;
+}
+
+static int
+isp_video_get_crop(struct file *file, void *fh, struct v4l2_crop *crop)
+{
+	struct isp_video *video = video_drvdata(file);
+	struct v4l2_subdev_format format;
+	struct v4l2_subdev *subdev;
+	u32 pad;
+	int ret;
+
+	subdev = isp_video_remote_subdev(video, &pad);
+	if (subdev == NULL)
+		return -EINVAL;
+
+	/* Try the get crop operation first and fallback to get format if not
+	 * implemented.
+	 */
+	ret = v4l2_subdev_call(subdev, video, g_crop, crop);
+	if (ret != -ENOIOCTLCMD)
+		return ret;
+
+	format.pad = pad;
+	format.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &format);
+	if (ret < 0)
+		return ret == -ENOIOCTLCMD ? -ENOTTY : ret;
+
+	crop->c.left = 0;
+	crop->c.top = 0;
+	crop->c.width = format.format.width;
+	crop->c.height = format.format.height;
+
+	return 0;
+}
+
+static int
+isp_video_set_crop(struct file *file, void *fh, const struct v4l2_crop *crop)
+{
+	struct isp_video *video = video_drvdata(file);
+	struct v4l2_subdev *subdev;
+	int ret;
+
+	subdev = isp_video_remote_subdev(video, NULL);
+	if (subdev == NULL)
+		return -EINVAL;
+
+	mutex_lock(&video->mutex);
+	ret = v4l2_subdev_call(subdev, video, s_crop, crop);
+	mutex_unlock(&video->mutex);
+
+	return ret == -ENOIOCTLCMD ? -ENOTTY : ret;
+}
+
+static int
+isp_video_get_param(struct file *file, void *fh, struct v4l2_streamparm *a)
+{
+	struct isp_video_fh *vfh = to_isp_video_fh(fh);
+	struct isp_video *video = video_drvdata(file);
+
+	if (video->type != V4L2_BUF_TYPE_VIDEO_OUTPUT ||
+	    video->type != a->type)
+		return -EINVAL;
+
+	memset(a, 0, sizeof(*a));
+	a->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	a->parm.output.capability = V4L2_CAP_TIMEPERFRAME;
+	a->parm.output.timeperframe = vfh->timeperframe;
+
+	return 0;
+}
+
+static int
+isp_video_set_param(struct file *file, void *fh, struct v4l2_streamparm *a)
+{
+	struct isp_video_fh *vfh = to_isp_video_fh(fh);
+	struct isp_video *video = video_drvdata(file);
+
+	if (video->type != V4L2_BUF_TYPE_VIDEO_OUTPUT ||
+	    video->type != a->type)
+		return -EINVAL;
+
+	if (a->parm.output.timeperframe.denominator == 0)
+		a->parm.output.timeperframe.denominator = 1;
+
+	vfh->timeperframe = a->parm.output.timeperframe;
+
+	return 0;
+}
+
+static int
+isp_video_reqbufs(struct file *file, void *fh, struct v4l2_requestbuffers *rb)
+{
+	struct isp_video_fh *vfh = to_isp_video_fh(fh);
+	struct isp_video *video = video_drvdata(file);
+	int ret;
+
+	mutex_lock(&video->queue_lock);
+	ret = vb2_reqbufs(&vfh->queue, rb);
+	mutex_unlock(&video->queue_lock);
+
+	return ret;
+}
+
+static int
+isp_video_querybuf(struct file *file, void *fh, struct v4l2_buffer *b)
+{
+	struct isp_video_fh *vfh = to_isp_video_fh(fh);
+	struct isp_video *video = video_drvdata(file);
+	int ret;
+
+	mutex_lock(&video->queue_lock);
+	ret = vb2_querybuf(&vfh->queue, b);
+	mutex_unlock(&video->queue_lock);
+
+	return ret;
+}
+
+static int
+isp_video_qbuf(struct file *file, void *fh, struct v4l2_buffer *b)
+{
+	struct isp_video_fh *vfh = to_isp_video_fh(fh);
+	struct isp_video *video = video_drvdata(file);
+	int ret;
+
+	mutex_lock(&video->queue_lock);
+	ret = vb2_qbuf(&vfh->queue, b);
+	mutex_unlock(&video->queue_lock);
+
+	return ret;
+}
+
+static int
+isp_video_dqbuf(struct file *file, void *fh, struct v4l2_buffer *b)
+{
+	struct isp_video_fh *vfh = to_isp_video_fh(fh);
+	struct isp_video *video = video_drvdata(file);
+	int ret;
+
+	mutex_lock(&video->queue_lock);
+	ret = vb2_dqbuf(&vfh->queue, b, file->f_flags & O_NONBLOCK);
+	mutex_unlock(&video->queue_lock);
+
+	return ret;
+}
+
+static int isp_video_check_external_subdevs(struct isp_video *video,
+					    struct isp_pipeline *pipe)
+{
+	struct isp_device *isp = video->isp;
+	struct media_entity *ents[] = {
+		&isp->isp_csi2a.subdev.entity,
+		&isp->isp_csi2c.subdev.entity,
+		&isp->isp_ccp2.subdev.entity,
+		&isp->isp_ccdc.subdev.entity
+	};
+	struct media_pad *source_pad;
+	struct media_entity *source = NULL;
+	struct media_entity *sink;
+	struct v4l2_subdev_format fmt;
+	struct v4l2_ext_controls ctrls;
+	struct v4l2_ext_control ctrl;
+	unsigned int i;
+	int ret;
+
+	/* Memory-to-memory pipelines have no external subdev. */
+	if (pipe->input != NULL)
+		return 0;
+
+	for (i = 0; i < ARRAY_SIZE(ents); i++) {
+		/* Is the entity part of the pipeline? */
+		if (!(pipe->entities & (1 << ents[i]->id)))
+			continue;
+
+		/* ISP entities have always sink pad == 0. Find source. */
+		source_pad = media_entity_remote_pad(&ents[i]->pads[0]);
+		if (source_pad == NULL)
+			continue;
+
+		source = source_pad->entity;
+		sink = ents[i];
+		break;
+	}
+
+	if (!source) {
+		dev_warn(isp->dev, "can't find source, failing now\n");
+		return -EINVAL;
+	}
+
+	if (media_entity_type(source) != MEDIA_ENT_T_V4L2_SUBDEV)
+		return 0;
+
+	pipe->external = media_entity_to_v4l2_subdev(source);
+
+	fmt.pad = source_pad->index;
+	fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	ret = v4l2_subdev_call(media_entity_to_v4l2_subdev(sink),
+			       pad, get_fmt, NULL, &fmt);
+	if (unlikely(ret < 0)) {
+		dev_warn(isp->dev, "get_fmt returned null!\n");
+		return ret;
+	}
+
+	pipe->external_width =
+		omap3isp_video_format_info(fmt.format.code)->width;
+
+	memset(&ctrls, 0, sizeof(ctrls));
+	memset(&ctrl, 0, sizeof(ctrl));
+
+	ctrl.id = V4L2_CID_PIXEL_RATE;
+
+	ctrls.count = 1;
+	ctrls.controls = &ctrl;
+
+	ret = v4l2_g_ext_ctrls(pipe->external->ctrl_handler, &ctrls);
+	if (ret < 0) {
+		dev_warn(isp->dev, "no pixel rate control in subdev %s\n",
+			 pipe->external->name);
+		return ret;
+	}
+
+	pipe->external_rate = ctrl.value64;
+
+	if (pipe->entities & (1 << isp->isp_ccdc.subdev.entity.id)) {
+		unsigned int rate = UINT_MAX;
+		/*
+		 * Check that maximum allowed CCDC pixel rate isn't
+		 * exceeded by the pixel rate.
+		 */
+		omap3isp_ccdc_max_rate(&isp->isp_ccdc, &rate);
+		if (pipe->external_rate > rate)
+			return -ENOSPC;
+	}
+
+	return 0;
+}
+
+/*
+ * Stream management
+ *
+ * Every ISP pipeline has a single input and a single output. The input can be
+ * either a sensor or a video node. The output is always a video node.
+ *
+ * As every pipeline has an output video node, the ISP video objects at the
+ * pipeline output stores the pipeline state. It tracks the streaming state of
+ * both the input and output, as well as the availability of buffers.
+ *
+ * In sensor-to-memory mode, frames are always available at the pipeline input.
+ * Starting the sensor usually requires I2C transfers and must be done in
+ * interruptible context. The pipeline is started and stopped synchronously
+ * to the stream on/off commands. All modules in the pipeline will get their
+ * subdev set stream handler called. The module at the end of the pipeline must
+ * delay starting the hardware until buffers are available at its output.
+ *
+ * In memory-to-memory mode, starting/stopping the stream requires
+ * synchronization between the input and output. ISP modules can't be stopped
+ * in the middle of a frame, and at least some of the modules seem to become
+ * busy as soon as they're started, even if they don't receive a frame start
+ * event. For that reason frames need to be processed in single-shot mode. The
+ * driver needs to wait until a frame is completely processed and written to
+ * memory before restarting the pipeline for the next frame. Pipelined
+ * processing might be possible but requires more testing.
+ *
+ * Stream start must be delayed until buffers are available at both the input
+ * and output. The pipeline must be started in the videobuf queue callback with
+ * the buffers queue spinlock held. The modules subdev set stream operation must
+ * not sleep.
+ */
+static int
+isp_video_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
+{
+	struct isp_video_fh *vfh = to_isp_video_fh(fh);
+	struct isp_video *video = video_drvdata(file);
+	enum isp_pipeline_state state;
+	struct isp_pipeline *pipe;
+	unsigned long flags;
+	int ret;
+
+	if (type != video->type)
+		return -EINVAL;
+
+	mutex_lock(&video->stream_lock);
+
+	/* Start streaming on the pipeline. No link touching an entity in the
+	 * pipeline can be activated or deactivated once streaming is started.
+	 */
+	pipe = video->video.entity.pipe
+	     ? to_isp_pipeline(&video->video.entity) : &video->pipe;
+
+	pipe->entities = 0;
+
+	/* TODO: Implement PM QoS */
+	pipe->l3_ick = clk_get_rate(video->isp->clock[ISP_CLK_L3_ICK]);
+	pipe->max_rate = pipe->l3_ick;
+
+	ret = media_entity_pipeline_start(&video->video.entity, &pipe->pipe);
+	if (ret < 0)
+		goto err_pipeline_start;
+
+	/* Verify that the currently configured format matches the output of
+	 * the connected subdev.
+	 */
+	ret = isp_video_check_format(video, vfh);
+	if (ret < 0)
+		goto err_check_format;
+
+	video->bpl_padding = ret;
+	video->bpl_value = vfh->format.fmt.pix.bytesperline;
+
+	ret = isp_video_get_graph_data(video, pipe);
+	if (ret < 0)
+		goto err_check_format;
+
+	if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		state = ISP_PIPELINE_STREAM_OUTPUT | ISP_PIPELINE_IDLE_OUTPUT;
+	else
+		state = ISP_PIPELINE_STREAM_INPUT | ISP_PIPELINE_IDLE_INPUT;
+
+	ret = isp_video_check_external_subdevs(video, pipe);
+	if (ret < 0)
+		goto err_check_format;
+
+	pipe->error = false;
+
+	spin_lock_irqsave(&pipe->lock, flags);
+	pipe->state &= ~ISP_PIPELINE_STREAM;
+	pipe->state |= state;
+	spin_unlock_irqrestore(&pipe->lock, flags);
+
+	/* Set the maximum time per frame as the value requested by userspace.
+	 * This is a soft limit that can be overridden if the hardware doesn't
+	 * support the request limit.
+	 */
+	if (video->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		pipe->max_timeperframe = vfh->timeperframe;
+
+	video->queue = &vfh->queue;
+	INIT_LIST_HEAD(&video->dmaqueue);
+	atomic_set(&pipe->frame_number, -1);
+	pipe->field = vfh->format.fmt.pix.field;
+
+	mutex_lock(&video->queue_lock);
+	ret = vb2_streamon(&vfh->queue, type);
+	mutex_unlock(&video->queue_lock);
+	if (ret < 0)
+		goto err_check_format;
+
+	/* In sensor-to-memory mode, the stream can be started synchronously
+	 * to the stream on command. In memory-to-memory mode, it will be
+	 * started when buffers are queued on both the input and output.
+	 */
+	if (pipe->input == NULL) {
+		ret = omap3isp_pipeline_set_stream(pipe,
+					      ISP_PIPELINE_STREAM_CONTINUOUS);
+		if (ret < 0)
+			goto err_set_stream;
+		spin_lock_irqsave(&video->irqlock, flags);
+		if (list_empty(&video->dmaqueue))
+			video->dmaqueue_flags |= ISP_VIDEO_DMAQUEUE_UNDERRUN;
+		spin_unlock_irqrestore(&video->irqlock, flags);
+	}
+
+	mutex_unlock(&video->stream_lock);
+	return 0;
+
+err_set_stream:
+	mutex_lock(&video->queue_lock);
+	vb2_streamoff(&vfh->queue, type);
+	mutex_unlock(&video->queue_lock);
+err_check_format:
+	media_entity_pipeline_stop(&video->video.entity);
+err_pipeline_start:
+	/* TODO: Implement PM QoS */
+	/* The DMA queue must be emptied here, otherwise CCDC interrupts that
+	 * will get triggered the next time the CCDC is powered up will try to
+	 * access buffers that might have been freed but still present in the
+	 * DMA queue. This can easily get triggered if the above
+	 * omap3isp_pipeline_set_stream() call fails on a system with a
+	 * free-running sensor.
+	 */
+	INIT_LIST_HEAD(&video->dmaqueue);
+	video->queue = NULL;
+
+	mutex_unlock(&video->stream_lock);
+	return ret;
+}
+
+static int
+isp_video_streamoff(struct file *file, void *fh, enum v4l2_buf_type type)
+{
+	struct isp_video_fh *vfh = to_isp_video_fh(fh);
+	struct isp_video *video = video_drvdata(file);
+	struct isp_pipeline *pipe = to_isp_pipeline(&video->video.entity);
+	enum isp_pipeline_state state;
+	unsigned int streaming;
+	unsigned long flags;
+
+	if (type != video->type)
+		return -EINVAL;
+
+	mutex_lock(&video->stream_lock);
+
+	/* Make sure we're not streaming yet. */
+	mutex_lock(&video->queue_lock);
+	streaming = vb2_is_streaming(&vfh->queue);
+	mutex_unlock(&video->queue_lock);
+
+	if (!streaming)
+		goto done;
+
+	/* Update the pipeline state. */
+	if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		state = ISP_PIPELINE_STREAM_OUTPUT
+		      | ISP_PIPELINE_QUEUE_OUTPUT;
+	else
+		state = ISP_PIPELINE_STREAM_INPUT
+		      | ISP_PIPELINE_QUEUE_INPUT;
+
+	spin_lock_irqsave(&pipe->lock, flags);
+	pipe->state &= ~state;
+	spin_unlock_irqrestore(&pipe->lock, flags);
+
+	/* Stop the stream. */
+	omap3isp_pipeline_set_stream(pipe, ISP_PIPELINE_STREAM_STOPPED);
+	omap3isp_video_cancel_stream(video);
+
+	mutex_lock(&video->queue_lock);
+	vb2_streamoff(&vfh->queue, type);
+	mutex_unlock(&video->queue_lock);
+	video->queue = NULL;
+	video->error = false;
+
+	/* TODO: Implement PM QoS */
+	media_entity_pipeline_stop(&video->video.entity);
+
+done:
+	mutex_unlock(&video->stream_lock);
+	return 0;
+}
+
+static int
+isp_video_enum_input(struct file *file, void *fh, struct v4l2_input *input)
+{
+	if (input->index > 0)
+		return -EINVAL;
+
+	strlcpy(input->name, "camera", sizeof(input->name));
+	input->type = V4L2_INPUT_TYPE_CAMERA;
+
+	return 0;
+}
+
+static int
+isp_video_g_input(struct file *file, void *fh, unsigned int *input)
+{
+	*input = 0;
+
+	return 0;
+}
+
+static int
+isp_video_s_input(struct file *file, void *fh, unsigned int input)
+{
+	return input == 0 ? 0 : -EINVAL;
+}
+
+static const struct v4l2_ioctl_ops isp_video_ioctl_ops = {
+	.vidioc_querycap		= isp_video_querycap,
+	.vidioc_g_fmt_vid_cap		= isp_video_get_format,
+	.vidioc_s_fmt_vid_cap		= isp_video_set_format,
+	.vidioc_try_fmt_vid_cap		= isp_video_try_format,
+	.vidioc_g_fmt_vid_out		= isp_video_get_format,
+	.vidioc_s_fmt_vid_out		= isp_video_set_format,
+	.vidioc_try_fmt_vid_out		= isp_video_try_format,
+	.vidioc_cropcap			= isp_video_cropcap,
+	.vidioc_g_crop			= isp_video_get_crop,
+	.vidioc_s_crop			= isp_video_set_crop,
+	.vidioc_g_parm			= isp_video_get_param,
+	.vidioc_s_parm			= isp_video_set_param,
+	.vidioc_reqbufs			= isp_video_reqbufs,
+	.vidioc_querybuf		= isp_video_querybuf,
+	.vidioc_qbuf			= isp_video_qbuf,
+	.vidioc_dqbuf			= isp_video_dqbuf,
+	.vidioc_streamon		= isp_video_streamon,
+	.vidioc_streamoff		= isp_video_streamoff,
+	.vidioc_enum_input		= isp_video_enum_input,
+	.vidioc_g_input			= isp_video_g_input,
+	.vidioc_s_input			= isp_video_s_input,
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 file operations
+ */
+
+static int isp_video_open(struct file *file)
+{
+	struct isp_video *video = video_drvdata(file);
+	struct isp_video_fh *handle;
+	struct vb2_queue *queue;
+	int ret = 0;
+
+	handle = kzalloc(sizeof(*handle), GFP_KERNEL);
+	if (handle == NULL)
+		return -ENOMEM;
+
+	v4l2_fh_init(&handle->vfh, &video->video);
+	v4l2_fh_add(&handle->vfh);
+
+	/* If this is the first user, initialise the pipeline. */
+	if (omap3isp_get(video->isp) == NULL) {
+		ret = -EBUSY;
+		goto done;
+	}
+
+	ret = omap3isp_pipeline_pm_use(&video->video.entity, 1);
+	if (ret < 0) {
+		omap3isp_put(video->isp);
+		goto done;
+	}
+
+	queue = &handle->queue;
+	queue->type = video->type;
+	queue->io_modes = VB2_MMAP | VB2_USERPTR;
+	queue->drv_priv = handle;
+	queue->ops = &isp_video_queue_ops;
+	queue->mem_ops = &vb2_dma_contig_memops;
+	queue->buf_struct_size = sizeof(struct isp_buffer);
+	queue->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+
+	ret = vb2_queue_init(&handle->queue);
+	if (ret < 0) {
+		omap3isp_put(video->isp);
+		goto done;
+	}
+
+	memset(&handle->format, 0, sizeof(handle->format));
+	handle->format.type = video->type;
+	handle->timeperframe.denominator = 1;
+
+	handle->video = video;
+	file->private_data = &handle->vfh;
+
+done:
+	if (ret < 0) {
+		v4l2_fh_del(&handle->vfh);
+		kfree(handle);
+	}
+
+	return ret;
+}
+
+static int isp_video_release(struct file *file)
+{
+	struct isp_video *video = video_drvdata(file);
+	struct v4l2_fh *vfh = file->private_data;
+	struct isp_video_fh *handle = to_isp_video_fh(vfh);
+
+	/* Disable streaming and free the buffers queue resources. */
+	isp_video_streamoff(file, vfh, video->type);
+
+	mutex_lock(&video->queue_lock);
+	vb2_queue_release(&handle->queue);
+	mutex_unlock(&video->queue_lock);
+
+	omap3isp_pipeline_pm_use(&video->video.entity, 0);
+
+	/* Release the file handle. */
+	v4l2_fh_del(vfh);
+	kfree(handle);
+	file->private_data = NULL;
+
+	omap3isp_put(video->isp);
+
+	return 0;
+}
+
+static unsigned int isp_video_poll(struct file *file, poll_table *wait)
+{
+	struct isp_video_fh *vfh = to_isp_video_fh(file->private_data);
+	struct isp_video *video = video_drvdata(file);
+	int ret;
+
+	mutex_lock(&video->queue_lock);
+	ret = vb2_poll(&vfh->queue, file, wait);
+	mutex_unlock(&video->queue_lock);
+
+	return ret;
+}
+
+static int isp_video_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct isp_video_fh *vfh = to_isp_video_fh(file->private_data);
+
+	return vb2_mmap(&vfh->queue, vma);
+}
+
+static struct v4l2_file_operations isp_video_fops = {
+	.owner = THIS_MODULE,
+	.unlocked_ioctl = video_ioctl2,
+	.open = isp_video_open,
+	.release = isp_video_release,
+	.poll = isp_video_poll,
+	.mmap = isp_video_mmap,
+};
+
+/* -----------------------------------------------------------------------------
+ * ISP video core
+ */
+
+static const struct isp_video_operations isp_video_dummy_ops = {
+};
+
+int omap3isp_video_init(struct isp_video *video, const char *name)
+{
+	const char *direction;
+	int ret;
+
+	switch (video->type) {
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		direction = "output";
+		video->pad.flags = MEDIA_PAD_FL_SINK
+				   | MEDIA_PAD_FL_MUST_CONNECT;
+		break;
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+		direction = "input";
+		video->pad.flags = MEDIA_PAD_FL_SOURCE
+				   | MEDIA_PAD_FL_MUST_CONNECT;
+		video->video.vfl_dir = VFL_DIR_TX;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	video->alloc_ctx = vb2_dma_contig_init_ctx(video->isp->dev);
+	if (IS_ERR(video->alloc_ctx))
+		return PTR_ERR(video->alloc_ctx);
+
+	ret = media_entity_init(&video->video.entity, 1, &video->pad, 0);
+	if (ret < 0) {
+		vb2_dma_contig_cleanup_ctx(video->alloc_ctx);
+		return ret;
+	}
+
+	mutex_init(&video->mutex);
+	atomic_set(&video->active, 0);
+
+	spin_lock_init(&video->pipe.lock);
+	mutex_init(&video->stream_lock);
+	mutex_init(&video->queue_lock);
+	spin_lock_init(&video->irqlock);
+
+	/* Initialize the video device. */
+	if (video->ops == NULL)
+		video->ops = &isp_video_dummy_ops;
+
+	video->video.fops = &isp_video_fops;
+	snprintf(video->video.name, sizeof(video->video.name),
+		 "OMAP3 ISP %s %s", name, direction);
+	video->video.vfl_type = VFL_TYPE_GRABBER;
+	video->video.release = video_device_release_empty;
+	video->video.ioctl_ops = &isp_video_ioctl_ops;
+	video->pipe.stream_state = ISP_PIPELINE_STREAM_STOPPED;
+
+	video_set_drvdata(&video->video, video);
+
+	return 0;
+}
+
+void omap3isp_video_cleanup(struct isp_video *video)
+{
+	vb2_dma_contig_cleanup_ctx(video->alloc_ctx);
+	media_entity_cleanup(&video->video.entity);
+	mutex_destroy(&video->queue_lock);
+	mutex_destroy(&video->stream_lock);
+	mutex_destroy(&video->mutex);
+}
+
+int omap3isp_video_register(struct isp_video *video, struct v4l2_device *vdev)
+{
+	int ret;
+
+	video->video.v4l2_dev = vdev;
+
+	ret = video_register_device(&video->video, VFL_TYPE_GRABBER, -1);
+	if (ret < 0)
+		dev_err(video->isp->dev,
+			"%s: could not register video device (%d)\n",
+			__func__, ret);
+
+	return ret;
+}
+
+void omap3isp_video_unregister(struct isp_video *video)
+{
+	if (video_is_registered(&video->video))
+		video_unregister_device(&video->video);
+}
diff --git a/drivers/media/platform/omap3isp/ispvideo.h b/drivers/media/platform/omap3isp/ispvideo.h
new file mode 100644
index 0000000..bcf0e0a
--- /dev/null
+++ b/drivers/media/platform/omap3isp/ispvideo.h
@@ -0,0 +1,211 @@
+/*
+ * ispvideo.h
+ *
+ * TI OMAP3 ISP - Generic video node
+ *
+ * Copyright (C) 2009-2010 Nokia Corporation
+ *
+ * Contacts: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *	     Sakari Ailus <sakari.ailus@iki.fi>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef OMAP3_ISP_VIDEO_H
+#define OMAP3_ISP_VIDEO_H
+
+#include <linux/v4l2-mediabus.h>
+#include <media/media-entity.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-fh.h>
+#include <media/videobuf2-v4l2.h>
+
+#define ISP_VIDEO_DRIVER_NAME		"ispvideo"
+#define ISP_VIDEO_DRIVER_VERSION	"0.0.2"
+
+struct isp_device;
+struct isp_video;
+struct v4l2_mbus_framefmt;
+struct v4l2_pix_format;
+
+/*
+ * struct isp_format_info - ISP media bus format information
+ * @code: V4L2 media bus format code
+ * @truncated: V4L2 media bus format code for the same format truncated to 10
+ *	bits. Identical to @code if the format is 10 bits wide or less.
+ * @uncompressed: V4L2 media bus format code for the corresponding uncompressed
+ *	format. Identical to @code if the format is not DPCM compressed.
+ * @flavor: V4L2 media bus format code for the same pixel layout but
+ *	shifted to be 8 bits per pixel. =0 if format is not shiftable.
+ * @pixelformat: V4L2 pixel format FCC identifier
+ * @width: Bits per pixel (when transferred over a bus)
+ * @bpp: Bytes per pixel (when stored in memory)
+ */
+struct isp_format_info {
+	u32 code;
+	u32 truncated;
+	u32 uncompressed;
+	u32 flavor;
+	u32 pixelformat;
+	unsigned int width;
+	unsigned int bpp;
+};
+
+enum isp_pipeline_stream_state {
+	ISP_PIPELINE_STREAM_STOPPED = 0,
+	ISP_PIPELINE_STREAM_CONTINUOUS = 1,
+	ISP_PIPELINE_STREAM_SINGLESHOT = 2,
+};
+
+enum isp_pipeline_state {
+	/* The stream has been started on the input video node. */
+	ISP_PIPELINE_STREAM_INPUT = 1,
+	/* The stream has been started on the output video node. */
+	ISP_PIPELINE_STREAM_OUTPUT = 2,
+	/* At least one buffer is queued on the input video node. */
+	ISP_PIPELINE_QUEUE_INPUT = 4,
+	/* At least one buffer is queued on the output video node. */
+	ISP_PIPELINE_QUEUE_OUTPUT = 8,
+	/* The input entity is idle, ready to be started. */
+	ISP_PIPELINE_IDLE_INPUT = 16,
+	/* The output entity is idle, ready to be started. */
+	ISP_PIPELINE_IDLE_OUTPUT = 32,
+	/* The pipeline is currently streaming. */
+	ISP_PIPELINE_STREAM = 64,
+};
+
+/*
+ * struct isp_pipeline - An ISP hardware pipeline
+ * @field: The field being processed by the pipeline
+ * @error: A hardware error occurred during capture
+ * @entities: Bitmask of entities in the pipeline (indexed by entity ID)
+ */
+struct isp_pipeline {
+	struct media_pipeline pipe;
+	spinlock_t lock;		/* Pipeline state and queue flags */
+	unsigned int state;
+	enum isp_pipeline_stream_state stream_state;
+	struct isp_video *input;
+	struct isp_video *output;
+	u32 entities;
+	unsigned long l3_ick;
+	unsigned int max_rate;
+	enum v4l2_field field;
+	atomic_t frame_number;
+	bool do_propagation; /* of frame number */
+	bool error;
+	struct v4l2_fract max_timeperframe;
+	struct v4l2_subdev *external;
+	unsigned int external_rate;
+	unsigned int external_width;
+};
+
+#define to_isp_pipeline(__e) \
+	container_of((__e)->pipe, struct isp_pipeline, pipe)
+
+static inline int isp_pipeline_ready(struct isp_pipeline *pipe)
+{
+	return pipe->state == (ISP_PIPELINE_STREAM_INPUT |
+			       ISP_PIPELINE_STREAM_OUTPUT |
+			       ISP_PIPELINE_QUEUE_INPUT |
+			       ISP_PIPELINE_QUEUE_OUTPUT |
+			       ISP_PIPELINE_IDLE_INPUT |
+			       ISP_PIPELINE_IDLE_OUTPUT);
+}
+
+/**
+ * struct isp_buffer - ISP video buffer
+ * @vb: videobuf2 buffer
+ * @irqlist: List head for insertion into IRQ queue
+ * @dma: DMA address
+ */
+struct isp_buffer {
+	struct vb2_v4l2_buffer vb;
+	struct list_head irqlist;
+	dma_addr_t dma;
+};
+
+#define to_isp_buffer(buf)	container_of(buf, struct isp_buffer, vb)
+
+enum isp_video_dmaqueue_flags {
+	/* Set if DMA queue becomes empty when ISP_PIPELINE_STREAM_CONTINUOUS */
+	ISP_VIDEO_DMAQUEUE_UNDERRUN = (1 << 0),
+	/* Set when queuing buffer to an empty DMA queue */
+	ISP_VIDEO_DMAQUEUE_QUEUED = (1 << 1),
+};
+
+#define isp_video_dmaqueue_flags_clr(video)	\
+			({ (video)->dmaqueue_flags = 0; })
+
+/*
+ * struct isp_video_operations - ISP video operations
+ * @queue:	Resume streaming when a buffer is queued. Called on VIDIOC_QBUF
+ *		if there was no buffer previously queued.
+ */
+struct isp_video_operations {
+	int(*queue)(struct isp_video *video, struct isp_buffer *buffer);
+};
+
+struct isp_video {
+	struct video_device video;
+	enum v4l2_buf_type type;
+	struct media_pad pad;
+
+	struct mutex mutex;		/* format and crop settings */
+	atomic_t active;
+
+	struct isp_device *isp;
+
+	unsigned int capture_mem;
+	unsigned int bpl_alignment;	/* alignment value */
+	unsigned int bpl_zero_padding;	/* whether the alignment is optional */
+	unsigned int bpl_max;		/* maximum bytes per line value */
+	unsigned int bpl_value;		/* bytes per line value */
+	unsigned int bpl_padding;	/* padding at end of line */
+
+	/* Pipeline state */
+	struct isp_pipeline pipe;
+	struct mutex stream_lock;	/* pipeline and stream states */
+	bool error;
+
+	/* Video buffers queue */
+	void *alloc_ctx;
+	struct vb2_queue *queue;
+	struct mutex queue_lock;	/* protects the queue */
+	spinlock_t irqlock;		/* protects dmaqueue */
+	struct list_head dmaqueue;
+	enum isp_video_dmaqueue_flags dmaqueue_flags;
+
+	const struct isp_video_operations *ops;
+};
+
+#define to_isp_video(vdev)	container_of(vdev, struct isp_video, video)
+
+struct isp_video_fh {
+	struct v4l2_fh vfh;
+	struct isp_video *video;
+	struct vb2_queue queue;
+	struct v4l2_format format;
+	struct v4l2_fract timeperframe;
+};
+
+#define to_isp_video_fh(fh)	container_of(fh, struct isp_video_fh, vfh)
+#define isp_video_queue_to_isp_video_fh(q) \
+				container_of(q, struct isp_video_fh, queue)
+
+int omap3isp_video_init(struct isp_video *video, const char *name);
+void omap3isp_video_cleanup(struct isp_video *video);
+int omap3isp_video_register(struct isp_video *video,
+			    struct v4l2_device *vdev);
+void omap3isp_video_unregister(struct isp_video *video);
+struct isp_buffer *omap3isp_video_buffer_next(struct isp_video *video);
+void omap3isp_video_cancel_stream(struct isp_video *video);
+void omap3isp_video_resume(struct isp_video *video, int continuous);
+struct media_pad *omap3isp_video_remote_pad(struct isp_video *video);
+
+const struct isp_format_info *
+omap3isp_video_format_info(u32 code);
+
+#endif /* OMAP3_ISP_VIDEO_H */
diff --git a/drivers/media/platform/omap3isp/luma_enhance_table.h b/drivers/media/platform/omap3isp/luma_enhance_table.h
new file mode 100644
index 0000000..81c5b15
--- /dev/null
+++ b/drivers/media/platform/omap3isp/luma_enhance_table.h
@@ -0,0 +1,32 @@
+/*
+ * luma_enhance_table.h
+ *
+ * TI OMAP3 ISP - Luminance enhancement table
+ *
+ * Copyright (C) 2010 Nokia Corporation
+ * Copyright (C) 2009 Texas Instruments, Inc.
+ *
+ * Contacts: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *	     Sakari Ailus <sakari.ailus@iki.fi>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+1047552, 1047552, 1047552, 1047552, 1047552, 1047552, 1047552, 1047552,
+1047552, 1047552, 1047552, 1047552, 1047552, 1047552, 1047552, 1047552,
+1047552, 1047552, 1047552, 1047552, 1047552, 1047552, 1047552, 1047552,
+1047552, 1047552, 1047552, 1047552, 1048575, 1047551, 1046527, 1045503,
+1044479, 1043455, 1042431, 1041407, 1040383, 1039359, 1038335, 1037311,
+1036287, 1035263, 1034239, 1033215, 1032191, 1031167, 1030143, 1028096,
+1028096, 1028096, 1028096, 1028096, 1028096, 1028096, 1028096, 1028096,
+1028096, 1028100, 1032196, 1036292, 1040388, 1044484,       0,       0,
+      0,       5,    5125,   10245,   15365,   20485,   25605,   30720,
+  30720,   30720,   30720,   30720,   30720,   30720,   30720,   30720,
+  30720,   30720,   31743,   30719,   29695,   28671,   27647,   26623,
+  25599,   24575,   23551,   22527,   21503,   20479,   19455,   18431,
+  17407,   16383,   15359,   14335,   13311,   12287,   11263,   10239,
+   9215,    8191,    7167,    6143,    5119,    4095,    3071,    1024,
+   1024,    1024,    1024,    1024,    1024,    1024,    1024,    1024,
+   1024,    1024,    1024,    1024,    1024,    1024,    1024,    1024
diff --git a/drivers/media/platform/omap3isp/noise_filter_table.h b/drivers/media/platform/omap3isp/noise_filter_table.h
new file mode 100644
index 0000000..5073f98
--- /dev/null
+++ b/drivers/media/platform/omap3isp/noise_filter_table.h
@@ -0,0 +1,20 @@
+/*
+ * noise_filter_table.h
+ *
+ * TI OMAP3 ISP - Noise filter table
+ *
+ * Copyright (C) 2010 Nokia Corporation
+ * Copyright (C) 2009 Texas Instruments, Inc.
+ *
+ * Contacts: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *	     Sakari Ailus <sakari.ailus@iki.fi>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16,
+16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16,
+31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31,
+31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, 31
diff --git a/drivers/media/platform/omap3isp/omap3isp.h b/drivers/media/platform/omap3isp/omap3isp.h
new file mode 100644
index 0000000..190e259
--- /dev/null
+++ b/drivers/media/platform/omap3isp/omap3isp.h
@@ -0,0 +1,132 @@
+/*
+ * omap3isp.h
+ *
+ * TI OMAP3 ISP - Bus Configuration
+ *
+ * Copyright (C) 2011 Nokia Corporation
+ *
+ * Contacts: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *	     Sakari Ailus <sakari.ailus@iki.fi>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ */
+
+#ifndef __OMAP3ISP_H__
+#define __OMAP3ISP_H__
+
+enum isp_interface_type {
+	ISP_INTERFACE_PARALLEL,
+	ISP_INTERFACE_CSI2A_PHY2,
+	ISP_INTERFACE_CCP2B_PHY1,
+	ISP_INTERFACE_CCP2B_PHY2,
+	ISP_INTERFACE_CSI2C_PHY1,
+};
+
+/**
+ * struct isp_parallel_cfg - Parallel interface configuration
+ * @data_lane_shift: Data lane shifter
+ *		0 - CAMEXT[13:0] -> CAM[13:0]
+ *		1 - CAMEXT[13:2] -> CAM[11:0]
+ *		2 - CAMEXT[13:4] -> CAM[9:0]
+ *		3 - CAMEXT[13:6] -> CAM[7:0]
+ * @clk_pol: Pixel clock polarity
+ *		0 - Sample on rising edge, 1 - Sample on falling edge
+ * @hs_pol: Horizontal synchronization polarity
+ *		0 - Active high, 1 - Active low
+ * @vs_pol: Vertical synchronization polarity
+ *		0 - Active high, 1 - Active low
+ * @fld_pol: Field signal polarity
+ *		0 - Positive, 1 - Negative
+ * @data_pol: Data polarity
+ *		0 - Normal, 1 - One's complement
+ */
+struct isp_parallel_cfg {
+	unsigned int data_lane_shift:2;
+	unsigned int clk_pol:1;
+	unsigned int hs_pol:1;
+	unsigned int vs_pol:1;
+	unsigned int fld_pol:1;
+	unsigned int data_pol:1;
+};
+
+enum {
+	ISP_CCP2_PHY_DATA_CLOCK = 0,
+	ISP_CCP2_PHY_DATA_STROBE = 1,
+};
+
+enum {
+	ISP_CCP2_MODE_MIPI = 0,
+	ISP_CCP2_MODE_CCP2 = 1,
+};
+
+/**
+ * struct isp_csiphy_lane: CCP2/CSI2 lane position and polarity
+ * @pos: position of the lane
+ * @pol: polarity of the lane
+ */
+struct isp_csiphy_lane {
+	u8 pos;
+	u8 pol;
+};
+
+#define ISP_CSIPHY1_NUM_DATA_LANES	1
+#define ISP_CSIPHY2_NUM_DATA_LANES	2
+
+/**
+ * struct isp_csiphy_lanes_cfg - CCP2/CSI2 lane configuration
+ * @data: Configuration of one or two data lanes
+ * @clk: Clock lane configuration
+ */
+struct isp_csiphy_lanes_cfg {
+	struct isp_csiphy_lane data[ISP_CSIPHY2_NUM_DATA_LANES];
+	struct isp_csiphy_lane clk;
+};
+
+/**
+ * struct isp_ccp2_cfg - CCP2 interface configuration
+ * @strobe_clk_pol: Strobe/clock polarity
+ *		0 - Non Inverted, 1 - Inverted
+ * @crc: Enable the cyclic redundancy check
+ * @ccp2_mode: Enable CCP2 compatibility mode
+ *		ISP_CCP2_MODE_MIPI - MIPI-CSI1 mode
+ *		ISP_CCP2_MODE_CCP2 - CCP2 mode
+ * @phy_layer: Physical layer selection
+ *		ISP_CCP2_PHY_DATA_CLOCK - Data/clock physical layer
+ *		ISP_CCP2_PHY_DATA_STROBE - Data/strobe physical layer
+ * @vpclk_div: Video port output clock control
+ */
+struct isp_ccp2_cfg {
+	unsigned int strobe_clk_pol:1;
+	unsigned int crc:1;
+	unsigned int ccp2_mode:1;
+	unsigned int phy_layer:1;
+	unsigned int vpclk_div:2;
+	struct isp_csiphy_lanes_cfg lanecfg;
+};
+
+/**
+ * struct isp_csi2_cfg - CSI2 interface configuration
+ * @crc: Enable the cyclic redundancy check
+ */
+struct isp_csi2_cfg {
+	unsigned crc:1;
+	struct isp_csiphy_lanes_cfg lanecfg;
+};
+
+struct isp_bus_cfg {
+	enum isp_interface_type interface;
+	union {
+		struct isp_parallel_cfg parallel;
+		struct isp_ccp2_cfg ccp2;
+		struct isp_csi2_cfg csi2;
+	} bus; /* gcc < 4.6.0 chokes on anonymous union initializers */
+};
+
+#endif	/* __OMAP3ISP_H__ */
diff --git a/drivers/media/platform/rcar_jpu.c b/drivers/media/platform/rcar_jpu.c
new file mode 100644
index 0000000..f8e3e83
--- /dev/null
+++ b/drivers/media/platform/rcar_jpu.c
@@ -0,0 +1,1802 @@
+/*
+ * Author: Mikhail Ulyanov
+ * Copyright (C) 2014-2015 Cogent Embedded, Inc.  <source@cogentembedded.com>
+ * Copyright (C) 2014-2015 Renesas Electronics Corporation
+ *
+ * This is based on the drivers/media/platform/s5p-jpeg driver by
+ * Andrzej Pietrasiewicz and Jacek Anaszewski.
+ * Some portions of code inspired by VSP1 driver by Laurent Pinchart.
+ *
+ * TODO in order of priority:
+ *      1) Rotation
+ *      2) Cropping
+ *      3) V4L2_CID_JPEG_ACTIVE_MARKER
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <asm/unaligned.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/string.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/videobuf2-dma-contig.h>
+
+
+#define DRV_NAME "rcar_jpu"
+
+/*
+ * Align JPEG header end to cache line to make sure we will not have any issues
+ * with cache; additionally to requerment (33.3.27 R01UH0501EJ0100 Rev.1.00)
+ */
+#define JPU_JPEG_HDR_SIZE		(ALIGN(0x258, L1_CACHE_BYTES))
+#define JPU_JPEG_MAX_BYTES_PER_PIXEL	2	/* 16 bit precision format */
+#define JPU_JPEG_MIN_SIZE		25	/* SOI + SOF + EOI */
+#define JPU_JPEG_QTBL_SIZE		0x40
+#define JPU_JPEG_HDCTBL_SIZE		0x1c
+#define JPU_JPEG_HACTBL_SIZE		0xb2
+#define JPU_JPEG_HEIGHT_OFFSET		0x91
+#define JPU_JPEG_WIDTH_OFFSET		0x93
+#define JPU_JPEG_SUBS_OFFSET		0x97
+#define JPU_JPEG_QTBL_LUM_OFFSET	0x07
+#define JPU_JPEG_QTBL_CHR_OFFSET	0x4c
+#define JPU_JPEG_HDCTBL_LUM_OFFSET	0xa4
+#define JPU_JPEG_HACTBL_LUM_OFFSET	0xc5
+#define JPU_JPEG_HDCTBL_CHR_OFFSET	0x17c
+#define JPU_JPEG_HACTBL_CHR_OFFSET	0x19d
+#define JPU_JPEG_PADDING_OFFSET		0x24f
+#define JPU_JPEG_LUM 0x00
+#define JPU_JPEG_CHR 0x01
+#define JPU_JPEG_DC  0x00
+#define JPU_JPEG_AC  0x10
+
+#define JPU_JPEG_422 0x21
+#define JPU_JPEG_420 0x22
+
+#define JPU_JPEG_DEFAULT_422_PIX_FMT V4L2_PIX_FMT_NV16M
+#define JPU_JPEG_DEFAULT_420_PIX_FMT V4L2_PIX_FMT_NV12M
+
+/* JPEG markers */
+#define TEM	0x01
+#define SOF0	0xc0
+#define RST	0xd0
+#define SOI	0xd8
+#define EOI	0xd9
+#define DHP	0xde
+#define DHT	0xc4
+#define COM	0xfe
+#define DQT	0xdb
+#define DRI	0xdd
+#define APP0	0xe0
+
+#define JPU_RESET_TIMEOUT	100 /* ms */
+#define JPU_JOB_TIMEOUT		300 /* ms */
+#define JPU_MAX_QUALITY		4
+#define JPU_WIDTH_MIN		16
+#define JPU_HEIGHT_MIN		16
+#define JPU_WIDTH_MAX		4096
+#define JPU_HEIGHT_MAX		4096
+#define JPU_MEMALIGN		8
+
+/* Flags that indicate a format can be used for capture/output */
+#define JPU_FMT_TYPE_OUTPUT	0
+#define JPU_FMT_TYPE_CAPTURE	1
+#define JPU_ENC_CAPTURE		(1 << 0)
+#define JPU_ENC_OUTPUT		(1 << 1)
+#define JPU_DEC_CAPTURE		(1 << 2)
+#define JPU_DEC_OUTPUT		(1 << 3)
+
+/*
+ * JPEG registers and bits
+ */
+
+/* JPEG code mode register */
+#define JCMOD	0x00
+#define JCMOD_PCTR		(1 << 7)
+#define JCMOD_MSKIP_ENABLE	(1 << 5)
+#define JCMOD_DSP_ENC		(0 << 3)
+#define JCMOD_DSP_DEC		(1 << 3)
+#define JCMOD_REDU		(7 << 0)
+#define JCMOD_REDU_422		(1 << 0)
+#define JCMOD_REDU_420		(2 << 0)
+
+/* JPEG code command register */
+#define JCCMD	0x04
+#define JCCMD_SRST	(1 << 12)
+#define JCCMD_JEND	(1 << 2)
+#define JCCMD_JSRT	(1 << 0)
+
+/* JPEG code quantanization table number register */
+#define JCQTN	0x0c
+#define JCQTN_SHIFT(t)		(((t) - 1) << 1)
+
+/* JPEG code Huffman table number register */
+#define JCHTN	0x10
+#define JCHTN_AC_SHIFT(t)	(((t) << 1) - 1)
+#define JCHTN_DC_SHIFT(t)	(((t) - 1) << 1)
+
+#define JCVSZU	0x1c /* JPEG code vertical size upper register */
+#define JCVSZD	0x20 /* JPEG code vertical size lower register */
+#define JCHSZU	0x24 /* JPEG code horizontal size upper register */
+#define JCHSZD	0x28 /* JPEG code horizontal size lower register */
+#define JCSZ_MASK 0xff /* JPEG code h/v size register contains only 1 byte*/
+
+#define JCDTCU	0x2c /* JPEG code data count upper register */
+#define JCDTCM	0x30 /* JPEG code data count middle register */
+#define JCDTCD	0x34 /* JPEG code data count lower register */
+
+/* JPEG interrupt enable register */
+#define JINTE	0x38
+#define JINTE_ERR		(7 << 5) /* INT5 + INT6 + INT7 */
+#define JINTE_TRANSF_COMPL	(1 << 10)
+
+/* JPEG interrupt status register */
+#define JINTS	0x3c
+#define JINTS_MASK	0x7c68
+#define JINTS_ERR		(1 << 5)
+#define JINTS_PROCESS_COMPL	(1 << 6)
+#define JINTS_TRANSF_COMPL	(1 << 10)
+
+#define JCDERR	0x40 /* JPEG code decode error register */
+#define JCDERR_MASK	0xf /* JPEG code decode error register mask*/
+
+/* JPEG interface encoding */
+#define JIFECNT	0x70
+#define JIFECNT_INFT_422	0
+#define JIFECNT_INFT_420	1
+#define JIFECNT_SWAP_WB		(3 << 4) /* to JPU */
+
+#define JIFESYA1	0x74	/* encode source Y address register 1 */
+#define JIFESCA1	0x78	/* encode source C address register 1 */
+#define JIFESYA2	0x7c	/* encode source Y address register 2 */
+#define JIFESCA2	0x80	/* encode source C address register 2 */
+#define JIFESMW		0x84	/* encode source memory width register */
+#define JIFESVSZ	0x88	/* encode source vertical size register */
+#define JIFESHSZ	0x8c	/* encode source horizontal size register */
+#define JIFEDA1		0x90	/* encode destination address register 1 */
+#define JIFEDA2		0x94	/* encode destination address register 2 */
+
+/* JPEG decoding control register */
+#define JIFDCNT	0xa0
+#define JIFDCNT_SWAP_WB		(3 << 1) /* from JPU */
+
+#define JIFDSA1		0xa4	/* decode source address register 1 */
+#define JIFDDMW		0xb0	/* decode destination  memory width register */
+#define JIFDDVSZ	0xb4	/* decode destination  vert. size register */
+#define JIFDDHSZ	0xb8	/* decode destination  horiz. size register */
+#define JIFDDYA1	0xbc	/* decode destination  Y address register 1 */
+#define JIFDDCA1	0xc0	/* decode destination  C address register 1 */
+
+#define JCQTBL(n)	(0x10000 + (n) * 0x40)	/* quantization tables regs */
+#define JCHTBD(n)	(0x10100 + (n) * 0x100)	/* Huffman table DC regs */
+#define JCHTBA(n)	(0x10120 + (n) * 0x100)	/* Huffman table AC regs */
+
+/**
+ * struct jpu - JPEG IP abstraction
+ * @mutex: the mutex protecting this structure
+ * @lock: spinlock protecting the device contexts
+ * @v4l2_dev: v4l2 device for mem2mem mode
+ * @vfd_encoder: video device node for encoder mem2mem mode
+ * @vfd_decoder: video device node for decoder mem2mem mode
+ * @m2m_dev: v4l2 mem2mem device data
+ * @curr: pointer to current context
+ * @irq_queue:	interrupt handler waitqueue
+ * @regs: JPEG IP registers mapping
+ * @irq: JPEG IP irq
+ * @clk: JPEG IP clock
+ * @dev: JPEG IP struct device
+ * @alloc_ctx: videobuf2 memory allocator's context
+ * @ref_count: reference counter
+ */
+struct jpu {
+	struct mutex	mutex;
+	spinlock_t	lock;
+	struct v4l2_device	v4l2_dev;
+	struct video_device	vfd_encoder;
+	struct video_device	vfd_decoder;
+	struct v4l2_m2m_dev	*m2m_dev;
+	struct jpu_ctx		*curr;
+	wait_queue_head_t	irq_queue;
+
+	void __iomem		*regs;
+	unsigned int		irq;
+	struct clk		*clk;
+	struct device		*dev;
+	void			*alloc_ctx;
+	int			ref_count;
+};
+
+/**
+ * struct jpu_buffer - driver's specific video buffer
+ * @buf: m2m buffer
+ * @compr_quality: destination image quality in compression mode
+ * @subsampling: source image subsampling in decompression mode
+ */
+struct jpu_buffer {
+	struct v4l2_m2m_buffer buf;
+	unsigned short	compr_quality;
+	unsigned char	subsampling;
+};
+
+/**
+ * struct jpu_fmt - driver's internal format data
+ * @fourcc: the fourcc code, 0 if not applicable
+ * @colorspace: the colorspace specifier
+ * @bpp: number of bits per pixel per plane
+ * @h_align: horizontal alignment order (align to 2^h_align)
+ * @v_align: vertical alignment order (align to 2^v_align)
+ * @subsampling: (horizontal:4 | vertical:4) subsampling factor
+ * @num_planes: number of planes
+ * @types: types of queue this format is applicable to
+ */
+struct jpu_fmt {
+	u32 fourcc;
+	u32 colorspace;
+	u8 bpp[2];
+	u8 h_align;
+	u8 v_align;
+	u8 subsampling;
+	u8 num_planes;
+	u16 types;
+};
+
+/**
+ * jpu_q_data - parameters of one queue
+ * @fmtinfo: driver-specific format of this queue
+ * @format: multiplanar format of this queue
+ * @sequence: sequence number
+ */
+struct jpu_q_data {
+	struct jpu_fmt *fmtinfo;
+	struct v4l2_pix_format_mplane format;
+	unsigned int sequence;
+};
+
+/**
+ * jpu_ctx - the device context data
+ * @jpu: JPEG IP device for this context
+ * @encoder: compression (encode) operation or decompression (decode)
+ * @compr_quality: destination image quality in compression (encode) mode
+ * @out_q: source (output) queue information
+ * @cap_q: destination (capture) queue information
+ * @fh: file handler
+ * @ctrl_handler: controls handler
+ */
+struct jpu_ctx {
+	struct jpu		*jpu;
+	bool			encoder;
+	unsigned short		compr_quality;
+	struct jpu_q_data	out_q;
+	struct jpu_q_data	cap_q;
+	struct v4l2_fh		fh;
+	struct v4l2_ctrl_handler ctrl_handler;
+};
+
+ /**
+ * jpeg_buffer - description of memory containing input JPEG data
+ * @end: end position in the buffer
+ * @curr: current position in the buffer
+ */
+struct jpeg_buffer {
+	void *end;
+	void *curr;
+};
+
+static struct jpu_fmt jpu_formats[] = {
+	{ V4L2_PIX_FMT_JPEG, V4L2_COLORSPACE_JPEG,
+	  {0, 0}, 0, 0, 0, 1, JPU_ENC_CAPTURE | JPU_DEC_OUTPUT },
+	{ V4L2_PIX_FMT_NV16M, V4L2_COLORSPACE_SRGB,
+	  {8, 8}, 2, 2, JPU_JPEG_422, 2, JPU_ENC_OUTPUT | JPU_DEC_CAPTURE },
+	{ V4L2_PIX_FMT_NV12M, V4L2_COLORSPACE_SRGB,
+	  {8, 4}, 2, 2, JPU_JPEG_420, 2, JPU_ENC_OUTPUT | JPU_DEC_CAPTURE },
+	{ V4L2_PIX_FMT_NV16, V4L2_COLORSPACE_SRGB,
+	  {16, 0}, 2, 2, JPU_JPEG_422, 1, JPU_ENC_OUTPUT | JPU_DEC_CAPTURE },
+	{ V4L2_PIX_FMT_NV12, V4L2_COLORSPACE_SRGB,
+	  {12, 0}, 2, 2, JPU_JPEG_420, 1, JPU_ENC_OUTPUT | JPU_DEC_CAPTURE },
+};
+
+static const u8 zigzag[] = {
+	0x03, 0x02, 0x0b, 0x13, 0x0a, 0x01, 0x00, 0x09,
+	0x12, 0x1b, 0x23, 0x1a, 0x11, 0x08, 0x07, 0x06,
+	0x0f, 0x10, 0x19, 0x22, 0x2b, 0x33, 0x2a, 0x21,
+	0x18, 0x17, 0x0e, 0x05, 0x04, 0x0d, 0x16, 0x1f,
+	0x20, 0x29, 0x32, 0x3b, 0x3a, 0x31, 0x28, 0x27,
+	0x1e, 0x15, 0x0e, 0x14, 0x10, 0x26, 0x2f, 0x30,
+	0x39, 0x38, 0x37, 0x2e, 0x25, 0x1c, 0x24, 0x2b,
+	0x36, 0x3f, 0x3e, 0x35, 0x2c, 0x34, 0x3d, 0x3c
+};
+
+#define QTBL_SIZE (ALIGN(JPU_JPEG_QTBL_SIZE, \
+			  sizeof(unsigned int)) / sizeof(unsigned int))
+#define HDCTBL_SIZE (ALIGN(JPU_JPEG_HDCTBL_SIZE, \
+			  sizeof(unsigned int)) / sizeof(unsigned int))
+#define HACTBL_SIZE (ALIGN(JPU_JPEG_HACTBL_SIZE, \
+			  sizeof(unsigned int)) / sizeof(unsigned int))
+/*
+ * Start of image; Quantization tables
+ * SOF0 (17 bytes payload) is Baseline DCT - Sample precision, height, width,
+ * Number of image components, (Ci:8 - Hi:4 - Vi:4 - Tq:8) * 3 - Y,Cb,Cr;
+ * Huffman tables; Padding with 0xff (33.3.27 R01UH0501EJ0100 Rev.1.00)
+ */
+#define JPU_JPEG_HDR_BLOB {                                                    \
+	0xff, SOI, 0xff, DQT, 0x00, JPU_JPEG_QTBL_SIZE + 0x3, JPU_JPEG_LUM,    \
+	[JPU_JPEG_QTBL_LUM_OFFSET ...                                          \
+		JPU_JPEG_QTBL_LUM_OFFSET + JPU_JPEG_QTBL_SIZE - 1] = 0x00,     \
+	0xff, DQT, 0x00, JPU_JPEG_QTBL_SIZE + 0x3, JPU_JPEG_CHR,               \
+	[JPU_JPEG_QTBL_CHR_OFFSET ... JPU_JPEG_QTBL_CHR_OFFSET +               \
+		JPU_JPEG_QTBL_SIZE - 1] = 0x00, 0xff, SOF0, 0x00, 0x11, 0x08,  \
+	[JPU_JPEG_HEIGHT_OFFSET ... JPU_JPEG_HEIGHT_OFFSET + 1] = 0x00,        \
+	[JPU_JPEG_WIDTH_OFFSET ... JPU_JPEG_WIDTH_OFFSET + 1] = 0x00,          \
+	0x03, 0x01, [JPU_JPEG_SUBS_OFFSET] = 0x00, JPU_JPEG_LUM,               \
+	0x02, 0x11, JPU_JPEG_CHR, 0x03, 0x11, JPU_JPEG_CHR,                    \
+	0xff, DHT, 0x00, JPU_JPEG_HDCTBL_SIZE + 0x3, JPU_JPEG_LUM|JPU_JPEG_DC, \
+	[JPU_JPEG_HDCTBL_LUM_OFFSET ...                                        \
+		JPU_JPEG_HDCTBL_LUM_OFFSET + JPU_JPEG_HDCTBL_SIZE - 1] = 0x00, \
+	0xff, DHT, 0x00, JPU_JPEG_HACTBL_SIZE + 0x3, JPU_JPEG_LUM|JPU_JPEG_AC, \
+	[JPU_JPEG_HACTBL_LUM_OFFSET ...                                        \
+		JPU_JPEG_HACTBL_LUM_OFFSET + JPU_JPEG_HACTBL_SIZE - 1] = 0x00, \
+	0xff, DHT, 0x00, JPU_JPEG_HDCTBL_SIZE + 0x3, JPU_JPEG_CHR|JPU_JPEG_DC, \
+	[JPU_JPEG_HDCTBL_CHR_OFFSET ...                                        \
+		JPU_JPEG_HDCTBL_CHR_OFFSET + JPU_JPEG_HDCTBL_SIZE - 1] = 0x00, \
+	0xff, DHT, 0x00, JPU_JPEG_HACTBL_SIZE + 0x3, JPU_JPEG_CHR|JPU_JPEG_AC, \
+	[JPU_JPEG_HACTBL_CHR_OFFSET ...                                        \
+		JPU_JPEG_HACTBL_CHR_OFFSET + JPU_JPEG_HACTBL_SIZE - 1] = 0x00, \
+	[JPU_JPEG_PADDING_OFFSET ... JPU_JPEG_HDR_SIZE - 1] = 0xff             \
+}
+
+static unsigned char jpeg_hdrs[JPU_MAX_QUALITY][JPU_JPEG_HDR_SIZE] = {
+	[0 ... JPU_MAX_QUALITY - 1] = JPU_JPEG_HDR_BLOB
+};
+
+static const unsigned int qtbl_lum[JPU_MAX_QUALITY][QTBL_SIZE] = {
+	{
+		0x14101927, 0x322e3e44, 0x10121726, 0x26354144,
+		0x19171f26, 0x35414444, 0x27262635, 0x41444444,
+		0x32263541, 0x44444444, 0x2e354144, 0x44444444,
+		0x3e414444, 0x44444444, 0x44444444, 0x44444444
+	},
+	{
+		0x100b0b10, 0x171b1f1e, 0x0b0c0c0f, 0x1417171e,
+		0x0b0c0d10, 0x171a232f, 0x100f1017, 0x1a252f40,
+		0x1714171a, 0x27334040, 0x1b171a25, 0x33404040,
+		0x1f17232f, 0x40404040, 0x1e1e2f40, 0x40404040
+	},
+	{
+		0x0c08080c, 0x11151817, 0x0809090b, 0x0f131217,
+		0x08090a0c, 0x13141b24, 0x0c0b0c15, 0x141c2435,
+		0x110f1314, 0x1e27333b, 0x1513141c, 0x27333b3b,
+		0x18121b24, 0x333b3b3b, 0x17172435, 0x3b3b3b3b
+	},
+	{
+		0x08060608, 0x0c0e1011, 0x06060608, 0x0a0d0c0f,
+		0x06060708, 0x0d0e1218, 0x0808080e, 0x0d131823,
+		0x0c0a0d0d, 0x141a2227, 0x0e0d0e13, 0x1a222727,
+		0x100c1318, 0x22272727, 0x110f1823, 0x27272727
+	}
+};
+
+static const unsigned int qtbl_chr[JPU_MAX_QUALITY][QTBL_SIZE] = {
+	{
+		0x15192026, 0x36444444, 0x191c1826, 0x36444444,
+		0x2018202b, 0x42444444, 0x26262b35, 0x44444444,
+		0x36424444, 0x44444444, 0x44444444, 0x44444444,
+		0x44444444, 0x44444444, 0x44444444, 0x44444444
+	},
+	{
+		0x110f1115, 0x141a2630, 0x0f131211, 0x141a232b,
+		0x11121416, 0x1a1e2e35, 0x1511161c, 0x1e273540,
+		0x14141a1e, 0x27304040, 0x1a1a1e27, 0x303f4040,
+		0x26232e35, 0x40404040, 0x302b3540, 0x40404040
+	},
+	{
+		0x0d0b0d10, 0x14141d25, 0x0b0e0e0e, 0x10141a20,
+		0x0d0e0f11, 0x14172328, 0x100e1115, 0x171e2832,
+		0x14101417, 0x1e25323b, 0x1414171e, 0x25303b3b,
+		0x1d1a2328, 0x323b3b3b, 0x25202832, 0x3b3b3b3b
+	},
+	{
+		0x0908090b, 0x0e111318, 0x080a090b, 0x0e0d1116,
+		0x09090d0e, 0x0d0f171a, 0x0b0b0e0e, 0x0f141a21,
+		0x0e0e0d0f, 0x14182127, 0x110d0f14, 0x18202727,
+		0x1311171a, 0x21272727, 0x18161a21, 0x27272727
+	}
+};
+
+static const unsigned int hdctbl_lum[HDCTBL_SIZE] = {
+	0x00010501, 0x01010101, 0x01000000, 0x00000000,
+	0x00010203, 0x04050607, 0x08090a0b
+};
+
+static const unsigned int hdctbl_chr[HDCTBL_SIZE] = {
+	0x00010501, 0x01010101, 0x01000000, 0x00000000,
+	0x00010203, 0x04050607, 0x08090a0b
+};
+
+static const unsigned int hactbl_lum[HACTBL_SIZE] = {
+	0x00020103, 0x03020403, 0x05050404, 0x0000017d, 0x01020300, 0x04110512,
+	0x21314106, 0x13516107,	0x22711432, 0x8191a108, 0x2342b1c1, 0x1552d1f0,
+	0x24336272, 0x82090a16, 0x1718191a, 0x25262728, 0x292a3435, 0x36373839,
+	0x3a434445, 0x46474849, 0x4a535455, 0x56575859, 0x5a636465, 0x66676869,
+	0x6a737475, 0x76777879, 0x7a838485, 0x86878889, 0x8a929394, 0x95969798,
+	0x999aa2a3, 0xa4a5a6a7, 0xa8a9aab2, 0xb3b4b5b6, 0xb7b8b9ba, 0xc2c3c4c5,
+	0xc6c7c8c9, 0xcad2d3d4, 0xd5d6d7d8, 0xd9dae1e2, 0xe3e4e5e6, 0xe7e8e9ea,
+	0xf1f2f3f4, 0xf5f6f7f8, 0xf9fa0000
+};
+
+static const unsigned int hactbl_chr[HACTBL_SIZE] = {
+	0x00020103, 0x03020403, 0x05050404, 0x0000017d, 0x01020300, 0x04110512,
+	0x21314106, 0x13516107,	0x22711432, 0x8191a108, 0x2342b1c1, 0x1552d1f0,
+	0x24336272, 0x82090a16, 0x1718191a, 0x25262728, 0x292a3435, 0x36373839,
+	0x3a434445, 0x46474849, 0x4a535455, 0x56575859, 0x5a636465, 0x66676869,
+	0x6a737475, 0x76777879, 0x7a838485, 0x86878889, 0x8a929394, 0x95969798,
+	0x999aa2a3, 0xa4a5a6a7, 0xa8a9aab2, 0xb3b4b5b6, 0xb7b8b9ba, 0xc2c3c4c5,
+	0xc6c7c8c9, 0xcad2d3d4, 0xd5d6d7d8, 0xd9dae1e2, 0xe3e4e5e6, 0xe7e8e9ea,
+	0xf1f2f3f4, 0xf5f6f7f8, 0xf9fa0000
+};
+
+static const char *error_to_text[16] = {
+	"Normal",
+	"SOI not detected",
+	"SOF1 to SOFF detected",
+	"Subsampling not detected",
+	"SOF accuracy error",
+	"DQT accuracy error",
+	"Component error 1",
+	"Component error 2",
+	"SOF0, DQT, and DHT not detected when SOS detected",
+	"SOS not detected",
+	"EOI not detected",
+	"Restart interval data number error detected",
+	"Image size error",
+	"Last MCU data number error",
+	"Block data number error",
+	"Unknown"
+};
+
+static struct jpu_buffer *vb2_to_jpu_buffer(struct vb2_v4l2_buffer *vb)
+{
+	struct v4l2_m2m_buffer *b =
+		container_of(vb, struct v4l2_m2m_buffer, vb);
+
+	return container_of(b, struct jpu_buffer, buf);
+}
+
+static u32 jpu_read(struct jpu *jpu, unsigned int reg)
+{
+	return ioread32(jpu->regs + reg);
+}
+
+static void jpu_write(struct jpu *jpu, u32 val, unsigned int reg)
+{
+	iowrite32(val, jpu->regs + reg);
+}
+
+static struct jpu_ctx *ctrl_to_ctx(struct v4l2_ctrl *c)
+{
+	return container_of(c->handler, struct jpu_ctx, ctrl_handler);
+}
+
+static struct jpu_ctx *fh_to_ctx(struct v4l2_fh *fh)
+{
+	return container_of(fh, struct jpu_ctx, fh);
+}
+
+static void jpu_set_tbl(struct jpu *jpu, u32 reg, const unsigned int *tbl,
+			unsigned int len) {
+	unsigned int i;
+
+	for (i = 0; i < len; i++)
+		jpu_write(jpu, tbl[i], reg + (i << 2));
+}
+
+static void jpu_set_qtbl(struct jpu *jpu, unsigned short quality)
+{
+	jpu_set_tbl(jpu, JCQTBL(0), qtbl_lum[quality], QTBL_SIZE);
+	jpu_set_tbl(jpu, JCQTBL(1), qtbl_chr[quality], QTBL_SIZE);
+}
+
+static void jpu_set_htbl(struct jpu *jpu)
+{
+	jpu_set_tbl(jpu, JCHTBD(0), hdctbl_lum, HDCTBL_SIZE);
+	jpu_set_tbl(jpu, JCHTBA(0), hactbl_lum, HACTBL_SIZE);
+	jpu_set_tbl(jpu, JCHTBD(1), hdctbl_chr, HDCTBL_SIZE);
+	jpu_set_tbl(jpu, JCHTBA(1), hactbl_chr, HACTBL_SIZE);
+}
+
+static int jpu_wait_reset(struct jpu *jpu)
+{
+	unsigned long timeout;
+
+	timeout = jiffies + msecs_to_jiffies(JPU_RESET_TIMEOUT);
+
+	while (jpu_read(jpu, JCCMD) & JCCMD_SRST) {
+		if (time_after(jiffies, timeout)) {
+			dev_err(jpu->dev, "timed out in reset\n");
+			return -ETIMEDOUT;
+		}
+		schedule();
+	}
+
+	return 0;
+}
+
+static int jpu_reset(struct jpu *jpu)
+{
+	jpu_write(jpu, JCCMD_SRST, JCCMD);
+	return jpu_wait_reset(jpu);
+}
+
+/*
+ * ============================================================================
+ * video ioctl operations
+ * ============================================================================
+ */
+static void put_qtbl(u8 *p, const u8 *qtbl)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(zigzag); i++)
+		p[i] = *(qtbl + zigzag[i]);
+}
+
+static void put_htbl(u8 *p, const u8 *htbl, unsigned int len)
+{
+	unsigned int i, j;
+
+	for (i = 0; i < len; i += 4)
+		for (j = 0; j < 4 && (i + j) < len; ++j)
+			p[i + j] = htbl[i + 3 - j];
+}
+
+static void jpu_generate_hdr(unsigned short quality, unsigned char *p)
+{
+	put_qtbl(p + JPU_JPEG_QTBL_LUM_OFFSET, (const u8 *)qtbl_lum[quality]);
+	put_qtbl(p + JPU_JPEG_QTBL_CHR_OFFSET, (const u8 *)qtbl_chr[quality]);
+
+	put_htbl(p + JPU_JPEG_HDCTBL_LUM_OFFSET, (const u8 *)hdctbl_lum,
+		 JPU_JPEG_HDCTBL_SIZE);
+	put_htbl(p + JPU_JPEG_HACTBL_LUM_OFFSET, (const u8 *)hactbl_lum,
+		 JPU_JPEG_HACTBL_SIZE);
+
+	put_htbl(p + JPU_JPEG_HDCTBL_CHR_OFFSET, (const u8 *)hdctbl_chr,
+		 JPU_JPEG_HDCTBL_SIZE);
+	put_htbl(p + JPU_JPEG_HACTBL_CHR_OFFSET, (const u8 *)hactbl_chr,
+		 JPU_JPEG_HACTBL_SIZE);
+}
+
+static int get_byte(struct jpeg_buffer *buf)
+{
+	if (buf->curr >= buf->end)
+		return -1;
+
+	return *(u8 *)buf->curr++;
+}
+
+static int get_word_be(struct jpeg_buffer *buf, unsigned int *word)
+{
+	if (buf->end - buf->curr < 2)
+		return -1;
+
+	*word = get_unaligned_be16(buf->curr);
+	buf->curr += 2;
+
+	return 0;
+}
+
+static void skip(struct jpeg_buffer *buf, unsigned long len)
+{
+	buf->curr += min((unsigned long)(buf->end - buf->curr), len);
+}
+
+static u8 jpu_parse_hdr(void *buffer, unsigned long size, unsigned int *width,
+			  unsigned int *height)
+{
+	struct jpeg_buffer jpeg_buffer;
+	unsigned int word;
+	bool soi = false;
+
+	jpeg_buffer.end = buffer + size;
+	jpeg_buffer.curr = buffer;
+
+	/*
+	 * basic size check and EOI - we don't want to let JPU cross
+	 * buffer bounds in any case. Hope it's stopping by EOI.
+	 */
+	if (size < JPU_JPEG_MIN_SIZE || *(u8 *)(buffer + size - 1) != EOI)
+		return 0;
+
+	for (;;) {
+		int c;
+
+		/* skip preceding filler bytes */
+		do
+			c = get_byte(&jpeg_buffer);
+		while (c == 0xff || c == 0);
+
+		if (!soi && c == SOI) {
+			soi = true;
+			continue;
+		} else if (soi != (c != SOI))
+			return 0;
+
+		switch (c) {
+		case SOF0: /* SOF0: baseline JPEG */
+			skip(&jpeg_buffer, 3); /* segment length and bpp */
+			if (get_word_be(&jpeg_buffer, height) ||
+			    get_word_be(&jpeg_buffer, width) ||
+			    get_byte(&jpeg_buffer) != 3) /* YCbCr only */
+				return 0;
+
+			skip(&jpeg_buffer, 1);
+			return get_byte(&jpeg_buffer);
+		case DHT:
+		case DQT:
+		case COM:
+		case DRI:
+		case APP0 ... APP0 + 0x0f:
+			if (get_word_be(&jpeg_buffer, &word))
+				return 0;
+			skip(&jpeg_buffer, (long)word - 2);
+		case 0:
+			break;
+		default:
+			return 0;
+		}
+	}
+
+	return 0;
+}
+
+static int jpu_querycap(struct file *file, void *priv,
+			struct v4l2_capability *cap)
+{
+	struct jpu_ctx *ctx = fh_to_ctx(priv);
+
+	if (ctx->encoder)
+		strlcpy(cap->card, DRV_NAME " encoder", sizeof(cap->card));
+	else
+		strlcpy(cap->card, DRV_NAME " decoder", sizeof(cap->card));
+
+	strlcpy(cap->driver, DRV_NAME, sizeof(cap->driver));
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
+		 dev_name(ctx->jpu->dev));
+	cap->device_caps |= V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_M2M_MPLANE;
+	cap->capabilities = V4L2_CAP_DEVICE_CAPS | cap->device_caps;
+	memset(cap->reserved, 0, sizeof(cap->reserved));
+
+	return 0;
+}
+
+static struct jpu_fmt *jpu_find_format(bool encoder, u32 pixelformat,
+				       unsigned int fmt_type)
+{
+	unsigned int i, fmt_flag;
+
+	if (encoder)
+		fmt_flag = fmt_type == JPU_FMT_TYPE_OUTPUT ? JPU_ENC_OUTPUT :
+							     JPU_ENC_CAPTURE;
+	else
+		fmt_flag = fmt_type == JPU_FMT_TYPE_OUTPUT ? JPU_DEC_OUTPUT :
+							     JPU_DEC_CAPTURE;
+
+	for (i = 0; i < ARRAY_SIZE(jpu_formats); i++) {
+		struct jpu_fmt *fmt = &jpu_formats[i];
+
+		if (fmt->fourcc == pixelformat && fmt->types & fmt_flag)
+			return fmt;
+	}
+
+	return NULL;
+}
+
+static int jpu_enum_fmt(struct v4l2_fmtdesc *f, u32 type)
+{
+	unsigned int i, num = 0;
+
+	for (i = 0; i < ARRAY_SIZE(jpu_formats); ++i) {
+		if (jpu_formats[i].types & type) {
+			if (num == f->index)
+				break;
+			++num;
+		}
+	}
+
+	if (i >= ARRAY_SIZE(jpu_formats))
+		return -EINVAL;
+
+	f->pixelformat = jpu_formats[i].fourcc;
+
+	return 0;
+}
+
+static int jpu_enum_fmt_cap(struct file *file, void *priv,
+			    struct v4l2_fmtdesc *f)
+{
+	struct jpu_ctx *ctx = fh_to_ctx(priv);
+
+	return jpu_enum_fmt(f, ctx->encoder ? JPU_ENC_CAPTURE :
+			    JPU_DEC_CAPTURE);
+}
+
+static int jpu_enum_fmt_out(struct file *file, void *priv,
+			    struct v4l2_fmtdesc *f)
+{
+	struct jpu_ctx *ctx = fh_to_ctx(priv);
+
+	return jpu_enum_fmt(f, ctx->encoder ? JPU_ENC_OUTPUT : JPU_DEC_OUTPUT);
+}
+
+static struct jpu_q_data *jpu_get_q_data(struct jpu_ctx *ctx,
+					 enum v4l2_buf_type type)
+{
+	if (V4L2_TYPE_IS_OUTPUT(type))
+		return &ctx->out_q;
+	else
+		return &ctx->cap_q;
+}
+
+static void jpu_bound_align_image(u32 *w, unsigned int w_min,
+				  unsigned int w_max, unsigned int w_align,
+				  u32 *h, unsigned int h_min,
+				  unsigned int h_max, unsigned int h_align)
+{
+	unsigned int width, height, w_step, h_step;
+
+	width = *w;
+	height = *h;
+
+	w_step = 1U << w_align;
+	h_step = 1U << h_align;
+	v4l_bound_align_image(w, w_min, w_max, w_align, h, h_min, h_max,
+			      h_align, 3);
+
+	if (*w < width && *w + w_step < w_max)
+		*w += w_step;
+	if (*h < height && *h + h_step < h_max)
+		*h += h_step;
+}
+
+static int __jpu_try_fmt(struct jpu_ctx *ctx, struct jpu_fmt **fmtinfo,
+			 struct v4l2_pix_format_mplane *pix,
+			 enum v4l2_buf_type type)
+{
+	struct jpu_fmt *fmt;
+	unsigned int f_type, w, h;
+
+	f_type = V4L2_TYPE_IS_OUTPUT(type) ? JPU_FMT_TYPE_OUTPUT :
+						JPU_FMT_TYPE_CAPTURE;
+
+	fmt = jpu_find_format(ctx->encoder, pix->pixelformat, f_type);
+	if (!fmt) {
+		unsigned int pixelformat;
+
+		dev_dbg(ctx->jpu->dev, "unknown format; set default format\n");
+		if (ctx->encoder)
+			pixelformat = f_type == JPU_FMT_TYPE_OUTPUT ?
+				V4L2_PIX_FMT_NV16M : V4L2_PIX_FMT_JPEG;
+		else
+			pixelformat = f_type == JPU_FMT_TYPE_CAPTURE ?
+				V4L2_PIX_FMT_NV16M : V4L2_PIX_FMT_JPEG;
+		fmt = jpu_find_format(ctx->encoder, pixelformat, f_type);
+	}
+
+	pix->pixelformat = fmt->fourcc;
+	pix->colorspace = fmt->colorspace;
+	pix->field = V4L2_FIELD_NONE;
+	pix->num_planes = fmt->num_planes;
+	memset(pix->reserved, 0, sizeof(pix->reserved));
+
+	jpu_bound_align_image(&pix->width, JPU_WIDTH_MIN, JPU_WIDTH_MAX,
+			      fmt->h_align, &pix->height, JPU_HEIGHT_MIN,
+			      JPU_HEIGHT_MAX, fmt->v_align);
+
+	w = pix->width;
+	h = pix->height;
+
+	if (fmt->fourcc == V4L2_PIX_FMT_JPEG) {
+		/* ignore userspaces's sizeimage for encoding */
+		if (pix->plane_fmt[0].sizeimage <= 0 || ctx->encoder)
+			pix->plane_fmt[0].sizeimage = JPU_JPEG_HDR_SIZE +
+				(JPU_JPEG_MAX_BYTES_PER_PIXEL * w * h);
+		pix->plane_fmt[0].bytesperline = 0;
+		memset(pix->plane_fmt[0].reserved, 0,
+		       sizeof(pix->plane_fmt[0].reserved));
+	} else {
+		unsigned int i, bpl = 0;
+
+		for (i = 0; i < pix->num_planes; ++i)
+			bpl = max(bpl, pix->plane_fmt[i].bytesperline);
+
+		bpl = clamp_t(unsigned int, bpl, w, JPU_WIDTH_MAX);
+		bpl = round_up(bpl, JPU_MEMALIGN);
+
+		for (i = 0; i < pix->num_planes; ++i) {
+			pix->plane_fmt[i].bytesperline = bpl;
+			pix->plane_fmt[i].sizeimage = bpl * h * fmt->bpp[i] / 8;
+			memset(pix->plane_fmt[i].reserved, 0,
+			       sizeof(pix->plane_fmt[i].reserved));
+		}
+	}
+
+	if (fmtinfo)
+		*fmtinfo = fmt;
+
+	return 0;
+}
+
+static int jpu_try_fmt(struct file *file, void *priv, struct v4l2_format *f)
+{
+	struct jpu_ctx *ctx = fh_to_ctx(priv);
+
+	if (!v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type))
+		return -EINVAL;
+
+	return __jpu_try_fmt(ctx, NULL, &f->fmt.pix_mp, f->type);
+}
+
+static int jpu_s_fmt(struct file *file, void *priv, struct v4l2_format *f)
+{
+	struct vb2_queue *vq;
+	struct jpu_ctx *ctx = fh_to_ctx(priv);
+	struct v4l2_m2m_ctx *m2m_ctx = ctx->fh.m2m_ctx;
+	struct jpu_fmt *fmtinfo;
+	struct jpu_q_data *q_data;
+	int ret;
+
+	vq = v4l2_m2m_get_vq(m2m_ctx, f->type);
+	if (!vq)
+		return -EINVAL;
+
+	if (vb2_is_busy(vq)) {
+		v4l2_err(&ctx->jpu->v4l2_dev, "%s queue busy\n", __func__);
+		return -EBUSY;
+	}
+
+	ret = __jpu_try_fmt(ctx, &fmtinfo, &f->fmt.pix_mp, f->type);
+	if (ret < 0)
+		return ret;
+
+	q_data = jpu_get_q_data(ctx, f->type);
+
+	q_data->format = f->fmt.pix_mp;
+	q_data->fmtinfo = fmtinfo;
+
+	return 0;
+}
+
+static int jpu_g_fmt(struct file *file, void *priv, struct v4l2_format *f)
+{
+	struct jpu_q_data *q_data;
+	struct jpu_ctx *ctx = fh_to_ctx(priv);
+
+	if (!v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type))
+		return -EINVAL;
+
+	q_data = jpu_get_q_data(ctx, f->type);
+	f->fmt.pix_mp = q_data->format;
+
+	return 0;
+}
+
+/*
+ * V4L2 controls
+ */
+static int jpu_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct jpu_ctx *ctx = ctrl_to_ctx(ctrl);
+	unsigned long flags;
+
+	spin_lock_irqsave(&ctx->jpu->lock, flags);
+	if (ctrl->id == V4L2_CID_JPEG_COMPRESSION_QUALITY)
+		ctx->compr_quality = ctrl->val;
+	spin_unlock_irqrestore(&ctx->jpu->lock, flags);
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops jpu_ctrl_ops = {
+	.s_ctrl		= jpu_s_ctrl,
+};
+
+static int jpu_streamon(struct file *file, void *priv, enum v4l2_buf_type type)
+{
+	struct jpu_ctx *ctx = fh_to_ctx(priv);
+	struct jpu_q_data *src_q_data, *dst_q_data, *orig, adj, *ref;
+	enum v4l2_buf_type adj_type;
+
+	src_q_data = jpu_get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+	dst_q_data = jpu_get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+
+	if (ctx->encoder) {
+		adj = *src_q_data;
+		orig = src_q_data;
+		ref = dst_q_data;
+		adj_type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+	} else {
+		adj = *dst_q_data;
+		orig = dst_q_data;
+		ref = src_q_data;
+		adj_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	}
+
+	adj.format.width = ref->format.width;
+	adj.format.height = ref->format.height;
+
+	__jpu_try_fmt(ctx, NULL, &adj.format, adj_type);
+
+	if (adj.format.width != orig->format.width ||
+	    adj.format.height != orig->format.height) {
+		dev_err(ctx->jpu->dev, "src and dst formats do not match.\n");
+		/* maybe we can return -EPIPE here? */
+		return -EINVAL;
+	}
+
+	return v4l2_m2m_streamon(file, ctx->fh.m2m_ctx, type);
+}
+
+static const struct v4l2_ioctl_ops jpu_ioctl_ops = {
+	.vidioc_querycap		= jpu_querycap,
+
+	.vidioc_enum_fmt_vid_cap_mplane = jpu_enum_fmt_cap,
+	.vidioc_enum_fmt_vid_out_mplane = jpu_enum_fmt_out,
+	.vidioc_g_fmt_vid_cap_mplane	= jpu_g_fmt,
+	.vidioc_g_fmt_vid_out_mplane	= jpu_g_fmt,
+	.vidioc_try_fmt_vid_cap_mplane	= jpu_try_fmt,
+	.vidioc_try_fmt_vid_out_mplane	= jpu_try_fmt,
+	.vidioc_s_fmt_vid_cap_mplane	= jpu_s_fmt,
+	.vidioc_s_fmt_vid_out_mplane	= jpu_s_fmt,
+
+	.vidioc_reqbufs			= v4l2_m2m_ioctl_reqbufs,
+	.vidioc_create_bufs             = v4l2_m2m_ioctl_create_bufs,
+	.vidioc_querybuf		= v4l2_m2m_ioctl_querybuf,
+	.vidioc_qbuf			= v4l2_m2m_ioctl_qbuf,
+	.vidioc_dqbuf			= v4l2_m2m_ioctl_dqbuf,
+	.vidioc_expbuf			= v4l2_m2m_ioctl_expbuf,
+
+	.vidioc_streamon		= jpu_streamon,
+	.vidioc_streamoff		= v4l2_m2m_ioctl_streamoff,
+
+	.vidioc_subscribe_event		= v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe
+};
+
+static int jpu_controls_create(struct jpu_ctx *ctx)
+{
+	struct v4l2_ctrl *ctrl;
+	int ret;
+
+	v4l2_ctrl_handler_init(&ctx->ctrl_handler, 1);
+
+	ctrl = v4l2_ctrl_new_std(&ctx->ctrl_handler, &jpu_ctrl_ops,
+				 V4L2_CID_JPEG_COMPRESSION_QUALITY,
+				 0, JPU_MAX_QUALITY - 1, 1, 0);
+
+	if (ctx->ctrl_handler.error) {
+		ret = ctx->ctrl_handler.error;
+		goto error_free;
+	}
+
+	if (!ctx->encoder)
+		ctrl->flags |= V4L2_CTRL_FLAG_VOLATILE |
+				V4L2_CTRL_FLAG_READ_ONLY;
+
+	ret = v4l2_ctrl_handler_setup(&ctx->ctrl_handler);
+	if (ret < 0)
+		goto error_free;
+
+	return 0;
+
+error_free:
+	v4l2_ctrl_handler_free(&ctx->ctrl_handler);
+	return ret;
+}
+
+/*
+ * ============================================================================
+ * Queue operations
+ * ============================================================================
+ */
+static int jpu_queue_setup(struct vb2_queue *vq,
+			   const void *parg,
+			   unsigned int *nbuffers, unsigned int *nplanes,
+			   unsigned int sizes[], void *alloc_ctxs[])
+{
+	const struct v4l2_format *fmt = parg;
+	struct jpu_ctx *ctx = vb2_get_drv_priv(vq);
+	struct jpu_q_data *q_data;
+	unsigned int i;
+
+	q_data = jpu_get_q_data(ctx, vq->type);
+
+	*nplanes = q_data->format.num_planes;
+
+	for (i = 0; i < *nplanes; i++) {
+		unsigned int q_size = q_data->format.plane_fmt[i].sizeimage;
+		unsigned int f_size = fmt ?
+			fmt->fmt.pix_mp.plane_fmt[i].sizeimage : 0;
+
+		if (fmt && f_size < q_size)
+			return -EINVAL;
+
+		sizes[i] = fmt ? f_size : q_size;
+		alloc_ctxs[i] = ctx->jpu->alloc_ctx;
+	}
+
+	return 0;
+}
+
+static int jpu_buf_prepare(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct jpu_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct jpu_q_data *q_data;
+	unsigned int i;
+
+	q_data = jpu_get_q_data(ctx, vb->vb2_queue->type);
+
+	if (V4L2_TYPE_IS_OUTPUT(vb->vb2_queue->type)) {
+		if (vbuf->field == V4L2_FIELD_ANY)
+			vbuf->field = V4L2_FIELD_NONE;
+		if (vbuf->field != V4L2_FIELD_NONE) {
+			dev_err(ctx->jpu->dev, "%s field isn't supported\n",
+					__func__);
+			return -EINVAL;
+		}
+	}
+
+	for (i = 0; i < q_data->format.num_planes; i++) {
+		unsigned long size = q_data->format.plane_fmt[i].sizeimage;
+
+		if (vb2_plane_size(vb, i) < size) {
+			dev_err(ctx->jpu->dev,
+				"%s: data will not fit into plane (%lu < %lu)\n",
+			       __func__, vb2_plane_size(vb, i), size);
+			return -EINVAL;
+		}
+
+		/* decoder capture queue */
+		if (!ctx->encoder && !V4L2_TYPE_IS_OUTPUT(vb->vb2_queue->type))
+			vb2_set_plane_payload(vb, i, size);
+	}
+
+	return 0;
+}
+
+static void jpu_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct jpu_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+
+	if (!ctx->encoder && V4L2_TYPE_IS_OUTPUT(vb->vb2_queue->type)) {
+		struct jpu_buffer *jpu_buf = vb2_to_jpu_buffer(vbuf);
+		struct jpu_q_data *q_data, adjust;
+		void *buffer = vb2_plane_vaddr(vb, 0);
+		unsigned long buf_size = vb2_get_plane_payload(vb, 0);
+		unsigned int width, height;
+
+		u8 subsampling = jpu_parse_hdr(buffer, buf_size, &width,
+						 &height);
+
+		/* check if JPEG data basic parsing was successful */
+		if (subsampling != JPU_JPEG_422 && subsampling != JPU_JPEG_420)
+			goto format_error;
+
+		q_data = &ctx->out_q;
+
+		adjust = *q_data;
+		adjust.format.width = width;
+		adjust.format.height = height;
+
+		__jpu_try_fmt(ctx, &adjust.fmtinfo, &adjust.format,
+			      V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+
+		if (adjust.format.width != q_data->format.width ||
+		    adjust.format.height != q_data->format.height)
+			goto format_error;
+
+		/*
+		 * keep subsampling in buffer to check it
+		 * for compatibility in device_run
+		 */
+		jpu_buf->subsampling = subsampling;
+	}
+
+	if (ctx->fh.m2m_ctx)
+		v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
+
+	return;
+
+format_error:
+	dev_err(ctx->jpu->dev, "incompatible or corrupted JPEG data\n");
+	vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
+}
+
+static void jpu_buf_finish(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct jpu_buffer *jpu_buf = vb2_to_jpu_buffer(vbuf);
+	struct jpu_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct jpu_q_data *q_data = &ctx->out_q;
+	enum v4l2_buf_type type = vb->vb2_queue->type;
+	u8 *buffer;
+
+	if (vb->state == VB2_BUF_STATE_DONE)
+		vbuf->sequence = jpu_get_q_data(ctx, type)->sequence++;
+
+	if (!ctx->encoder || vb->state != VB2_BUF_STATE_DONE ||
+	    V4L2_TYPE_IS_OUTPUT(type))
+		return;
+
+	buffer = vb2_plane_vaddr(vb, 0);
+
+	memcpy(buffer, jpeg_hdrs[jpu_buf->compr_quality], JPU_JPEG_HDR_SIZE);
+	*(__be16 *)(buffer + JPU_JPEG_HEIGHT_OFFSET) =
+					cpu_to_be16(q_data->format.height);
+	*(__be16 *)(buffer + JPU_JPEG_WIDTH_OFFSET) =
+					cpu_to_be16(q_data->format.width);
+	*(buffer + JPU_JPEG_SUBS_OFFSET) = q_data->fmtinfo->subsampling;
+}
+
+static int jpu_start_streaming(struct vb2_queue *vq, unsigned count)
+{
+	struct jpu_ctx *ctx = vb2_get_drv_priv(vq);
+	struct jpu_q_data *q_data = jpu_get_q_data(ctx, vq->type);
+
+	q_data->sequence = 0;
+	return 0;
+}
+
+static void jpu_stop_streaming(struct vb2_queue *vq)
+{
+	struct jpu_ctx *ctx = vb2_get_drv_priv(vq);
+	struct vb2_v4l2_buffer *vb;
+	unsigned long flags;
+
+	for (;;) {
+		if (V4L2_TYPE_IS_OUTPUT(vq->type))
+			vb = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+		else
+			vb = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
+		if (vb == NULL)
+			return;
+		spin_lock_irqsave(&ctx->jpu->lock, flags);
+		v4l2_m2m_buf_done(vb, VB2_BUF_STATE_ERROR);
+		spin_unlock_irqrestore(&ctx->jpu->lock, flags);
+	}
+}
+
+static struct vb2_ops jpu_qops = {
+	.queue_setup		= jpu_queue_setup,
+	.buf_prepare		= jpu_buf_prepare,
+	.buf_queue		= jpu_buf_queue,
+	.buf_finish		= jpu_buf_finish,
+	.start_streaming	= jpu_start_streaming,
+	.stop_streaming		= jpu_stop_streaming,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
+};
+
+static int jpu_queue_init(void *priv, struct vb2_queue *src_vq,
+			  struct vb2_queue *dst_vq)
+{
+	struct jpu_ctx *ctx = priv;
+	int ret;
+
+	memset(src_vq, 0, sizeof(*src_vq));
+	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+	src_vq->io_modes = VB2_MMAP | VB2_DMABUF;
+	src_vq->drv_priv = ctx;
+	src_vq->buf_struct_size = sizeof(struct jpu_buffer);
+	src_vq->ops = &jpu_qops;
+	src_vq->mem_ops = &vb2_dma_contig_memops;
+	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	src_vq->lock = &ctx->jpu->mutex;
+
+	ret = vb2_queue_init(src_vq);
+	if (ret)
+		return ret;
+
+	memset(dst_vq, 0, sizeof(*dst_vq));
+	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	dst_vq->io_modes = VB2_MMAP | VB2_DMABUF;
+	dst_vq->drv_priv = ctx;
+	dst_vq->buf_struct_size = sizeof(struct jpu_buffer);
+	dst_vq->ops = &jpu_qops;
+	dst_vq->mem_ops = &vb2_dma_contig_memops;
+	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	dst_vq->lock = &ctx->jpu->mutex;
+
+	return vb2_queue_init(dst_vq);
+}
+
+/*
+ * ============================================================================
+ * Device file operations
+ * ============================================================================
+ */
+static int jpu_open(struct file *file)
+{
+	struct jpu *jpu = video_drvdata(file);
+	struct video_device *vfd = video_devdata(file);
+	struct jpu_ctx *ctx;
+	int ret;
+
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx)
+		return -ENOMEM;
+
+	v4l2_fh_init(&ctx->fh, vfd);
+	ctx->fh.ctrl_handler = &ctx->ctrl_handler;
+	file->private_data = &ctx->fh;
+	v4l2_fh_add(&ctx->fh);
+
+	ctx->jpu = jpu;
+	ctx->encoder = vfd == &jpu->vfd_encoder;
+
+	__jpu_try_fmt(ctx, &ctx->out_q.fmtinfo, &ctx->out_q.format,
+		      V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+	__jpu_try_fmt(ctx, &ctx->cap_q.fmtinfo, &ctx->cap_q.format,
+		      V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+
+	ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(jpu->m2m_dev, ctx, jpu_queue_init);
+	if (IS_ERR(ctx->fh.m2m_ctx)) {
+		ret = PTR_ERR(ctx->fh.m2m_ctx);
+		goto v4l_prepare_rollback;
+	}
+
+	ret = jpu_controls_create(ctx);
+	if (ret < 0)
+		goto v4l_prepare_rollback;
+
+	if (mutex_lock_interruptible(&jpu->mutex)) {
+		ret = -ERESTARTSYS;
+		goto v4l_prepare_rollback;
+	}
+
+	if (jpu->ref_count == 0) {
+		ret = clk_prepare_enable(jpu->clk);
+		if (ret < 0)
+			goto device_prepare_rollback;
+		/* ...issue software reset */
+		ret = jpu_reset(jpu);
+		if (ret)
+			goto device_prepare_rollback;
+	}
+
+	jpu->ref_count++;
+
+	mutex_unlock(&jpu->mutex);
+	return 0;
+
+device_prepare_rollback:
+	mutex_unlock(&jpu->mutex);
+v4l_prepare_rollback:
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	kfree(ctx);
+	return ret;
+}
+
+static int jpu_release(struct file *file)
+{
+	struct jpu *jpu = video_drvdata(file);
+	struct jpu_ctx *ctx = fh_to_ctx(file->private_data);
+
+	mutex_lock(&jpu->mutex);
+	if (--jpu->ref_count == 0)
+		clk_disable_unprepare(jpu->clk);
+	mutex_unlock(&jpu->mutex);
+
+	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
+	v4l2_ctrl_handler_free(&ctx->ctrl_handler);
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	kfree(ctx);
+
+	return 0;
+}
+
+static const struct v4l2_file_operations jpu_fops = {
+	.owner		= THIS_MODULE,
+	.open		= jpu_open,
+	.release	= jpu_release,
+	.unlocked_ioctl	= video_ioctl2,
+	.poll		= v4l2_m2m_fop_poll,
+	.mmap		= v4l2_m2m_fop_mmap,
+};
+
+/*
+ * ============================================================================
+ * mem2mem callbacks
+ * ============================================================================
+ */
+static void jpu_cleanup(struct jpu_ctx *ctx, bool reset)
+{
+	/* remove current buffers and finish job */
+	struct vb2_v4l2_buffer *src_buf, *dst_buf;
+	unsigned long flags;
+
+	spin_lock_irqsave(&ctx->jpu->lock, flags);
+
+	src_buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+	dst_buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
+
+	v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_ERROR);
+	v4l2_m2m_buf_done(dst_buf, VB2_BUF_STATE_ERROR);
+
+	/* ...and give it a chance on next run */
+	if (reset)
+		jpu_write(ctx->jpu, JCCMD_SRST, JCCMD);
+
+	spin_unlock_irqrestore(&ctx->jpu->lock, flags);
+
+	v4l2_m2m_job_finish(ctx->jpu->m2m_dev, ctx->fh.m2m_ctx);
+}
+
+static void jpu_device_run(void *priv)
+{
+	struct jpu_ctx *ctx = priv;
+	struct jpu *jpu = ctx->jpu;
+	struct jpu_buffer *jpu_buf;
+	struct jpu_q_data *q_data;
+	struct vb2_v4l2_buffer *src_buf, *dst_buf;
+	unsigned int w, h, bpl;
+	unsigned char num_planes, subsampling;
+	unsigned long flags;
+
+	/* ...wait until module reset completes; we have mutex locked here */
+	if (jpu_wait_reset(jpu)) {
+		jpu_cleanup(ctx, true);
+		return;
+	}
+
+	spin_lock_irqsave(&ctx->jpu->lock, flags);
+
+	jpu->curr = ctx;
+
+	src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+	dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
+
+	if (ctx->encoder) {
+		jpu_buf = vb2_to_jpu_buffer(dst_buf);
+		q_data = &ctx->out_q;
+	} else {
+		jpu_buf = vb2_to_jpu_buffer(src_buf);
+		q_data = &ctx->cap_q;
+	}
+
+	w = q_data->format.width;
+	h = q_data->format.height;
+	bpl = q_data->format.plane_fmt[0].bytesperline;
+	num_planes = q_data->fmtinfo->num_planes;
+	subsampling = q_data->fmtinfo->subsampling;
+
+	if (ctx->encoder) {
+		unsigned long src_1_addr, src_2_addr, dst_addr;
+		unsigned int redu, inft;
+
+		dst_addr = vb2_dma_contig_plane_dma_addr(&dst_buf->vb2_buf, 0);
+		src_1_addr =
+			vb2_dma_contig_plane_dma_addr(&src_buf->vb2_buf, 0);
+		if (num_planes > 1)
+			src_2_addr = vb2_dma_contig_plane_dma_addr(
+					&src_buf->vb2_buf, 1);
+		else
+			src_2_addr = src_1_addr + w * h;
+
+		jpu_buf->compr_quality = ctx->compr_quality;
+
+		if (subsampling == JPU_JPEG_420) {
+			redu = JCMOD_REDU_420;
+			inft = JIFECNT_INFT_420;
+		} else {
+			redu = JCMOD_REDU_422;
+			inft = JIFECNT_INFT_422;
+		}
+
+		/* only no marker mode works for encoding */
+		jpu_write(jpu, JCMOD_DSP_ENC | JCMOD_PCTR | redu |
+			  JCMOD_MSKIP_ENABLE, JCMOD);
+
+		jpu_write(jpu, JIFECNT_SWAP_WB | inft, JIFECNT);
+		jpu_write(jpu, JIFDCNT_SWAP_WB, JIFDCNT);
+		jpu_write(jpu, JINTE_TRANSF_COMPL, JINTE);
+
+		/* Y and C components source addresses */
+		jpu_write(jpu, src_1_addr, JIFESYA1);
+		jpu_write(jpu, src_2_addr, JIFESCA1);
+
+		/* memory width */
+		jpu_write(jpu, bpl, JIFESMW);
+
+		jpu_write(jpu, (w >> 8) & JCSZ_MASK, JCHSZU);
+		jpu_write(jpu, w & JCSZ_MASK, JCHSZD);
+
+		jpu_write(jpu, (h >> 8) & JCSZ_MASK, JCVSZU);
+		jpu_write(jpu, h & JCSZ_MASK, JCVSZD);
+
+		jpu_write(jpu, w, JIFESHSZ);
+		jpu_write(jpu, h, JIFESVSZ);
+
+		jpu_write(jpu, dst_addr + JPU_JPEG_HDR_SIZE, JIFEDA1);
+
+		jpu_write(jpu, 0 << JCQTN_SHIFT(1) | 1 << JCQTN_SHIFT(2) |
+			  1 << JCQTN_SHIFT(3), JCQTN);
+
+		jpu_write(jpu, 0 << JCHTN_AC_SHIFT(1) | 0 << JCHTN_DC_SHIFT(1) |
+			  1 << JCHTN_AC_SHIFT(2) | 1 << JCHTN_DC_SHIFT(2) |
+			  1 << JCHTN_AC_SHIFT(3) | 1 << JCHTN_DC_SHIFT(3),
+			  JCHTN);
+
+		jpu_set_qtbl(jpu, ctx->compr_quality);
+		jpu_set_htbl(jpu);
+	} else {
+		unsigned long src_addr, dst_1_addr, dst_2_addr;
+
+		if (jpu_buf->subsampling != subsampling) {
+			dev_err(ctx->jpu->dev,
+				"src and dst formats do not match.\n");
+			spin_unlock_irqrestore(&ctx->jpu->lock, flags);
+			jpu_cleanup(ctx, false);
+			return;
+		}
+
+		src_addr = vb2_dma_contig_plane_dma_addr(&src_buf->vb2_buf, 0);
+		dst_1_addr =
+			vb2_dma_contig_plane_dma_addr(&dst_buf->vb2_buf, 0);
+		if (q_data->fmtinfo->num_planes > 1)
+			dst_2_addr = vb2_dma_contig_plane_dma_addr(
+					&dst_buf->vb2_buf, 1);
+		else
+			dst_2_addr = dst_1_addr + w * h;
+
+		/* ...set up decoder operation */
+		jpu_write(jpu, JCMOD_DSP_DEC | JCMOD_PCTR, JCMOD);
+		jpu_write(jpu, JIFECNT_SWAP_WB, JIFECNT);
+		jpu_write(jpu, JIFDCNT_SWAP_WB, JIFDCNT);
+
+		/* ...enable interrupts on transfer completion and d-g error */
+		jpu_write(jpu, JINTE_TRANSF_COMPL | JINTE_ERR, JINTE);
+
+		/* ...set source/destination addresses of encoded data */
+		jpu_write(jpu, src_addr, JIFDSA1);
+		jpu_write(jpu, dst_1_addr, JIFDDYA1);
+		jpu_write(jpu, dst_2_addr, JIFDDCA1);
+
+		jpu_write(jpu, bpl, JIFDDMW);
+	}
+
+	/* ...start encoder/decoder operation */
+	jpu_write(jpu, JCCMD_JSRT, JCCMD);
+
+	spin_unlock_irqrestore(&ctx->jpu->lock, flags);
+}
+
+static int jpu_job_ready(void *priv)
+{
+	return 1;
+}
+
+static void jpu_job_abort(void *priv)
+{
+	struct jpu_ctx *ctx = priv;
+
+	if (!wait_event_timeout(ctx->jpu->irq_queue, !ctx->jpu->curr,
+				msecs_to_jiffies(JPU_JOB_TIMEOUT)))
+		jpu_cleanup(ctx, true);
+}
+
+static struct v4l2_m2m_ops jpu_m2m_ops = {
+	.device_run	= jpu_device_run,
+	.job_ready	= jpu_job_ready,
+	.job_abort	= jpu_job_abort,
+};
+
+/*
+ * ============================================================================
+ * IRQ handler
+ * ============================================================================
+ */
+static irqreturn_t jpu_irq_handler(int irq, void *dev_id)
+{
+	struct jpu *jpu = dev_id;
+	struct jpu_ctx *curr_ctx;
+	struct vb2_v4l2_buffer *src_buf, *dst_buf;
+	unsigned int int_status;
+
+	int_status = jpu_read(jpu, JINTS);
+
+	/* ...spurious interrupt */
+	if (!((JINTS_TRANSF_COMPL | JINTS_PROCESS_COMPL | JINTS_ERR) &
+	    int_status))
+		return IRQ_NONE;
+
+	/* ...clear interrupts */
+	jpu_write(jpu, ~(int_status & JINTS_MASK), JINTS);
+	if (int_status & (JINTS_ERR | JINTS_PROCESS_COMPL))
+		jpu_write(jpu, JCCMD_JEND, JCCMD);
+
+	spin_lock(&jpu->lock);
+
+	if ((int_status & JINTS_PROCESS_COMPL) &&
+	   !(int_status & JINTS_TRANSF_COMPL))
+		goto handled;
+
+	curr_ctx = v4l2_m2m_get_curr_priv(jpu->m2m_dev);
+	if (!curr_ctx) {
+		/* ...instance is not running */
+		dev_err(jpu->dev, "no active context for m2m\n");
+		goto handled;
+	}
+
+	src_buf = v4l2_m2m_src_buf_remove(curr_ctx->fh.m2m_ctx);
+	dst_buf = v4l2_m2m_dst_buf_remove(curr_ctx->fh.m2m_ctx);
+
+	if (int_status & JINTS_TRANSF_COMPL) {
+		if (curr_ctx->encoder) {
+			unsigned long payload_size = jpu_read(jpu, JCDTCU) << 16
+						   | jpu_read(jpu, JCDTCM) << 8
+						   | jpu_read(jpu, JCDTCD);
+			vb2_set_plane_payload(&dst_buf->vb2_buf, 0,
+				payload_size + JPU_JPEG_HDR_SIZE);
+		}
+
+		dst_buf->field = src_buf->field;
+		dst_buf->timestamp = src_buf->timestamp;
+		if (src_buf->flags & V4L2_BUF_FLAG_TIMECODE)
+			dst_buf->timecode = src_buf->timecode;
+		dst_buf->flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+		dst_buf->flags |= src_buf->flags &
+					V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+		dst_buf->flags = src_buf->flags &
+			(V4L2_BUF_FLAG_TIMECODE | V4L2_BUF_FLAG_KEYFRAME |
+			 V4L2_BUF_FLAG_PFRAME | V4L2_BUF_FLAG_BFRAME |
+			 V4L2_BUF_FLAG_TSTAMP_SRC_MASK);
+
+		v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_DONE);
+		v4l2_m2m_buf_done(dst_buf, VB2_BUF_STATE_DONE);
+	} else if (int_status & JINTS_ERR) {
+		unsigned char error = jpu_read(jpu, JCDERR) & JCDERR_MASK;
+
+		dev_dbg(jpu->dev, "processing error: %#X: %s\n", error,
+			error_to_text[error]);
+
+		v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_ERROR);
+		v4l2_m2m_buf_done(dst_buf, VB2_BUF_STATE_ERROR);
+	}
+
+	jpu->curr = NULL;
+
+	/* ...reset JPU after completion */
+	jpu_write(jpu, JCCMD_SRST, JCCMD);
+	spin_unlock(&jpu->lock);
+
+	v4l2_m2m_job_finish(jpu->m2m_dev, curr_ctx->fh.m2m_ctx);
+
+	/* ...wakeup abort routine if needed */
+	wake_up(&jpu->irq_queue);
+
+	return IRQ_HANDLED;
+
+handled:
+	spin_unlock(&jpu->lock);
+	return IRQ_HANDLED;
+}
+
+/*
+ * ============================================================================
+ * Driver basic infrastructure
+ * ============================================================================
+ */
+static const struct of_device_id jpu_dt_ids[] = {
+	{ .compatible = "renesas,jpu-r8a7790" }, /* H2 */
+	{ .compatible = "renesas,jpu-r8a7791" }, /* M2-W */
+	{ .compatible = "renesas,jpu-r8a7792" }, /* V2H */
+	{ .compatible = "renesas,jpu-r8a7793" }, /* M2-N */
+	{ },
+};
+MODULE_DEVICE_TABLE(of, jpu_dt_ids);
+
+static int jpu_probe(struct platform_device *pdev)
+{
+	struct jpu *jpu;
+	struct resource *res;
+	int ret;
+	unsigned int i;
+
+	jpu = devm_kzalloc(&pdev->dev, sizeof(*jpu), GFP_KERNEL);
+	if (!jpu)
+		return -ENOMEM;
+
+	init_waitqueue_head(&jpu->irq_queue);
+	mutex_init(&jpu->mutex);
+	spin_lock_init(&jpu->lock);
+	jpu->dev = &pdev->dev;
+
+	/* memory-mapped registers */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	jpu->regs = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(jpu->regs))
+		return PTR_ERR(jpu->regs);
+
+	/* interrupt service routine registration */
+	jpu->irq = ret = platform_get_irq(pdev, 0);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "cannot find IRQ\n");
+		return ret;
+	}
+
+	ret = devm_request_irq(&pdev->dev, jpu->irq, jpu_irq_handler, 0,
+			       dev_name(&pdev->dev), jpu);
+	if (ret) {
+		dev_err(&pdev->dev, "cannot claim IRQ %d\n", jpu->irq);
+		return ret;
+	}
+
+	/* clocks */
+	jpu->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(jpu->clk)) {
+		dev_err(&pdev->dev, "cannot get clock\n");
+		return PTR_ERR(jpu->clk);
+	}
+
+	/* v4l2 device */
+	ret = v4l2_device_register(&pdev->dev, &jpu->v4l2_dev);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to register v4l2 device\n");
+		return ret;
+	}
+
+	/* mem2mem device */
+	jpu->m2m_dev = v4l2_m2m_init(&jpu_m2m_ops);
+	if (IS_ERR(jpu->m2m_dev)) {
+		v4l2_err(&jpu->v4l2_dev, "Failed to init mem2mem device\n");
+		ret = PTR_ERR(jpu->m2m_dev);
+		goto device_register_rollback;
+	}
+
+	jpu->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
+	if (IS_ERR(jpu->alloc_ctx)) {
+		v4l2_err(&jpu->v4l2_dev, "Failed to init memory allocator\n");
+		ret = PTR_ERR(jpu->alloc_ctx);
+		goto m2m_init_rollback;
+	}
+
+	/* fill in qantization and Huffman tables for encoder */
+	for (i = 0; i < JPU_MAX_QUALITY; i++)
+		jpu_generate_hdr(i, (unsigned char *)jpeg_hdrs[i]);
+
+	strlcpy(jpu->vfd_encoder.name, DRV_NAME, sizeof(jpu->vfd_encoder.name));
+	jpu->vfd_encoder.fops		= &jpu_fops;
+	jpu->vfd_encoder.ioctl_ops	= &jpu_ioctl_ops;
+	jpu->vfd_encoder.minor		= -1;
+	jpu->vfd_encoder.release	= video_device_release_empty;
+	jpu->vfd_encoder.lock		= &jpu->mutex;
+	jpu->vfd_encoder.v4l2_dev	= &jpu->v4l2_dev;
+	jpu->vfd_encoder.vfl_dir	= VFL_DIR_M2M;
+
+	ret = video_register_device(&jpu->vfd_encoder, VFL_TYPE_GRABBER, -1);
+	if (ret) {
+		v4l2_err(&jpu->v4l2_dev, "Failed to register video device\n");
+		goto vb2_allocator_rollback;
+	}
+
+	video_set_drvdata(&jpu->vfd_encoder, jpu);
+
+	strlcpy(jpu->vfd_decoder.name, DRV_NAME, sizeof(jpu->vfd_decoder.name));
+	jpu->vfd_decoder.fops		= &jpu_fops;
+	jpu->vfd_decoder.ioctl_ops	= &jpu_ioctl_ops;
+	jpu->vfd_decoder.minor		= -1;
+	jpu->vfd_decoder.release	= video_device_release_empty;
+	jpu->vfd_decoder.lock		= &jpu->mutex;
+	jpu->vfd_decoder.v4l2_dev	= &jpu->v4l2_dev;
+	jpu->vfd_decoder.vfl_dir	= VFL_DIR_M2M;
+
+	ret = video_register_device(&jpu->vfd_decoder, VFL_TYPE_GRABBER, -1);
+	if (ret) {
+		v4l2_err(&jpu->v4l2_dev, "Failed to register video device\n");
+		goto enc_vdev_register_rollback;
+	}
+
+	video_set_drvdata(&jpu->vfd_decoder, jpu);
+	platform_set_drvdata(pdev, jpu);
+
+	v4l2_info(&jpu->v4l2_dev, "encoder device registered as /dev/video%d\n",
+		  jpu->vfd_encoder.num);
+	v4l2_info(&jpu->v4l2_dev, "decoder device registered as /dev/video%d\n",
+		  jpu->vfd_decoder.num);
+
+	return 0;
+
+enc_vdev_register_rollback:
+	video_unregister_device(&jpu->vfd_encoder);
+
+vb2_allocator_rollback:
+	vb2_dma_contig_cleanup_ctx(jpu->alloc_ctx);
+
+m2m_init_rollback:
+	v4l2_m2m_release(jpu->m2m_dev);
+
+device_register_rollback:
+	v4l2_device_unregister(&jpu->v4l2_dev);
+
+	return ret;
+}
+
+static int jpu_remove(struct platform_device *pdev)
+{
+	struct jpu *jpu = platform_get_drvdata(pdev);
+
+	video_unregister_device(&jpu->vfd_decoder);
+	video_unregister_device(&jpu->vfd_encoder);
+	vb2_dma_contig_cleanup_ctx(jpu->alloc_ctx);
+	v4l2_m2m_release(jpu->m2m_dev);
+	v4l2_device_unregister(&jpu->v4l2_dev);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int jpu_suspend(struct device *dev)
+{
+	struct jpu *jpu = dev_get_drvdata(dev);
+
+	if (jpu->ref_count == 0)
+		return 0;
+
+	clk_disable_unprepare(jpu->clk);
+
+	return 0;
+}
+
+static int jpu_resume(struct device *dev)
+{
+	struct jpu *jpu = dev_get_drvdata(dev);
+
+	if (jpu->ref_count == 0)
+		return 0;
+
+	clk_prepare_enable(jpu->clk);
+
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops jpu_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(jpu_suspend, jpu_resume)
+};
+
+static struct platform_driver jpu_driver = {
+	.probe = jpu_probe,
+	.remove = jpu_remove,
+	.driver = {
+		.of_match_table = jpu_dt_ids,
+		.name = DRV_NAME,
+		.pm = &jpu_pm_ops,
+	},
+};
+
+module_platform_driver(jpu_driver);
+
+MODULE_ALIAS("platform:" DRV_NAME);
+MODULE_AUTHOR("Mikhail Ulianov <mikhail.ulyanov@cogentembedded.com>");
+MODULE_DESCRIPTION("Renesas R-Car JPEG processing unit driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/platform/s3c-camif/Makefile b/drivers/media/platform/s3c-camif/Makefile
new file mode 100644
index 0000000..50bf8c5
--- /dev/null
+++ b/drivers/media/platform/s3c-camif/Makefile
@@ -0,0 +1,5 @@
+# Makefile for s3c244x/s3c64xx CAMIF driver
+
+s3c-camif-objs := camif-core.o camif-capture.o camif-regs.o
+
+obj-$(CONFIG_VIDEO_S3C_CAMIF) += s3c-camif.o
diff --git a/drivers/media/platform/s3c-camif/camif-capture.c b/drivers/media/platform/s3c-camif/camif-capture.c
new file mode 100644
index 0000000..537b858
--- /dev/null
+++ b/drivers/media/platform/s3c-camif/camif-capture.c
@@ -0,0 +1,1670 @@
+/*
+ * s3c24xx/s3c64xx SoC series Camera Interface (CAMIF) driver
+ *
+ * Copyright (C) 2012 Sylwester Nawrocki <sylvester.nawrocki@gmail.com>
+ * Copyright (C) 2012 Tomasz Figa <tomasz.figa@gmail.com>
+ *
+ * Based on drivers/media/platform/s5p-fimc,
+ * Copyright (C) 2010 - 2012 Samsung Electronics Co., Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+*/
+#define pr_fmt(fmt) "%s:%d " fmt, __func__, __LINE__
+
+#include <linux/bug.h>
+#include <linux/clk.h>
+#include <linux/device.h>
+#include <linux/errno.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/ratelimit.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <linux/videodev2.h>
+
+#include <media/media-device.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "camif-core.h"
+#include "camif-regs.h"
+
+static int debug;
+module_param(debug, int, 0644);
+
+/* Locking: called with vp->camif->slock spinlock held */
+static void camif_cfg_video_path(struct camif_vp *vp)
+{
+	WARN_ON(s3c_camif_get_scaler_config(vp, &vp->scaler));
+	camif_hw_set_scaler(vp);
+	camif_hw_set_flip(vp);
+	camif_hw_set_target_format(vp);
+	camif_hw_set_output_dma(vp);
+}
+
+static void camif_prepare_dma_offset(struct camif_vp *vp)
+{
+	struct camif_frame *f = &vp->out_frame;
+
+	f->dma_offset.initial = f->rect.top * f->f_width + f->rect.left;
+	f->dma_offset.line = f->f_width - (f->rect.left + f->rect.width);
+
+	pr_debug("dma_offset: initial: %d, line: %d\n",
+		 f->dma_offset.initial, f->dma_offset.line);
+}
+
+/* Locking: called with camif->slock spinlock held */
+static int s3c_camif_hw_init(struct camif_dev *camif, struct camif_vp *vp)
+{
+	const struct s3c_camif_variant *variant = camif->variant;
+
+	if (camif->sensor.sd == NULL || vp->out_fmt == NULL)
+		return -EINVAL;
+
+	if (variant->ip_revision == S3C244X_CAMIF_IP_REV)
+		camif_hw_clear_fifo_overflow(vp);
+	camif_hw_set_camera_bus(camif);
+	camif_hw_set_source_format(camif);
+	camif_hw_set_camera_crop(camif);
+	camif_hw_set_test_pattern(camif, camif->test_pattern);
+	if (variant->has_img_effect)
+		camif_hw_set_effect(camif, camif->colorfx,
+				camif->colorfx_cb, camif->colorfx_cr);
+	if (variant->ip_revision == S3C6410_CAMIF_IP_REV)
+		camif_hw_set_input_path(vp);
+	camif_cfg_video_path(vp);
+	vp->state &= ~ST_VP_CONFIG;
+
+	return 0;
+}
+
+/*
+ * Initialize the video path, only up from the scaler stage. The camera
+ * input interface set up is skipped. This is useful to enable one of the
+ * video paths when the other is already running.
+ * Locking: called with camif->slock spinlock held.
+ */
+static int s3c_camif_hw_vp_init(struct camif_dev *camif, struct camif_vp *vp)
+{
+	unsigned int ip_rev = camif->variant->ip_revision;
+
+	if (vp->out_fmt == NULL)
+		return -EINVAL;
+
+	camif_prepare_dma_offset(vp);
+	if (ip_rev == S3C244X_CAMIF_IP_REV)
+		camif_hw_clear_fifo_overflow(vp);
+	camif_cfg_video_path(vp);
+	vp->state &= ~ST_VP_CONFIG;
+	return 0;
+}
+
+static int sensor_set_power(struct camif_dev *camif, int on)
+{
+	struct cam_sensor *sensor = &camif->sensor;
+	int err = 0;
+
+	if (camif->sensor.power_count == !on)
+		err = v4l2_subdev_call(sensor->sd, core, s_power, on);
+	if (!err)
+		sensor->power_count += on ? 1 : -1;
+
+	pr_debug("on: %d, power_count: %d, err: %d\n",
+		 on, sensor->power_count, err);
+
+	return err;
+}
+
+static int sensor_set_streaming(struct camif_dev *camif, int on)
+{
+	struct cam_sensor *sensor = &camif->sensor;
+	int err = 0;
+
+	if (camif->sensor.stream_count == !on)
+		err = v4l2_subdev_call(sensor->sd, video, s_stream, on);
+	if (!err)
+		sensor->stream_count += on ? 1 : -1;
+
+	pr_debug("on: %d, stream_count: %d, err: %d\n",
+		 on, sensor->stream_count, err);
+
+	return err;
+}
+
+/*
+ * Reinitialize the driver so it is ready to start streaming again.
+ * Return any buffers to vb2, perform CAMIF software reset and
+ * turn off streaming at the data pipeline (sensor) if required.
+ */
+static int camif_reinitialize(struct camif_vp *vp)
+{
+	struct camif_dev *camif = vp->camif;
+	struct camif_buffer *buf;
+	unsigned long flags;
+	bool streaming;
+
+	spin_lock_irqsave(&camif->slock, flags);
+	streaming = vp->state & ST_VP_SENSOR_STREAMING;
+
+	vp->state &= ~(ST_VP_PENDING | ST_VP_RUNNING | ST_VP_OFF |
+		       ST_VP_ABORTING | ST_VP_STREAMING |
+		       ST_VP_SENSOR_STREAMING | ST_VP_LASTIRQ);
+
+	/* Release unused buffers */
+	while (!list_empty(&vp->pending_buf_q)) {
+		buf = camif_pending_queue_pop(vp);
+		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+	}
+
+	while (!list_empty(&vp->active_buf_q)) {
+		buf = camif_active_queue_pop(vp);
+		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+	}
+
+	spin_unlock_irqrestore(&camif->slock, flags);
+
+	if (!streaming)
+		return 0;
+
+	return sensor_set_streaming(camif, 0);
+}
+
+static bool s3c_vp_active(struct camif_vp *vp)
+{
+	struct camif_dev *camif = vp->camif;
+	unsigned long flags;
+	bool ret;
+
+	spin_lock_irqsave(&camif->slock, flags);
+	ret = (vp->state & ST_VP_RUNNING) || (vp->state & ST_VP_PENDING);
+	spin_unlock_irqrestore(&camif->slock, flags);
+
+	return ret;
+}
+
+static bool camif_is_streaming(struct camif_dev *camif)
+{
+	unsigned long flags;
+	bool status;
+
+	spin_lock_irqsave(&camif->slock, flags);
+	status = camif->stream_count > 0;
+	spin_unlock_irqrestore(&camif->slock, flags);
+
+	return status;
+}
+
+static int camif_stop_capture(struct camif_vp *vp)
+{
+	struct camif_dev *camif = vp->camif;
+	unsigned long flags;
+	int ret;
+
+	if (!s3c_vp_active(vp))
+		return 0;
+
+	spin_lock_irqsave(&camif->slock, flags);
+	vp->state &= ~(ST_VP_OFF | ST_VP_LASTIRQ);
+	vp->state |= ST_VP_ABORTING;
+	spin_unlock_irqrestore(&camif->slock, flags);
+
+	ret = wait_event_timeout(vp->irq_queue,
+			   !(vp->state & ST_VP_ABORTING),
+			   msecs_to_jiffies(CAMIF_STOP_TIMEOUT));
+
+	spin_lock_irqsave(&camif->slock, flags);
+
+	if (ret == 0 && !(vp->state & ST_VP_OFF)) {
+		/* Timed out, forcibly stop capture */
+		vp->state &= ~(ST_VP_OFF | ST_VP_ABORTING |
+			       ST_VP_LASTIRQ);
+
+		camif_hw_disable_capture(vp);
+		camif_hw_enable_scaler(vp, false);
+	}
+
+	spin_unlock_irqrestore(&camif->slock, flags);
+
+	return camif_reinitialize(vp);
+}
+
+static int camif_prepare_addr(struct camif_vp *vp, struct vb2_buffer *vb,
+			      struct camif_addr *paddr)
+{
+	struct camif_frame *frame = &vp->out_frame;
+	u32 pix_size;
+
+	if (vb == NULL || frame == NULL)
+		return -EINVAL;
+
+	pix_size = frame->rect.width * frame->rect.height;
+
+	pr_debug("colplanes: %d, pix_size: %u\n",
+		 vp->out_fmt->colplanes, pix_size);
+
+	paddr->y = vb2_dma_contig_plane_dma_addr(vb, 0);
+
+	switch (vp->out_fmt->colplanes) {
+	case 1:
+		paddr->cb = 0;
+		paddr->cr = 0;
+		break;
+	case 2:
+		/* decompose Y into Y/Cb */
+		paddr->cb = (u32)(paddr->y + pix_size);
+		paddr->cr = 0;
+		break;
+	case 3:
+		paddr->cb = (u32)(paddr->y + pix_size);
+		/* decompose Y into Y/Cb/Cr */
+		if (vp->out_fmt->color == IMG_FMT_YCBCR422P)
+			paddr->cr = (u32)(paddr->cb + (pix_size >> 1));
+		else /* 420 */
+			paddr->cr = (u32)(paddr->cb + (pix_size >> 2));
+
+		if (vp->out_fmt->color == IMG_FMT_YCRCB420)
+			swap(paddr->cb, paddr->cr);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	pr_debug("DMA address: y: %pad  cb: %pad cr: %pad\n",
+		 &paddr->y, &paddr->cb, &paddr->cr);
+
+	return 0;
+}
+
+irqreturn_t s3c_camif_irq_handler(int irq, void *priv)
+{
+	struct camif_vp *vp = priv;
+	struct camif_dev *camif = vp->camif;
+	unsigned int ip_rev = camif->variant->ip_revision;
+	unsigned int status;
+
+	spin_lock(&camif->slock);
+
+	if (ip_rev == S3C6410_CAMIF_IP_REV)
+		camif_hw_clear_pending_irq(vp);
+
+	status = camif_hw_get_status(vp);
+
+	if (ip_rev == S3C244X_CAMIF_IP_REV && (status & CISTATUS_OVF_MASK)) {
+		camif_hw_clear_fifo_overflow(vp);
+		goto unlock;
+	}
+
+	if (vp->state & ST_VP_ABORTING) {
+		if (vp->state & ST_VP_OFF) {
+			/* Last IRQ */
+			vp->state &= ~(ST_VP_OFF | ST_VP_ABORTING |
+				       ST_VP_LASTIRQ);
+			wake_up(&vp->irq_queue);
+			goto unlock;
+		} else if (vp->state & ST_VP_LASTIRQ) {
+			camif_hw_disable_capture(vp);
+			camif_hw_enable_scaler(vp, false);
+			camif_hw_set_lastirq(vp, false);
+			vp->state |= ST_VP_OFF;
+		} else {
+			/* Disable capture, enable last IRQ */
+			camif_hw_set_lastirq(vp, true);
+			vp->state |= ST_VP_LASTIRQ;
+		}
+	}
+
+	if (!list_empty(&vp->pending_buf_q) && (vp->state & ST_VP_RUNNING) &&
+	    !list_empty(&vp->active_buf_q)) {
+		unsigned int index;
+		struct camif_buffer *vbuf;
+		/*
+		 * Get previous DMA write buffer index:
+		 * 0 => DMA buffer 0, 2;
+		 * 1 => DMA buffer 1, 3.
+		 */
+		index = (CISTATUS_FRAMECNT(status) + 2) & 1;
+		vbuf = camif_active_queue_peek(vp, index);
+
+		if (!WARN_ON(vbuf == NULL)) {
+			/* Dequeue a filled buffer */
+			v4l2_get_timestamp(&vbuf->vb.timestamp);
+			vbuf->vb.sequence = vp->frame_sequence++;
+			vb2_buffer_done(&vbuf->vb.vb2_buf, VB2_BUF_STATE_DONE);
+
+			/* Set up an empty buffer at the DMA engine */
+			vbuf = camif_pending_queue_pop(vp);
+			vbuf->index = index;
+			camif_hw_set_output_addr(vp, &vbuf->paddr, index);
+			camif_hw_set_output_addr(vp, &vbuf->paddr, index + 2);
+
+			/* Scheduled in H/W, add to the queue */
+			camif_active_queue_add(vp, vbuf);
+		}
+	} else if (!(vp->state & ST_VP_ABORTING) &&
+		   (vp->state & ST_VP_PENDING))  {
+		vp->state |= ST_VP_RUNNING;
+	}
+
+	if (vp->state & ST_VP_CONFIG) {
+		camif_prepare_dma_offset(vp);
+		camif_hw_set_camera_crop(camif);
+		camif_hw_set_scaler(vp);
+		camif_hw_set_flip(vp);
+		camif_hw_set_test_pattern(camif, camif->test_pattern);
+		if (camif->variant->has_img_effect)
+			camif_hw_set_effect(camif, camif->colorfx,
+				    camif->colorfx_cb, camif->colorfx_cr);
+		vp->state &= ~ST_VP_CONFIG;
+	}
+unlock:
+	spin_unlock(&camif->slock);
+	return IRQ_HANDLED;
+}
+
+static int start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct camif_vp *vp = vb2_get_drv_priv(vq);
+	struct camif_dev *camif = vp->camif;
+	unsigned long flags;
+	int ret;
+
+	/*
+	 * We assume the codec capture path is always activated
+	 * first, before the preview path starts streaming.
+	 * This is required to avoid internal FIFO overflow and
+	 * a need for CAMIF software reset.
+	 */
+	spin_lock_irqsave(&camif->slock, flags);
+
+	if (camif->stream_count == 0) {
+		camif_hw_reset(camif);
+		ret = s3c_camif_hw_init(camif, vp);
+	} else {
+		ret = s3c_camif_hw_vp_init(camif, vp);
+	}
+	spin_unlock_irqrestore(&camif->slock, flags);
+
+	if (ret < 0) {
+		camif_reinitialize(vp);
+		return ret;
+	}
+
+	spin_lock_irqsave(&camif->slock, flags);
+	vp->frame_sequence = 0;
+	vp->state |= ST_VP_PENDING;
+
+	if (!list_empty(&vp->pending_buf_q) &&
+	    (!(vp->state & ST_VP_STREAMING) ||
+	     !(vp->state & ST_VP_SENSOR_STREAMING))) {
+
+		camif_hw_enable_scaler(vp, vp->scaler.enable);
+		camif_hw_enable_capture(vp);
+		vp->state |= ST_VP_STREAMING;
+
+		if (!(vp->state & ST_VP_SENSOR_STREAMING)) {
+			vp->state |= ST_VP_SENSOR_STREAMING;
+			spin_unlock_irqrestore(&camif->slock, flags);
+			ret = sensor_set_streaming(camif, 1);
+			if (ret)
+				v4l2_err(&vp->vdev, "Sensor s_stream failed\n");
+			if (debug)
+				camif_hw_dump_regs(camif, __func__);
+
+			return ret;
+		}
+	}
+
+	spin_unlock_irqrestore(&camif->slock, flags);
+	return 0;
+}
+
+static void stop_streaming(struct vb2_queue *vq)
+{
+	struct camif_vp *vp = vb2_get_drv_priv(vq);
+	camif_stop_capture(vp);
+}
+
+static int queue_setup(struct vb2_queue *vq, const void *parg,
+		       unsigned int *num_buffers, unsigned int *num_planes,
+		       unsigned int sizes[], void *allocators[])
+{
+	const struct v4l2_format *pfmt = parg;
+	const struct v4l2_pix_format *pix = NULL;
+	struct camif_vp *vp = vb2_get_drv_priv(vq);
+	struct camif_dev *camif = vp->camif;
+	struct camif_frame *frame = &vp->out_frame;
+	const struct camif_fmt *fmt;
+	unsigned int size;
+
+	if (pfmt) {
+		pix = &pfmt->fmt.pix;
+		fmt = s3c_camif_find_format(vp, &pix->pixelformat, -1);
+		if (fmt == NULL)
+			return -EINVAL;
+		size = (pix->width * pix->height * fmt->depth) / 8;
+	} else {
+		fmt = vp->out_fmt;
+		if (fmt == NULL)
+			return -EINVAL;
+		size = (frame->f_width * frame->f_height * fmt->depth) / 8;
+	}
+
+	*num_planes = 1;
+
+	if (pix)
+		sizes[0] = max(size, pix->sizeimage);
+	else
+		sizes[0] = size;
+	allocators[0] = camif->alloc_ctx;
+
+	pr_debug("size: %u\n", sizes[0]);
+	return 0;
+}
+
+static int buffer_prepare(struct vb2_buffer *vb)
+{
+	struct camif_vp *vp = vb2_get_drv_priv(vb->vb2_queue);
+
+	if (vp->out_fmt == NULL)
+		return -EINVAL;
+
+	if (vb2_plane_size(vb, 0) < vp->payload) {
+		v4l2_err(&vp->vdev, "buffer too small: %lu, required: %u\n",
+			 vb2_plane_size(vb, 0), vp->payload);
+		return -EINVAL;
+	}
+	vb2_set_plane_payload(vb, 0, vp->payload);
+
+	return 0;
+}
+
+static void buffer_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct camif_buffer *buf = container_of(vbuf, struct camif_buffer, vb);
+	struct camif_vp *vp = vb2_get_drv_priv(vb->vb2_queue);
+	struct camif_dev *camif = vp->camif;
+	unsigned long flags;
+
+	spin_lock_irqsave(&camif->slock, flags);
+	WARN_ON(camif_prepare_addr(vp, &buf->vb.vb2_buf, &buf->paddr));
+
+	if (!(vp->state & ST_VP_STREAMING) && vp->active_buffers < 2) {
+		/* Schedule an empty buffer in H/W */
+		buf->index = vp->buf_index;
+
+		camif_hw_set_output_addr(vp, &buf->paddr, buf->index);
+		camif_hw_set_output_addr(vp, &buf->paddr, buf->index + 2);
+
+		camif_active_queue_add(vp, buf);
+		vp->buf_index = !vp->buf_index;
+	} else {
+		camif_pending_queue_add(vp, buf);
+	}
+
+	if (vb2_is_streaming(&vp->vb_queue) && !list_empty(&vp->pending_buf_q)
+		&& !(vp->state & ST_VP_STREAMING)) {
+
+		vp->state |= ST_VP_STREAMING;
+		camif_hw_enable_scaler(vp, vp->scaler.enable);
+		camif_hw_enable_capture(vp);
+		spin_unlock_irqrestore(&camif->slock, flags);
+
+		if (!(vp->state & ST_VP_SENSOR_STREAMING)) {
+			if (sensor_set_streaming(camif, 1) == 0)
+				vp->state |= ST_VP_SENSOR_STREAMING;
+			else
+				v4l2_err(&vp->vdev, "Sensor s_stream failed\n");
+
+			if (debug)
+				camif_hw_dump_regs(camif, __func__);
+		}
+		return;
+	}
+	spin_unlock_irqrestore(&camif->slock, flags);
+}
+
+static const struct vb2_ops s3c_camif_qops = {
+	.queue_setup	 = queue_setup,
+	.buf_prepare	 = buffer_prepare,
+	.buf_queue	 = buffer_queue,
+	.wait_prepare	 = vb2_ops_wait_prepare,
+	.wait_finish	 = vb2_ops_wait_finish,
+	.start_streaming = start_streaming,
+	.stop_streaming	 = stop_streaming,
+};
+
+static int s3c_camif_open(struct file *file)
+{
+	struct camif_vp *vp = video_drvdata(file);
+	struct camif_dev *camif = vp->camif;
+	int ret;
+
+	pr_debug("[vp%d] state: %#x,  owner: %p, pid: %d\n", vp->id,
+		 vp->state, vp->owner, task_pid_nr(current));
+
+	if (mutex_lock_interruptible(&camif->lock))
+		return -ERESTARTSYS;
+
+	ret = v4l2_fh_open(file);
+	if (ret < 0)
+		goto unlock;
+
+	ret = pm_runtime_get_sync(camif->dev);
+	if (ret < 0)
+		goto err_pm;
+
+	ret = sensor_set_power(camif, 1);
+	if (!ret)
+		goto unlock;
+
+	pm_runtime_put(camif->dev);
+err_pm:
+	v4l2_fh_release(file);
+unlock:
+	mutex_unlock(&camif->lock);
+	return ret;
+}
+
+static int s3c_camif_close(struct file *file)
+{
+	struct camif_vp *vp = video_drvdata(file);
+	struct camif_dev *camif = vp->camif;
+	int ret;
+
+	pr_debug("[vp%d] state: %#x, owner: %p, pid: %d\n", vp->id,
+		 vp->state, vp->owner, task_pid_nr(current));
+
+	mutex_lock(&camif->lock);
+
+	if (vp->owner == file->private_data) {
+		camif_stop_capture(vp);
+		vb2_queue_release(&vp->vb_queue);
+		vp->owner = NULL;
+	}
+
+	sensor_set_power(camif, 0);
+
+	pm_runtime_put(camif->dev);
+	ret = v4l2_fh_release(file);
+
+	mutex_unlock(&camif->lock);
+	return ret;
+}
+
+static unsigned int s3c_camif_poll(struct file *file,
+				   struct poll_table_struct *wait)
+{
+	struct camif_vp *vp = video_drvdata(file);
+	struct camif_dev *camif = vp->camif;
+	int ret;
+
+	mutex_lock(&camif->lock);
+	if (vp->owner && vp->owner != file->private_data)
+		ret = -EBUSY;
+	else
+		ret = vb2_poll(&vp->vb_queue, file, wait);
+
+	mutex_unlock(&camif->lock);
+	return ret;
+}
+
+static int s3c_camif_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct camif_vp *vp = video_drvdata(file);
+	int ret;
+
+	if (vp->owner && vp->owner != file->private_data)
+		ret = -EBUSY;
+	else
+		ret = vb2_mmap(&vp->vb_queue, vma);
+
+	return ret;
+}
+
+static const struct v4l2_file_operations s3c_camif_fops = {
+	.owner		= THIS_MODULE,
+	.open		= s3c_camif_open,
+	.release	= s3c_camif_close,
+	.poll		= s3c_camif_poll,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap		= s3c_camif_mmap,
+};
+
+/*
+ * Video node IOCTLs
+ */
+
+static int s3c_camif_vidioc_querycap(struct file *file, void *priv,
+				     struct v4l2_capability *cap)
+{
+	struct camif_vp *vp = video_drvdata(file);
+
+	strlcpy(cap->driver, S3C_CAMIF_DRIVER_NAME, sizeof(cap->driver));
+	strlcpy(cap->card, S3C_CAMIF_DRIVER_NAME, sizeof(cap->card));
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s.%d",
+		 dev_name(vp->camif->dev), vp->id);
+
+	cap->device_caps = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_CAPTURE;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+
+	return 0;
+}
+
+static int s3c_camif_vidioc_enum_input(struct file *file, void *priv,
+				       struct v4l2_input *input)
+{
+	struct camif_vp *vp = video_drvdata(file);
+	struct v4l2_subdev *sensor = vp->camif->sensor.sd;
+
+	if (input->index || sensor == NULL)
+		return -EINVAL;
+
+	input->type = V4L2_INPUT_TYPE_CAMERA;
+	strlcpy(input->name, sensor->name, sizeof(input->name));
+	return 0;
+}
+
+static int s3c_camif_vidioc_s_input(struct file *file, void *priv,
+				    unsigned int i)
+{
+	return i == 0 ? 0 : -EINVAL;
+}
+
+static int s3c_camif_vidioc_g_input(struct file *file, void *priv,
+				    unsigned int *i)
+{
+	*i = 0;
+	return 0;
+}
+
+static int s3c_camif_vidioc_enum_fmt(struct file *file, void *priv,
+				     struct v4l2_fmtdesc *f)
+{
+	struct camif_vp *vp = video_drvdata(file);
+	const struct camif_fmt *fmt;
+
+	fmt = s3c_camif_find_format(vp, NULL, f->index);
+	if (!fmt)
+		return -EINVAL;
+
+	strlcpy(f->description, fmt->name, sizeof(f->description));
+	f->pixelformat = fmt->fourcc;
+
+	pr_debug("fmt(%d): %s\n", f->index, f->description);
+	return 0;
+}
+
+static int s3c_camif_vidioc_g_fmt(struct file *file, void *priv,
+				  struct v4l2_format *f)
+{
+	struct camif_vp *vp = video_drvdata(file);
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct camif_frame *frame = &vp->out_frame;
+	const struct camif_fmt *fmt = vp->out_fmt;
+
+	pix->bytesperline = frame->f_width * fmt->ybpp;
+	pix->sizeimage = vp->payload;
+
+	pix->pixelformat = fmt->fourcc;
+	pix->width = frame->f_width;
+	pix->height = frame->f_height;
+	pix->field = V4L2_FIELD_NONE;
+	pix->colorspace = V4L2_COLORSPACE_JPEG;
+
+	return 0;
+}
+
+static int __camif_video_try_format(struct camif_vp *vp,
+				    struct v4l2_pix_format *pix,
+				    const struct camif_fmt **ffmt)
+{
+	struct camif_dev *camif = vp->camif;
+	struct v4l2_rect *crop = &camif->camif_crop;
+	unsigned int wmin, hmin, sc_hrmax, sc_vrmax;
+	const struct vp_pix_limits *pix_lim;
+	const struct camif_fmt *fmt;
+
+	fmt = s3c_camif_find_format(vp, &pix->pixelformat, 0);
+
+	if (WARN_ON(fmt == NULL))
+		return -EINVAL;
+
+	if (ffmt)
+		*ffmt = fmt;
+
+	pix_lim = &camif->variant->vp_pix_limits[vp->id];
+
+	pr_debug("fmt: %ux%u, crop: %ux%u, bytesperline: %u\n",
+		 pix->width, pix->height, crop->width, crop->height,
+		 pix->bytesperline);
+	/*
+	 * Calculate minimum width and height according to the configured
+	 * camera input interface crop rectangle and the resizer's capabilities.
+	 */
+	sc_hrmax = min(SCALER_MAX_RATIO, 1 << (ffs(crop->width) - 3));
+	sc_vrmax = min(SCALER_MAX_RATIO, 1 << (ffs(crop->height) - 1));
+
+	wmin = max_t(u32, pix_lim->min_out_width, crop->width / sc_hrmax);
+	wmin = round_up(wmin, pix_lim->out_width_align);
+	hmin = max_t(u32, 8, crop->height / sc_vrmax);
+	hmin = round_up(hmin, 8);
+
+	v4l_bound_align_image(&pix->width, wmin, pix_lim->max_sc_out_width,
+			      ffs(pix_lim->out_width_align) - 1,
+			      &pix->height, hmin, pix_lim->max_height, 0, 0);
+
+	pix->bytesperline = pix->width * fmt->ybpp;
+	pix->sizeimage = (pix->width * pix->height * fmt->depth) / 8;
+	pix->pixelformat = fmt->fourcc;
+	pix->colorspace = V4L2_COLORSPACE_JPEG;
+	pix->field = V4L2_FIELD_NONE;
+
+	pr_debug("%ux%u, wmin: %d, hmin: %d, sc_hrmax: %d, sc_vrmax: %d\n",
+		 pix->width, pix->height, wmin, hmin, sc_hrmax, sc_vrmax);
+
+	return 0;
+}
+
+static int s3c_camif_vidioc_try_fmt(struct file *file, void *priv,
+				    struct v4l2_format *f)
+{
+	struct camif_vp *vp = video_drvdata(file);
+	return __camif_video_try_format(vp, &f->fmt.pix, NULL);
+}
+
+static int s3c_camif_vidioc_s_fmt(struct file *file, void *priv,
+				  struct v4l2_format *f)
+{
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct camif_vp *vp = video_drvdata(file);
+	struct camif_frame *out_frame = &vp->out_frame;
+	const struct camif_fmt *fmt = NULL;
+	int ret;
+
+	pr_debug("[vp%d]\n", vp->id);
+
+	if (vb2_is_busy(&vp->vb_queue))
+		return -EBUSY;
+
+	ret = __camif_video_try_format(vp, &f->fmt.pix, &fmt);
+	if (ret < 0)
+		return ret;
+
+	vp->out_fmt = fmt;
+	vp->payload = pix->sizeimage;
+	out_frame->f_width = pix->width;
+	out_frame->f_height = pix->height;
+
+	/* Reset composition rectangle */
+	out_frame->rect.width = pix->width;
+	out_frame->rect.height = pix->height;
+	out_frame->rect.left = 0;
+	out_frame->rect.top = 0;
+
+	if (vp->owner == NULL)
+		vp->owner = priv;
+
+	pr_debug("%ux%u. payload: %u. fmt: %s. %d %d. sizeimage: %d. bpl: %d\n",
+		out_frame->f_width, out_frame->f_height, vp->payload, fmt->name,
+		pix->width * pix->height * fmt->depth, fmt->depth,
+		pix->sizeimage, pix->bytesperline);
+
+	return 0;
+}
+
+/* Only check pixel formats at the sensor and the camif subdev pads */
+static int camif_pipeline_validate(struct camif_dev *camif)
+{
+	struct v4l2_subdev_format src_fmt;
+	struct media_pad *pad;
+	int ret;
+
+	/* Retrieve format at the sensor subdev source pad */
+	pad = media_entity_remote_pad(&camif->pads[0]);
+	if (!pad || media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+		return -EPIPE;
+
+	src_fmt.pad = pad->index;
+	src_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	ret = v4l2_subdev_call(camif->sensor.sd, pad, get_fmt, NULL, &src_fmt);
+	if (ret < 0 && ret != -ENOIOCTLCMD)
+		return -EPIPE;
+
+	if (src_fmt.format.width != camif->mbus_fmt.width ||
+	    src_fmt.format.height != camif->mbus_fmt.height ||
+	    src_fmt.format.code != camif->mbus_fmt.code)
+		return -EPIPE;
+
+	return 0;
+}
+
+static int s3c_camif_streamon(struct file *file, void *priv,
+			      enum v4l2_buf_type type)
+{
+	struct camif_vp *vp = video_drvdata(file);
+	struct camif_dev *camif = vp->camif;
+	struct media_entity *sensor = &camif->sensor.sd->entity;
+	int ret;
+
+	pr_debug("[vp%d]\n", vp->id);
+
+	if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	if (vp->owner && vp->owner != priv)
+		return -EBUSY;
+
+	if (s3c_vp_active(vp))
+		return 0;
+
+	ret = media_entity_pipeline_start(sensor, camif->m_pipeline);
+	if (ret < 0)
+		return ret;
+
+	ret = camif_pipeline_validate(camif);
+	if (ret < 0) {
+		media_entity_pipeline_stop(sensor);
+		return ret;
+	}
+
+	return vb2_streamon(&vp->vb_queue, type);
+}
+
+static int s3c_camif_streamoff(struct file *file, void *priv,
+			       enum v4l2_buf_type type)
+{
+	struct camif_vp *vp = video_drvdata(file);
+	struct camif_dev *camif = vp->camif;
+	int ret;
+
+	pr_debug("[vp%d]\n", vp->id);
+
+	if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	if (vp->owner && vp->owner != priv)
+		return -EBUSY;
+
+	ret = vb2_streamoff(&vp->vb_queue, type);
+	if (ret == 0)
+		media_entity_pipeline_stop(&camif->sensor.sd->entity);
+	return ret;
+}
+
+static int s3c_camif_reqbufs(struct file *file, void *priv,
+			     struct v4l2_requestbuffers *rb)
+{
+	struct camif_vp *vp = video_drvdata(file);
+	int ret;
+
+	pr_debug("[vp%d] rb count: %d, owner: %p, priv: %p\n",
+		 vp->id, rb->count, vp->owner, priv);
+
+	if (vp->owner && vp->owner != priv)
+		return -EBUSY;
+
+	if (rb->count)
+		rb->count = max_t(u32, CAMIF_REQ_BUFS_MIN, rb->count);
+	else
+		vp->owner = NULL;
+
+	ret = vb2_reqbufs(&vp->vb_queue, rb);
+	if (ret < 0)
+		return ret;
+
+	if (rb->count && rb->count < CAMIF_REQ_BUFS_MIN) {
+		rb->count = 0;
+		vb2_reqbufs(&vp->vb_queue, rb);
+		ret = -ENOMEM;
+	}
+
+	vp->reqbufs_count = rb->count;
+	if (vp->owner == NULL && rb->count > 0)
+		vp->owner = priv;
+
+	return ret;
+}
+
+static int s3c_camif_querybuf(struct file *file, void *priv,
+			      struct v4l2_buffer *buf)
+{
+	struct camif_vp *vp = video_drvdata(file);
+	return vb2_querybuf(&vp->vb_queue, buf);
+}
+
+static int s3c_camif_qbuf(struct file *file, void *priv,
+			  struct v4l2_buffer *buf)
+{
+	struct camif_vp *vp = video_drvdata(file);
+
+	pr_debug("[vp%d]\n", vp->id);
+
+	if (vp->owner && vp->owner != priv)
+		return -EBUSY;
+
+	return vb2_qbuf(&vp->vb_queue, buf);
+}
+
+static int s3c_camif_dqbuf(struct file *file, void *priv,
+			   struct v4l2_buffer *buf)
+{
+	struct camif_vp *vp = video_drvdata(file);
+
+	pr_debug("[vp%d] sequence: %d\n", vp->id, vp->frame_sequence);
+
+	if (vp->owner && vp->owner != priv)
+		return -EBUSY;
+
+	return vb2_dqbuf(&vp->vb_queue, buf, file->f_flags & O_NONBLOCK);
+}
+
+static int s3c_camif_create_bufs(struct file *file, void *priv,
+				 struct v4l2_create_buffers *create)
+{
+	struct camif_vp *vp = video_drvdata(file);
+	int ret;
+
+	if (vp->owner && vp->owner != priv)
+		return -EBUSY;
+
+	create->count = max_t(u32, 1, create->count);
+	ret = vb2_create_bufs(&vp->vb_queue, create);
+
+	if (!ret && vp->owner == NULL)
+		vp->owner = priv;
+
+	return ret;
+}
+
+static int s3c_camif_prepare_buf(struct file *file, void *priv,
+				 struct v4l2_buffer *b)
+{
+	struct camif_vp *vp = video_drvdata(file);
+	return vb2_prepare_buf(&vp->vb_queue, b);
+}
+
+static int s3c_camif_g_selection(struct file *file, void *priv,
+				 struct v4l2_selection *sel)
+{
+	struct camif_vp *vp = video_drvdata(file);
+
+	if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = vp->out_frame.f_width;
+		sel->r.height = vp->out_frame.f_height;
+		return 0;
+
+	case V4L2_SEL_TGT_COMPOSE:
+		sel->r = vp->out_frame.rect;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static void __camif_try_compose(struct camif_dev *camif, struct camif_vp *vp,
+				struct v4l2_rect *r)
+{
+	/* s3c244x doesn't support composition */
+	if (camif->variant->ip_revision == S3C244X_CAMIF_IP_REV) {
+		*r = vp->out_frame.rect;
+		return;
+	}
+
+	/* TODO: s3c64xx */
+}
+
+static int s3c_camif_s_selection(struct file *file, void *priv,
+				 struct v4l2_selection *sel)
+{
+	struct camif_vp *vp = video_drvdata(file);
+	struct camif_dev *camif = vp->camif;
+	struct v4l2_rect rect = sel->r;
+	unsigned long flags;
+
+	if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
+	    sel->target != V4L2_SEL_TGT_COMPOSE)
+		return -EINVAL;
+
+	__camif_try_compose(camif, vp, &rect);
+
+	sel->r = rect;
+	spin_lock_irqsave(&camif->slock, flags);
+	vp->out_frame.rect = rect;
+	vp->state |= ST_VP_CONFIG;
+	spin_unlock_irqrestore(&camif->slock, flags);
+
+	pr_debug("type: %#x, target: %#x, flags: %#x, (%d,%d)/%dx%d\n",
+		sel->type, sel->target, sel->flags,
+		sel->r.left, sel->r.top, sel->r.width, sel->r.height);
+
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops s3c_camif_ioctl_ops = {
+	.vidioc_querycap	  = s3c_camif_vidioc_querycap,
+	.vidioc_enum_input	  = s3c_camif_vidioc_enum_input,
+	.vidioc_g_input		  = s3c_camif_vidioc_g_input,
+	.vidioc_s_input		  = s3c_camif_vidioc_s_input,
+	.vidioc_enum_fmt_vid_cap  = s3c_camif_vidioc_enum_fmt,
+	.vidioc_try_fmt_vid_cap	  = s3c_camif_vidioc_try_fmt,
+	.vidioc_s_fmt_vid_cap	  = s3c_camif_vidioc_s_fmt,
+	.vidioc_g_fmt_vid_cap	  = s3c_camif_vidioc_g_fmt,
+	.vidioc_g_selection	  = s3c_camif_g_selection,
+	.vidioc_s_selection	  = s3c_camif_s_selection,
+	.vidioc_reqbufs		  = s3c_camif_reqbufs,
+	.vidioc_querybuf	  = s3c_camif_querybuf,
+	.vidioc_prepare_buf	  = s3c_camif_prepare_buf,
+	.vidioc_create_bufs	  = s3c_camif_create_bufs,
+	.vidioc_qbuf		  = s3c_camif_qbuf,
+	.vidioc_dqbuf		  = s3c_camif_dqbuf,
+	.vidioc_streamon	  = s3c_camif_streamon,
+	.vidioc_streamoff	  = s3c_camif_streamoff,
+	.vidioc_subscribe_event	  = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+	.vidioc_log_status	  = v4l2_ctrl_log_status,
+};
+
+/*
+ * Video node controls
+ */
+static int s3c_camif_video_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct camif_vp *vp = ctrl->priv;
+	struct camif_dev *camif = vp->camif;
+	unsigned long flags;
+
+	pr_debug("[vp%d] ctrl: %s, value: %d\n", vp->id,
+		 ctrl->name, ctrl->val);
+
+	spin_lock_irqsave(&camif->slock, flags);
+
+	switch (ctrl->id) {
+	case V4L2_CID_HFLIP:
+		vp->hflip = ctrl->val;
+		break;
+
+	case V4L2_CID_VFLIP:
+		vp->vflip = ctrl->val;
+		break;
+	}
+
+	vp->state |= ST_VP_CONFIG;
+	spin_unlock_irqrestore(&camif->slock, flags);
+	return 0;
+}
+
+/* Codec and preview video node control ops */
+static const struct v4l2_ctrl_ops s3c_camif_video_ctrl_ops = {
+	.s_ctrl = s3c_camif_video_s_ctrl,
+};
+
+int s3c_camif_register_video_node(struct camif_dev *camif, int idx)
+{
+	struct camif_vp *vp = &camif->vp[idx];
+	struct vb2_queue *q = &vp->vb_queue;
+	struct video_device *vfd = &vp->vdev;
+	struct v4l2_ctrl *ctrl;
+	int ret;
+
+	memset(vfd, 0, sizeof(*vfd));
+	snprintf(vfd->name, sizeof(vfd->name), "camif-%s",
+		 vp->id == 0 ? "codec" : "preview");
+
+	vfd->fops = &s3c_camif_fops;
+	vfd->ioctl_ops = &s3c_camif_ioctl_ops;
+	vfd->v4l2_dev = &camif->v4l2_dev;
+	vfd->minor = -1;
+	vfd->release = video_device_release_empty;
+	vfd->lock = &camif->lock;
+	vp->reqbufs_count = 0;
+
+	INIT_LIST_HEAD(&vp->pending_buf_q);
+	INIT_LIST_HEAD(&vp->active_buf_q);
+
+	memset(q, 0, sizeof(*q));
+	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	q->io_modes = VB2_MMAP | VB2_USERPTR;
+	q->ops = &s3c_camif_qops;
+	q->mem_ops = &vb2_dma_contig_memops;
+	q->buf_struct_size = sizeof(struct camif_buffer);
+	q->drv_priv = vp;
+	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	q->lock = &vp->camif->lock;
+
+	ret = vb2_queue_init(q);
+	if (ret)
+		goto err_vd_rel;
+
+	vp->pad.flags = MEDIA_PAD_FL_SINK;
+	ret = media_entity_init(&vfd->entity, 1, &vp->pad, 0);
+	if (ret)
+		goto err_vd_rel;
+
+	video_set_drvdata(vfd, vp);
+
+	v4l2_ctrl_handler_init(&vp->ctrl_handler, 1);
+	ctrl = v4l2_ctrl_new_std(&vp->ctrl_handler, &s3c_camif_video_ctrl_ops,
+				 V4L2_CID_HFLIP, 0, 1, 1, 0);
+	if (ctrl)
+		ctrl->priv = vp;
+	ctrl = v4l2_ctrl_new_std(&vp->ctrl_handler, &s3c_camif_video_ctrl_ops,
+				 V4L2_CID_VFLIP, 0, 1, 1, 0);
+	if (ctrl)
+		ctrl->priv = vp;
+
+	ret = vp->ctrl_handler.error;
+	if (ret < 0)
+		goto err_me_cleanup;
+
+	vfd->ctrl_handler = &vp->ctrl_handler;
+
+	ret = video_register_device(vfd, VFL_TYPE_GRABBER, -1);
+	if (ret)
+		goto err_ctrlh_free;
+
+	v4l2_info(&camif->v4l2_dev, "registered %s as /dev/%s\n",
+		  vfd->name, video_device_node_name(vfd));
+	return 0;
+
+err_ctrlh_free:
+	v4l2_ctrl_handler_free(&vp->ctrl_handler);
+err_me_cleanup:
+	media_entity_cleanup(&vfd->entity);
+err_vd_rel:
+	video_device_release(vfd);
+	return ret;
+}
+
+void s3c_camif_unregister_video_node(struct camif_dev *camif, int idx)
+{
+	struct video_device *vfd = &camif->vp[idx].vdev;
+
+	if (video_is_registered(vfd)) {
+		video_unregister_device(vfd);
+		media_entity_cleanup(&vfd->entity);
+		v4l2_ctrl_handler_free(vfd->ctrl_handler);
+	}
+}
+
+/* Media bus pixel formats supported at the camif input */
+static const u32 camif_mbus_formats[] = {
+	MEDIA_BUS_FMT_YUYV8_2X8,
+	MEDIA_BUS_FMT_YVYU8_2X8,
+	MEDIA_BUS_FMT_UYVY8_2X8,
+	MEDIA_BUS_FMT_VYUY8_2X8,
+};
+
+/*
+ *  Camera input interface subdev operations
+ */
+
+static int s3c_camif_subdev_enum_mbus_code(struct v4l2_subdev *sd,
+					struct v4l2_subdev_pad_config *cfg,
+					struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index >= ARRAY_SIZE(camif_mbus_formats))
+		return -EINVAL;
+
+	code->code = camif_mbus_formats[code->index];
+	return 0;
+}
+
+static int s3c_camif_subdev_get_fmt(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_pad_config *cfg,
+				    struct v4l2_subdev_format *fmt)
+{
+	struct camif_dev *camif = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *mf = &fmt->format;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		mf = v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
+		fmt->format = *mf;
+		return 0;
+	}
+
+	mutex_lock(&camif->lock);
+
+	switch (fmt->pad) {
+	case CAMIF_SD_PAD_SINK:
+		/* full camera input pixel size */
+		*mf = camif->mbus_fmt;
+		break;
+
+	case CAMIF_SD_PAD_SOURCE_C...CAMIF_SD_PAD_SOURCE_P:
+		/* crop rectangle at camera interface input */
+		mf->width = camif->camif_crop.width;
+		mf->height = camif->camif_crop.height;
+		mf->code = camif->mbus_fmt.code;
+		break;
+	}
+
+	mutex_unlock(&camif->lock);
+	mf->field = V4L2_FIELD_NONE;
+	mf->colorspace = V4L2_COLORSPACE_JPEG;
+	return 0;
+}
+
+static void __camif_subdev_try_format(struct camif_dev *camif,
+				struct v4l2_mbus_framefmt *mf, int pad)
+{
+	const struct s3c_camif_variant *variant = camif->variant;
+	const struct vp_pix_limits *pix_lim;
+	int i = ARRAY_SIZE(camif_mbus_formats);
+
+	/* FIXME: constraints against codec or preview path ? */
+	pix_lim = &variant->vp_pix_limits[VP_CODEC];
+
+	while (i-- >= 0)
+		if (camif_mbus_formats[i] == mf->code)
+			break;
+
+	mf->code = camif_mbus_formats[i];
+
+	if (pad == CAMIF_SD_PAD_SINK) {
+		v4l_bound_align_image(&mf->width, 8, CAMIF_MAX_PIX_WIDTH,
+				      ffs(pix_lim->out_width_align) - 1,
+				      &mf->height, 8, CAMIF_MAX_PIX_HEIGHT, 0,
+				      0);
+	} else {
+		struct v4l2_rect *crop = &camif->camif_crop;
+		v4l_bound_align_image(&mf->width, 8, crop->width,
+				      ffs(pix_lim->out_width_align) - 1,
+				      &mf->height, 8, crop->height,
+				      0, 0);
+	}
+
+	v4l2_dbg(1, debug, &camif->subdev, "%ux%u\n", mf->width, mf->height);
+}
+
+static int s3c_camif_subdev_set_fmt(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_pad_config *cfg,
+				    struct v4l2_subdev_format *fmt)
+{
+	struct camif_dev *camif = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *mf = &fmt->format;
+	struct v4l2_rect *crop = &camif->camif_crop;
+	int i;
+
+	v4l2_dbg(1, debug, sd, "pad%d: code: 0x%x, %ux%u\n",
+		 fmt->pad, mf->code, mf->width, mf->height);
+
+	mf->field = V4L2_FIELD_NONE;
+	mf->colorspace = V4L2_COLORSPACE_JPEG;
+	mutex_lock(&camif->lock);
+
+	/*
+	 * No pixel format change at the camera input is allowed
+	 * while streaming.
+	 */
+	if (vb2_is_busy(&camif->vp[VP_CODEC].vb_queue) ||
+	    vb2_is_busy(&camif->vp[VP_PREVIEW].vb_queue)) {
+		mutex_unlock(&camif->lock);
+		return -EBUSY;
+	}
+
+	__camif_subdev_try_format(camif, mf, fmt->pad);
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		mf = v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
+		*mf = fmt->format;
+		mutex_unlock(&camif->lock);
+		return 0;
+	}
+
+	switch (fmt->pad) {
+	case CAMIF_SD_PAD_SINK:
+		camif->mbus_fmt = *mf;
+		/* Reset sink crop rectangle. */
+		crop->width = mf->width;
+		crop->height = mf->height;
+		crop->left = 0;
+		crop->top = 0;
+		/*
+		 * Reset source format (the camif's crop rectangle)
+		 * and the video output resolution.
+		 */
+		for (i = 0; i < CAMIF_VP_NUM; i++) {
+			struct camif_frame *frame = &camif->vp[i].out_frame;
+			frame->rect = *crop;
+			frame->f_width = mf->width;
+			frame->f_height = mf->height;
+		}
+		break;
+
+	case CAMIF_SD_PAD_SOURCE_C...CAMIF_SD_PAD_SOURCE_P:
+		/* Pixel format can be only changed on the sink pad. */
+		mf->code = camif->mbus_fmt.code;
+		mf->width = crop->width;
+		mf->height = crop->height;
+		break;
+	}
+
+	mutex_unlock(&camif->lock);
+	return 0;
+}
+
+static int s3c_camif_subdev_get_selection(struct v4l2_subdev *sd,
+					  struct v4l2_subdev_pad_config *cfg,
+					  struct v4l2_subdev_selection *sel)
+{
+	struct camif_dev *camif = v4l2_get_subdevdata(sd);
+	struct v4l2_rect *crop = &camif->camif_crop;
+	struct v4l2_mbus_framefmt *mf = &camif->mbus_fmt;
+
+	if ((sel->target != V4L2_SEL_TGT_CROP &&
+	    sel->target != V4L2_SEL_TGT_CROP_BOUNDS) ||
+	    sel->pad != CAMIF_SD_PAD_SINK)
+		return -EINVAL;
+
+	if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
+		sel->r = *v4l2_subdev_get_try_crop(sd, cfg, sel->pad);
+		return 0;
+	}
+
+	mutex_lock(&camif->lock);
+
+	if (sel->target == V4L2_SEL_TGT_CROP) {
+		sel->r = *crop;
+	} else { /* crop bounds */
+		sel->r.width = mf->width;
+		sel->r.height = mf->height;
+		sel->r.left = 0;
+		sel->r.top = 0;
+	}
+
+	mutex_unlock(&camif->lock);
+
+	v4l2_dbg(1, debug, sd, "%s: crop: (%d,%d) %dx%d, size: %ux%u\n",
+		 __func__, crop->left, crop->top, crop->width,
+		 crop->height, mf->width, mf->height);
+
+	return 0;
+}
+
+static void __camif_try_crop(struct camif_dev *camif, struct v4l2_rect *r)
+{
+	struct v4l2_mbus_framefmt *mf = &camif->mbus_fmt;
+	const struct camif_pix_limits *pix_lim = &camif->variant->pix_limits;
+	unsigned int left = 2 * r->left;
+	unsigned int top = 2 * r->top;
+
+	/*
+	 * Following constraints must be met:
+	 *  - r->width + 2 * r->left = mf->width;
+	 *  - r->height + 2 * r->top = mf->height;
+	 *  - crop rectangle size and position must be aligned
+	 *    to 8 or 2 pixels, depending on SoC version.
+	 */
+	v4l_bound_align_image(&r->width, 0, mf->width,
+			      ffs(pix_lim->win_hor_offset_align) - 1,
+			      &r->height, 0, mf->height, 1, 0);
+
+	v4l_bound_align_image(&left, 0, mf->width - r->width,
+			      ffs(pix_lim->win_hor_offset_align),
+			      &top, 0, mf->height - r->height, 2, 0);
+
+	r->left = left / 2;
+	r->top = top / 2;
+	r->width = mf->width - left;
+	r->height = mf->height - top;
+	/*
+	 * Make sure we either downscale or upscale both the pixel
+	 * width and height. Just return current crop rectangle if
+	 * this scaler constraint is not met.
+	 */
+	if (camif->variant->ip_revision == S3C244X_CAMIF_IP_REV &&
+	    camif_is_streaming(camif)) {
+		unsigned int i;
+
+		for (i = 0; i < CAMIF_VP_NUM; i++) {
+			struct v4l2_rect *or = &camif->vp[i].out_frame.rect;
+			if ((or->width > r->width) == (or->height > r->height))
+				continue;
+			*r = camif->camif_crop;
+			pr_debug("Width/height scaling direction limitation\n");
+			break;
+		}
+	}
+
+	v4l2_dbg(1, debug, &camif->v4l2_dev, "crop: (%d,%d)/%dx%d, fmt: %ux%u\n",
+		 r->left, r->top, r->width, r->height, mf->width, mf->height);
+}
+
+static int s3c_camif_subdev_set_selection(struct v4l2_subdev *sd,
+					  struct v4l2_subdev_pad_config *cfg,
+					  struct v4l2_subdev_selection *sel)
+{
+	struct camif_dev *camif = v4l2_get_subdevdata(sd);
+	struct v4l2_rect *crop = &camif->camif_crop;
+	struct camif_scaler scaler;
+
+	if (sel->target != V4L2_SEL_TGT_CROP || sel->pad != CAMIF_SD_PAD_SINK)
+		return -EINVAL;
+
+	mutex_lock(&camif->lock);
+	__camif_try_crop(camif, &sel->r);
+
+	if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
+		*v4l2_subdev_get_try_crop(sd, cfg, sel->pad) = sel->r;
+	} else {
+		unsigned long flags;
+		unsigned int i;
+
+		spin_lock_irqsave(&camif->slock, flags);
+		*crop = sel->r;
+
+		for (i = 0; i < CAMIF_VP_NUM; i++) {
+			struct camif_vp *vp = &camif->vp[i];
+			scaler = vp->scaler;
+			if (s3c_camif_get_scaler_config(vp, &scaler))
+				continue;
+			vp->scaler = scaler;
+			vp->state |= ST_VP_CONFIG;
+		}
+
+		spin_unlock_irqrestore(&camif->slock, flags);
+	}
+	mutex_unlock(&camif->lock);
+
+	v4l2_dbg(1, debug, sd, "%s: (%d,%d) %dx%d, f_w: %u, f_h: %u\n",
+		 __func__, crop->left, crop->top, crop->width, crop->height,
+		 camif->mbus_fmt.width, camif->mbus_fmt.height);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_pad_ops s3c_camif_subdev_pad_ops = {
+	.enum_mbus_code = s3c_camif_subdev_enum_mbus_code,
+	.get_selection = s3c_camif_subdev_get_selection,
+	.set_selection = s3c_camif_subdev_set_selection,
+	.get_fmt = s3c_camif_subdev_get_fmt,
+	.set_fmt = s3c_camif_subdev_set_fmt,
+};
+
+static struct v4l2_subdev_ops s3c_camif_subdev_ops = {
+	.pad = &s3c_camif_subdev_pad_ops,
+};
+
+static int s3c_camif_subdev_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct camif_dev *camif = container_of(ctrl->handler, struct camif_dev,
+					       ctrl_handler);
+	unsigned long flags;
+
+	spin_lock_irqsave(&camif->slock, flags);
+
+	switch (ctrl->id) {
+	case V4L2_CID_COLORFX:
+		camif->colorfx = camif->ctrl_colorfx->val;
+		/* Set Cb, Cr */
+		switch (ctrl->val) {
+		case V4L2_COLORFX_SEPIA:
+			camif->colorfx_cb = 115;
+			camif->colorfx_cr = 145;
+			break;
+		case V4L2_COLORFX_SET_CBCR:
+			camif->colorfx_cb = camif->ctrl_colorfx_cbcr->val >> 8;
+			camif->colorfx_cr = camif->ctrl_colorfx_cbcr->val & 0xff;
+			break;
+		default:
+			/* for V4L2_COLORFX_BW and others */
+			camif->colorfx_cb = 128;
+			camif->colorfx_cr = 128;
+		}
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		camif->test_pattern = camif->ctrl_test_pattern->val;
+		break;
+	default:
+		WARN_ON(1);
+	}
+
+	camif->vp[VP_CODEC].state |= ST_VP_CONFIG;
+	camif->vp[VP_PREVIEW].state |= ST_VP_CONFIG;
+	spin_unlock_irqrestore(&camif->slock, flags);
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops s3c_camif_subdev_ctrl_ops = {
+	.s_ctrl	= s3c_camif_subdev_s_ctrl,
+};
+
+static const char * const s3c_camif_test_pattern_menu[] = {
+	"Disabled",
+	"Color bars",
+	"Horizontal increment",
+	"Vertical increment",
+};
+
+int s3c_camif_create_subdev(struct camif_dev *camif)
+{
+	struct v4l2_ctrl_handler *handler = &camif->ctrl_handler;
+	struct v4l2_subdev *sd = &camif->subdev;
+	int ret;
+
+	v4l2_subdev_init(sd, &s3c_camif_subdev_ops);
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	strlcpy(sd->name, "S3C-CAMIF", sizeof(sd->name));
+
+	camif->pads[CAMIF_SD_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+	camif->pads[CAMIF_SD_PAD_SOURCE_C].flags = MEDIA_PAD_FL_SOURCE;
+	camif->pads[CAMIF_SD_PAD_SOURCE_P].flags = MEDIA_PAD_FL_SOURCE;
+
+	ret = media_entity_init(&sd->entity, CAMIF_SD_PADS_NUM,
+				camif->pads, 0);
+	if (ret)
+		return ret;
+
+	v4l2_ctrl_handler_init(handler, 3);
+	camif->ctrl_test_pattern = v4l2_ctrl_new_std_menu_items(handler,
+			&s3c_camif_subdev_ctrl_ops, V4L2_CID_TEST_PATTERN,
+			ARRAY_SIZE(s3c_camif_test_pattern_menu) - 1, 0, 0,
+			s3c_camif_test_pattern_menu);
+
+	if (camif->variant->has_img_effect) {
+		camif->ctrl_colorfx = v4l2_ctrl_new_std_menu(handler,
+				&s3c_camif_subdev_ctrl_ops,
+				V4L2_CID_COLORFX, V4L2_COLORFX_SET_CBCR,
+				~0x981f, V4L2_COLORFX_NONE);
+
+		camif->ctrl_colorfx_cbcr = v4l2_ctrl_new_std(handler,
+				&s3c_camif_subdev_ctrl_ops,
+				V4L2_CID_COLORFX_CBCR, 0, 0xffff, 1, 0);
+	}
+
+	if (handler->error) {
+		v4l2_ctrl_handler_free(handler);
+		media_entity_cleanup(&sd->entity);
+		return handler->error;
+	}
+
+	if (camif->variant->has_img_effect)
+		v4l2_ctrl_auto_cluster(2, &camif->ctrl_colorfx,
+			       V4L2_COLORFX_SET_CBCR, false);
+
+	sd->ctrl_handler = handler;
+	v4l2_set_subdevdata(sd, camif);
+
+	return 0;
+}
+
+void s3c_camif_unregister_subdev(struct camif_dev *camif)
+{
+	struct v4l2_subdev *sd = &camif->subdev;
+
+	/* Return if not registered */
+	if (v4l2_get_subdevdata(sd) == NULL)
+		return;
+
+	v4l2_device_unregister_subdev(sd);
+	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(&camif->ctrl_handler);
+	v4l2_set_subdevdata(sd, NULL);
+}
+
+int s3c_camif_set_defaults(struct camif_dev *camif)
+{
+	unsigned int ip_rev = camif->variant->ip_revision;
+	int i;
+
+	for (i = 0; i < CAMIF_VP_NUM; i++) {
+		struct camif_vp *vp = &camif->vp[i];
+		struct camif_frame *f = &vp->out_frame;
+
+		vp->camif = camif;
+		vp->id = i;
+		vp->offset = camif->variant->vp_offset;
+
+		if (ip_rev == S3C244X_CAMIF_IP_REV)
+			vp->fmt_flags = i ? FMT_FL_S3C24XX_PREVIEW :
+					FMT_FL_S3C24XX_CODEC;
+		else
+			vp->fmt_flags = FMT_FL_S3C64XX;
+
+		vp->out_fmt = s3c_camif_find_format(vp, NULL, 0);
+		BUG_ON(vp->out_fmt == NULL);
+
+		memset(f, 0, sizeof(*f));
+		f->f_width = CAMIF_DEF_WIDTH;
+		f->f_height = CAMIF_DEF_HEIGHT;
+		f->rect.width = CAMIF_DEF_WIDTH;
+		f->rect.height = CAMIF_DEF_HEIGHT;
+
+		/* Scaler is always enabled */
+		vp->scaler.enable = 1;
+
+		vp->payload = (f->f_width * f->f_height *
+			       vp->out_fmt->depth) / 8;
+	}
+
+	memset(&camif->mbus_fmt, 0, sizeof(camif->mbus_fmt));
+	camif->mbus_fmt.width = CAMIF_DEF_WIDTH;
+	camif->mbus_fmt.height = CAMIF_DEF_HEIGHT;
+	camif->mbus_fmt.code  = camif_mbus_formats[0];
+
+	memset(&camif->camif_crop, 0, sizeof(camif->camif_crop));
+	camif->camif_crop.width = CAMIF_DEF_WIDTH;
+	camif->camif_crop.height = CAMIF_DEF_HEIGHT;
+
+	return 0;
+}
diff --git a/drivers/media/platform/s3c-camif/camif-core.c b/drivers/media/platform/s3c-camif/camif-core.c
new file mode 100644
index 0000000..1ba9bb0
--- /dev/null
+++ b/drivers/media/platform/s3c-camif/camif-core.c
@@ -0,0 +1,663 @@
+/*
+ * s3c24xx/s3c64xx SoC series Camera Interface (CAMIF) driver
+ *
+ * Copyright (C) 2012 Sylwester Nawrocki <sylvester.nawrocki@gmail.com>
+ * Copyright (C) 2012 Tomasz Figa <tomasz.figa@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published
+ * by the Free Software Foundation, either version 2 of the License,
+ * or (at your option) any later version.
+ */
+#define pr_fmt(fmt) "%s:%d " fmt, __func__, __LINE__
+
+#include <linux/bug.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/errno.h>
+#include <linux/gpio.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <linux/version.h>
+
+#include <media/media-device.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "camif-core.h"
+
+static char *camif_clocks[CLK_MAX_NUM] = {
+	/* HCLK CAMIF clock */
+	[CLK_GATE]	= "camif",
+	/* CAMIF / external camera sensor master clock */
+	[CLK_CAM]	= "camera",
+};
+
+static const struct camif_fmt camif_formats[] = {
+	{
+		.name		= "YUV 4:2:2 planar, Y/Cb/Cr",
+		.fourcc		= V4L2_PIX_FMT_YUV422P,
+		.depth		= 16,
+		.ybpp		= 1,
+		.color		= IMG_FMT_YCBCR422P,
+		.colplanes	= 3,
+		.flags		= FMT_FL_S3C24XX_CODEC |
+				  FMT_FL_S3C64XX,
+	}, {
+		.name		= "YUV 4:2:0 planar, Y/Cb/Cr",
+		.fourcc		= V4L2_PIX_FMT_YUV420,
+		.depth		= 12,
+		.ybpp		= 1,
+		.color		= IMG_FMT_YCBCR420,
+		.colplanes	= 3,
+		.flags		= FMT_FL_S3C24XX_CODEC |
+				  FMT_FL_S3C64XX,
+	}, {
+		.name		= "YVU 4:2:0 planar, Y/Cr/Cb",
+		.fourcc		= V4L2_PIX_FMT_YVU420,
+		.depth		= 12,
+		.ybpp		= 1,
+		.color		= IMG_FMT_YCRCB420,
+		.colplanes	= 3,
+		.flags		= FMT_FL_S3C24XX_CODEC |
+				  FMT_FL_S3C64XX,
+	}, {
+		.name		= "RGB565, 16 bpp",
+		.fourcc		= V4L2_PIX_FMT_RGB565X,
+		.depth		= 16,
+		.ybpp		= 2,
+		.color		= IMG_FMT_RGB565,
+		.colplanes	= 1,
+		.flags		= FMT_FL_S3C24XX_PREVIEW |
+				  FMT_FL_S3C64XX,
+	}, {
+		.name		= "XRGB8888, 32 bpp",
+		.fourcc		= V4L2_PIX_FMT_RGB32,
+		.depth		= 32,
+		.ybpp		= 4,
+		.color		= IMG_FMT_XRGB8888,
+		.colplanes	= 1,
+		.flags		= FMT_FL_S3C24XX_PREVIEW |
+				  FMT_FL_S3C64XX,
+	}, {
+		.name		= "BGR666",
+		.fourcc		= V4L2_PIX_FMT_BGR666,
+		.depth		= 32,
+		.ybpp		= 4,
+		.color		= IMG_FMT_RGB666,
+		.colplanes	= 1,
+		.flags		= FMT_FL_S3C64XX,
+	}
+};
+
+/**
+ * s3c_camif_find_format() - lookup camif color format by fourcc or an index
+ * @pixelformat: fourcc to match, ignored if null
+ * @index: index to the camif_formats array, ignored if negative
+ */
+const struct camif_fmt *s3c_camif_find_format(struct camif_vp *vp,
+					      const u32 *pixelformat,
+					      int index)
+{
+	const struct camif_fmt *fmt, *def_fmt = NULL;
+	unsigned int i;
+	int id = 0;
+
+	if (index >= (int)ARRAY_SIZE(camif_formats))
+		return NULL;
+
+	for (i = 0; i < ARRAY_SIZE(camif_formats); ++i) {
+		fmt = &camif_formats[i];
+		if (vp && !(vp->fmt_flags & fmt->flags))
+			continue;
+		if (pixelformat && fmt->fourcc == *pixelformat)
+			return fmt;
+		if (index == id)
+			def_fmt = fmt;
+		id++;
+	}
+	return def_fmt;
+}
+
+static int camif_get_scaler_factor(u32 src, u32 tar, u32 *ratio, u32 *shift)
+{
+	unsigned int sh = 6;
+
+	if (src >= 64 * tar)
+		return -EINVAL;
+
+	while (sh--) {
+		unsigned int tmp = 1 << sh;
+		if (src >= tar * tmp) {
+			*shift = sh, *ratio = tmp;
+			return 0;
+		}
+	}
+	*shift = 0, *ratio = 1;
+	return 0;
+}
+
+int s3c_camif_get_scaler_config(struct camif_vp *vp,
+				struct camif_scaler *scaler)
+{
+	struct v4l2_rect *camif_crop = &vp->camif->camif_crop;
+	int source_x = camif_crop->width;
+	int source_y = camif_crop->height;
+	int target_x = vp->out_frame.rect.width;
+	int target_y = vp->out_frame.rect.height;
+	int ret;
+
+	if (vp->rotation == 90 || vp->rotation == 270)
+		swap(target_x, target_y);
+
+	ret = camif_get_scaler_factor(source_x, target_x, &scaler->pre_h_ratio,
+				      &scaler->h_shift);
+	if (ret < 0)
+		return ret;
+
+	ret = camif_get_scaler_factor(source_y, target_y, &scaler->pre_v_ratio,
+				      &scaler->v_shift);
+	if (ret < 0)
+		return ret;
+
+	scaler->pre_dst_width = source_x / scaler->pre_h_ratio;
+	scaler->pre_dst_height = source_y / scaler->pre_v_ratio;
+
+	scaler->main_h_ratio = (source_x << 8) / (target_x << scaler->h_shift);
+	scaler->main_v_ratio = (source_y << 8) / (target_y << scaler->v_shift);
+
+	scaler->scaleup_h = (target_x >= source_x);
+	scaler->scaleup_v = (target_y >= source_y);
+
+	scaler->copy = 0;
+
+	pr_debug("H: ratio: %u, shift: %u. V: ratio: %u, shift: %u.\n",
+		 scaler->pre_h_ratio, scaler->h_shift,
+		 scaler->pre_v_ratio, scaler->v_shift);
+
+	pr_debug("Source: %dx%d, Target: %dx%d, scaleup_h/v: %d/%d\n",
+		 source_x, source_y, target_x, target_y,
+		 scaler->scaleup_h, scaler->scaleup_v);
+
+	return 0;
+}
+
+static int camif_register_sensor(struct camif_dev *camif)
+{
+	struct s3c_camif_sensor_info *sensor = &camif->pdata.sensor;
+	struct v4l2_device *v4l2_dev = &camif->v4l2_dev;
+	struct i2c_adapter *adapter;
+	struct v4l2_subdev_format format;
+	struct v4l2_subdev *sd;
+	int ret;
+
+	camif->sensor.sd = NULL;
+
+	if (sensor->i2c_board_info.addr == 0)
+		return -EINVAL;
+
+	adapter = i2c_get_adapter(sensor->i2c_bus_num);
+	if (adapter == NULL) {
+		v4l2_warn(v4l2_dev, "failed to get I2C adapter %d\n",
+			  sensor->i2c_bus_num);
+		return -EPROBE_DEFER;
+	}
+
+	sd = v4l2_i2c_new_subdev_board(v4l2_dev, adapter,
+				       &sensor->i2c_board_info, NULL);
+	if (sd == NULL) {
+		i2c_put_adapter(adapter);
+		v4l2_warn(v4l2_dev, "failed to acquire subdev %s\n",
+			  sensor->i2c_board_info.type);
+		return -EPROBE_DEFER;
+	}
+	camif->sensor.sd = sd;
+
+	v4l2_info(v4l2_dev, "registered sensor subdevice %s\n", sd->name);
+
+	/* Get initial pixel format and set it at the camif sink pad */
+	format.pad = 0;
+	format.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &format);
+
+	if (ret < 0)
+		return 0;
+
+	format.pad = CAMIF_SD_PAD_SINK;
+	v4l2_subdev_call(&camif->subdev, pad, set_fmt, NULL, &format);
+
+	v4l2_info(sd, "Initial format from sensor: %dx%d, %#x\n",
+		  format.format.width, format.format.height,
+		  format.format.code);
+	return 0;
+}
+
+static void camif_unregister_sensor(struct camif_dev *camif)
+{
+	struct v4l2_subdev *sd = camif->sensor.sd;
+	struct i2c_client *client = sd ? v4l2_get_subdevdata(sd) : NULL;
+	struct i2c_adapter *adapter;
+
+	if (client == NULL)
+		return;
+
+	adapter = client->adapter;
+	v4l2_device_unregister_subdev(sd);
+	camif->sensor.sd = NULL;
+	i2c_unregister_device(client);
+	i2c_put_adapter(adapter);
+}
+
+static int camif_create_media_links(struct camif_dev *camif)
+{
+	int i, ret;
+
+	ret = media_entity_create_link(&camif->sensor.sd->entity, 0,
+				&camif->subdev.entity, CAMIF_SD_PAD_SINK,
+				MEDIA_LNK_FL_IMMUTABLE |
+				MEDIA_LNK_FL_ENABLED);
+	if (ret)
+		return ret;
+
+	for (i = 1; i < CAMIF_SD_PADS_NUM && !ret; i++) {
+		ret = media_entity_create_link(&camif->subdev.entity, i,
+				&camif->vp[i - 1].vdev.entity, 0,
+				MEDIA_LNK_FL_IMMUTABLE |
+				MEDIA_LNK_FL_ENABLED);
+	}
+
+	return ret;
+}
+
+static int camif_register_video_nodes(struct camif_dev *camif)
+{
+	int ret = s3c_camif_register_video_node(camif, VP_CODEC);
+	if (ret < 0)
+		return ret;
+
+	return s3c_camif_register_video_node(camif, VP_PREVIEW);
+}
+
+static void camif_unregister_video_nodes(struct camif_dev *camif)
+{
+	s3c_camif_unregister_video_node(camif, VP_CODEC);
+	s3c_camif_unregister_video_node(camif, VP_PREVIEW);
+}
+
+static void camif_unregister_media_entities(struct camif_dev *camif)
+{
+	camif_unregister_video_nodes(camif);
+	camif_unregister_sensor(camif);
+	s3c_camif_unregister_subdev(camif);
+}
+
+/*
+ * Media device
+ */
+static int camif_media_dev_register(struct camif_dev *camif)
+{
+	struct media_device *md = &camif->media_dev;
+	struct v4l2_device *v4l2_dev = &camif->v4l2_dev;
+	unsigned int ip_rev = camif->variant->ip_revision;
+	int ret;
+
+	memset(md, 0, sizeof(*md));
+	snprintf(md->model, sizeof(md->model), "SAMSUNG S3C%s CAMIF",
+		 ip_rev == S3C6410_CAMIF_IP_REV ? "6410" : "244X");
+	strlcpy(md->bus_info, "platform", sizeof(md->bus_info));
+	md->hw_revision = ip_rev;
+	md->driver_version = KERNEL_VERSION(1, 0, 0);
+
+	md->dev = camif->dev;
+
+	strlcpy(v4l2_dev->name, "s3c-camif", sizeof(v4l2_dev->name));
+	v4l2_dev->mdev = md;
+
+	ret = v4l2_device_register(camif->dev, v4l2_dev);
+	if (ret < 0)
+		return ret;
+
+	ret = media_device_register(md);
+	if (ret < 0)
+		v4l2_device_unregister(v4l2_dev);
+
+	return ret;
+}
+
+static void camif_clk_put(struct camif_dev *camif)
+{
+	int i;
+
+	for (i = 0; i < CLK_MAX_NUM; i++) {
+		if (IS_ERR(camif->clock[i]))
+			continue;
+		clk_unprepare(camif->clock[i]);
+		clk_put(camif->clock[i]);
+		camif->clock[i] = ERR_PTR(-EINVAL);
+	}
+}
+
+static int camif_clk_get(struct camif_dev *camif)
+{
+	int ret, i;
+
+	for (i = 1; i < CLK_MAX_NUM; i++)
+		camif->clock[i] = ERR_PTR(-EINVAL);
+
+	for (i = 0; i < CLK_MAX_NUM; i++) {
+		camif->clock[i] = clk_get(camif->dev, camif_clocks[i]);
+		if (IS_ERR(camif->clock[i])) {
+			ret = PTR_ERR(camif->clock[i]);
+			goto err;
+		}
+		ret = clk_prepare(camif->clock[i]);
+		if (ret < 0) {
+			clk_put(camif->clock[i]);
+			camif->clock[i] = NULL;
+			goto err;
+		}
+	}
+	return 0;
+err:
+	camif_clk_put(camif);
+	dev_err(camif->dev, "failed to get clock: %s\n",
+		camif_clocks[i]);
+	return ret;
+}
+
+/*
+ * The CAMIF device has two relatively independent data processing paths
+ * that can source data from memory or the common camera input frontend.
+ * Register interrupts for each data processing path (camif_vp).
+ */
+static int camif_request_irqs(struct platform_device *pdev,
+			      struct camif_dev *camif)
+{
+	int irq, ret, i;
+
+	for (i = 0; i < CAMIF_VP_NUM; i++) {
+		struct camif_vp *vp = &camif->vp[i];
+
+		init_waitqueue_head(&vp->irq_queue);
+
+		irq = platform_get_irq(pdev, i);
+		if (irq <= 0) {
+			dev_err(&pdev->dev, "failed to get IRQ %d\n", i);
+			return -ENXIO;
+		}
+
+		ret = devm_request_irq(&pdev->dev, irq, s3c_camif_irq_handler,
+				       0, dev_name(&pdev->dev), vp);
+		if (ret < 0) {
+			dev_err(&pdev->dev, "failed to install IRQ: %d\n", ret);
+			break;
+		}
+	}
+
+	return ret;
+}
+
+static int s3c_camif_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct s3c_camif_plat_data *pdata = dev->platform_data;
+	struct s3c_camif_drvdata *drvdata;
+	struct camif_dev *camif;
+	struct resource *mres;
+	int ret = 0;
+
+	camif = devm_kzalloc(dev, sizeof(*camif), GFP_KERNEL);
+	if (!camif)
+		return -ENOMEM;
+
+	spin_lock_init(&camif->slock);
+	mutex_init(&camif->lock);
+
+	camif->dev = dev;
+
+	if (!pdata || !pdata->gpio_get || !pdata->gpio_put) {
+		dev_err(dev, "wrong platform data\n");
+		return -EINVAL;
+	}
+
+	camif->pdata = *pdata;
+	drvdata = (void *)platform_get_device_id(pdev)->driver_data;
+	camif->variant = drvdata->variant;
+
+	mres = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	camif->io_base = devm_ioremap_resource(dev, mres);
+	if (IS_ERR(camif->io_base))
+		return PTR_ERR(camif->io_base);
+
+	ret = camif_request_irqs(pdev, camif);
+	if (ret < 0)
+		return ret;
+
+	ret = pdata->gpio_get();
+	if (ret < 0)
+		return ret;
+
+	ret = s3c_camif_create_subdev(camif);
+	if (ret < 0)
+		goto err_sd;
+
+	ret = camif_clk_get(camif);
+	if (ret < 0)
+		goto err_clk;
+
+	platform_set_drvdata(pdev, camif);
+	clk_set_rate(camif->clock[CLK_CAM],
+			camif->pdata.sensor.clock_frequency);
+
+	dev_info(dev, "sensor clock frequency: %lu\n",
+		 clk_get_rate(camif->clock[CLK_CAM]));
+	/*
+	 * Set initial pixel format, resolution and crop rectangle.
+	 * Must be done before a sensor subdev is registered as some
+	 * settings are overrode with values from sensor subdev.
+	 */
+	s3c_camif_set_defaults(camif);
+
+	pm_runtime_enable(dev);
+
+	ret = pm_runtime_get_sync(dev);
+	if (ret < 0)
+		goto err_pm;
+
+	/* Initialize contiguous memory allocator */
+	camif->alloc_ctx = vb2_dma_contig_init_ctx(dev);
+	if (IS_ERR(camif->alloc_ctx)) {
+		ret = PTR_ERR(camif->alloc_ctx);
+		goto err_alloc;
+	}
+
+	ret = camif_media_dev_register(camif);
+	if (ret < 0)
+		goto err_mdev;
+
+	ret = camif_register_sensor(camif);
+	if (ret < 0)
+		goto err_sens;
+
+	ret = v4l2_device_register_subdev(&camif->v4l2_dev, &camif->subdev);
+	if (ret < 0)
+		goto err_sens;
+
+	mutex_lock(&camif->media_dev.graph_mutex);
+
+	ret = v4l2_device_register_subdev_nodes(&camif->v4l2_dev);
+	if (ret < 0)
+		goto err_unlock;
+
+	ret = camif_register_video_nodes(camif);
+	if (ret < 0)
+		goto err_unlock;
+
+	ret = camif_create_media_links(camif);
+	if (ret < 0)
+		goto err_unlock;
+
+	mutex_unlock(&camif->media_dev.graph_mutex);
+	pm_runtime_put(dev);
+	return 0;
+
+err_unlock:
+	mutex_unlock(&camif->media_dev.graph_mutex);
+err_sens:
+	v4l2_device_unregister(&camif->v4l2_dev);
+	media_device_unregister(&camif->media_dev);
+	camif_unregister_media_entities(camif);
+err_mdev:
+	vb2_dma_contig_cleanup_ctx(camif->alloc_ctx);
+err_alloc:
+	pm_runtime_put(dev);
+	pm_runtime_disable(dev);
+err_pm:
+	camif_clk_put(camif);
+err_clk:
+	s3c_camif_unregister_subdev(camif);
+err_sd:
+	pdata->gpio_put();
+	return ret;
+}
+
+static int s3c_camif_remove(struct platform_device *pdev)
+{
+	struct camif_dev *camif = platform_get_drvdata(pdev);
+	struct s3c_camif_plat_data *pdata = &camif->pdata;
+
+	media_device_unregister(&camif->media_dev);
+	camif_unregister_media_entities(camif);
+	v4l2_device_unregister(&camif->v4l2_dev);
+
+	pm_runtime_disable(&pdev->dev);
+	camif_clk_put(camif);
+	pdata->gpio_put();
+
+	return 0;
+}
+
+static int s3c_camif_runtime_resume(struct device *dev)
+{
+	struct camif_dev *camif = dev_get_drvdata(dev);
+
+	clk_enable(camif->clock[CLK_GATE]);
+	/* null op on s3c244x */
+	clk_enable(camif->clock[CLK_CAM]);
+	return 0;
+}
+
+static int s3c_camif_runtime_suspend(struct device *dev)
+{
+	struct camif_dev *camif = dev_get_drvdata(dev);
+
+	/* null op on s3c244x */
+	clk_disable(camif->clock[CLK_CAM]);
+
+	clk_disable(camif->clock[CLK_GATE]);
+	return 0;
+}
+
+static const struct s3c_camif_variant s3c244x_camif_variant = {
+	.vp_pix_limits = {
+		[VP_CODEC] = {
+			.max_out_width		= 4096,
+			.max_sc_out_width	= 2048,
+			.out_width_align	= 16,
+			.min_out_width		= 16,
+			.max_height		= 4096,
+		},
+		[VP_PREVIEW] = {
+			.max_out_width		= 640,
+			.max_sc_out_width	= 640,
+			.out_width_align	= 16,
+			.min_out_width		= 16,
+			.max_height		= 480,
+		}
+	},
+	.pix_limits = {
+		.win_hor_offset_align	= 8,
+	},
+	.ip_revision = S3C244X_CAMIF_IP_REV,
+};
+
+static struct s3c_camif_drvdata s3c244x_camif_drvdata = {
+	.variant	= &s3c244x_camif_variant,
+	.bus_clk_freq	= 24000000UL,
+};
+
+static const struct s3c_camif_variant s3c6410_camif_variant = {
+	.vp_pix_limits = {
+		[VP_CODEC] = {
+			.max_out_width		= 4096,
+			.max_sc_out_width	= 2048,
+			.out_width_align	= 16,
+			.min_out_width		= 16,
+			.max_height		= 4096,
+		},
+		[VP_PREVIEW] = {
+			.max_out_width		= 4096,
+			.max_sc_out_width	= 720,
+			.out_width_align	= 16,
+			.min_out_width		= 16,
+			.max_height		= 4096,
+		}
+	},
+	.pix_limits = {
+		.win_hor_offset_align	= 8,
+	},
+	.ip_revision = S3C6410_CAMIF_IP_REV,
+	.has_img_effect = 1,
+	.vp_offset = 0x20,
+};
+
+static struct s3c_camif_drvdata s3c6410_camif_drvdata = {
+	.variant	= &s3c6410_camif_variant,
+	.bus_clk_freq	= 133000000UL,
+};
+
+static const struct platform_device_id s3c_camif_driver_ids[] = {
+	{
+		.name		= "s3c2440-camif",
+		.driver_data	= (unsigned long)&s3c244x_camif_drvdata,
+	}, {
+		.name		= "s3c6410-camif",
+		.driver_data	= (unsigned long)&s3c6410_camif_drvdata,
+	},
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(platform, s3c_camif_driver_ids);
+
+static const struct dev_pm_ops s3c_camif_pm_ops = {
+	.runtime_suspend	= s3c_camif_runtime_suspend,
+	.runtime_resume		= s3c_camif_runtime_resume,
+};
+
+static struct platform_driver s3c_camif_driver = {
+	.probe		= s3c_camif_probe,
+	.remove		= s3c_camif_remove,
+	.id_table	= s3c_camif_driver_ids,
+	.driver = {
+		.name	= S3C_CAMIF_DRIVER_NAME,
+		.pm	= &s3c_camif_pm_ops,
+	}
+};
+
+module_platform_driver(s3c_camif_driver);
+
+MODULE_AUTHOR("Sylwester Nawrocki <sylvester.nawrocki@gmail.com>");
+MODULE_AUTHOR("Tomasz Figa <tomasz.figa@gmail.com>");
+MODULE_DESCRIPTION("S3C24XX/S3C64XX SoC camera interface driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/s3c-camif/camif-core.h b/drivers/media/platform/s3c-camif/camif-core.h
new file mode 100644
index 0000000..adaf196
--- /dev/null
+++ b/drivers/media/platform/s3c-camif/camif-core.h
@@ -0,0 +1,393 @@
+/*
+ * s3c24xx/s3c64xx SoC series Camera Interface (CAMIF) driver
+ *
+ * Copyright (C) 2012 Sylwester Nawrocki <sylvester.nawrocki@gmail.com>
+ * Copyright (C) 2012 Tomasz Figa <tomasz.figa@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+*/
+
+#ifndef CAMIF_CORE_H_
+#define CAMIF_CORE_H_
+
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/spinlock.h>
+#include <linux/types.h>
+#include <linux/videodev2.h>
+
+#include <media/media-entity.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-mediabus.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/s3c_camif.h>
+
+#define S3C_CAMIF_DRIVER_NAME	"s3c-camif"
+#define CAMIF_REQ_BUFS_MIN	3
+#define CAMIF_MAX_OUT_BUFS	4
+#define CAMIF_MAX_PIX_WIDTH	4096
+#define CAMIF_MAX_PIX_HEIGHT	4096
+#define SCALER_MAX_RATIO	64
+#define CAMIF_DEF_WIDTH		640
+#define CAMIF_DEF_HEIGHT	480
+#define CAMIF_STOP_TIMEOUT	1500 /* ms */
+
+#define S3C244X_CAMIF_IP_REV	0x20 /* 2.0 */
+#define S3C2450_CAMIF_IP_REV	0x30 /* 3.0 - not implemented, not tested */
+#define S3C6400_CAMIF_IP_REV	0x31 /* 3.1 - not implemented, not tested */
+#define S3C6410_CAMIF_IP_REV	0x32 /* 3.2 */
+
+/* struct camif_vp::state */
+
+#define ST_VP_PENDING		(1 << 0)
+#define ST_VP_RUNNING		(1 << 1)
+#define ST_VP_STREAMING		(1 << 2)
+#define ST_VP_SENSOR_STREAMING	(1 << 3)
+
+#define ST_VP_ABORTING		(1 << 4)
+#define ST_VP_OFF		(1 << 5)
+#define ST_VP_LASTIRQ		(1 << 6)
+
+#define ST_VP_CONFIG		(1 << 8)
+
+#define CAMIF_SD_PAD_SINK	0
+#define CAMIF_SD_PAD_SOURCE_C	1
+#define CAMIF_SD_PAD_SOURCE_P	2
+#define CAMIF_SD_PADS_NUM	3
+
+enum img_fmt {
+	IMG_FMT_RGB565 = 0x0010,
+	IMG_FMT_RGB666,
+	IMG_FMT_XRGB8888,
+	IMG_FMT_YCBCR420 = 0x0020,
+	IMG_FMT_YCRCB420,
+	IMG_FMT_YCBCR422P,
+	IMG_FMT_YCBYCR422 = 0x0040,
+	IMG_FMT_YCRYCB422,
+	IMG_FMT_CBYCRY422,
+	IMG_FMT_CRYCBY422,
+};
+
+#define img_fmt_is_rgb(x) ((x) & 0x10)
+#define img_fmt_is_ycbcr(x) ((x) & 0x60)
+
+/* Possible values for struct camif_fmt::flags */
+#define FMT_FL_S3C24XX_CODEC	(1 << 0)
+#define FMT_FL_S3C24XX_PREVIEW	(1 << 1)
+#define FMT_FL_S3C64XX		(1 << 2)
+
+/**
+ * struct camif_fmt - pixel format description
+ * @fourcc:    fourcc code for this format, 0 if not applicable
+ * @color:     a corresponding enum img_fmt
+ * @colplanes: number of physically contiguous data planes
+ * @flags:     indicate for which SoCs revisions this format is valid
+ * @depth:     bits per pixel (total)
+ * @ybpp:      number of luminance bytes per pixel
+ */
+struct camif_fmt {
+	char *name;
+	u32 fourcc;
+	u32 color;
+	u16 colplanes;
+	u16 flags;
+	u8 depth;
+	u8 ybpp;
+};
+
+/**
+ * struct camif_dma_offset - pixel offset information for DMA
+ * @initial: offset (in pixels) to first pixel
+ * @line: offset (in pixels) from end of line to start of next line
+ */
+struct camif_dma_offset {
+	int	initial;
+	int	line;
+};
+
+/**
+ * struct camif_frame - source/target frame properties
+ * @f_width: full pixel width
+ * @f_height: full pixel height
+ * @rect: crop/composition rectangle
+ * @dma_offset: DMA offset configuration
+ */
+struct camif_frame {
+	u16 f_width;
+	u16 f_height;
+	struct v4l2_rect rect;
+	struct camif_dma_offset dma_offset;
+};
+
+/* CAMIF clocks enumeration */
+enum {
+	CLK_GATE,
+	CLK_CAM,
+	CLK_MAX_NUM,
+};
+
+struct vp_pix_limits {
+	u16 max_out_width;
+	u16 max_sc_out_width;
+	u16 out_width_align;
+	u16 max_height;
+	u8 min_out_width;
+	u16 out_hor_offset_align;
+};
+
+struct camif_pix_limits {
+	u16 win_hor_offset_align;
+};
+
+/**
+ * struct s3c_camif_variant - CAMIF variant structure
+ * @vp_pix_limits:    pixel limits for the codec and preview paths
+ * @camif_pix_limits: pixel limits for the camera input interface
+ * @ip_revision:      the CAMIF IP revision: 0x20 for s3c244x, 0x32 for s3c6410
+ */
+struct s3c_camif_variant {
+	struct vp_pix_limits vp_pix_limits[2];
+	struct camif_pix_limits pix_limits;
+	u8 ip_revision;
+	u8 has_img_effect;
+	unsigned int vp_offset;
+};
+
+struct s3c_camif_drvdata {
+	const struct s3c_camif_variant *variant;
+	unsigned long bus_clk_freq;
+};
+
+struct camif_scaler {
+	u8 scaleup_h;
+	u8 scaleup_v;
+	u8 copy;
+	u8 enable;
+	u32 h_shift;
+	u32 v_shift;
+	u32 pre_h_ratio;
+	u32 pre_v_ratio;
+	u32 pre_dst_width;
+	u32 pre_dst_height;
+	u32 main_h_ratio;
+	u32 main_v_ratio;
+};
+
+struct camif_dev;
+
+/**
+ * struct camif_vp - CAMIF data processing path structure (codec/preview)
+ * @irq_queue:	    interrupt handling waitqueue
+ * @irq:	    interrupt number for this data path
+ * @camif:	    pointer to the camif structure
+ * @pad:	    media pad for the video node
+ * @vdev            video device
+ * @ctrl_handler:   video node controls handler
+ * @owner:	    file handle that own the streaming
+ * @pending_buf_q:  pending (empty) buffers queue head
+ * @active_buf_q:   active (being written) buffers queue head
+ * @active_buffers: counter of buffer set up at the DMA engine
+ * @buf_index:	    identifier of a last empty buffer set up in H/W
+ * @frame_sequence: image frame sequence counter
+ * @reqbufs_count:  the number of buffers requested
+ * @scaler:	    the scaler structure
+ * @out_fmt:	    pixel format at this video path output
+ * @payload:	    the output data frame payload size
+ * @out_frame:	    the output pixel resolution
+ * @state:	    the video path's state
+ * @fmt_flags:	    flags determining supported pixel formats
+ * @id:		    CAMIF id, 0 - codec, 1 - preview
+ * @rotation:	    current image rotation value
+ * @hflip:	    apply horizontal flip if set
+ * @vflip:	    apply vertical flip if set
+ */
+struct camif_vp {
+	wait_queue_head_t	irq_queue;
+	int			irq;
+	struct camif_dev	*camif;
+	struct media_pad	pad;
+	struct video_device	vdev;
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct v4l2_fh		*owner;
+	struct vb2_queue	vb_queue;
+	struct list_head	pending_buf_q;
+	struct list_head	active_buf_q;
+	unsigned int		active_buffers;
+	unsigned int		buf_index;
+	unsigned int		frame_sequence;
+	unsigned int		reqbufs_count;
+	struct camif_scaler	scaler;
+	const struct camif_fmt	*out_fmt;
+	unsigned int		payload;
+	struct camif_frame	out_frame;
+	unsigned int		state;
+	u16			fmt_flags;
+	u8			id;
+	u16			rotation;
+	u8			hflip;
+	u8			vflip;
+	unsigned int		offset;
+};
+
+/* Video processing path enumeration */
+#define VP_CODEC	0
+#define VP_PREVIEW	1
+#define CAMIF_VP_NUM	2
+
+/**
+ * struct camif_dev - the CAMIF driver private data structure
+ * @media_dev:    top-level media device structure
+ * @v4l2_dev:	  root v4l2_device
+ * @subdev:       camera interface ("catchcam") subdev
+ * @mbus_fmt:	  camera input media bus format
+ * @camif_crop:   camera input interface crop rectangle
+ * @pads:	  the camif subdev's media pads
+ * @stream_count: the camera interface streaming reference counter
+ * @sensor:       image sensor data structure
+ * @m_pipeline:	  video entity pipeline description
+ * @ctrl_handler: v4l2 control handler (owned by @subdev)
+ * @test_pattern: test pattern controls
+ * @vp:           video path (DMA) description (codec/preview)
+ * @alloc_ctx:    memory buffer allocator context
+ * @variant:      variant information for this device
+ * @dev:	  pointer to the CAMIF device struct
+ * @pdata:	  a copy of the driver's platform data
+ * @clock:	  clocks required for the CAMIF operation
+ * @lock:	  mutex protecting this data structure
+ * @slock:	  spinlock protecting CAMIF registers
+ * @io_base:	  start address of the mmaped CAMIF registers
+ */
+struct camif_dev {
+	struct media_device		media_dev;
+	struct v4l2_device		v4l2_dev;
+	struct v4l2_subdev		subdev;
+	struct v4l2_mbus_framefmt	mbus_fmt;
+	struct v4l2_rect		camif_crop;
+	struct media_pad		pads[CAMIF_SD_PADS_NUM];
+	int				stream_count;
+
+	struct cam_sensor {
+		struct v4l2_subdev	*sd;
+		short			power_count;
+		short			stream_count;
+	} sensor;
+	struct media_pipeline		*m_pipeline;
+
+	struct v4l2_ctrl_handler	ctrl_handler;
+	struct v4l2_ctrl		*ctrl_test_pattern;
+	struct {
+		struct v4l2_ctrl	*ctrl_colorfx;
+		struct v4l2_ctrl	*ctrl_colorfx_cbcr;
+	};
+	u8				test_pattern;
+	u8				colorfx;
+	u8				colorfx_cb;
+	u8				colorfx_cr;
+
+	struct camif_vp			vp[CAMIF_VP_NUM];
+	struct vb2_alloc_ctx		*alloc_ctx;
+
+	const struct s3c_camif_variant	*variant;
+	struct device			*dev;
+	struct s3c_camif_plat_data	pdata;
+	struct clk			*clock[CLK_MAX_NUM];
+	struct mutex			lock;
+	spinlock_t			slock;
+	void __iomem			*io_base;
+};
+
+/**
+ * struct camif_addr - Y/Cb/Cr DMA start address structure
+ * @y:	 luminance plane dma address
+ * @cb:	 Cb plane dma address
+ * @cr:	 Cr plane dma address
+ */
+struct camif_addr {
+	dma_addr_t y;
+	dma_addr_t cb;
+	dma_addr_t cr;
+};
+
+/**
+ * struct camif_buffer - the camif video buffer structure
+ * @vb:    vb2 buffer
+ * @list:  list head for the buffers queue
+ * @paddr: DMA start addresses
+ * @index: an identifier of this buffer at the DMA engine
+ */
+struct camif_buffer {
+	struct vb2_v4l2_buffer vb;
+	struct list_head list;
+	struct camif_addr paddr;
+	unsigned int index;
+};
+
+const struct camif_fmt *s3c_camif_find_format(struct camif_vp *vp,
+	      const u32 *pixelformat, int index);
+int s3c_camif_register_video_node(struct camif_dev *camif, int idx);
+void s3c_camif_unregister_video_node(struct camif_dev *camif, int idx);
+irqreturn_t s3c_camif_irq_handler(int irq, void *priv);
+int s3c_camif_create_subdev(struct camif_dev *camif);
+void s3c_camif_unregister_subdev(struct camif_dev *camif);
+int s3c_camif_set_defaults(struct camif_dev *camif);
+int s3c_camif_get_scaler_config(struct camif_vp *vp,
+				struct camif_scaler *scaler);
+
+static inline void camif_active_queue_add(struct camif_vp *vp,
+					  struct camif_buffer *buf)
+{
+	list_add_tail(&buf->list, &vp->active_buf_q);
+	vp->active_buffers++;
+}
+
+static inline struct camif_buffer *camif_active_queue_pop(
+					struct camif_vp *vp)
+{
+	struct camif_buffer *buf = list_first_entry(&vp->active_buf_q,
+					      struct camif_buffer, list);
+	list_del(&buf->list);
+	vp->active_buffers--;
+	return buf;
+}
+
+static inline struct camif_buffer *camif_active_queue_peek(
+			   struct camif_vp *vp, int index)
+{
+	struct camif_buffer *tmp, *buf;
+
+	if (WARN_ON(list_empty(&vp->active_buf_q)))
+		return NULL;
+
+	list_for_each_entry_safe(buf, tmp, &vp->active_buf_q, list) {
+		if (buf->index == index) {
+			list_del(&buf->list);
+			vp->active_buffers--;
+			return buf;
+		}
+	}
+
+	return NULL;
+}
+
+static inline void camif_pending_queue_add(struct camif_vp *vp,
+					   struct camif_buffer *buf)
+{
+	list_add_tail(&buf->list, &vp->pending_buf_q);
+}
+
+static inline struct camif_buffer *camif_pending_queue_pop(
+					struct camif_vp *vp)
+{
+	struct camif_buffer *buf = list_first_entry(&vp->pending_buf_q,
+					      struct camif_buffer, list);
+	list_del(&buf->list);
+	return buf;
+}
+
+#endif /* CAMIF_CORE_H_ */
diff --git a/drivers/media/platform/s3c-camif/camif-regs.c b/drivers/media/platform/s3c-camif/camif-regs.c
new file mode 100644
index 0000000..812fb3a
--- /dev/null
+++ b/drivers/media/platform/s3c-camif/camif-regs.c
@@ -0,0 +1,606 @@
+/*
+ * Samsung s3c24xx/s3c64xx SoC CAMIF driver
+ *
+ * Copyright (C) 2012 Sylwester Nawrocki <sylvester.nawrocki@gmail.com>
+ * Copyright (C) 2012 Tomasz Figa <tomasz.figa@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+*/
+#define pr_fmt(fmt) "%s:%d " fmt, __func__, __LINE__
+
+#include <linux/delay.h>
+#include "camif-regs.h"
+
+#define camif_write(_camif, _off, _val)	writel(_val, (_camif)->io_base + (_off))
+#define camif_read(_camif, _off)	readl((_camif)->io_base + (_off))
+
+void camif_hw_reset(struct camif_dev *camif)
+{
+	u32 cfg;
+
+	cfg = camif_read(camif, S3C_CAMIF_REG_CISRCFMT);
+	cfg |= CISRCFMT_ITU601_8BIT;
+	camif_write(camif, S3C_CAMIF_REG_CISRCFMT, cfg);
+
+	/* S/W reset */
+	cfg = camif_read(camif, S3C_CAMIF_REG_CIGCTRL);
+	cfg |= CIGCTRL_SWRST;
+	if (camif->variant->ip_revision == S3C6410_CAMIF_IP_REV)
+		cfg |= CIGCTRL_IRQ_LEVEL;
+	camif_write(camif, S3C_CAMIF_REG_CIGCTRL, cfg);
+	udelay(10);
+
+	cfg = camif_read(camif, S3C_CAMIF_REG_CIGCTRL);
+	cfg &= ~CIGCTRL_SWRST;
+	camif_write(camif, S3C_CAMIF_REG_CIGCTRL, cfg);
+	udelay(10);
+}
+
+void camif_hw_clear_pending_irq(struct camif_vp *vp)
+{
+	u32 cfg = camif_read(vp->camif, S3C_CAMIF_REG_CIGCTRL);
+	cfg |= CIGCTRL_IRQ_CLR(vp->id);
+	camif_write(vp->camif, S3C_CAMIF_REG_CIGCTRL, cfg);
+}
+
+/*
+ * Sets video test pattern (off, color bar, horizontal or vertical gradient).
+ * External sensor pixel clock must be active for the test pattern to work.
+ */
+void camif_hw_set_test_pattern(struct camif_dev *camif, unsigned int pattern)
+{
+	u32 cfg = camif_read(camif, S3C_CAMIF_REG_CIGCTRL);
+	cfg &= ~CIGCTRL_TESTPATTERN_MASK;
+	cfg |= (pattern << 27);
+	camif_write(camif, S3C_CAMIF_REG_CIGCTRL, cfg);
+}
+
+void camif_hw_set_effect(struct camif_dev *camif, unsigned int effect,
+			unsigned int cr, unsigned int cb)
+{
+	static const struct v4l2_control colorfx[] = {
+		{ V4L2_COLORFX_NONE,		CIIMGEFF_FIN_BYPASS },
+		{ V4L2_COLORFX_BW,		CIIMGEFF_FIN_ARBITRARY },
+		{ V4L2_COLORFX_SEPIA,		CIIMGEFF_FIN_ARBITRARY },
+		{ V4L2_COLORFX_NEGATIVE,	CIIMGEFF_FIN_NEGATIVE },
+		{ V4L2_COLORFX_ART_FREEZE,	CIIMGEFF_FIN_ARTFREEZE },
+		{ V4L2_COLORFX_EMBOSS,		CIIMGEFF_FIN_EMBOSSING },
+		{ V4L2_COLORFX_SILHOUETTE,	CIIMGEFF_FIN_SILHOUETTE },
+		{ V4L2_COLORFX_SET_CBCR,	CIIMGEFF_FIN_ARBITRARY },
+	};
+	unsigned int i, cfg;
+
+	for (i = 0; i < ARRAY_SIZE(colorfx); i++)
+		if (colorfx[i].id == effect)
+			break;
+
+	if (i == ARRAY_SIZE(colorfx))
+		return;
+
+	cfg = camif_read(camif, S3C_CAMIF_REG_CIIMGEFF(camif->vp->offset));
+	/* Set effect */
+	cfg &= ~CIIMGEFF_FIN_MASK;
+	cfg |= colorfx[i].value;
+	/* Set both paths */
+	if (camif->variant->ip_revision >= S3C6400_CAMIF_IP_REV) {
+		if (effect == V4L2_COLORFX_NONE)
+			cfg &= ~CIIMGEFF_IE_ENABLE_MASK;
+		else
+			cfg |= CIIMGEFF_IE_ENABLE_MASK;
+	}
+	cfg &= ~CIIMGEFF_PAT_CBCR_MASK;
+	cfg |= cr | (cb << 13);
+	camif_write(camif, S3C_CAMIF_REG_CIIMGEFF(camif->vp->offset), cfg);
+}
+
+static const u32 src_pixfmt_map[8][2] = {
+	{ MEDIA_BUS_FMT_YUYV8_2X8, CISRCFMT_ORDER422_YCBYCR },
+	{ MEDIA_BUS_FMT_YVYU8_2X8, CISRCFMT_ORDER422_YCRYCB },
+	{ MEDIA_BUS_FMT_UYVY8_2X8, CISRCFMT_ORDER422_CBYCRY },
+	{ MEDIA_BUS_FMT_VYUY8_2X8, CISRCFMT_ORDER422_CRYCBY },
+};
+
+/* Set camera input pixel format and resolution */
+void camif_hw_set_source_format(struct camif_dev *camif)
+{
+	struct v4l2_mbus_framefmt *mf = &camif->mbus_fmt;
+	int i;
+	u32 cfg;
+
+	for (i = ARRAY_SIZE(src_pixfmt_map) - 1; i >= 0; i--) {
+		if (src_pixfmt_map[i][0] == mf->code)
+			break;
+	}
+	if (i < 0) {
+		i = 0;
+		dev_err(camif->dev,
+			"Unsupported pixel code, falling back to %#08x\n",
+			src_pixfmt_map[i][0]);
+	}
+
+	cfg = camif_read(camif, S3C_CAMIF_REG_CISRCFMT);
+	cfg &= ~(CISRCFMT_ORDER422_MASK | CISRCFMT_SIZE_CAM_MASK);
+	cfg |= (mf->width << 16) | mf->height;
+	cfg |= src_pixfmt_map[i][1];
+	camif_write(camif, S3C_CAMIF_REG_CISRCFMT, cfg);
+}
+
+/* Set the camera host input window offsets (cropping) */
+void camif_hw_set_camera_crop(struct camif_dev *camif)
+{
+	struct v4l2_mbus_framefmt *mf = &camif->mbus_fmt;
+	struct v4l2_rect *crop = &camif->camif_crop;
+	u32 hoff2, voff2;
+	u32 cfg;
+
+	/* Note: s3c244x requirement: left = f_width - rect.width / 2 */
+	cfg = camif_read(camif, S3C_CAMIF_REG_CIWDOFST);
+	cfg &= ~(CIWDOFST_OFST_MASK | CIWDOFST_WINOFSEN);
+	cfg |= (crop->left << 16) | crop->top;
+	if (crop->left != 0 || crop->top != 0)
+		cfg |= CIWDOFST_WINOFSEN;
+	camif_write(camif, S3C_CAMIF_REG_CIWDOFST, cfg);
+
+	if (camif->variant->ip_revision == S3C6410_CAMIF_IP_REV) {
+		hoff2 = mf->width - crop->width - crop->left;
+		voff2 = mf->height - crop->height - crop->top;
+		cfg = (hoff2 << 16) | voff2;
+		camif_write(camif, S3C_CAMIF_REG_CIWDOFST2, cfg);
+	}
+}
+
+void camif_hw_clear_fifo_overflow(struct camif_vp *vp)
+{
+	struct camif_dev *camif = vp->camif;
+	u32 cfg;
+
+	cfg = camif_read(camif, S3C_CAMIF_REG_CIWDOFST);
+	if (vp->id == 0)
+		cfg |= (CIWDOFST_CLROVCOFIY | CIWDOFST_CLROVCOFICB |
+			CIWDOFST_CLROVCOFICR);
+	else
+		cfg |= (/* CIWDOFST_CLROVPRFIY | */ CIWDOFST_CLROVPRFICB |
+			CIWDOFST_CLROVPRFICR);
+	camif_write(camif, S3C_CAMIF_REG_CIWDOFST, cfg);
+}
+
+/* Set video bus signals polarity */
+void camif_hw_set_camera_bus(struct camif_dev *camif)
+{
+	unsigned int flags = camif->pdata.sensor.flags;
+
+	u32 cfg = camif_read(camif, S3C_CAMIF_REG_CIGCTRL);
+
+	cfg &= ~(CIGCTRL_INVPOLPCLK | CIGCTRL_INVPOLVSYNC |
+		 CIGCTRL_INVPOLHREF | CIGCTRL_INVPOLFIELD);
+
+	if (flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
+		cfg |= CIGCTRL_INVPOLPCLK;
+
+	if (flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
+		cfg |= CIGCTRL_INVPOLVSYNC;
+	/*
+	 * HREF is normally high during frame active data
+	 * transmission and low during horizontal synchronization
+	 * period. Thus HREF active high means HSYNC active low.
+	 */
+	if (flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
+		cfg |= CIGCTRL_INVPOLHREF; /* HREF active low */
+
+	if (camif->variant->ip_revision == S3C6410_CAMIF_IP_REV) {
+		if (flags & V4L2_MBUS_FIELD_EVEN_LOW)
+			cfg |= CIGCTRL_INVPOLFIELD;
+		cfg |= CIGCTRL_FIELDMODE;
+	}
+
+	pr_debug("Setting CIGCTRL to: %#x\n", cfg);
+
+	camif_write(camif, S3C_CAMIF_REG_CIGCTRL, cfg);
+}
+
+void camif_hw_set_output_addr(struct camif_vp *vp,
+			      struct camif_addr *paddr, int i)
+{
+	struct camif_dev *camif = vp->camif;
+
+	camif_write(camif, S3C_CAMIF_REG_CIYSA(vp->id, i), paddr->y);
+	if (camif->variant->ip_revision == S3C6410_CAMIF_IP_REV
+		|| vp->id == VP_CODEC) {
+		camif_write(camif, S3C_CAMIF_REG_CICBSA(vp->id, i),
+								paddr->cb);
+		camif_write(camif, S3C_CAMIF_REG_CICRSA(vp->id, i),
+								paddr->cr);
+	}
+
+	pr_debug("dst_buf[%d]: %pad, cb: %pad, cr: %pad\n",
+		 i, &paddr->y, &paddr->cb, &paddr->cr);
+}
+
+static void camif_hw_set_out_dma_size(struct camif_vp *vp)
+{
+	struct camif_frame *frame = &vp->out_frame;
+	u32 cfg;
+
+	cfg = camif_read(vp->camif, S3C_CAMIF_REG_CITRGFMT(vp->id, vp->offset));
+	cfg &= ~CITRGFMT_TARGETSIZE_MASK;
+	cfg |= (frame->f_width << 16) | frame->f_height;
+	camif_write(vp->camif, S3C_CAMIF_REG_CITRGFMT(vp->id, vp->offset), cfg);
+}
+
+static void camif_get_dma_burst(u32 width, u32 ybpp, u32 *mburst, u32 *rburst)
+{
+	unsigned int nwords = width * ybpp / 4;
+	unsigned int div, rem;
+
+	if (WARN_ON(width < 8 || (width * ybpp) & 7))
+		return;
+
+	for (div = 16; div >= 2; div /= 2) {
+		if (nwords < div)
+			continue;
+
+		rem = nwords & (div - 1);
+		if (rem == 0) {
+			*mburst = div;
+			*rburst = div;
+			break;
+		}
+		if (rem == div / 2 || rem == div / 4) {
+			*mburst = div;
+			*rburst = rem;
+			break;
+		}
+	}
+}
+
+void camif_hw_set_output_dma(struct camif_vp *vp)
+{
+	struct camif_dev *camif = vp->camif;
+	struct camif_frame *frame = &vp->out_frame;
+	const struct camif_fmt *fmt = vp->out_fmt;
+	unsigned int ymburst = 0, yrburst = 0;
+	u32 cfg;
+
+	camif_hw_set_out_dma_size(vp);
+
+	if (camif->variant->ip_revision == S3C6410_CAMIF_IP_REV) {
+		struct camif_dma_offset *offset = &frame->dma_offset;
+		/* Set the input dma offsets. */
+		cfg = S3C_CISS_OFFS_INITIAL(offset->initial);
+		cfg |= S3C_CISS_OFFS_LINE(offset->line);
+		camif_write(camif, S3C_CAMIF_REG_CISSY(vp->id), cfg);
+		camif_write(camif, S3C_CAMIF_REG_CISSCB(vp->id), cfg);
+		camif_write(camif, S3C_CAMIF_REG_CISSCR(vp->id), cfg);
+	}
+
+	/* Configure DMA burst values */
+	camif_get_dma_burst(frame->rect.width, fmt->ybpp, &ymburst, &yrburst);
+
+	cfg = camif_read(camif, S3C_CAMIF_REG_CICTRL(vp->id, vp->offset));
+	cfg &= ~CICTRL_BURST_MASK;
+
+	cfg |= CICTRL_YBURST1(ymburst) | CICTRL_YBURST2(yrburst);
+	cfg |= CICTRL_CBURST1(ymburst / 2) | CICTRL_CBURST2(yrburst / 2);
+
+	camif_write(camif, S3C_CAMIF_REG_CICTRL(vp->id, vp->offset), cfg);
+
+	pr_debug("ymburst: %u, yrburst: %u\n", ymburst, yrburst);
+}
+
+void camif_hw_set_input_path(struct camif_vp *vp)
+{
+	u32 cfg = camif_read(vp->camif, S3C_CAMIF_REG_MSCTRL(vp->id));
+	cfg &= ~MSCTRL_SEL_DMA_CAM;
+	camif_write(vp->camif, S3C_CAMIF_REG_MSCTRL(vp->id), cfg);
+}
+
+void camif_hw_set_target_format(struct camif_vp *vp)
+{
+	struct camif_dev *camif = vp->camif;
+	struct camif_frame *frame = &vp->out_frame;
+	u32 cfg;
+
+	pr_debug("fw: %d, fh: %d color: %d\n", frame->f_width,
+		 frame->f_height, vp->out_fmt->color);
+
+	cfg = camif_read(camif, S3C_CAMIF_REG_CITRGFMT(vp->id, vp->offset));
+	cfg &= ~CITRGFMT_TARGETSIZE_MASK;
+
+	if (camif->variant->ip_revision == S3C244X_CAMIF_IP_REV) {
+		/* We currently support only YCbCr 4:2:2 at the camera input */
+		cfg |= CITRGFMT_IN422;
+		cfg &= ~CITRGFMT_OUT422;
+		if (vp->out_fmt->color == IMG_FMT_YCBCR422P)
+			cfg |= CITRGFMT_OUT422;
+	} else {
+		cfg &= ~CITRGFMT_OUTFORMAT_MASK;
+		switch (vp->out_fmt->color) {
+		case IMG_FMT_RGB565...IMG_FMT_XRGB8888:
+			cfg |= CITRGFMT_OUTFORMAT_RGB;
+			break;
+		case IMG_FMT_YCBCR420...IMG_FMT_YCRCB420:
+			cfg |= CITRGFMT_OUTFORMAT_YCBCR420;
+			break;
+		case IMG_FMT_YCBCR422P:
+			cfg |= CITRGFMT_OUTFORMAT_YCBCR422;
+			break;
+		case IMG_FMT_YCBYCR422...IMG_FMT_CRYCBY422:
+			cfg |= CITRGFMT_OUTFORMAT_YCBCR422I;
+			break;
+		}
+	}
+
+	/* Rotation is only supported by s3c64xx */
+	if (vp->rotation == 90 || vp->rotation == 270)
+		cfg |= (frame->f_height << 16) | frame->f_width;
+	else
+		cfg |= (frame->f_width << 16) | frame->f_height;
+	camif_write(camif, S3C_CAMIF_REG_CITRGFMT(vp->id, vp->offset), cfg);
+
+	/* Target area, output pixel width * height */
+	cfg = camif_read(camif, S3C_CAMIF_REG_CITAREA(vp->id, vp->offset));
+	cfg &= ~CITAREA_MASK;
+	cfg |= (frame->f_width * frame->f_height);
+	camif_write(camif, S3C_CAMIF_REG_CITAREA(vp->id, vp->offset), cfg);
+}
+
+void camif_hw_set_flip(struct camif_vp *vp)
+{
+	u32 cfg = camif_read(vp->camif,
+				S3C_CAMIF_REG_CITRGFMT(vp->id, vp->offset));
+
+	cfg &= ~CITRGFMT_FLIP_MASK;
+
+	if (vp->hflip)
+		cfg |= CITRGFMT_FLIP_Y_MIRROR;
+	if (vp->vflip)
+		cfg |= CITRGFMT_FLIP_X_MIRROR;
+
+	camif_write(vp->camif, S3C_CAMIF_REG_CITRGFMT(vp->id, vp->offset), cfg);
+}
+
+static void camif_hw_set_prescaler(struct camif_vp *vp)
+{
+	struct camif_dev *camif = vp->camif;
+	struct camif_scaler *sc = &vp->scaler;
+	u32 cfg, shfactor, addr;
+
+	addr = S3C_CAMIF_REG_CISCPRERATIO(vp->id, vp->offset);
+
+	shfactor = 10 - (sc->h_shift + sc->v_shift);
+	cfg = shfactor << 28;
+
+	cfg |= (sc->pre_h_ratio << 16) | sc->pre_v_ratio;
+	camif_write(camif, addr, cfg);
+
+	cfg = (sc->pre_dst_width << 16) | sc->pre_dst_height;
+	camif_write(camif, S3C_CAMIF_REG_CISCPREDST(vp->id, vp->offset), cfg);
+}
+
+static void camif_s3c244x_hw_set_scaler(struct camif_vp *vp)
+{
+	struct camif_dev *camif = vp->camif;
+	struct camif_scaler *scaler = &vp->scaler;
+	unsigned int color = vp->out_fmt->color;
+	u32 cfg;
+
+	camif_hw_set_prescaler(vp);
+
+	cfg = camif_read(camif, S3C_CAMIF_REG_CISCCTRL(vp->id, vp->offset));
+
+	cfg &= ~(CISCCTRL_SCALEUP_MASK | CISCCTRL_SCALERBYPASS |
+		 CISCCTRL_MAIN_RATIO_MASK | CIPRSCCTRL_RGB_FORMAT_24BIT);
+
+	if (scaler->enable) {
+		if (scaler->scaleup_h) {
+			if (vp->id == VP_CODEC)
+				cfg |= CISCCTRL_SCALEUP_H;
+			else
+				cfg |= CIPRSCCTRL_SCALEUP_H;
+		}
+		if (scaler->scaleup_v) {
+			if (vp->id == VP_CODEC)
+				cfg |= CISCCTRL_SCALEUP_V;
+			else
+				cfg |= CIPRSCCTRL_SCALEUP_V;
+		}
+	} else {
+		if (vp->id == VP_CODEC)
+			cfg |= CISCCTRL_SCALERBYPASS;
+	}
+
+	cfg |= ((scaler->main_h_ratio & 0x1ff) << 16);
+	cfg |= scaler->main_v_ratio & 0x1ff;
+
+	if (vp->id == VP_PREVIEW) {
+		if (color == IMG_FMT_XRGB8888)
+			cfg |= CIPRSCCTRL_RGB_FORMAT_24BIT;
+		cfg |= CIPRSCCTRL_SAMPLE;
+	}
+
+	camif_write(camif, S3C_CAMIF_REG_CISCCTRL(vp->id, vp->offset), cfg);
+
+	pr_debug("main: h_ratio: %#x, v_ratio: %#x",
+		 scaler->main_h_ratio, scaler->main_v_ratio);
+}
+
+static void camif_s3c64xx_hw_set_scaler(struct camif_vp *vp)
+{
+	struct camif_dev *camif = vp->camif;
+	struct camif_scaler *scaler = &vp->scaler;
+	unsigned int color = vp->out_fmt->color;
+	u32 cfg;
+
+	camif_hw_set_prescaler(vp);
+
+	cfg = camif_read(camif, S3C_CAMIF_REG_CISCCTRL(vp->id, vp->offset));
+
+	cfg &= ~(CISCCTRL_CSCR2Y_WIDE | CISCCTRL_CSCY2R_WIDE
+		| CISCCTRL_SCALEUP_H | CISCCTRL_SCALEUP_V
+		| CISCCTRL_SCALERBYPASS | CISCCTRL_ONE2ONE
+		| CISCCTRL_INRGB_FMT_MASK | CISCCTRL_OUTRGB_FMT_MASK
+		| CISCCTRL_INTERLACE | CISCCTRL_EXTRGB_EXTENSION
+		| CISCCTRL_MAIN_RATIO_MASK);
+
+	cfg |= (CISCCTRL_CSCR2Y_WIDE | CISCCTRL_CSCY2R_WIDE);
+
+	if (!scaler->enable) {
+		cfg |= CISCCTRL_SCALERBYPASS;
+	} else {
+		if (scaler->scaleup_h)
+			cfg |= CISCCTRL_SCALEUP_H;
+		if (scaler->scaleup_v)
+			cfg |= CISCCTRL_SCALEUP_V;
+		if (scaler->copy)
+			cfg |= CISCCTRL_ONE2ONE;
+	}
+
+	switch (color) {
+	case IMG_FMT_RGB666:
+		cfg |= CISCCTRL_OUTRGB_FMT_RGB666;
+		break;
+	case IMG_FMT_XRGB8888:
+		cfg |= CISCCTRL_OUTRGB_FMT_RGB888;
+		break;
+	}
+
+	cfg |= (scaler->main_h_ratio & 0x1ff) << 16;
+	cfg |= scaler->main_v_ratio & 0x1ff;
+
+	camif_write(camif, S3C_CAMIF_REG_CISCCTRL(vp->id, vp->offset), cfg);
+
+	pr_debug("main: h_ratio: %#x, v_ratio: %#x",
+		 scaler->main_h_ratio, scaler->main_v_ratio);
+}
+
+void camif_hw_set_scaler(struct camif_vp *vp)
+{
+	unsigned int ip_rev = vp->camif->variant->ip_revision;
+
+	if (ip_rev == S3C244X_CAMIF_IP_REV)
+		camif_s3c244x_hw_set_scaler(vp);
+	else
+		camif_s3c64xx_hw_set_scaler(vp);
+}
+
+void camif_hw_enable_scaler(struct camif_vp *vp, bool on)
+{
+	u32 addr = S3C_CAMIF_REG_CISCCTRL(vp->id, vp->offset);
+	u32 cfg;
+
+	cfg = camif_read(vp->camif, addr);
+	if (on)
+		cfg |= CISCCTRL_SCALERSTART;
+	else
+		cfg &= ~CISCCTRL_SCALERSTART;
+	camif_write(vp->camif, addr, cfg);
+}
+
+void camif_hw_set_lastirq(struct camif_vp *vp, int enable)
+{
+	u32 addr = S3C_CAMIF_REG_CICTRL(vp->id, vp->offset);
+	u32 cfg;
+
+	cfg = camif_read(vp->camif, addr);
+	if (enable)
+		cfg |= CICTRL_LASTIRQ_ENABLE;
+	else
+		cfg &= ~CICTRL_LASTIRQ_ENABLE;
+	camif_write(vp->camif, addr, cfg);
+}
+
+void camif_hw_enable_capture(struct camif_vp *vp)
+{
+	struct camif_dev *camif = vp->camif;
+	u32 cfg;
+
+	cfg = camif_read(camif, S3C_CAMIF_REG_CIIMGCPT(vp->offset));
+	camif->stream_count++;
+
+	if (camif->variant->ip_revision == S3C6410_CAMIF_IP_REV)
+		cfg |= CIIMGCPT_CPT_FREN_ENABLE(vp->id);
+
+	if (vp->scaler.enable)
+		cfg |= CIIMGCPT_IMGCPTEN_SC(vp->id);
+
+	if (camif->stream_count == 1)
+		cfg |= CIIMGCPT_IMGCPTEN;
+
+	camif_write(camif, S3C_CAMIF_REG_CIIMGCPT(vp->offset), cfg);
+
+	pr_debug("CIIMGCPT: %#x, camif->stream_count: %d\n",
+		 cfg, camif->stream_count);
+}
+
+void camif_hw_disable_capture(struct camif_vp *vp)
+{
+	struct camif_dev *camif = vp->camif;
+	u32 cfg;
+
+	cfg = camif_read(camif, S3C_CAMIF_REG_CIIMGCPT(vp->offset));
+	cfg &= ~CIIMGCPT_IMGCPTEN_SC(vp->id);
+
+	if (WARN_ON(--(camif->stream_count) < 0))
+		camif->stream_count = 0;
+
+	if (camif->stream_count == 0)
+		cfg &= ~CIIMGCPT_IMGCPTEN;
+
+	pr_debug("CIIMGCPT: %#x, camif->stream_count: %d\n",
+		 cfg, camif->stream_count);
+
+	camif_write(camif, S3C_CAMIF_REG_CIIMGCPT(vp->offset), cfg);
+}
+
+void camif_hw_dump_regs(struct camif_dev *camif, const char *label)
+{
+	struct {
+		u32 offset;
+		const char * const name;
+	} registers[] = {
+		{ S3C_CAMIF_REG_CISRCFMT,		"CISRCFMT" },
+		{ S3C_CAMIF_REG_CIWDOFST,		"CIWDOFST" },
+		{ S3C_CAMIF_REG_CIGCTRL,		"CIGCTRL" },
+		{ S3C_CAMIF_REG_CIWDOFST2,		"CIWDOFST2" },
+		{ S3C_CAMIF_REG_CIYSA(0, 0),		"CICOYSA0" },
+		{ S3C_CAMIF_REG_CICBSA(0, 0),		"CICOCBSA0" },
+		{ S3C_CAMIF_REG_CICRSA(0, 0),		"CICOCRSA0" },
+		{ S3C_CAMIF_REG_CIYSA(0, 1),		"CICOYSA1" },
+		{ S3C_CAMIF_REG_CICBSA(0, 1),		"CICOCBSA1" },
+		{ S3C_CAMIF_REG_CICRSA(0, 1),		"CICOCRSA1" },
+		{ S3C_CAMIF_REG_CIYSA(0, 2),		"CICOYSA2" },
+		{ S3C_CAMIF_REG_CICBSA(0, 2),		"CICOCBSA2" },
+		{ S3C_CAMIF_REG_CICRSA(0, 2),		"CICOCRSA2" },
+		{ S3C_CAMIF_REG_CIYSA(0, 3),		"CICOYSA3" },
+		{ S3C_CAMIF_REG_CICBSA(0, 3),		"CICOCBSA3" },
+		{ S3C_CAMIF_REG_CICRSA(0, 3),		"CICOCRSA3" },
+		{ S3C_CAMIF_REG_CIYSA(1, 0),		"CIPRYSA0" },
+		{ S3C_CAMIF_REG_CIYSA(1, 1),		"CIPRYSA1" },
+		{ S3C_CAMIF_REG_CIYSA(1, 2),		"CIPRYSA2" },
+		{ S3C_CAMIF_REG_CIYSA(1, 3),		"CIPRYSA3" },
+		{ S3C_CAMIF_REG_CITRGFMT(0, 0),		"CICOTRGFMT" },
+		{ S3C_CAMIF_REG_CITRGFMT(1, 0),		"CIPRTRGFMT" },
+		{ S3C_CAMIF_REG_CICTRL(0, 0),		"CICOCTRL" },
+		{ S3C_CAMIF_REG_CICTRL(1, 0),		"CIPRCTRL" },
+		{ S3C_CAMIF_REG_CISCPREDST(0, 0),	"CICOSCPREDST" },
+		{ S3C_CAMIF_REG_CISCPREDST(1, 0),	"CIPRSCPREDST" },
+		{ S3C_CAMIF_REG_CISCPRERATIO(0, 0),	"CICOSCPRERATIO" },
+		{ S3C_CAMIF_REG_CISCPRERATIO(1, 0),	"CIPRSCPRERATIO" },
+		{ S3C_CAMIF_REG_CISCCTRL(0, 0),		"CICOSCCTRL" },
+		{ S3C_CAMIF_REG_CISCCTRL(1, 0),		"CIPRSCCTRL" },
+		{ S3C_CAMIF_REG_CITAREA(0, 0),		"CICOTAREA" },
+		{ S3C_CAMIF_REG_CITAREA(1, 0),		"CIPRTAREA" },
+		{ S3C_CAMIF_REG_CISTATUS(0, 0),		"CICOSTATUS" },
+		{ S3C_CAMIF_REG_CISTATUS(1, 0),		"CIPRSTATUS" },
+		{ S3C_CAMIF_REG_CIIMGCPT(0),		"CIIMGCPT" },
+	};
+	u32 i;
+
+	pr_info("--- %s ---\n", label);
+	for (i = 0; i < ARRAY_SIZE(registers); i++) {
+		u32 cfg = readl(camif->io_base + registers[i].offset);
+		dev_info(camif->dev, "%s:\t0x%08x\n", registers[i].name, cfg);
+	}
+}
diff --git a/drivers/media/platform/s3c-camif/camif-regs.h b/drivers/media/platform/s3c-camif/camif-regs.h
new file mode 100644
index 0000000..af2d472
--- /dev/null
+++ b/drivers/media/platform/s3c-camif/camif-regs.h
@@ -0,0 +1,269 @@
+/*
+ * Register definition file for s3c24xx/s3c64xx SoC CAMIF driver
+ *
+ * Copyright (C) 2012 Sylwester Nawrocki <sylvester.nawrocki@gmail.com>
+ * Copyright (C) 2012 Tomasz Figa <tomasz.figa@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+*/
+
+#ifndef CAMIF_REGS_H_
+#define CAMIF_REGS_H_
+
+#include "camif-core.h"
+#include <media/s3c_camif.h>
+
+/*
+ * The id argument indicates the processing path:
+ * id = 0 - codec (FIMC C), 1 - preview (FIMC P).
+ */
+
+/* Camera input format */
+#define S3C_CAMIF_REG_CISRCFMT			0x00
+#define  CISRCFMT_ITU601_8BIT			(1 << 31)
+#define  CISRCFMT_ITU656_8BIT			(0 << 31)
+#define  CISRCFMT_ORDER422_YCBYCR		(0 << 14)
+#define  CISRCFMT_ORDER422_YCRYCB		(1 << 14)
+#define  CISRCFMT_ORDER422_CBYCRY		(2 << 14)
+#define  CISRCFMT_ORDER422_CRYCBY		(3 << 14)
+#define  CISRCFMT_ORDER422_MASK			(3 << 14)
+#define  CISRCFMT_SIZE_CAM_MASK			(0x1fff << 16 | 0x1fff)
+
+/* Window offset */
+#define S3C_CAMIF_REG_CIWDOFST			0x04
+#define  CIWDOFST_WINOFSEN			(1 << 31)
+#define  CIWDOFST_CLROVCOFIY			(1 << 30)
+#define  CIWDOFST_CLROVRLB_PR			(1 << 28)
+/* #define  CIWDOFST_CLROVPRFIY			(1 << 27) */
+#define  CIWDOFST_CLROVCOFICB			(1 << 15)
+#define  CIWDOFST_CLROVCOFICR			(1 << 14)
+#define  CIWDOFST_CLROVPRFICB			(1 << 13)
+#define  CIWDOFST_CLROVPRFICR			(1 << 12)
+#define  CIWDOFST_OFST_MASK			(0x7ff << 16 | 0x7ff)
+
+/* Window offset 2 */
+#define S3C_CAMIF_REG_CIWDOFST2			0x14
+#define  CIWDOFST2_OFST2_MASK			(0xfff << 16 | 0xfff)
+
+/* Global control */
+#define S3C_CAMIF_REG_CIGCTRL			0x08
+#define  CIGCTRL_SWRST				(1 << 31)
+#define  CIGCTRL_CAMRST				(1 << 30)
+#define  CIGCTRL_TESTPATTERN_NORMAL		(0 << 27)
+#define  CIGCTRL_TESTPATTERN_COLOR_BAR		(1 << 27)
+#define  CIGCTRL_TESTPATTERN_HOR_INC		(2 << 27)
+#define  CIGCTRL_TESTPATTERN_VER_INC		(3 << 27)
+#define  CIGCTRL_TESTPATTERN_MASK		(3 << 27)
+#define  CIGCTRL_INVPOLPCLK			(1 << 26)
+#define  CIGCTRL_INVPOLVSYNC			(1 << 25)
+#define  CIGCTRL_INVPOLHREF			(1 << 24)
+#define  CIGCTRL_IRQ_OVFEN			(1 << 22)
+#define  CIGCTRL_HREF_MASK			(1 << 21)
+#define  CIGCTRL_IRQ_LEVEL			(1 << 20)
+/* IRQ_CLR_C, IRQ_CLR_P */
+#define  CIGCTRL_IRQ_CLR(id)			(1 << (19 - (id)))
+#define  CIGCTRL_FIELDMODE			(1 << 2)
+#define  CIGCTRL_INVPOLFIELD			(1 << 1)
+#define  CIGCTRL_CAM_INTERLACE			(1 << 0)
+
+/* Y DMA output frame start address. n = 0..3. */
+#define S3C_CAMIF_REG_CIYSA(id, n)		(0x18 + (id) * 0x54 + (n) * 4)
+/* Cb plane output DMA start address. n = 0..3. Only codec path. */
+#define S3C_CAMIF_REG_CICBSA(id, n)		(0x28 + (id) * 0x54 + (n) * 4)
+/* Cr plane output DMA start address. n = 0..3. Only codec path. */
+#define S3C_CAMIF_REG_CICRSA(id, n)		(0x38 + (id) * 0x54 + (n) * 4)
+
+/* CICOTRGFMT, CIPRTRGFMT - Target format */
+#define S3C_CAMIF_REG_CITRGFMT(id, _offs)	(0x48 + (id) * (0x34 + (_offs)))
+#define  CITRGFMT_IN422				(1 << 31) /* only for s3c24xx */
+#define  CITRGFMT_OUT422			(1 << 30) /* only for s3c24xx */
+#define  CITRGFMT_OUTFORMAT_YCBCR420		(0 << 29) /* only for s3c6410 */
+#define  CITRGFMT_OUTFORMAT_YCBCR422		(1 << 29) /* only for s3c6410 */
+#define  CITRGFMT_OUTFORMAT_YCBCR422I		(2 << 29) /* only for s3c6410 */
+#define  CITRGFMT_OUTFORMAT_RGB			(3 << 29) /* only for s3c6410 */
+#define  CITRGFMT_OUTFORMAT_MASK		(3 << 29) /* only for s3c6410 */
+#define  CITRGFMT_TARGETHSIZE(x)		((x) << 16)
+#define  CITRGFMT_FLIP_NORMAL			(0 << 14)
+#define  CITRGFMT_FLIP_X_MIRROR			(1 << 14)
+#define  CITRGFMT_FLIP_Y_MIRROR			(2 << 14)
+#define  CITRGFMT_FLIP_180			(3 << 14)
+#define  CITRGFMT_FLIP_MASK			(3 << 14)
+/* Preview path only */
+#define  CITRGFMT_ROT90_PR			(1 << 13)
+#define  CITRGFMT_TARGETVSIZE(x)		((x) << 0)
+#define  CITRGFMT_TARGETSIZE_MASK		((0x1fff << 16) | 0x1fff)
+
+/* CICOCTRL, CIPRCTRL. Output DMA control. */
+#define S3C_CAMIF_REG_CICTRL(id, _offs)		(0x4c + (id) * (0x34 + (_offs)))
+#define  CICTRL_BURST_MASK			(0xfffff << 4)
+/* xBURSTn - 5-bits width */
+#define  CICTRL_YBURST1(x)			((x) << 19)
+#define  CICTRL_YBURST2(x)			((x) << 14)
+#define  CICTRL_RGBBURST1(x)			((x) << 19)
+#define  CICTRL_RGBBURST2(x)			((x) << 14)
+#define  CICTRL_CBURST1(x)			((x) << 9)
+#define  CICTRL_CBURST2(x)			((x) << 4)
+#define  CICTRL_LASTIRQ_ENABLE			(1 << 2)
+#define  CICTRL_ORDER422_MASK			(3 << 0)
+
+/* CICOSCPRERATIO, CIPRSCPRERATIO. Pre-scaler control 1. */
+#define S3C_CAMIF_REG_CISCPRERATIO(id, _offs)	(0x50 + (id) * (0x34 + (_offs)))
+
+/* CICOSCPREDST, CIPRSCPREDST. Pre-scaler control 2. */
+#define S3C_CAMIF_REG_CISCPREDST(id, _offs)	(0x54 + (id) * (0x34 + (_offs)))
+
+/* CICOSCCTRL, CIPRSCCTRL. Main scaler control. */
+#define S3C_CAMIF_REG_CISCCTRL(id, _offs)	(0x58 + (id) * (0x34 + (_offs)))
+#define  CISCCTRL_SCALERBYPASS			(1 << 31)
+/* s3c244x preview path only, s3c64xx both */
+#define  CIPRSCCTRL_SAMPLE			(1 << 31)
+/* 0 - 16-bit RGB, 1 - 24-bit RGB */
+#define  CIPRSCCTRL_RGB_FORMAT_24BIT		(1 << 30) /* only for s3c244x */
+#define  CIPRSCCTRL_SCALEUP_H			(1 << 29) /* only for s3c244x */
+#define  CIPRSCCTRL_SCALEUP_V			(1 << 28) /* only for s3c244x */
+/* s3c64xx */
+#define  CISCCTRL_SCALEUP_H			(1 << 30)
+#define  CISCCTRL_SCALEUP_V			(1 << 29)
+#define  CISCCTRL_SCALEUP_MASK			(0x3 << 29)
+#define  CISCCTRL_CSCR2Y_WIDE			(1 << 28)
+#define  CISCCTRL_CSCY2R_WIDE			(1 << 27)
+#define  CISCCTRL_LCDPATHEN_FIFO		(1 << 26)
+#define  CISCCTRL_INTERLACE			(1 << 25)
+#define  CISCCTRL_SCALERSTART			(1 << 15)
+#define  CISCCTRL_INRGB_FMT_RGB565		(0 << 13)
+#define  CISCCTRL_INRGB_FMT_RGB666		(1 << 13)
+#define  CISCCTRL_INRGB_FMT_RGB888		(2 << 13)
+#define  CISCCTRL_INRGB_FMT_MASK		(3 << 13)
+#define  CISCCTRL_OUTRGB_FMT_RGB565		(0 << 11)
+#define  CISCCTRL_OUTRGB_FMT_RGB666		(1 << 11)
+#define  CISCCTRL_OUTRGB_FMT_RGB888		(2 << 11)
+#define  CISCCTRL_OUTRGB_FMT_MASK		(3 << 11)
+#define  CISCCTRL_EXTRGB_EXTENSION		(1 << 10)
+#define  CISCCTRL_ONE2ONE			(1 << 9)
+#define  CISCCTRL_MAIN_RATIO_MASK		(0x1ff << 16 | 0x1ff)
+
+/* CICOTAREA, CIPRTAREA. Target area for DMA (Hsize x Vsize). */
+#define S3C_CAMIF_REG_CITAREA(id, _offs)	(0x5c + (id) * (0x34 + (_offs)))
+#define CITAREA_MASK				0xfffffff
+
+/* Codec (id = 0) or preview (id = 1) path status. */
+#define S3C_CAMIF_REG_CISTATUS(id, _offs)	(0x64 + (id) * (0x34 + (_offs)))
+#define  CISTATUS_OVFIY_STATUS			(1 << 31)
+#define  CISTATUS_OVFICB_STATUS			(1 << 30)
+#define  CISTATUS_OVFICR_STATUS			(1 << 29)
+#define  CISTATUS_OVF_MASK			(0x7 << 29)
+#define  CIPRSTATUS_OVF_MASK			(0x3 << 30)
+#define  CISTATUS_VSYNC_STATUS			(1 << 28)
+#define  CISTATUS_FRAMECNT_MASK			(3 << 26)
+#define  CISTATUS_FRAMECNT(__reg)		(((__reg) >> 26) & 0x3)
+#define  CISTATUS_WINOFSTEN_STATUS		(1 << 25)
+#define  CISTATUS_IMGCPTEN_STATUS		(1 << 22)
+#define  CISTATUS_IMGCPTENSC_STATUS		(1 << 21)
+#define  CISTATUS_VSYNC_A_STATUS		(1 << 20)
+#define  CISTATUS_FRAMEEND_STATUS		(1 << 19) /* 17 on s3c64xx */
+
+/* Image capture enable */
+#define S3C_CAMIF_REG_CIIMGCPT(_offs)		(0xa0 + (_offs))
+#define  CIIMGCPT_IMGCPTEN			(1 << 31)
+#define  CIIMGCPT_IMGCPTEN_SC(id)		(1 << (30 - (id)))
+/* Frame control: 1 - one-shot, 0 - free run */
+#define  CIIMGCPT_CPT_FREN_ENABLE(id)		(1 << (25 - (id)))
+#define  CIIMGCPT_CPT_FRMOD_ENABLE		(0 << 18)
+#define  CIIMGCPT_CPT_FRMOD_CNT			(1 << 18)
+
+/* Capture sequence */
+#define S3C_CAMIF_REG_CICPTSEQ			0xc4
+
+/* Image effects */
+#define S3C_CAMIF_REG_CIIMGEFF(_offs)		(0xb0 + (_offs))
+#define  CIIMGEFF_IE_ENABLE(id)			(1 << (30 + (id)))
+#define  CIIMGEFF_IE_ENABLE_MASK		(3 << 30)
+/* Image effect: 1 - after scaler, 0 - before scaler */
+#define  CIIMGEFF_IE_AFTER_SC			(1 << 29)
+#define  CIIMGEFF_FIN_MASK			(7 << 26)
+#define  CIIMGEFF_FIN_BYPASS			(0 << 26)
+#define  CIIMGEFF_FIN_ARBITRARY			(1 << 26)
+#define  CIIMGEFF_FIN_NEGATIVE			(2 << 26)
+#define  CIIMGEFF_FIN_ARTFREEZE			(3 << 26)
+#define  CIIMGEFF_FIN_EMBOSSING			(4 << 26)
+#define  CIIMGEFF_FIN_SILHOUETTE		(5 << 26)
+#define  CIIMGEFF_PAT_CBCR_MASK			((0xff << 13) | 0xff)
+#define  CIIMGEFF_PAT_CB(x)			((x) << 13)
+#define  CIIMGEFF_PAT_CR(x)			(x)
+
+/* MSCOY0SA, MSPRY0SA. Y/Cb/Cr frame start address for input DMA. */
+#define S3C_CAMIF_REG_MSY0SA(id)		(0xd4 + ((id) * 0x2c))
+#define S3C_CAMIF_REG_MSCB0SA(id)		(0xd8 + ((id) * 0x2c))
+#define S3C_CAMIF_REG_MSCR0SA(id)		(0xdc + ((id) * 0x2c))
+
+/* MSCOY0END, MSCOY0END. Y/Cb/Cr frame end address for input DMA. */
+#define S3C_CAMIF_REG_MSY0END(id)		(0xe0 + ((id) * 0x2c))
+#define S3C_CAMIF_REG_MSCB0END(id)		(0xe4 + ((id) * 0x2c))
+#define S3C_CAMIF_REG_MSCR0END(id)		(0xe8 + ((id) * 0x2c))
+
+/* MSPRYOFF, MSPRYOFF. Y/Cb/Cr offset. n: 0 - codec, 1 - preview. */
+#define S3C_CAMIF_REG_MSYOFF(id)		(0x118 + ((id) * 0x2c))
+#define S3C_CAMIF_REG_MSCBOFF(id)		(0x11c + ((id) * 0x2c))
+#define S3C_CAMIF_REG_MSCROFF(id)		(0x120 + ((id) * 0x2c))
+
+/* Real input DMA data size. n = 0 - codec, 1 - preview. */
+#define S3C_CAMIF_REG_MSWIDTH(id)		(0xf8 + (id) * 0x2c)
+#define  AUTOLOAD_ENABLE			(1 << 31)
+#define  ADDR_CH_DIS				(1 << 30)
+#define  MSHEIGHT(x)				(((x) & 0x3ff) << 16)
+#define  MSWIDTH(x)				((x) & 0x3ff)
+
+/* Input DMA control. n = 0 - codec, 1 - preview */
+#define S3C_CAMIF_REG_MSCTRL(id)		(0xfc + (id) * 0x2c)
+#define  MSCTRL_ORDER422_M_YCBYCR		(0 << 4)
+#define  MSCTRL_ORDER422_M_YCRYCB		(1 << 4)
+#define  MSCTRL_ORDER422_M_CBYCRY		(2 << 4)
+#define  MSCTRL_ORDER422_M_CRYCBY		(3 << 4)
+/* 0 - camera, 1 - DMA */
+#define  MSCTRL_SEL_DMA_CAM			(1 << 3)
+#define  MSCTRL_INFORMAT_M_YCBCR420		(0 << 1)
+#define  MSCTRL_INFORMAT_M_YCBCR422		(1 << 1)
+#define  MSCTRL_INFORMAT_M_YCBCR422I		(2 << 1)
+#define  MSCTRL_INFORMAT_M_RGB			(3 << 1)
+#define  MSCTRL_ENVID_M				(1 << 0)
+
+/* CICOSCOSY, CIPRSCOSY. Scan line Y/Cb/Cr offset. */
+#define S3C_CAMIF_REG_CISSY(id)			(0x12c + (id) * 0x0c)
+#define S3C_CAMIF_REG_CISSCB(id)		(0x130 + (id) * 0x0c)
+#define S3C_CAMIF_REG_CISSCR(id)		(0x134 + (id) * 0x0c)
+#define S3C_CISS_OFFS_INITIAL(x)		((x) << 16)
+#define S3C_CISS_OFFS_LINE(x)			((x) << 0)
+
+/* ------------------------------------------------------------------ */
+
+void camif_hw_reset(struct camif_dev *camif);
+void camif_hw_clear_pending_irq(struct camif_vp *vp);
+void camif_hw_clear_fifo_overflow(struct camif_vp *vp);
+void camif_hw_set_lastirq(struct camif_vp *vp, int enable);
+void camif_hw_set_input_path(struct camif_vp *vp);
+void camif_hw_enable_scaler(struct camif_vp *vp, bool on);
+void camif_hw_enable_capture(struct camif_vp *vp);
+void camif_hw_disable_capture(struct camif_vp *vp);
+void camif_hw_set_camera_bus(struct camif_dev *camif);
+void camif_hw_set_source_format(struct camif_dev *camif);
+void camif_hw_set_camera_crop(struct camif_dev *camif);
+void camif_hw_set_scaler(struct camif_vp *vp);
+void camif_hw_set_flip(struct camif_vp *vp);
+void camif_hw_set_output_dma(struct camif_vp *vp);
+void camif_hw_set_target_format(struct camif_vp *vp);
+void camif_hw_set_test_pattern(struct camif_dev *camif, unsigned int pattern);
+void camif_hw_set_effect(struct camif_dev *camif, unsigned int effect,
+			unsigned int cr, unsigned int cb);
+void camif_hw_set_output_addr(struct camif_vp *vp, struct camif_addr *paddr,
+			      int index);
+void camif_hw_dump_regs(struct camif_dev *camif, const char *label);
+
+static inline u32 camif_hw_get_status(struct camif_vp *vp)
+{
+	return readl(vp->camif->io_base + S3C_CAMIF_REG_CISTATUS(vp->id,
+								vp->offset));
+}
+
+#endif /* CAMIF_REGS_H_ */
diff --git a/drivers/media/platform/s5p-g2d/Makefile b/drivers/media/platform/s5p-g2d/Makefile
new file mode 100644
index 0000000..2c48c41
--- /dev/null
+++ b/drivers/media/platform/s5p-g2d/Makefile
@@ -0,0 +1,3 @@
+s5p-g2d-objs := g2d.o g2d-hw.o
+
+obj-$(CONFIG_VIDEO_SAMSUNG_S5P_G2D)	+= s5p-g2d.o
diff --git a/drivers/media/platform/s5p-g2d/g2d-hw.c b/drivers/media/platform/s5p-g2d/g2d-hw.c
new file mode 100644
index 0000000..e87bd93
--- /dev/null
+++ b/drivers/media/platform/s5p-g2d/g2d-hw.c
@@ -0,0 +1,117 @@
+/*
+ * Samsung S5P G2D - 2D Graphics Accelerator Driver
+ *
+ * Copyright (c) 2011 Samsung Electronics Co., Ltd.
+ * Kamil Debski, <k.debski@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation; either version 2 of the
+ * License, or (at your option) any later version
+ */
+
+#include <linux/io.h>
+
+#include "g2d.h"
+#include "g2d-regs.h"
+
+#define w(x, a)	writel((x), d->regs + (a))
+#define r(a)	readl(d->regs + (a))
+
+/* g2d_reset clears all g2d registers */
+void g2d_reset(struct g2d_dev *d)
+{
+	w(1, SOFT_RESET_REG);
+}
+
+void g2d_set_src_size(struct g2d_dev *d, struct g2d_frame *f)
+{
+	u32 n;
+
+	w(0, SRC_SELECT_REG);
+	w(f->stride & 0xFFFF, SRC_STRIDE_REG);
+
+	n = f->o_height & 0xFFF;
+	n <<= 16;
+	n |= f->o_width & 0xFFF;
+	w(n, SRC_LEFT_TOP_REG);
+
+	n = f->bottom & 0xFFF;
+	n <<= 16;
+	n |= f->right & 0xFFF;
+	w(n, SRC_RIGHT_BOTTOM_REG);
+
+	w(f->fmt->hw, SRC_COLOR_MODE_REG);
+}
+
+void g2d_set_src_addr(struct g2d_dev *d, dma_addr_t a)
+{
+	w(a, SRC_BASE_ADDR_REG);
+}
+
+void g2d_set_dst_size(struct g2d_dev *d, struct g2d_frame *f)
+{
+	u32 n;
+
+	w(0, DST_SELECT_REG);
+	w(f->stride & 0xFFFF, DST_STRIDE_REG);
+
+	n = f->o_height & 0xFFF;
+	n <<= 16;
+	n |= f->o_width & 0xFFF;
+	w(n, DST_LEFT_TOP_REG);
+
+	n = f->bottom & 0xFFF;
+	n <<= 16;
+	n |= f->right & 0xFFF;
+	w(n, DST_RIGHT_BOTTOM_REG);
+
+	w(f->fmt->hw, DST_COLOR_MODE_REG);
+}
+
+void g2d_set_dst_addr(struct g2d_dev *d, dma_addr_t a)
+{
+	w(a, DST_BASE_ADDR_REG);
+}
+
+void g2d_set_rop4(struct g2d_dev *d, u32 r)
+{
+	w(r, ROP4_REG);
+}
+
+void g2d_set_flip(struct g2d_dev *d, u32 r)
+{
+	w(r, SRC_MSK_DIRECT_REG);
+}
+
+void g2d_set_v41_stretch(struct g2d_dev *d, struct g2d_frame *src,
+					struct g2d_frame *dst)
+{
+	w(DEFAULT_SCALE_MODE, SRC_SCALE_CTRL_REG);
+
+	/* inversed scaling factor: src is numerator */
+	w((src->c_width << 16) / dst->c_width, SRC_XSCALE_REG);
+	w((src->c_height << 16) / dst->c_height, SRC_YSCALE_REG);
+}
+
+void g2d_set_cmd(struct g2d_dev *d, u32 c)
+{
+	w(c, BITBLT_COMMAND_REG);
+}
+
+void g2d_start(struct g2d_dev *d)
+{
+	/* Clear cache */
+	if (d->variant->hw_rev == TYPE_G2D_3X)
+		w(0x7, CACHECTL_REG);
+
+	/* Enable interrupt */
+	w(1, INTEN_REG);
+	/* Start G2D engine */
+	w(1, BITBLT_START_REG);
+}
+
+void g2d_clear_int(struct g2d_dev *d)
+{
+	w(1, INTC_PEND_REG);
+}
diff --git a/drivers/media/platform/s5p-g2d/g2d-regs.h b/drivers/media/platform/s5p-g2d/g2d-regs.h
new file mode 100644
index 0000000..9bf31ad
--- /dev/null
+++ b/drivers/media/platform/s5p-g2d/g2d-regs.h
@@ -0,0 +1,122 @@
+/*
+ * Samsung S5P G2D - 2D Graphics Accelerator Driver
+ *
+ * Copyright (c) 2011 Samsung Electronics Co., Ltd.
+ * Kamil Debski, <k.debski@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation; either version 2 of the
+ * License, or (at your option) any later version
+ */
+
+/* General Registers */
+#define SOFT_RESET_REG		0x0000	/* Software reset reg */
+#define INTEN_REG		0x0004	/* Interrupt Enable reg */
+#define INTC_PEND_REG		0x000C	/* Interrupt Control Pending reg */
+#define FIFO_STAT_REG		0x0010	/* Command FIFO Status reg */
+#define AXI_ID_MODE_REG		0x0014	/* AXI Read ID Mode reg */
+#define CACHECTL_REG		0x0018	/* Cache & Buffer clear reg */
+#define AXI_MODE_REG		0x001C	/* AXI Mode reg */
+
+/* Command Registers */
+#define BITBLT_START_REG	0x0100	/* BitBLT Start reg */
+#define BITBLT_COMMAND_REG	0x0104	/* Command reg for BitBLT */
+
+/* Parameter Setting Registers (Rotate & Direction) */
+#define ROTATE_REG		0x0200	/* Rotation reg */
+#define SRC_MSK_DIRECT_REG	0x0204	/* Src and Mask Direction reg */
+#define DST_PAT_DIRECT_REG	0x0208	/* Dest and Pattern Direction reg */
+
+/* Parameter Setting Registers (Src) */
+#define SRC_SELECT_REG		0x0300	/* Src Image Selection reg */
+#define SRC_BASE_ADDR_REG	0x0304	/* Src Image Base Address reg */
+#define SRC_STRIDE_REG		0x0308	/* Src Stride reg */
+#define SRC_COLOR_MODE_REG	0x030C	/* Src Image Color Mode reg */
+#define SRC_LEFT_TOP_REG	0x0310	/* Src Left Top Coordinate reg */
+#define SRC_RIGHT_BOTTOM_REG	0x0314	/* Src Right Bottom Coordinate reg */
+#define SRC_SCALE_CTRL_REG	0x0328	/* Src Scaling type select */
+#define SRC_XSCALE_REG		0x032c	/* Src X Scaling ratio */
+#define SRC_YSCALE_REG		0x0330	/* Src Y Scaling ratio */
+
+/* Parameter Setting Registers (Dest) */
+#define DST_SELECT_REG		0x0400	/* Dest Image Selection reg */
+#define DST_BASE_ADDR_REG	0x0404	/* Dest Image Base Address reg */
+#define DST_STRIDE_REG		0x0408	/* Dest Stride reg */
+#define DST_COLOR_MODE_REG	0x040C	/* Dest Image Color Mode reg */
+#define DST_LEFT_TOP_REG	0x0410	/* Dest Left Top Coordinate reg */
+#define DST_RIGHT_BOTTOM_REG	0x0414	/* Dest Right Bottom Coordinate reg */
+
+/* Parameter Setting Registers (Pattern) */
+#define PAT_BASE_ADDR_REG	0x0500	/* Pattern Image Base Address reg */
+#define PAT_SIZE_REG		0x0504	/* Pattern Image Size reg */
+#define PAT_COLOR_MODE_REG	0x0508	/* Pattern Image Color Mode reg */
+#define PAT_OFFSET_REG		0x050C	/* Pattern Left Top Coordinate reg */
+#define PAT_STRIDE_REG		0x0510	/* Pattern Stride reg */
+
+/* Parameter Setting Registers (Mask) */
+#define MASK_BASE_ADDR_REG	0x0520	/* Mask Base Address reg */
+#define MASK_STRIDE_REG		0x0524	/* Mask Stride reg */
+
+/* Parameter Setting Registers (Clipping Window) */
+#define CW_LT_REG		0x0600	/* LeftTop coordinates of Clip Window */
+#define CW_RB_REG		0x0604	/* RightBottom coordinates of Clip
+								Window */
+
+/* Parameter Setting Registers (ROP & Alpha Setting) */
+#define THIRD_OPERAND_REG	0x0610	/* Third Operand Selection reg */
+#define ROP4_REG		0x0614	/* Raster Operation reg */
+#define ALPHA_REG		0x0618	/* Alpha value, Fading offset value */
+
+/* Parameter Setting Registers (Color) */
+#define FG_COLOR_REG		0x0700	/* Foreground Color reg */
+#define BG_COLOR_REG		0x0704	/* Background Color reg */
+#define BS_COLOR_REG		0x0708	/* Blue Screen Color reg */
+
+/* Parameter Setting Registers (Color Key) */
+#define SRC_COLORKEY_CTRL_REG	0x0710	/* Src Colorkey control reg */
+#define SRC_COLORKEY_DR_MIN_REG	0x0714	/* Src Colorkey Decision Reference
+								Min reg */
+#define SRC_COLORKEY_DR_MAX_REG	0x0718	/* Src Colorkey Decision Reference
+								Max reg */
+#define DST_COLORKEY_CTRL_REG	0x071C	/* Dest Colorkey control reg */
+#define DST_COLORKEY_DR_MIN_REG	0x0720	/* Dest Colorkey Decision Reference
+								Min reg */
+#define DST_COLORKEY_DR_MAX_REG	0x0724	/* Dest Colorkey Decision Reference
+								Max reg */
+
+/* Color mode values */
+
+#define ORDER_XRGB		0
+#define ORDER_RGBX		1
+#define ORDER_XBGR		2
+#define ORDER_BGRX		3
+
+#define MODE_XRGB_8888		0
+#define MODE_ARGB_8888		1
+#define MODE_RGB_565		2
+#define MODE_XRGB_1555		3
+#define MODE_ARGB_1555		4
+#define MODE_XRGB_4444		5
+#define MODE_ARGB_4444		6
+#define MODE_PACKED_RGB_888	7
+
+#define COLOR_MODE(o, m)	(((o) << 4) | (m))
+
+/* ROP4 operation values */
+#define ROP4_COPY		0xCCCC
+#define ROP4_INVERT		0x3333
+
+/* Hardware limits */
+#define MAX_WIDTH		8000
+#define MAX_HEIGHT		8000
+
+#define G2D_TIMEOUT		500
+
+#define DEFAULT_WIDTH		100
+#define DEFAULT_HEIGHT		100
+
+#define DEFAULT_SCALE_MODE	(2 << 0)
+
+/* Command mode register values */
+#define CMD_V3_ENABLE_STRETCH	(1 << 4)
diff --git a/drivers/media/platform/s5p-g2d/g2d.c b/drivers/media/platform/s5p-g2d/g2d.c
new file mode 100644
index 0000000..e1936d9
--- /dev/null
+++ b/drivers/media/platform/s5p-g2d/g2d.c
@@ -0,0 +1,817 @@
+/*
+ * Samsung S5P G2D - 2D Graphics Accelerator Driver
+ *
+ * Copyright (c) 2011 Samsung Electronics Co., Ltd.
+ * Kamil Debski, <k.debski@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation; either version 2 of the
+ * License, or (at your option) any later version
+ */
+
+#include <linux/module.h>
+#include <linux/fs.h>
+#include <linux/timer.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/clk.h>
+#include <linux/interrupt.h>
+#include <linux/of.h>
+
+#include <linux/platform_device.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "g2d.h"
+#include "g2d-regs.h"
+
+#define fh2ctx(__fh) container_of(__fh, struct g2d_ctx, fh)
+
+static struct g2d_fmt formats[] = {
+	{
+		.name	= "XRGB_8888",
+		.fourcc	= V4L2_PIX_FMT_RGB32,
+		.depth	= 32,
+		.hw	= COLOR_MODE(ORDER_XRGB, MODE_XRGB_8888),
+	},
+	{
+		.name	= "RGB_565",
+		.fourcc	= V4L2_PIX_FMT_RGB565X,
+		.depth	= 16,
+		.hw	= COLOR_MODE(ORDER_XRGB, MODE_RGB_565),
+	},
+	{
+		.name	= "XRGB_1555",
+		.fourcc	= V4L2_PIX_FMT_RGB555X,
+		.depth	= 16,
+		.hw	= COLOR_MODE(ORDER_XRGB, MODE_XRGB_1555),
+	},
+	{
+		.name	= "XRGB_4444",
+		.fourcc	= V4L2_PIX_FMT_RGB444,
+		.depth	= 16,
+		.hw	= COLOR_MODE(ORDER_XRGB, MODE_XRGB_4444),
+	},
+	{
+		.name	= "PACKED_RGB_888",
+		.fourcc	= V4L2_PIX_FMT_RGB24,
+		.depth	= 24,
+		.hw	= COLOR_MODE(ORDER_XRGB, MODE_PACKED_RGB_888),
+	},
+};
+#define NUM_FORMATS ARRAY_SIZE(formats)
+
+static struct g2d_frame def_frame = {
+	.width		= DEFAULT_WIDTH,
+	.height		= DEFAULT_HEIGHT,
+	.c_width	= DEFAULT_WIDTH,
+	.c_height	= DEFAULT_HEIGHT,
+	.o_width	= 0,
+	.o_height	= 0,
+	.fmt		= &formats[0],
+	.right		= DEFAULT_WIDTH,
+	.bottom		= DEFAULT_HEIGHT,
+};
+
+static struct g2d_fmt *find_fmt(struct v4l2_format *f)
+{
+	unsigned int i;
+	for (i = 0; i < NUM_FORMATS; i++) {
+		if (formats[i].fourcc == f->fmt.pix.pixelformat)
+			return &formats[i];
+	}
+	return NULL;
+}
+
+
+static struct g2d_frame *get_frame(struct g2d_ctx *ctx,
+							enum v4l2_buf_type type)
+{
+	switch (type) {
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+		return &ctx->in;
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		return &ctx->out;
+	default:
+		return ERR_PTR(-EINVAL);
+	}
+}
+
+static int g2d_queue_setup(struct vb2_queue *vq, const void *parg,
+			   unsigned int *nbuffers, unsigned int *nplanes,
+			   unsigned int sizes[], void *alloc_ctxs[])
+{
+	struct g2d_ctx *ctx = vb2_get_drv_priv(vq);
+	struct g2d_frame *f = get_frame(ctx, vq->type);
+
+	if (IS_ERR(f))
+		return PTR_ERR(f);
+
+	sizes[0] = f->size;
+	*nplanes = 1;
+	alloc_ctxs[0] = ctx->dev->alloc_ctx;
+
+	if (*nbuffers == 0)
+		*nbuffers = 1;
+
+	return 0;
+}
+
+static int g2d_buf_prepare(struct vb2_buffer *vb)
+{
+	struct g2d_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct g2d_frame *f = get_frame(ctx, vb->vb2_queue->type);
+
+	if (IS_ERR(f))
+		return PTR_ERR(f);
+	vb2_set_plane_payload(vb, 0, f->size);
+	return 0;
+}
+
+static void g2d_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct g2d_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
+}
+
+static struct vb2_ops g2d_qops = {
+	.queue_setup	= g2d_queue_setup,
+	.buf_prepare	= g2d_buf_prepare,
+	.buf_queue	= g2d_buf_queue,
+};
+
+static int queue_init(void *priv, struct vb2_queue *src_vq,
+						struct vb2_queue *dst_vq)
+{
+	struct g2d_ctx *ctx = priv;
+	int ret;
+
+	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	src_vq->io_modes = VB2_MMAP | VB2_USERPTR;
+	src_vq->drv_priv = ctx;
+	src_vq->ops = &g2d_qops;
+	src_vq->mem_ops = &vb2_dma_contig_memops;
+	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	src_vq->lock = &ctx->dev->mutex;
+
+	ret = vb2_queue_init(src_vq);
+	if (ret)
+		return ret;
+
+	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	dst_vq->io_modes = VB2_MMAP | VB2_USERPTR;
+	dst_vq->drv_priv = ctx;
+	dst_vq->ops = &g2d_qops;
+	dst_vq->mem_ops = &vb2_dma_contig_memops;
+	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	dst_vq->lock = &ctx->dev->mutex;
+
+	return vb2_queue_init(dst_vq);
+}
+
+static int g2d_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct g2d_ctx *ctx = container_of(ctrl->handler, struct g2d_ctx,
+								ctrl_handler);
+	unsigned long flags;
+
+	spin_lock_irqsave(&ctx->dev->ctrl_lock, flags);
+	switch (ctrl->id) {
+	case V4L2_CID_COLORFX:
+		if (ctrl->val == V4L2_COLORFX_NEGATIVE)
+			ctx->rop = ROP4_INVERT;
+		else
+			ctx->rop = ROP4_COPY;
+		break;
+
+	case V4L2_CID_HFLIP:
+		ctx->flip = ctx->ctrl_hflip->val | (ctx->ctrl_vflip->val << 1);
+		break;
+
+	}
+	spin_unlock_irqrestore(&ctx->dev->ctrl_lock, flags);
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops g2d_ctrl_ops = {
+	.s_ctrl		= g2d_s_ctrl,
+};
+
+static int g2d_setup_ctrls(struct g2d_ctx *ctx)
+{
+	struct g2d_dev *dev = ctx->dev;
+
+	v4l2_ctrl_handler_init(&ctx->ctrl_handler, 3);
+
+	ctx->ctrl_hflip = v4l2_ctrl_new_std(&ctx->ctrl_handler, &g2d_ctrl_ops,
+						V4L2_CID_HFLIP, 0, 1, 1, 0);
+
+	ctx->ctrl_vflip = v4l2_ctrl_new_std(&ctx->ctrl_handler, &g2d_ctrl_ops,
+						V4L2_CID_VFLIP, 0, 1, 1, 0);
+
+	v4l2_ctrl_new_std_menu(
+		&ctx->ctrl_handler,
+		&g2d_ctrl_ops,
+		V4L2_CID_COLORFX,
+		V4L2_COLORFX_NEGATIVE,
+		~((1 << V4L2_COLORFX_NONE) | (1 << V4L2_COLORFX_NEGATIVE)),
+		V4L2_COLORFX_NONE);
+
+	if (ctx->ctrl_handler.error) {
+		int err = ctx->ctrl_handler.error;
+		v4l2_err(&dev->v4l2_dev, "g2d_setup_ctrls failed\n");
+		v4l2_ctrl_handler_free(&ctx->ctrl_handler);
+		return err;
+	}
+
+	v4l2_ctrl_cluster(2, &ctx->ctrl_hflip);
+
+	return 0;
+}
+
+static int g2d_open(struct file *file)
+{
+	struct g2d_dev *dev = video_drvdata(file);
+	struct g2d_ctx *ctx = NULL;
+	int ret = 0;
+
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx)
+		return -ENOMEM;
+	ctx->dev = dev;
+	/* Set default formats */
+	ctx->in		= def_frame;
+	ctx->out	= def_frame;
+
+	if (mutex_lock_interruptible(&dev->mutex)) {
+		kfree(ctx);
+		return -ERESTARTSYS;
+	}
+	ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(dev->m2m_dev, ctx, &queue_init);
+	if (IS_ERR(ctx->fh.m2m_ctx)) {
+		ret = PTR_ERR(ctx->fh.m2m_ctx);
+		mutex_unlock(&dev->mutex);
+		kfree(ctx);
+		return ret;
+	}
+	v4l2_fh_init(&ctx->fh, video_devdata(file));
+	file->private_data = &ctx->fh;
+	v4l2_fh_add(&ctx->fh);
+
+	g2d_setup_ctrls(ctx);
+
+	/* Write the default values to the ctx struct */
+	v4l2_ctrl_handler_setup(&ctx->ctrl_handler);
+
+	ctx->fh.ctrl_handler = &ctx->ctrl_handler;
+	mutex_unlock(&dev->mutex);
+
+	v4l2_info(&dev->v4l2_dev, "instance opened\n");
+	return 0;
+}
+
+static int g2d_release(struct file *file)
+{
+	struct g2d_dev *dev = video_drvdata(file);
+	struct g2d_ctx *ctx = fh2ctx(file->private_data);
+
+	v4l2_ctrl_handler_free(&ctx->ctrl_handler);
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	kfree(ctx);
+	v4l2_info(&dev->v4l2_dev, "instance closed\n");
+	return 0;
+}
+
+
+static int vidioc_querycap(struct file *file, void *priv,
+				struct v4l2_capability *cap)
+{
+	strncpy(cap->driver, G2D_NAME, sizeof(cap->driver) - 1);
+	strncpy(cap->card, G2D_NAME, sizeof(cap->card) - 1);
+	cap->bus_info[0] = 0;
+	cap->device_caps = V4L2_CAP_VIDEO_M2M | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	return 0;
+}
+
+static int vidioc_enum_fmt(struct file *file, void *prv, struct v4l2_fmtdesc *f)
+{
+	struct g2d_fmt *fmt;
+	if (f->index >= NUM_FORMATS)
+		return -EINVAL;
+	fmt = &formats[f->index];
+	f->pixelformat = fmt->fourcc;
+	strncpy(f->description, fmt->name, sizeof(f->description) - 1);
+	return 0;
+}
+
+static int vidioc_g_fmt(struct file *file, void *prv, struct v4l2_format *f)
+{
+	struct g2d_ctx *ctx = prv;
+	struct vb2_queue *vq;
+	struct g2d_frame *frm;
+
+	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
+	if (!vq)
+		return -EINVAL;
+	frm = get_frame(ctx, f->type);
+	if (IS_ERR(frm))
+		return PTR_ERR(frm);
+
+	f->fmt.pix.width		= frm->width;
+	f->fmt.pix.height		= frm->height;
+	f->fmt.pix.field		= V4L2_FIELD_NONE;
+	f->fmt.pix.pixelformat		= frm->fmt->fourcc;
+	f->fmt.pix.bytesperline		= (frm->width * frm->fmt->depth) >> 3;
+	f->fmt.pix.sizeimage		= frm->size;
+	return 0;
+}
+
+static int vidioc_try_fmt(struct file *file, void *prv, struct v4l2_format *f)
+{
+	struct g2d_fmt *fmt;
+	enum v4l2_field *field;
+
+	fmt = find_fmt(f);
+	if (!fmt)
+		return -EINVAL;
+
+	field = &f->fmt.pix.field;
+	if (*field == V4L2_FIELD_ANY)
+		*field = V4L2_FIELD_NONE;
+	else if (*field != V4L2_FIELD_NONE)
+		return -EINVAL;
+
+	if (f->fmt.pix.width > MAX_WIDTH)
+		f->fmt.pix.width = MAX_WIDTH;
+	if (f->fmt.pix.height > MAX_HEIGHT)
+		f->fmt.pix.height = MAX_HEIGHT;
+
+	if (f->fmt.pix.width < 1)
+		f->fmt.pix.width = 1;
+	if (f->fmt.pix.height < 1)
+		f->fmt.pix.height = 1;
+
+	f->fmt.pix.bytesperline = (f->fmt.pix.width * fmt->depth) >> 3;
+	f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
+	return 0;
+}
+
+static int vidioc_s_fmt(struct file *file, void *prv, struct v4l2_format *f)
+{
+	struct g2d_ctx *ctx = prv;
+	struct g2d_dev *dev = ctx->dev;
+	struct vb2_queue *vq;
+	struct g2d_frame *frm;
+	struct g2d_fmt *fmt;
+	int ret = 0;
+
+	/* Adjust all values accordingly to the hardware capabilities
+	 * and chosen format. */
+	ret = vidioc_try_fmt(file, prv, f);
+	if (ret)
+		return ret;
+	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
+	if (vb2_is_busy(vq)) {
+		v4l2_err(&dev->v4l2_dev, "queue (%d) bust\n", f->type);
+		return -EBUSY;
+	}
+	frm = get_frame(ctx, f->type);
+	if (IS_ERR(frm))
+		return PTR_ERR(frm);
+	fmt = find_fmt(f);
+	if (!fmt)
+		return -EINVAL;
+	frm->width	= f->fmt.pix.width;
+	frm->height	= f->fmt.pix.height;
+	frm->size	= f->fmt.pix.sizeimage;
+	/* Reset crop settings */
+	frm->o_width	= 0;
+	frm->o_height	= 0;
+	frm->c_width	= frm->width;
+	frm->c_height	= frm->height;
+	frm->right	= frm->width;
+	frm->bottom	= frm->height;
+	frm->fmt	= fmt;
+	frm->stride	= f->fmt.pix.bytesperline;
+	return 0;
+}
+
+static int vidioc_cropcap(struct file *file, void *priv,
+					struct v4l2_cropcap *cr)
+{
+	struct g2d_ctx *ctx = priv;
+	struct g2d_frame *f;
+
+	f = get_frame(ctx, cr->type);
+	if (IS_ERR(f))
+		return PTR_ERR(f);
+
+	cr->bounds.left		= 0;
+	cr->bounds.top		= 0;
+	cr->bounds.width	= f->width;
+	cr->bounds.height	= f->height;
+	cr->defrect		= cr->bounds;
+	return 0;
+}
+
+static int vidioc_g_crop(struct file *file, void *prv, struct v4l2_crop *cr)
+{
+	struct g2d_ctx *ctx = prv;
+	struct g2d_frame *f;
+
+	f = get_frame(ctx, cr->type);
+	if (IS_ERR(f))
+		return PTR_ERR(f);
+
+	cr->c.left	= f->o_height;
+	cr->c.top	= f->o_width;
+	cr->c.width	= f->c_width;
+	cr->c.height	= f->c_height;
+	return 0;
+}
+
+static int vidioc_try_crop(struct file *file, void *prv, const struct v4l2_crop *cr)
+{
+	struct g2d_ctx *ctx = prv;
+	struct g2d_dev *dev = ctx->dev;
+	struct g2d_frame *f;
+
+	f = get_frame(ctx, cr->type);
+	if (IS_ERR(f))
+		return PTR_ERR(f);
+
+	if (cr->c.top < 0 || cr->c.left < 0) {
+		v4l2_err(&dev->v4l2_dev,
+			"doesn't support negative values for top & left\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int vidioc_s_crop(struct file *file, void *prv, const struct v4l2_crop *cr)
+{
+	struct g2d_ctx *ctx = prv;
+	struct g2d_frame *f;
+	int ret;
+
+	ret = vidioc_try_crop(file, prv, cr);
+	if (ret)
+		return ret;
+	f = get_frame(ctx, cr->type);
+	if (IS_ERR(f))
+		return PTR_ERR(f);
+
+	f->c_width	= cr->c.width;
+	f->c_height	= cr->c.height;
+	f->o_width	= cr->c.left;
+	f->o_height	= cr->c.top;
+	f->bottom	= f->o_height + f->c_height;
+	f->right	= f->o_width + f->c_width;
+	return 0;
+}
+
+static void job_abort(void *prv)
+{
+	struct g2d_ctx *ctx = prv;
+	struct g2d_dev *dev = ctx->dev;
+
+	if (dev->curr == NULL) /* No job currently running */
+		return;
+
+	wait_event_timeout(dev->irq_queue,
+			   dev->curr == NULL,
+			   msecs_to_jiffies(G2D_TIMEOUT));
+}
+
+static void device_run(void *prv)
+{
+	struct g2d_ctx *ctx = prv;
+	struct g2d_dev *dev = ctx->dev;
+	struct vb2_buffer *src, *dst;
+	unsigned long flags;
+	u32 cmd = 0;
+
+	dev->curr = ctx;
+
+	src = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+	dst = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
+
+	clk_enable(dev->gate);
+	g2d_reset(dev);
+
+	spin_lock_irqsave(&dev->ctrl_lock, flags);
+
+	g2d_set_src_size(dev, &ctx->in);
+	g2d_set_src_addr(dev, vb2_dma_contig_plane_dma_addr(src, 0));
+
+	g2d_set_dst_size(dev, &ctx->out);
+	g2d_set_dst_addr(dev, vb2_dma_contig_plane_dma_addr(dst, 0));
+
+	g2d_set_rop4(dev, ctx->rop);
+	g2d_set_flip(dev, ctx->flip);
+
+	if (ctx->in.c_width != ctx->out.c_width ||
+		ctx->in.c_height != ctx->out.c_height) {
+		if (dev->variant->hw_rev == TYPE_G2D_3X)
+			cmd |= CMD_V3_ENABLE_STRETCH;
+		else
+			g2d_set_v41_stretch(dev, &ctx->in, &ctx->out);
+	}
+
+	g2d_set_cmd(dev, cmd);
+	g2d_start(dev);
+
+	spin_unlock_irqrestore(&dev->ctrl_lock, flags);
+}
+
+static irqreturn_t g2d_isr(int irq, void *prv)
+{
+	struct g2d_dev *dev = prv;
+	struct g2d_ctx *ctx = dev->curr;
+	struct vb2_v4l2_buffer *src, *dst;
+
+	g2d_clear_int(dev);
+	clk_disable(dev->gate);
+
+	BUG_ON(ctx == NULL);
+
+	src = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+	dst = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
+
+	BUG_ON(src == NULL);
+	BUG_ON(dst == NULL);
+
+	dst->timecode = src->timecode;
+	dst->timestamp = src->timestamp;
+	dst->flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+	dst->flags |=
+		src->flags & V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+
+	v4l2_m2m_buf_done(src, VB2_BUF_STATE_DONE);
+	v4l2_m2m_buf_done(dst, VB2_BUF_STATE_DONE);
+	v4l2_m2m_job_finish(dev->m2m_dev, ctx->fh.m2m_ctx);
+
+	dev->curr = NULL;
+	wake_up(&dev->irq_queue);
+	return IRQ_HANDLED;
+}
+
+static const struct v4l2_file_operations g2d_fops = {
+	.owner		= THIS_MODULE,
+	.open		= g2d_open,
+	.release	= g2d_release,
+	.poll		= v4l2_m2m_fop_poll,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap		= v4l2_m2m_fop_mmap,
+};
+
+static const struct v4l2_ioctl_ops g2d_ioctl_ops = {
+	.vidioc_querycap	= vidioc_querycap,
+
+	.vidioc_enum_fmt_vid_cap	= vidioc_enum_fmt,
+	.vidioc_g_fmt_vid_cap		= vidioc_g_fmt,
+	.vidioc_try_fmt_vid_cap		= vidioc_try_fmt,
+	.vidioc_s_fmt_vid_cap		= vidioc_s_fmt,
+
+	.vidioc_enum_fmt_vid_out	= vidioc_enum_fmt,
+	.vidioc_g_fmt_vid_out		= vidioc_g_fmt,
+	.vidioc_try_fmt_vid_out		= vidioc_try_fmt,
+	.vidioc_s_fmt_vid_out		= vidioc_s_fmt,
+
+	.vidioc_reqbufs			= v4l2_m2m_ioctl_reqbufs,
+	.vidioc_querybuf		= v4l2_m2m_ioctl_querybuf,
+	.vidioc_qbuf			= v4l2_m2m_ioctl_qbuf,
+	.vidioc_dqbuf			= v4l2_m2m_ioctl_dqbuf,
+
+	.vidioc_streamon		= v4l2_m2m_ioctl_streamon,
+	.vidioc_streamoff		= v4l2_m2m_ioctl_streamoff,
+
+	.vidioc_g_crop			= vidioc_g_crop,
+	.vidioc_s_crop			= vidioc_s_crop,
+	.vidioc_cropcap			= vidioc_cropcap,
+};
+
+static struct video_device g2d_videodev = {
+	.name		= G2D_NAME,
+	.fops		= &g2d_fops,
+	.ioctl_ops	= &g2d_ioctl_ops,
+	.minor		= -1,
+	.release	= video_device_release,
+	.vfl_dir	= VFL_DIR_M2M,
+};
+
+static struct v4l2_m2m_ops g2d_m2m_ops = {
+	.device_run	= device_run,
+	.job_abort	= job_abort,
+};
+
+static const struct of_device_id exynos_g2d_match[];
+
+static int g2d_probe(struct platform_device *pdev)
+{
+	struct g2d_dev *dev;
+	struct video_device *vfd;
+	struct resource *res;
+	const struct of_device_id *of_id;
+	int ret = 0;
+
+	dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
+	if (!dev)
+		return -ENOMEM;
+
+	spin_lock_init(&dev->ctrl_lock);
+	mutex_init(&dev->mutex);
+	atomic_set(&dev->num_inst, 0);
+	init_waitqueue_head(&dev->irq_queue);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	dev->regs = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(dev->regs))
+		return PTR_ERR(dev->regs);
+
+	dev->clk = clk_get(&pdev->dev, "sclk_fimg2d");
+	if (IS_ERR(dev->clk)) {
+		dev_err(&pdev->dev, "failed to get g2d clock\n");
+		return -ENXIO;
+	}
+
+	ret = clk_prepare(dev->clk);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to prepare g2d clock\n");
+		goto put_clk;
+	}
+
+	dev->gate = clk_get(&pdev->dev, "fimg2d");
+	if (IS_ERR(dev->gate)) {
+		dev_err(&pdev->dev, "failed to get g2d clock gate\n");
+		ret = -ENXIO;
+		goto unprep_clk;
+	}
+
+	ret = clk_prepare(dev->gate);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to prepare g2d clock gate\n");
+		goto put_clk_gate;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "failed to find IRQ\n");
+		ret = -ENXIO;
+		goto unprep_clk_gate;
+	}
+
+	dev->irq = res->start;
+
+	ret = devm_request_irq(&pdev->dev, dev->irq, g2d_isr,
+						0, pdev->name, dev);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to install IRQ\n");
+		goto put_clk_gate;
+	}
+
+	dev->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
+	if (IS_ERR(dev->alloc_ctx)) {
+		ret = PTR_ERR(dev->alloc_ctx);
+		goto unprep_clk_gate;
+	}
+
+	ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
+	if (ret)
+		goto alloc_ctx_cleanup;
+	vfd = video_device_alloc();
+	if (!vfd) {
+		v4l2_err(&dev->v4l2_dev, "Failed to allocate video device\n");
+		ret = -ENOMEM;
+		goto unreg_v4l2_dev;
+	}
+	*vfd = g2d_videodev;
+	vfd->lock = &dev->mutex;
+	vfd->v4l2_dev = &dev->v4l2_dev;
+	ret = video_register_device(vfd, VFL_TYPE_GRABBER, 0);
+	if (ret) {
+		v4l2_err(&dev->v4l2_dev, "Failed to register video device\n");
+		goto rel_vdev;
+	}
+	video_set_drvdata(vfd, dev);
+	snprintf(vfd->name, sizeof(vfd->name), "%s", g2d_videodev.name);
+	dev->vfd = vfd;
+	v4l2_info(&dev->v4l2_dev, "device registered as /dev/video%d\n",
+								vfd->num);
+	platform_set_drvdata(pdev, dev);
+	dev->m2m_dev = v4l2_m2m_init(&g2d_m2m_ops);
+	if (IS_ERR(dev->m2m_dev)) {
+		v4l2_err(&dev->v4l2_dev, "Failed to init mem2mem device\n");
+		ret = PTR_ERR(dev->m2m_dev);
+		goto unreg_video_dev;
+	}
+
+	def_frame.stride = (def_frame.width * def_frame.fmt->depth) >> 3;
+
+	if (!pdev->dev.of_node) {
+		dev->variant = g2d_get_drv_data(pdev);
+	} else {
+		of_id = of_match_node(exynos_g2d_match, pdev->dev.of_node);
+		if (!of_id) {
+			ret = -ENODEV;
+			goto unreg_video_dev;
+		}
+		dev->variant = (struct g2d_variant *)of_id->data;
+	}
+
+	return 0;
+
+unreg_video_dev:
+	video_unregister_device(dev->vfd);
+rel_vdev:
+	video_device_release(vfd);
+unreg_v4l2_dev:
+	v4l2_device_unregister(&dev->v4l2_dev);
+alloc_ctx_cleanup:
+	vb2_dma_contig_cleanup_ctx(dev->alloc_ctx);
+unprep_clk_gate:
+	clk_unprepare(dev->gate);
+put_clk_gate:
+	clk_put(dev->gate);
+unprep_clk:
+	clk_unprepare(dev->clk);
+put_clk:
+	clk_put(dev->clk);
+
+	return ret;
+}
+
+static int g2d_remove(struct platform_device *pdev)
+{
+	struct g2d_dev *dev = platform_get_drvdata(pdev);
+
+	v4l2_info(&dev->v4l2_dev, "Removing " G2D_NAME);
+	v4l2_m2m_release(dev->m2m_dev);
+	video_unregister_device(dev->vfd);
+	v4l2_device_unregister(&dev->v4l2_dev);
+	vb2_dma_contig_cleanup_ctx(dev->alloc_ctx);
+	clk_unprepare(dev->gate);
+	clk_put(dev->gate);
+	clk_unprepare(dev->clk);
+	clk_put(dev->clk);
+	return 0;
+}
+
+static struct g2d_variant g2d_drvdata_v3x = {
+	.hw_rev = TYPE_G2D_3X, /* Revision 3.0 for S5PV210 and Exynos4210 */
+};
+
+static struct g2d_variant g2d_drvdata_v4x = {
+	.hw_rev = TYPE_G2D_4X, /* Revision 4.1 for Exynos4X12 and Exynos5 */
+};
+
+static const struct of_device_id exynos_g2d_match[] = {
+	{
+		.compatible = "samsung,s5pv210-g2d",
+		.data = &g2d_drvdata_v3x,
+	}, {
+		.compatible = "samsung,exynos4212-g2d",
+		.data = &g2d_drvdata_v4x,
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(of, exynos_g2d_match);
+
+static const struct platform_device_id g2d_driver_ids[] = {
+	{
+		.name = "s5p-g2d",
+		.driver_data = (unsigned long)&g2d_drvdata_v3x,
+	}, {
+		.name = "s5p-g2d-v4x",
+		.driver_data = (unsigned long)&g2d_drvdata_v4x,
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(platform, g2d_driver_ids);
+
+static struct platform_driver g2d_pdrv = {
+	.probe		= g2d_probe,
+	.remove		= g2d_remove,
+	.id_table	= g2d_driver_ids,
+	.driver		= {
+		.name = G2D_NAME,
+		.of_match_table = exynos_g2d_match,
+	},
+};
+
+module_platform_driver(g2d_pdrv);
+
+MODULE_AUTHOR("Kamil Debski <k.debski@samsung.com>");
+MODULE_DESCRIPTION("S5P G2D 2d graphics accelerator driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/s5p-g2d/g2d.h b/drivers/media/platform/s5p-g2d/g2d.h
new file mode 100644
index 0000000..b0e52ab
--- /dev/null
+++ b/drivers/media/platform/s5p-g2d/g2d.h
@@ -0,0 +1,96 @@
+/*
+ * Samsung S5P G2D - 2D Graphics Accelerator Driver
+ *
+ * Copyright (c) 2011 Samsung Electronics Co., Ltd.
+ * Kamil Debski, <k.debski@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation; either version 2 of the
+ * License, or (at your option) any later version
+ */
+
+#include <linux/platform_device.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+
+#define G2D_NAME "s5p-g2d"
+#define TYPE_G2D_3X 3
+#define TYPE_G2D_4X 4
+
+struct g2d_dev {
+	struct v4l2_device	v4l2_dev;
+	struct v4l2_m2m_dev	*m2m_dev;
+	struct video_device	*vfd;
+	struct mutex		mutex;
+	spinlock_t		ctrl_lock;
+	atomic_t		num_inst;
+	struct vb2_alloc_ctx	*alloc_ctx;
+	void __iomem		*regs;
+	struct clk		*clk;
+	struct clk		*gate;
+	struct g2d_ctx		*curr;
+	struct g2d_variant	*variant;
+	int irq;
+	wait_queue_head_t	irq_queue;
+};
+
+struct g2d_frame {
+	/* Original dimensions */
+	u32	width;
+	u32	height;
+	/* Crop size */
+	u32	c_width;
+	u32	c_height;
+	/* Offset */
+	u32	o_width;
+	u32	o_height;
+	/* Image format */
+	struct g2d_fmt *fmt;
+	/* Variables that can calculated once and reused */
+	u32	stride;
+	u32	bottom;
+	u32	right;
+	u32	size;
+};
+
+struct g2d_ctx {
+	struct v4l2_fh fh;
+	struct g2d_dev		*dev;
+	struct g2d_frame	in;
+	struct g2d_frame	out;
+	struct v4l2_ctrl	*ctrl_hflip;
+	struct v4l2_ctrl	*ctrl_vflip;
+	struct v4l2_ctrl_handler ctrl_handler;
+	u32 rop;
+	u32 flip;
+};
+
+struct g2d_fmt {
+	char	*name;
+	u32	fourcc;
+	int	depth;
+	u32	hw;
+};
+
+struct g2d_variant {
+	unsigned short hw_rev;
+};
+
+void g2d_reset(struct g2d_dev *d);
+void g2d_set_src_size(struct g2d_dev *d, struct g2d_frame *f);
+void g2d_set_src_addr(struct g2d_dev *d, dma_addr_t a);
+void g2d_set_dst_size(struct g2d_dev *d, struct g2d_frame *f);
+void g2d_set_dst_addr(struct g2d_dev *d, dma_addr_t a);
+void g2d_start(struct g2d_dev *d);
+void g2d_clear_int(struct g2d_dev *d);
+void g2d_set_rop4(struct g2d_dev *d, u32 r);
+void g2d_set_flip(struct g2d_dev *d, u32 r);
+void g2d_set_v41_stretch(struct g2d_dev *d,
+			struct g2d_frame *src, struct g2d_frame *dst);
+void g2d_set_cmd(struct g2d_dev *d, u32 c);
+
+static inline struct g2d_variant *g2d_get_drv_data(struct platform_device *pdev)
+{
+	return (struct g2d_variant *)platform_get_device_id(pdev)->driver_data;
+}
diff --git a/drivers/media/platform/s5p-jpeg/Makefile b/drivers/media/platform/s5p-jpeg/Makefile
new file mode 100644
index 0000000..9e5f214
--- /dev/null
+++ b/drivers/media/platform/s5p-jpeg/Makefile
@@ -0,0 +1,2 @@
+s5p-jpeg-objs := jpeg-core.o jpeg-hw-exynos3250.o jpeg-hw-exynos4.o jpeg-hw-s5p.o
+obj-$(CONFIG_VIDEO_SAMSUNG_S5P_JPEG) += s5p-jpeg.o
diff --git a/drivers/media/platform/s5p-jpeg/jpeg-core.c b/drivers/media/platform/s5p-jpeg/jpeg-core.c
new file mode 100644
index 0000000..9c6fc09
--- /dev/null
+++ b/drivers/media/platform/s5p-jpeg/jpeg-core.c
@@ -0,0 +1,3133 @@
+/* linux/drivers/media/platform/s5p-jpeg/jpeg-core.c
+ *
+ * Copyright (c) 2011-2014 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ *
+ * Author: Andrzej Pietrasiewicz <andrzej.p@samsung.com>
+ * Author: Jacek Anaszewski <j.anaszewski@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/gfp.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/string.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "jpeg-core.h"
+#include "jpeg-hw-s5p.h"
+#include "jpeg-hw-exynos4.h"
+#include "jpeg-hw-exynos3250.h"
+#include "jpeg-regs.h"
+
+static struct s5p_jpeg_fmt sjpeg_formats[] = {
+	{
+		.name		= "JPEG JFIF",
+		.fourcc		= V4L2_PIX_FMT_JPEG,
+		.flags		= SJPEG_FMT_FLAG_ENC_CAPTURE |
+				  SJPEG_FMT_FLAG_DEC_OUTPUT |
+				  SJPEG_FMT_FLAG_S5P |
+				  SJPEG_FMT_FLAG_EXYNOS3250 |
+				  SJPEG_FMT_FLAG_EXYNOS4,
+	},
+	{
+		.name		= "YUV 4:2:2 packed, YCbYCr",
+		.fourcc		= V4L2_PIX_FMT_YUYV,
+		.depth		= 16,
+		.colplanes	= 1,
+		.h_align	= 4,
+		.v_align	= 3,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_S5P |
+				  SJPEG_FMT_NON_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_422,
+	},
+	{
+		.name		= "YUV 4:2:2 packed, YCbYCr",
+		.fourcc		= V4L2_PIX_FMT_YUYV,
+		.depth		= 16,
+		.colplanes	= 1,
+		.h_align	= 1,
+		.v_align	= 0,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_EXYNOS4 |
+				  SJPEG_FMT_NON_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_422,
+	},
+	{
+		.name		= "YUV 4:2:2 packed, YCbYCr",
+		.fourcc		= V4L2_PIX_FMT_YUYV,
+		.depth		= 16,
+		.colplanes	= 1,
+		.h_align	= 2,
+		.v_align	= 0,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_EXYNOS3250 |
+				  SJPEG_FMT_NON_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_422,
+	},
+	{
+		.name		= "YUV 4:2:2 packed, YCrYCb",
+		.fourcc		= V4L2_PIX_FMT_YVYU,
+		.depth		= 16,
+		.colplanes	= 1,
+		.h_align	= 1,
+		.v_align	= 0,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_EXYNOS4 |
+				  SJPEG_FMT_NON_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_422,
+	},
+	{
+		.name		= "YUV 4:2:2 packed, YCrYCb",
+		.fourcc		= V4L2_PIX_FMT_YVYU,
+		.depth		= 16,
+		.colplanes	= 1,
+		.h_align	= 2,
+		.v_align	= 0,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_EXYNOS3250 |
+				  SJPEG_FMT_NON_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_422,
+	},
+	{
+		.name		= "YUV 4:2:2 packed, YCrYCb",
+		.fourcc		= V4L2_PIX_FMT_UYVY,
+		.depth		= 16,
+		.colplanes	= 1,
+		.h_align	= 2,
+		.v_align	= 0,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_EXYNOS3250 |
+				  SJPEG_FMT_NON_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_422,
+	},
+	{
+		.name		= "YUV 4:2:2 packed, YCrYCb",
+		.fourcc		= V4L2_PIX_FMT_VYUY,
+		.depth		= 16,
+		.colplanes	= 1,
+		.h_align	= 2,
+		.v_align	= 0,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_EXYNOS3250 |
+				  SJPEG_FMT_NON_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_422,
+	},
+	{
+		.name		= "RGB565",
+		.fourcc		= V4L2_PIX_FMT_RGB565,
+		.depth		= 16,
+		.colplanes	= 1,
+		.h_align	= 0,
+		.v_align	= 0,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_EXYNOS4 |
+				  SJPEG_FMT_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_444,
+	},
+	{
+		.name		= "RGB565",
+		.fourcc		= V4L2_PIX_FMT_RGB565,
+		.depth		= 16,
+		.colplanes	= 1,
+		.h_align	= 2,
+		.v_align	= 0,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_EXYNOS3250 |
+				  SJPEG_FMT_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_444,
+	},
+	{
+		.name		= "RGB565X",
+		.fourcc		= V4L2_PIX_FMT_RGB565X,
+		.depth		= 16,
+		.colplanes	= 1,
+		.h_align	= 2,
+		.v_align	= 0,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_EXYNOS3250 |
+				  SJPEG_FMT_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_444,
+	},
+	{
+		.name		= "RGB565",
+		.fourcc		= V4L2_PIX_FMT_RGB565,
+		.depth		= 16,
+		.colplanes	= 1,
+		.h_align	= 0,
+		.v_align	= 0,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_S5P |
+				  SJPEG_FMT_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_444,
+	},
+	{
+		.name		= "ARGB8888, 32 bpp",
+		.fourcc		= V4L2_PIX_FMT_RGB32,
+		.depth		= 32,
+		.colplanes	= 1,
+		.h_align	= 0,
+		.v_align	= 0,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_EXYNOS4 |
+				  SJPEG_FMT_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_444,
+	},
+	{
+		.name		= "ARGB8888, 32 bpp",
+		.fourcc		= V4L2_PIX_FMT_RGB32,
+		.depth		= 32,
+		.colplanes	= 1,
+		.h_align	= 2,
+		.v_align	= 0,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_EXYNOS3250 |
+				  SJPEG_FMT_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_444,
+	},
+	{
+		.name		= "YUV 4:4:4 planar, Y/CbCr",
+		.fourcc		= V4L2_PIX_FMT_NV24,
+		.depth		= 24,
+		.colplanes	= 2,
+		.h_align	= 0,
+		.v_align	= 0,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_EXYNOS4 |
+				  SJPEG_FMT_NON_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_444,
+	},
+	{
+		.name		= "YUV 4:4:4 planar, Y/CrCb",
+		.fourcc		= V4L2_PIX_FMT_NV42,
+		.depth		= 24,
+		.colplanes	= 2,
+		.h_align	= 0,
+		.v_align	= 0,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_EXYNOS4 |
+				  SJPEG_FMT_NON_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_444,
+	},
+	{
+		.name		= "YUV 4:2:2 planar, Y/CrCb",
+		.fourcc		= V4L2_PIX_FMT_NV61,
+		.depth		= 16,
+		.colplanes	= 2,
+		.h_align	= 1,
+		.v_align	= 0,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_EXYNOS4 |
+				  SJPEG_FMT_NON_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_422,
+	},
+	{
+		.name		= "YUV 4:2:2 planar, Y/CbCr",
+		.fourcc		= V4L2_PIX_FMT_NV16,
+		.depth		= 16,
+		.colplanes	= 2,
+		.h_align	= 1,
+		.v_align	= 0,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_EXYNOS4 |
+				  SJPEG_FMT_NON_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_422,
+	},
+	{
+		.name		= "YUV 4:2:0 planar, Y/CbCr",
+		.fourcc		= V4L2_PIX_FMT_NV12,
+		.depth		= 12,
+		.colplanes	= 2,
+		.h_align	= 1,
+		.v_align	= 1,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_EXYNOS4 |
+				  SJPEG_FMT_NON_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_420,
+	},
+	{
+		.name		= "YUV 4:2:0 planar, Y/CbCr",
+		.fourcc		= V4L2_PIX_FMT_NV12,
+		.depth		= 12,
+		.colplanes	= 2,
+		.h_align	= 3,
+		.v_align	= 3,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_EXYNOS3250 |
+				  SJPEG_FMT_NON_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_420,
+	},
+	{
+		.name		= "YUV 4:2:0 planar, Y/CbCr",
+		.fourcc		= V4L2_PIX_FMT_NV12,
+		.depth		= 12,
+		.colplanes	= 2,
+		.h_align	= 4,
+		.v_align	= 4,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_S5P |
+				  SJPEG_FMT_NON_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_420,
+	},
+	{
+		.name		= "YUV 4:2:0 planar, Y/CrCb",
+		.fourcc		= V4L2_PIX_FMT_NV21,
+		.depth		= 12,
+		.colplanes	= 2,
+		.h_align	= 3,
+		.v_align	= 3,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_EXYNOS3250 |
+				  SJPEG_FMT_NON_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_420,
+	},
+	{
+		.name		= "YUV 4:2:0 planar, Y/CrCb",
+		.fourcc		= V4L2_PIX_FMT_NV21,
+		.depth		= 12,
+		.colplanes	= 2,
+		.h_align	= 1,
+		.v_align	= 1,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_EXYNOS3250 |
+				  SJPEG_FMT_FLAG_EXYNOS4 |
+				  SJPEG_FMT_NON_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_420,
+	},
+	{
+		.name		= "YUV 4:2:0 contiguous 3-planar, Y/Cb/Cr",
+		.fourcc		= V4L2_PIX_FMT_YUV420,
+		.depth		= 12,
+		.colplanes	= 3,
+		.h_align	= 1,
+		.v_align	= 1,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_EXYNOS4 |
+				  SJPEG_FMT_NON_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_420,
+	},
+	{
+		.name		= "YUV 4:2:0 contiguous 3-planar, Y/Cb/Cr",
+		.fourcc		= V4L2_PIX_FMT_YUV420,
+		.depth		= 12,
+		.colplanes	= 3,
+		.h_align	= 4,
+		.v_align	= 4,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_EXYNOS3250 |
+				  SJPEG_FMT_NON_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_420,
+	},
+	{
+		.name		= "Gray",
+		.fourcc		= V4L2_PIX_FMT_GREY,
+		.depth		= 8,
+		.colplanes	= 1,
+		.flags		= SJPEG_FMT_FLAG_ENC_OUTPUT |
+				  SJPEG_FMT_FLAG_DEC_CAPTURE |
+				  SJPEG_FMT_FLAG_EXYNOS4 |
+				  SJPEG_FMT_NON_RGB,
+		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_GRAY,
+	},
+};
+#define SJPEG_NUM_FORMATS ARRAY_SIZE(sjpeg_formats)
+
+static const unsigned char qtbl_luminance[4][64] = {
+	{/*level 0 - high compression quality */
+		20, 16, 25, 39, 50, 46, 62, 68,
+		16, 18, 23, 38, 38, 53, 65, 68,
+		25, 23, 31, 38, 53, 65, 68, 68,
+		39, 38, 38, 53, 65, 68, 68, 68,
+		50, 38, 53, 65, 68, 68, 68, 68,
+		46, 53, 65, 68, 68, 68, 68, 68,
+		62, 65, 68, 68, 68, 68, 68, 68,
+		68, 68, 68, 68, 68, 68, 68, 68
+	},
+	{/* level 1 */
+		16, 11, 11, 16, 23, 27, 31, 30,
+		11, 12, 12, 15, 20, 23, 23, 30,
+		11, 12, 13, 16, 23, 26, 35, 47,
+		16, 15, 16, 23, 26, 37, 47, 64,
+		23, 20, 23, 26, 39, 51, 64, 64,
+		27, 23, 26, 37, 51, 64, 64, 64,
+		31, 23, 35, 47, 64, 64, 64, 64,
+		30, 30, 47, 64, 64, 64, 64, 64
+	},
+	{/* level 2 */
+		12,  8,  8, 12, 17, 21, 24, 23,
+		 8,  9,  9, 11, 15, 19, 18, 23,
+		 8,  9, 10, 12, 19, 20, 27, 36,
+		12, 11, 12, 21, 20, 28, 36, 53,
+		17, 15, 19, 20, 30, 39, 51, 59,
+		21, 19, 20, 28, 39, 51, 59, 59,
+		24, 18, 27, 36, 51, 59, 59, 59,
+		23, 23, 36, 53, 59, 59, 59, 59
+	},
+	{/* level 3 - low compression quality */
+		 8,  6,  6,  8, 12, 14, 16, 17,
+		 6,  6,  6,  8, 10, 13, 12, 15,
+		 6,  6,  7,  8, 13, 14, 18, 24,
+		 8,  8,  8, 14, 13, 19, 24, 35,
+		12, 10, 13, 13, 20, 26, 34, 39,
+		14, 13, 14, 19, 26, 34, 39, 39,
+		16, 12, 18, 24, 34, 39, 39, 39,
+		17, 15, 24, 35, 39, 39, 39, 39
+	}
+};
+
+static const unsigned char qtbl_chrominance[4][64] = {
+	{/*level 0 - high compression quality */
+		21, 25, 32, 38, 54, 68, 68, 68,
+		25, 28, 24, 38, 54, 68, 68, 68,
+		32, 24, 32, 43, 66, 68, 68, 68,
+		38, 38, 43, 53, 68, 68, 68, 68,
+		54, 54, 66, 68, 68, 68, 68, 68,
+		68, 68, 68, 68, 68, 68, 68, 68,
+		68, 68, 68, 68, 68, 68, 68, 68,
+		68, 68, 68, 68, 68, 68, 68, 68
+	},
+	{/* level 1 */
+		17, 15, 17, 21, 20, 26, 38, 48,
+		15, 19, 18, 17, 20, 26, 35, 43,
+		17, 18, 20, 22, 26, 30, 46, 53,
+		21, 17, 22, 28, 30, 39, 53, 64,
+		20, 20, 26, 30, 39, 48, 64, 64,
+		26, 26, 30, 39, 48, 63, 64, 64,
+		38, 35, 46, 53, 64, 64, 64, 64,
+		48, 43, 53, 64, 64, 64, 64, 64
+	},
+	{/* level 2 */
+		13, 11, 13, 16, 20, 20, 29, 37,
+		11, 14, 14, 14, 16, 20, 26, 32,
+		13, 14, 15, 17, 20, 23, 35, 40,
+		16, 14, 17, 21, 23, 30, 40, 50,
+		20, 16, 20, 23, 30, 37, 50, 59,
+		20, 20, 23, 30, 37, 48, 59, 59,
+		29, 26, 35, 40, 50, 59, 59, 59,
+		37, 32, 40, 50, 59, 59, 59, 59
+	},
+	{/* level 3 - low compression quality */
+		 9,  8,  9, 11, 14, 17, 19, 24,
+		 8, 10,  9, 11, 14, 13, 17, 22,
+		 9,  9, 13, 14, 13, 15, 23, 26,
+		11, 11, 14, 14, 15, 20, 26, 33,
+		14, 14, 13, 15, 20, 24, 33, 39,
+		17, 13, 15, 20, 24, 32, 39, 39,
+		19, 17, 23, 26, 33, 39, 39, 39,
+		24, 22, 26, 33, 39, 39, 39, 39
+	}
+};
+
+static const unsigned char hdctbl0[16] = {
+	0, 1, 5, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0
+};
+
+static const unsigned char hdctblg0[12] = {
+	0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 0xa, 0xb
+};
+static const unsigned char hactbl0[16] = {
+	0, 2, 1, 3, 3, 2, 4, 3, 5, 5, 4, 4, 0, 0, 1, 0x7d
+};
+static const unsigned char hactblg0[162] = {
+	0x01, 0x02, 0x03, 0x00, 0x04, 0x11, 0x05, 0x12,
+	0x21, 0x31, 0x41, 0x06, 0x13, 0x51, 0x61, 0x07,
+	0x22, 0x71, 0x14, 0x32, 0x81, 0x91, 0xa1, 0x08,
+	0x23, 0x42, 0xb1, 0xc1, 0x15, 0x52, 0xd1, 0xf0,
+	0x24, 0x33, 0x62, 0x72, 0x82, 0x09, 0x0a, 0x16,
+	0x17, 0x18, 0x19, 0x1a, 0x25, 0x26, 0x27, 0x28,
+	0x29, 0x2a, 0x34, 0x35, 0x36, 0x37, 0x38, 0x39,
+	0x3a, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48, 0x49,
+	0x4a, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58, 0x59,
+	0x5a, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68, 0x69,
+	0x6a, 0x73, 0x74, 0x75, 0x76, 0x77, 0x78, 0x79,
+	0x7a, 0x83, 0x84, 0x85, 0x86, 0x87, 0x88, 0x89,
+	0x8a, 0x92, 0x93, 0x94, 0x95, 0x96, 0x97, 0x98,
+	0x99, 0x9a, 0xa2, 0xa3, 0xa4, 0xa5, 0xa6, 0xa7,
+	0xa8, 0xa9, 0xaa, 0xb2, 0xb3, 0xb4, 0xb5, 0xb6,
+	0xb7, 0xb8, 0xb9, 0xba, 0xc2, 0xc3, 0xc4, 0xc5,
+	0xc6, 0xc7, 0xc8, 0xc9, 0xca, 0xd2, 0xd3, 0xd4,
+	0xd5, 0xd6, 0xd7, 0xd8, 0xd9, 0xda, 0xe1, 0xe2,
+	0xe3, 0xe4, 0xe5, 0xe6, 0xe7, 0xe8, 0xe9, 0xea,
+	0xf1, 0xf2, 0xf3, 0xf4, 0xf5, 0xf6, 0xf7, 0xf8,
+	0xf9, 0xfa
+};
+
+/*
+ * Fourcc downgrade schema lookup tables for 422 and 420
+ * chroma subsampling - fourcc on each position maps on the
+ * fourcc from the table fourcc_to_dwngrd_schema_id which allows
+ * to get the most suitable fourcc counterpart for the given
+ * downgraded subsampling property.
+ */
+static const u32 subs422_fourcc_dwngrd_schema[] = {
+	V4L2_PIX_FMT_NV16,
+	V4L2_PIX_FMT_NV61,
+};
+
+static const u32 subs420_fourcc_dwngrd_schema[] = {
+	V4L2_PIX_FMT_NV12,
+	V4L2_PIX_FMT_NV21,
+	V4L2_PIX_FMT_NV12,
+	V4L2_PIX_FMT_NV21,
+	V4L2_PIX_FMT_NV12,
+	V4L2_PIX_FMT_NV21,
+	V4L2_PIX_FMT_GREY,
+	V4L2_PIX_FMT_GREY,
+	V4L2_PIX_FMT_GREY,
+	V4L2_PIX_FMT_GREY,
+};
+
+/*
+ * Lookup table for translation of a fourcc to the position
+ * of its downgraded counterpart in the *fourcc_dwngrd_schema
+ * tables.
+ */
+static const u32 fourcc_to_dwngrd_schema_id[] = {
+	V4L2_PIX_FMT_NV24,
+	V4L2_PIX_FMT_NV42,
+	V4L2_PIX_FMT_NV16,
+	V4L2_PIX_FMT_NV61,
+	V4L2_PIX_FMT_YUYV,
+	V4L2_PIX_FMT_YVYU,
+	V4L2_PIX_FMT_NV12,
+	V4L2_PIX_FMT_NV21,
+	V4L2_PIX_FMT_YUV420,
+	V4L2_PIX_FMT_GREY,
+};
+
+static int s5p_jpeg_get_dwngrd_sch_id_by_fourcc(u32 fourcc)
+{
+	int i;
+	for (i = 0; i < ARRAY_SIZE(fourcc_to_dwngrd_schema_id); ++i) {
+		if (fourcc_to_dwngrd_schema_id[i] == fourcc)
+			return i;
+	}
+
+	return -EINVAL;
+}
+
+static int s5p_jpeg_adjust_fourcc_to_subsampling(
+					enum v4l2_jpeg_chroma_subsampling subs,
+					u32 in_fourcc,
+					u32 *out_fourcc,
+					struct s5p_jpeg_ctx *ctx)
+{
+	int dwngrd_sch_id;
+
+	if (ctx->subsampling != V4L2_JPEG_CHROMA_SUBSAMPLING_GRAY) {
+		dwngrd_sch_id =
+			s5p_jpeg_get_dwngrd_sch_id_by_fourcc(in_fourcc);
+		if (dwngrd_sch_id < 0)
+			return -EINVAL;
+	}
+
+	switch (ctx->subsampling) {
+	case V4L2_JPEG_CHROMA_SUBSAMPLING_GRAY:
+		*out_fourcc = V4L2_PIX_FMT_GREY;
+		break;
+	case V4L2_JPEG_CHROMA_SUBSAMPLING_420:
+		if (dwngrd_sch_id >
+				ARRAY_SIZE(subs420_fourcc_dwngrd_schema) - 1)
+			return -EINVAL;
+		*out_fourcc = subs420_fourcc_dwngrd_schema[dwngrd_sch_id];
+		break;
+	case V4L2_JPEG_CHROMA_SUBSAMPLING_422:
+		if (dwngrd_sch_id >
+				ARRAY_SIZE(subs422_fourcc_dwngrd_schema) - 1)
+			return -EINVAL;
+		*out_fourcc = subs422_fourcc_dwngrd_schema[dwngrd_sch_id];
+		break;
+	default:
+		*out_fourcc = V4L2_PIX_FMT_GREY;
+		break;
+	}
+
+	return 0;
+}
+
+static int exynos4x12_decoded_subsampling[] = {
+	V4L2_JPEG_CHROMA_SUBSAMPLING_GRAY,
+	V4L2_JPEG_CHROMA_SUBSAMPLING_444,
+	V4L2_JPEG_CHROMA_SUBSAMPLING_422,
+	V4L2_JPEG_CHROMA_SUBSAMPLING_420,
+};
+
+static int exynos3250_decoded_subsampling[] = {
+	V4L2_JPEG_CHROMA_SUBSAMPLING_444,
+	V4L2_JPEG_CHROMA_SUBSAMPLING_422,
+	V4L2_JPEG_CHROMA_SUBSAMPLING_420,
+	V4L2_JPEG_CHROMA_SUBSAMPLING_GRAY,
+	-1,
+	-1,
+	V4L2_JPEG_CHROMA_SUBSAMPLING_411,
+};
+
+static inline struct s5p_jpeg_ctx *ctrl_to_ctx(struct v4l2_ctrl *c)
+{
+	return container_of(c->handler, struct s5p_jpeg_ctx, ctrl_handler);
+}
+
+static inline struct s5p_jpeg_ctx *fh_to_ctx(struct v4l2_fh *fh)
+{
+	return container_of(fh, struct s5p_jpeg_ctx, fh);
+}
+
+static int s5p_jpeg_to_user_subsampling(struct s5p_jpeg_ctx *ctx)
+{
+	WARN_ON(ctx->subsampling > 3);
+
+	switch (ctx->jpeg->variant->version) {
+	case SJPEG_S5P:
+		if (ctx->subsampling > 2)
+			return V4L2_JPEG_CHROMA_SUBSAMPLING_GRAY;
+		return ctx->subsampling;
+	case SJPEG_EXYNOS3250:
+	case SJPEG_EXYNOS5420:
+		if (ctx->subsampling > 3)
+			return V4L2_JPEG_CHROMA_SUBSAMPLING_411;
+		return exynos3250_decoded_subsampling[ctx->subsampling];
+	case SJPEG_EXYNOS4:
+	case SJPEG_EXYNOS5433:
+		if (ctx->subsampling > 2)
+			return V4L2_JPEG_CHROMA_SUBSAMPLING_420;
+		return exynos4x12_decoded_subsampling[ctx->subsampling];
+	default:
+		return V4L2_JPEG_CHROMA_SUBSAMPLING_GRAY;
+	}
+}
+
+static inline void s5p_jpeg_set_qtbl(void __iomem *regs,
+				     const unsigned char *qtbl,
+				     unsigned long tab, int len)
+{
+	int i;
+
+	for (i = 0; i < len; i++)
+		writel((unsigned int)qtbl[i], regs + tab + (i * 0x04));
+}
+
+static inline void s5p_jpeg_set_qtbl_lum(void __iomem *regs, int quality)
+{
+	/* this driver fills quantisation table 0 with data for luma */
+	s5p_jpeg_set_qtbl(regs, qtbl_luminance[quality],
+			  S5P_JPG_QTBL_CONTENT(0),
+			  ARRAY_SIZE(qtbl_luminance[quality]));
+}
+
+static inline void s5p_jpeg_set_qtbl_chr(void __iomem *regs, int quality)
+{
+	/* this driver fills quantisation table 1 with data for chroma */
+	s5p_jpeg_set_qtbl(regs, qtbl_chrominance[quality],
+			  S5P_JPG_QTBL_CONTENT(1),
+			  ARRAY_SIZE(qtbl_chrominance[quality]));
+}
+
+static inline void s5p_jpeg_set_htbl(void __iomem *regs,
+				     const unsigned char *htbl,
+				     unsigned long tab, int len)
+{
+	int i;
+
+	for (i = 0; i < len; i++)
+		writel((unsigned int)htbl[i], regs + tab + (i * 0x04));
+}
+
+static inline void s5p_jpeg_set_hdctbl(void __iomem *regs)
+{
+	/* this driver fills table 0 for this component */
+	s5p_jpeg_set_htbl(regs, hdctbl0, S5P_JPG_HDCTBL(0),
+						ARRAY_SIZE(hdctbl0));
+}
+
+static inline void s5p_jpeg_set_hdctblg(void __iomem *regs)
+{
+	/* this driver fills table 0 for this component */
+	s5p_jpeg_set_htbl(regs, hdctblg0, S5P_JPG_HDCTBLG(0),
+						ARRAY_SIZE(hdctblg0));
+}
+
+static inline void s5p_jpeg_set_hactbl(void __iomem *regs)
+{
+	/* this driver fills table 0 for this component */
+	s5p_jpeg_set_htbl(regs, hactbl0, S5P_JPG_HACTBL(0),
+						ARRAY_SIZE(hactbl0));
+}
+
+static inline void s5p_jpeg_set_hactblg(void __iomem *regs)
+{
+	/* this driver fills table 0 for this component */
+	s5p_jpeg_set_htbl(regs, hactblg0, S5P_JPG_HACTBLG(0),
+						ARRAY_SIZE(hactblg0));
+}
+
+static inline void exynos4_jpeg_set_tbl(void __iomem *regs,
+					const unsigned char *tbl,
+					unsigned long tab, int len)
+{
+	int i;
+	unsigned int dword;
+
+	for (i = 0; i < len; i += 4) {
+		dword = tbl[i] |
+			(tbl[i + 1] << 8) |
+			(tbl[i + 2] << 16) |
+			(tbl[i + 3] << 24);
+		writel(dword, regs + tab + i);
+	}
+}
+
+static inline void exynos4_jpeg_set_qtbl_lum(void __iomem *regs, int quality)
+{
+	/* this driver fills quantisation table 0 with data for luma */
+	exynos4_jpeg_set_tbl(regs, qtbl_luminance[quality],
+			     EXYNOS4_QTBL_CONTENT(0),
+			     ARRAY_SIZE(qtbl_luminance[quality]));
+}
+
+static inline void exynos4_jpeg_set_qtbl_chr(void __iomem *regs, int quality)
+{
+	/* this driver fills quantisation table 1 with data for chroma */
+	exynos4_jpeg_set_tbl(regs, qtbl_chrominance[quality],
+			     EXYNOS4_QTBL_CONTENT(1),
+			     ARRAY_SIZE(qtbl_chrominance[quality]));
+}
+
+static void exynos4_jpeg_set_huff_tbl(void __iomem *base)
+{
+	exynos4_jpeg_set_tbl(base, hdctbl0, EXYNOS4_HUFF_TBL_HDCLL,
+							ARRAY_SIZE(hdctbl0));
+	exynos4_jpeg_set_tbl(base, hdctbl0, EXYNOS4_HUFF_TBL_HDCCL,
+							ARRAY_SIZE(hdctbl0));
+	exynos4_jpeg_set_tbl(base, hdctblg0, EXYNOS4_HUFF_TBL_HDCLV,
+							ARRAY_SIZE(hdctblg0));
+	exynos4_jpeg_set_tbl(base, hdctblg0, EXYNOS4_HUFF_TBL_HDCCV,
+							ARRAY_SIZE(hdctblg0));
+	exynos4_jpeg_set_tbl(base, hactbl0, EXYNOS4_HUFF_TBL_HACLL,
+							ARRAY_SIZE(hactbl0));
+	exynos4_jpeg_set_tbl(base, hactbl0, EXYNOS4_HUFF_TBL_HACCL,
+							ARRAY_SIZE(hactbl0));
+	exynos4_jpeg_set_tbl(base, hactblg0, EXYNOS4_HUFF_TBL_HACLV,
+							ARRAY_SIZE(hactblg0));
+	exynos4_jpeg_set_tbl(base, hactblg0, EXYNOS4_HUFF_TBL_HACCV,
+							ARRAY_SIZE(hactblg0));
+}
+
+static inline int __exynos4_huff_tbl(int class, int id, bool lenval)
+{
+	/*
+	 * class: 0 - DC, 1 - AC
+	 * id: 0 - Y, 1 - Cb/Cr
+	 */
+	if (class) {
+		if (id)
+			return lenval ? EXYNOS4_HUFF_TBL_HACCL :
+				EXYNOS4_HUFF_TBL_HACCV;
+		return lenval ? EXYNOS4_HUFF_TBL_HACLL : EXYNOS4_HUFF_TBL_HACLV;
+
+	}
+	/* class == 0 */
+	if (id)
+		return lenval ? EXYNOS4_HUFF_TBL_HDCCL : EXYNOS4_HUFF_TBL_HDCCV;
+
+	return lenval ? EXYNOS4_HUFF_TBL_HDCLL : EXYNOS4_HUFF_TBL_HDCLV;
+}
+
+static inline int exynos4_huff_tbl_len(int class, int id)
+{
+	return __exynos4_huff_tbl(class, id, true);
+}
+
+static inline int exynos4_huff_tbl_val(int class, int id)
+{
+	return __exynos4_huff_tbl(class, id, false);
+}
+
+static int get_byte(struct s5p_jpeg_buffer *buf);
+static int get_word_be(struct s5p_jpeg_buffer *buf, unsigned int *word);
+static void skip(struct s5p_jpeg_buffer *buf, long len);
+
+static void exynos4_jpeg_parse_decode_h_tbl(struct s5p_jpeg_ctx *ctx)
+{
+	struct s5p_jpeg *jpeg = ctx->jpeg;
+	struct vb2_buffer *vb = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+	struct s5p_jpeg_buffer jpeg_buffer;
+	unsigned int word;
+	int c, x, components;
+
+	jpeg_buffer.size = 2; /* Ls */
+	jpeg_buffer.data =
+		(unsigned long)vb2_plane_vaddr(vb, 0) + ctx->out_q.sos + 2;
+	jpeg_buffer.curr = 0;
+
+	word = 0;
+
+	if (get_word_be(&jpeg_buffer, &word))
+		return;
+	jpeg_buffer.size = (long)word - 2;
+	jpeg_buffer.data += 2;
+	jpeg_buffer.curr = 0;
+
+	components = get_byte(&jpeg_buffer);
+	if (components == -1)
+		return;
+	while (components--) {
+		c = get_byte(&jpeg_buffer);
+		if (c == -1)
+			return;
+		x = get_byte(&jpeg_buffer);
+		if (x == -1)
+			return;
+		exynos4_jpeg_select_dec_h_tbl(jpeg->regs, c,
+					(((x >> 4) & 0x1) << 1) | (x & 0x1));
+	}
+
+}
+
+static void exynos4_jpeg_parse_huff_tbl(struct s5p_jpeg_ctx *ctx)
+{
+	struct s5p_jpeg *jpeg = ctx->jpeg;
+	struct vb2_buffer *vb = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+	struct s5p_jpeg_buffer jpeg_buffer;
+	unsigned int word;
+	int c, i, n, j;
+
+	for (j = 0; j < ctx->out_q.dht.n; ++j) {
+		jpeg_buffer.size = ctx->out_q.dht.len[j];
+		jpeg_buffer.data = (unsigned long)vb2_plane_vaddr(vb, 0) +
+				   ctx->out_q.dht.marker[j];
+		jpeg_buffer.curr = 0;
+
+		word = 0;
+		while (jpeg_buffer.curr < jpeg_buffer.size) {
+			char id, class;
+
+			c = get_byte(&jpeg_buffer);
+			if (c == -1)
+				return;
+			id = c & 0xf;
+			class = (c >> 4) & 0xf;
+			n = 0;
+			for (i = 0; i < 16; ++i) {
+				c = get_byte(&jpeg_buffer);
+				if (c == -1)
+					return;
+				word |= c << ((i % 4) * 8);
+				if ((i + 1) % 4 == 0) {
+					writel(word, jpeg->regs +
+					exynos4_huff_tbl_len(class, id) +
+					(i / 4) * 4);
+					word = 0;
+				}
+				n += c;
+			}
+			word = 0;
+			for (i = 0; i < n; ++i) {
+				c = get_byte(&jpeg_buffer);
+				if (c == -1)
+					return;
+				word |= c << ((i % 4) * 8);
+				if ((i + 1) % 4 == 0) {
+					writel(word, jpeg->regs +
+					exynos4_huff_tbl_val(class, id) +
+					(i / 4) * 4);
+					word = 0;
+				}
+			}
+			if (i % 4) {
+				writel(word, jpeg->regs +
+				exynos4_huff_tbl_val(class, id) + (i / 4) * 4);
+			}
+			word = 0;
+		}
+	}
+}
+
+static void exynos4_jpeg_parse_decode_q_tbl(struct s5p_jpeg_ctx *ctx)
+{
+	struct s5p_jpeg *jpeg = ctx->jpeg;
+	struct vb2_buffer *vb = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+	struct s5p_jpeg_buffer jpeg_buffer;
+	int c, x, components;
+
+	jpeg_buffer.size = ctx->out_q.sof_len;
+	jpeg_buffer.data =
+		(unsigned long)vb2_plane_vaddr(vb, 0) + ctx->out_q.sof;
+	jpeg_buffer.curr = 0;
+
+	skip(&jpeg_buffer, 5); /* P, Y, X */
+	components = get_byte(&jpeg_buffer);
+	if (components == -1)
+		return;
+
+	exynos4_jpeg_set_dec_components(jpeg->regs, components);
+
+	while (components--) {
+		c = get_byte(&jpeg_buffer);
+		if (c == -1)
+			return;
+		skip(&jpeg_buffer, 1);
+		x = get_byte(&jpeg_buffer);
+		if (x == -1)
+			return;
+		exynos4_jpeg_select_dec_q_tbl(jpeg->regs, c, x);
+	}
+}
+
+static void exynos4_jpeg_parse_q_tbl(struct s5p_jpeg_ctx *ctx)
+{
+	struct s5p_jpeg *jpeg = ctx->jpeg;
+	struct vb2_buffer *vb = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+	struct s5p_jpeg_buffer jpeg_buffer;
+	unsigned int word;
+	int c, i, j;
+
+	for (j = 0; j < ctx->out_q.dqt.n; ++j) {
+		jpeg_buffer.size = ctx->out_q.dqt.len[j];
+		jpeg_buffer.data = (unsigned long)vb2_plane_vaddr(vb, 0) +
+				   ctx->out_q.dqt.marker[j];
+		jpeg_buffer.curr = 0;
+
+		word = 0;
+		while (jpeg_buffer.size - jpeg_buffer.curr >= 65) {
+			char id;
+
+			c = get_byte(&jpeg_buffer);
+			if (c == -1)
+				return;
+			id = c & 0xf;
+			/* nonzero means extended mode - not supported */
+			if ((c >> 4) & 0xf)
+				return;
+			for (i = 0; i < 64; ++i) {
+				c = get_byte(&jpeg_buffer);
+				if (c == -1)
+					return;
+				word |= c << ((i % 4) * 8);
+				if ((i + 1) % 4 == 0) {
+					writel(word, jpeg->regs +
+					EXYNOS4_QTBL_CONTENT(id) + (i / 4) * 4);
+					word = 0;
+				}
+			}
+			word = 0;
+		}
+	}
+}
+
+/*
+ * ============================================================================
+ * Device file operations
+ * ============================================================================
+ */
+
+static int queue_init(void *priv, struct vb2_queue *src_vq,
+		      struct vb2_queue *dst_vq);
+static struct s5p_jpeg_fmt *s5p_jpeg_find_format(struct s5p_jpeg_ctx *ctx,
+				__u32 pixelformat, unsigned int fmt_type);
+static int s5p_jpeg_controls_create(struct s5p_jpeg_ctx *ctx);
+
+static int s5p_jpeg_open(struct file *file)
+{
+	struct s5p_jpeg *jpeg = video_drvdata(file);
+	struct video_device *vfd = video_devdata(file);
+	struct s5p_jpeg_ctx *ctx;
+	struct s5p_jpeg_fmt *out_fmt, *cap_fmt;
+	int ret = 0;
+
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx)
+		return -ENOMEM;
+
+	if (mutex_lock_interruptible(&jpeg->lock)) {
+		ret = -ERESTARTSYS;
+		goto free;
+	}
+
+	v4l2_fh_init(&ctx->fh, vfd);
+	/* Use separate control handler per file handle */
+	ctx->fh.ctrl_handler = &ctx->ctrl_handler;
+	file->private_data = &ctx->fh;
+	v4l2_fh_add(&ctx->fh);
+
+	ctx->jpeg = jpeg;
+	if (vfd == jpeg->vfd_encoder) {
+		ctx->mode = S5P_JPEG_ENCODE;
+		out_fmt = s5p_jpeg_find_format(ctx, V4L2_PIX_FMT_RGB565,
+							FMT_TYPE_OUTPUT);
+		cap_fmt = s5p_jpeg_find_format(ctx, V4L2_PIX_FMT_JPEG,
+							FMT_TYPE_CAPTURE);
+	} else {
+		ctx->mode = S5P_JPEG_DECODE;
+		out_fmt = s5p_jpeg_find_format(ctx, V4L2_PIX_FMT_JPEG,
+							FMT_TYPE_OUTPUT);
+		cap_fmt = s5p_jpeg_find_format(ctx, V4L2_PIX_FMT_YUYV,
+							FMT_TYPE_CAPTURE);
+		ctx->scale_factor = EXYNOS3250_DEC_SCALE_FACTOR_8_8;
+	}
+
+	ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(jpeg->m2m_dev, ctx, queue_init);
+	if (IS_ERR(ctx->fh.m2m_ctx)) {
+		ret = PTR_ERR(ctx->fh.m2m_ctx);
+		goto error;
+	}
+
+	ctx->out_q.fmt = out_fmt;
+	ctx->cap_q.fmt = cap_fmt;
+
+	ret = s5p_jpeg_controls_create(ctx);
+	if (ret < 0)
+		goto error;
+
+	mutex_unlock(&jpeg->lock);
+	return 0;
+
+error:
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	mutex_unlock(&jpeg->lock);
+free:
+	kfree(ctx);
+	return ret;
+}
+
+static int s5p_jpeg_release(struct file *file)
+{
+	struct s5p_jpeg *jpeg = video_drvdata(file);
+	struct s5p_jpeg_ctx *ctx = fh_to_ctx(file->private_data);
+
+	mutex_lock(&jpeg->lock);
+	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
+	v4l2_ctrl_handler_free(&ctx->ctrl_handler);
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	kfree(ctx);
+	mutex_unlock(&jpeg->lock);
+
+	return 0;
+}
+
+static const struct v4l2_file_operations s5p_jpeg_fops = {
+	.owner		= THIS_MODULE,
+	.open		= s5p_jpeg_open,
+	.release	= s5p_jpeg_release,
+	.poll		= v4l2_m2m_fop_poll,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap		= v4l2_m2m_fop_mmap,
+};
+
+/*
+ * ============================================================================
+ * video ioctl operations
+ * ============================================================================
+ */
+
+static int get_byte(struct s5p_jpeg_buffer *buf)
+{
+	if (buf->curr >= buf->size)
+		return -1;
+
+	return ((unsigned char *)buf->data)[buf->curr++];
+}
+
+static int get_word_be(struct s5p_jpeg_buffer *buf, unsigned int *word)
+{
+	unsigned int temp;
+	int byte;
+
+	byte = get_byte(buf);
+	if (byte == -1)
+		return -1;
+	temp = byte << 8;
+	byte = get_byte(buf);
+	if (byte == -1)
+		return -1;
+	*word = (unsigned int)byte | temp;
+	return 0;
+}
+
+static void skip(struct s5p_jpeg_buffer *buf, long len)
+{
+	if (len <= 0)
+		return;
+
+	while (len--)
+		get_byte(buf);
+}
+
+static bool s5p_jpeg_parse_hdr(struct s5p_jpeg_q_data *result,
+			       unsigned long buffer, unsigned long size,
+			       struct s5p_jpeg_ctx *ctx)
+{
+	int c, components = 0, notfound, n_dht = 0, n_dqt = 0;
+	unsigned int height = 0, width = 0, word, subsampling = 0;
+	unsigned int sos = 0, sof = 0, sof_len = 0;
+	unsigned int dht[S5P_JPEG_MAX_MARKER], dht_len[S5P_JPEG_MAX_MARKER];
+	unsigned int dqt[S5P_JPEG_MAX_MARKER], dqt_len[S5P_JPEG_MAX_MARKER];
+	long length;
+	struct s5p_jpeg_buffer jpeg_buffer;
+
+	jpeg_buffer.size = size;
+	jpeg_buffer.data = buffer;
+	jpeg_buffer.curr = 0;
+
+	notfound = 1;
+	while (notfound || !sos) {
+		c = get_byte(&jpeg_buffer);
+		if (c == -1)
+			return false;
+		if (c != 0xff)
+			continue;
+		do
+			c = get_byte(&jpeg_buffer);
+		while (c == 0xff);
+		if (c == -1)
+			return false;
+		if (c == 0)
+			continue;
+		length = 0;
+		switch (c) {
+		/* SOF0: baseline JPEG */
+		case SOF0:
+			if (get_word_be(&jpeg_buffer, &word))
+				break;
+			length = (long)word - 2;
+			if (!length)
+				return false;
+			sof = jpeg_buffer.curr; /* after 0xffc0 */
+			sof_len = length;
+			if (get_byte(&jpeg_buffer) == -1)
+				break;
+			if (get_word_be(&jpeg_buffer, &height))
+				break;
+			if (get_word_be(&jpeg_buffer, &width))
+				break;
+			components = get_byte(&jpeg_buffer);
+			if (components == -1)
+				break;
+
+			if (components == 1) {
+				subsampling = 0x33;
+			} else {
+				skip(&jpeg_buffer, 1);
+				subsampling = get_byte(&jpeg_buffer);
+				skip(&jpeg_buffer, 1);
+			}
+			if (components > 3)
+				return false;
+			skip(&jpeg_buffer, components * 2);
+			notfound = 0;
+			break;
+
+		case DQT:
+			if (get_word_be(&jpeg_buffer, &word))
+				break;
+			length = (long)word - 2;
+			if (!length)
+				return false;
+			if (n_dqt >= S5P_JPEG_MAX_MARKER)
+				return false;
+			dqt[n_dqt] = jpeg_buffer.curr; /* after 0xffdb */
+			dqt_len[n_dqt++] = length;
+			skip(&jpeg_buffer, length);
+			break;
+
+		case DHT:
+			if (get_word_be(&jpeg_buffer, &word))
+				break;
+			length = (long)word - 2;
+			if (!length)
+				return false;
+			if (n_dht >= S5P_JPEG_MAX_MARKER)
+				return false;
+			dht[n_dht] = jpeg_buffer.curr; /* after 0xffc4 */
+			dht_len[n_dht++] = length;
+			skip(&jpeg_buffer, length);
+			break;
+
+		case SOS:
+			sos = jpeg_buffer.curr - 2; /* 0xffda */
+			break;
+
+		/* skip payload-less markers */
+		case RST ... RST + 7:
+		case SOI:
+		case EOI:
+		case TEM:
+			break;
+
+		/* skip uninteresting payload markers */
+		default:
+			if (get_word_be(&jpeg_buffer, &word))
+				break;
+			length = (long)word - 2;
+			skip(&jpeg_buffer, length);
+			break;
+		}
+	}
+	result->w = width;
+	result->h = height;
+	result->sos = sos;
+	result->dht.n = n_dht;
+	while (n_dht--) {
+		result->dht.marker[n_dht] = dht[n_dht];
+		result->dht.len[n_dht] = dht_len[n_dht];
+	}
+	result->dqt.n = n_dqt;
+	while (n_dqt--) {
+		result->dqt.marker[n_dqt] = dqt[n_dqt];
+		result->dqt.len[n_dqt] = dqt_len[n_dqt];
+	}
+	result->sof = sof;
+	result->sof_len = sof_len;
+	result->size = result->components = components;
+
+	switch (subsampling) {
+	case 0x11:
+		ctx->subsampling = V4L2_JPEG_CHROMA_SUBSAMPLING_444;
+		break;
+	case 0x21:
+		ctx->subsampling = V4L2_JPEG_CHROMA_SUBSAMPLING_422;
+		break;
+	case 0x22:
+		ctx->subsampling = V4L2_JPEG_CHROMA_SUBSAMPLING_420;
+		break;
+	case 0x33:
+		ctx->subsampling = V4L2_JPEG_CHROMA_SUBSAMPLING_GRAY;
+		break;
+	default:
+		return false;
+	}
+
+	return !notfound && sos;
+}
+
+static int s5p_jpeg_querycap(struct file *file, void *priv,
+			   struct v4l2_capability *cap)
+{
+	struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
+
+	if (ctx->mode == S5P_JPEG_ENCODE) {
+		strlcpy(cap->driver, S5P_JPEG_M2M_NAME " encoder",
+			sizeof(cap->driver));
+		strlcpy(cap->card, S5P_JPEG_M2M_NAME " encoder",
+			sizeof(cap->card));
+	} else {
+		strlcpy(cap->driver, S5P_JPEG_M2M_NAME " decoder",
+			sizeof(cap->driver));
+		strlcpy(cap->card, S5P_JPEG_M2M_NAME " decoder",
+			sizeof(cap->card));
+	}
+	cap->bus_info[0] = 0;
+	cap->device_caps = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_M2M;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	return 0;
+}
+
+static int enum_fmt(struct s5p_jpeg_fmt *sjpeg_formats, int n,
+		    struct v4l2_fmtdesc *f, u32 type)
+{
+	int i, num = 0;
+
+	for (i = 0; i < n; ++i) {
+		if (sjpeg_formats[i].flags & type) {
+			/* index-th format of type type found ? */
+			if (num == f->index)
+				break;
+			/* Correct type but haven't reached our index yet,
+			 * just increment per-type index */
+			++num;
+		}
+	}
+
+	/* Format not found */
+	if (i >= n)
+		return -EINVAL;
+
+	strlcpy(f->description, sjpeg_formats[i].name, sizeof(f->description));
+	f->pixelformat = sjpeg_formats[i].fourcc;
+
+	return 0;
+}
+
+static int s5p_jpeg_enum_fmt_vid_cap(struct file *file, void *priv,
+				   struct v4l2_fmtdesc *f)
+{
+	struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
+
+	if (ctx->mode == S5P_JPEG_ENCODE)
+		return enum_fmt(sjpeg_formats, SJPEG_NUM_FORMATS, f,
+				SJPEG_FMT_FLAG_ENC_CAPTURE);
+
+	return enum_fmt(sjpeg_formats, SJPEG_NUM_FORMATS, f,
+					SJPEG_FMT_FLAG_DEC_CAPTURE);
+}
+
+static int s5p_jpeg_enum_fmt_vid_out(struct file *file, void *priv,
+				   struct v4l2_fmtdesc *f)
+{
+	struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
+
+	if (ctx->mode == S5P_JPEG_ENCODE)
+		return enum_fmt(sjpeg_formats, SJPEG_NUM_FORMATS, f,
+				SJPEG_FMT_FLAG_ENC_OUTPUT);
+
+	return enum_fmt(sjpeg_formats, SJPEG_NUM_FORMATS, f,
+					SJPEG_FMT_FLAG_DEC_OUTPUT);
+}
+
+static struct s5p_jpeg_q_data *get_q_data(struct s5p_jpeg_ctx *ctx,
+					  enum v4l2_buf_type type)
+{
+	if (type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		return &ctx->out_q;
+	if (type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return &ctx->cap_q;
+
+	return NULL;
+}
+
+static int s5p_jpeg_g_fmt(struct file *file, void *priv, struct v4l2_format *f)
+{
+	struct vb2_queue *vq;
+	struct s5p_jpeg_q_data *q_data = NULL;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct s5p_jpeg_ctx *ct = fh_to_ctx(priv);
+
+	vq = v4l2_m2m_get_vq(ct->fh.m2m_ctx, f->type);
+	if (!vq)
+		return -EINVAL;
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+	    ct->mode == S5P_JPEG_DECODE && !ct->hdr_parsed)
+		return -EINVAL;
+	q_data = get_q_data(ct, f->type);
+	BUG_ON(q_data == NULL);
+
+	pix->width = q_data->w;
+	pix->height = q_data->h;
+	pix->field = V4L2_FIELD_NONE;
+	pix->pixelformat = q_data->fmt->fourcc;
+	pix->bytesperline = 0;
+	if (q_data->fmt->fourcc != V4L2_PIX_FMT_JPEG) {
+		u32 bpl = q_data->w;
+		if (q_data->fmt->colplanes == 1)
+			bpl = (bpl * q_data->fmt->depth) >> 3;
+		pix->bytesperline = bpl;
+	}
+	pix->sizeimage = q_data->size;
+
+	return 0;
+}
+
+static struct s5p_jpeg_fmt *s5p_jpeg_find_format(struct s5p_jpeg_ctx *ctx,
+				u32 pixelformat, unsigned int fmt_type)
+{
+	unsigned int k, fmt_flag;
+
+	if (ctx->mode == S5P_JPEG_ENCODE)
+		fmt_flag = (fmt_type == FMT_TYPE_OUTPUT) ?
+				SJPEG_FMT_FLAG_ENC_OUTPUT :
+				SJPEG_FMT_FLAG_ENC_CAPTURE;
+	else
+		fmt_flag = (fmt_type == FMT_TYPE_OUTPUT) ?
+				SJPEG_FMT_FLAG_DEC_OUTPUT :
+				SJPEG_FMT_FLAG_DEC_CAPTURE;
+
+	for (k = 0; k < ARRAY_SIZE(sjpeg_formats); k++) {
+		struct s5p_jpeg_fmt *fmt = &sjpeg_formats[k];
+		if (fmt->fourcc == pixelformat &&
+		    fmt->flags & fmt_flag &&
+		    fmt->flags & ctx->jpeg->variant->fmt_ver_flag) {
+			return fmt;
+		}
+	}
+
+	return NULL;
+}
+
+static void jpeg_bound_align_image(struct s5p_jpeg_ctx *ctx,
+				   u32 *w, unsigned int wmin, unsigned int wmax,
+				   unsigned int walign,
+				   u32 *h, unsigned int hmin, unsigned int hmax,
+				   unsigned int halign)
+{
+	int width, height, w_step, h_step;
+
+	width = *w;
+	height = *h;
+
+	w_step = 1 << walign;
+	h_step = 1 << halign;
+
+	if (ctx->jpeg->variant->hw3250_compat) {
+		/*
+		 * Rightmost and bottommost pixels are cropped by the
+		 * Exynos3250/compatible JPEG IP for RGB formats, for the
+		 * specific width and height values respectively. This
+		 * assignment will result in v4l_bound_align_image returning
+		 * dimensions reduced by 1 for the aforementioned cases.
+		 */
+		if (w_step == 4 && ((width & 3) == 1)) {
+			wmax = width;
+			hmax = height;
+		}
+	}
+
+	v4l_bound_align_image(w, wmin, wmax, walign, h, hmin, hmax, halign, 0);
+
+	if (*w < width && (*w + w_step) < wmax)
+		*w += w_step;
+	if (*h < height && (*h + h_step) < hmax)
+		*h += h_step;
+}
+
+static int vidioc_try_fmt(struct v4l2_format *f, struct s5p_jpeg_fmt *fmt,
+			  struct s5p_jpeg_ctx *ctx, int q_type)
+{
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+
+	if (pix->field == V4L2_FIELD_ANY)
+		pix->field = V4L2_FIELD_NONE;
+	else if (pix->field != V4L2_FIELD_NONE)
+		return -EINVAL;
+
+	/* V4L2 specification suggests the driver corrects the format struct
+	 * if any of the dimensions is unsupported */
+	if (q_type == FMT_TYPE_OUTPUT)
+		jpeg_bound_align_image(ctx, &pix->width, S5P_JPEG_MIN_WIDTH,
+				       S5P_JPEG_MAX_WIDTH, 0,
+				       &pix->height, S5P_JPEG_MIN_HEIGHT,
+				       S5P_JPEG_MAX_HEIGHT, 0);
+	else
+		jpeg_bound_align_image(ctx, &pix->width, S5P_JPEG_MIN_WIDTH,
+				       S5P_JPEG_MAX_WIDTH, fmt->h_align,
+				       &pix->height, S5P_JPEG_MIN_HEIGHT,
+				       S5P_JPEG_MAX_HEIGHT, fmt->v_align);
+
+	if (fmt->fourcc == V4L2_PIX_FMT_JPEG) {
+		if (pix->sizeimage <= 0)
+			pix->sizeimage = PAGE_SIZE;
+		pix->bytesperline = 0;
+	} else {
+		u32 bpl = pix->bytesperline;
+
+		if (fmt->colplanes > 1 && bpl < pix->width)
+			bpl = pix->width; /* planar */
+
+		if (fmt->colplanes == 1 && /* packed */
+		    (bpl << 3) / fmt->depth < pix->width)
+			bpl = (pix->width * fmt->depth) >> 3;
+
+		pix->bytesperline = bpl;
+		pix->sizeimage = (pix->width * pix->height * fmt->depth) >> 3;
+	}
+
+	return 0;
+}
+
+static int s5p_jpeg_try_fmt_vid_cap(struct file *file, void *priv,
+				  struct v4l2_format *f)
+{
+	struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct s5p_jpeg_fmt *fmt;
+	int ret;
+
+	fmt = s5p_jpeg_find_format(ctx, f->fmt.pix.pixelformat,
+						FMT_TYPE_CAPTURE);
+	if (!fmt) {
+		v4l2_err(&ctx->jpeg->v4l2_dev,
+			 "Fourcc format (0x%08x) invalid.\n",
+			 f->fmt.pix.pixelformat);
+		return -EINVAL;
+	}
+
+	if (!ctx->jpeg->variant->hw_ex4_compat || ctx->mode != S5P_JPEG_DECODE)
+		goto exit;
+
+	/*
+	 * The exynos4x12 device requires resulting YUV image
+	 * subsampling not to be lower than the input jpeg subsampling.
+	 * If this requirement is not met then downgrade the requested
+	 * capture format to the one with subsampling equal to the input jpeg.
+	 */
+	if ((fmt->flags & SJPEG_FMT_NON_RGB) &&
+	    (fmt->subsampling < ctx->subsampling)) {
+		ret = s5p_jpeg_adjust_fourcc_to_subsampling(ctx->subsampling,
+							    fmt->fourcc,
+							    &pix->pixelformat,
+							    ctx);
+		if (ret < 0)
+			pix->pixelformat = V4L2_PIX_FMT_GREY;
+
+		fmt = s5p_jpeg_find_format(ctx, pix->pixelformat,
+							FMT_TYPE_CAPTURE);
+	}
+
+	/*
+	 * Decompression of a JPEG file with 4:2:0 subsampling and odd
+	 * width to the YUV 4:2:0 compliant formats produces a raw image
+	 * with broken luma component. Adjust capture format to RGB565
+	 * in such a case.
+	 */
+	if (ctx->subsampling == V4L2_JPEG_CHROMA_SUBSAMPLING_420 &&
+	    (ctx->out_q.w & 1) &&
+	    (pix->pixelformat == V4L2_PIX_FMT_NV12 ||
+	     pix->pixelformat == V4L2_PIX_FMT_NV21 ||
+	     pix->pixelformat == V4L2_PIX_FMT_YUV420)) {
+		pix->pixelformat = V4L2_PIX_FMT_RGB565;
+		fmt = s5p_jpeg_find_format(ctx, pix->pixelformat,
+							FMT_TYPE_CAPTURE);
+	}
+
+exit:
+	return vidioc_try_fmt(f, fmt, ctx, FMT_TYPE_CAPTURE);
+}
+
+static int s5p_jpeg_try_fmt_vid_out(struct file *file, void *priv,
+				  struct v4l2_format *f)
+{
+	struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
+	struct s5p_jpeg_fmt *fmt;
+
+	fmt = s5p_jpeg_find_format(ctx, f->fmt.pix.pixelformat,
+						FMT_TYPE_OUTPUT);
+	if (!fmt) {
+		v4l2_err(&ctx->jpeg->v4l2_dev,
+			 "Fourcc format (0x%08x) invalid.\n",
+			 f->fmt.pix.pixelformat);
+		return -EINVAL;
+	}
+
+	return vidioc_try_fmt(f, fmt, ctx, FMT_TYPE_OUTPUT);
+}
+
+static int exynos4_jpeg_get_output_buffer_size(struct s5p_jpeg_ctx *ctx,
+						struct v4l2_format *f,
+						int fmt_depth)
+{
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	u32 pix_fmt = f->fmt.pix.pixelformat;
+	int w = pix->width, h = pix->height, wh_align;
+
+	if (pix_fmt == V4L2_PIX_FMT_RGB32 ||
+	    pix_fmt == V4L2_PIX_FMT_NV24 ||
+	    pix_fmt == V4L2_PIX_FMT_NV42 ||
+	    pix_fmt == V4L2_PIX_FMT_NV12 ||
+	    pix_fmt == V4L2_PIX_FMT_NV21 ||
+	    pix_fmt == V4L2_PIX_FMT_YUV420)
+		wh_align = 4;
+	else
+		wh_align = 1;
+
+	jpeg_bound_align_image(ctx, &w, S5P_JPEG_MIN_WIDTH,
+			       S5P_JPEG_MAX_WIDTH, wh_align,
+			       &h, S5P_JPEG_MIN_HEIGHT,
+			       S5P_JPEG_MAX_HEIGHT, wh_align);
+
+	return w * h * fmt_depth >> 3;
+}
+
+static int exynos3250_jpeg_try_downscale(struct s5p_jpeg_ctx *ctx,
+				   struct v4l2_rect *r);
+
+static int s5p_jpeg_s_fmt(struct s5p_jpeg_ctx *ct, struct v4l2_format *f)
+{
+	struct vb2_queue *vq;
+	struct s5p_jpeg_q_data *q_data = NULL;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct v4l2_ctrl *ctrl_subs;
+	struct v4l2_rect scale_rect;
+	unsigned int f_type;
+
+	vq = v4l2_m2m_get_vq(ct->fh.m2m_ctx, f->type);
+	if (!vq)
+		return -EINVAL;
+
+	q_data = get_q_data(ct, f->type);
+	BUG_ON(q_data == NULL);
+
+	if (vb2_is_busy(vq)) {
+		v4l2_err(&ct->jpeg->v4l2_dev, "%s queue busy\n", __func__);
+		return -EBUSY;
+	}
+
+	f_type = V4L2_TYPE_IS_OUTPUT(f->type) ?
+			FMT_TYPE_OUTPUT : FMT_TYPE_CAPTURE;
+
+	q_data->fmt = s5p_jpeg_find_format(ct, pix->pixelformat, f_type);
+	q_data->w = pix->width;
+	q_data->h = pix->height;
+	if (q_data->fmt->fourcc != V4L2_PIX_FMT_JPEG) {
+		/*
+		 * During encoding Exynos4x12 SoCs access wider memory area
+		 * than it results from Image_x and Image_y values written to
+		 * the JPEG_IMAGE_SIZE register. In order to avoid sysmmu
+		 * page fault calculate proper buffer size in such a case.
+		 */
+		if (ct->jpeg->variant->hw_ex4_compat &&
+		    f_type == FMT_TYPE_OUTPUT && ct->mode == S5P_JPEG_ENCODE)
+			q_data->size = exynos4_jpeg_get_output_buffer_size(ct,
+							f,
+							q_data->fmt->depth);
+		else
+			q_data->size = q_data->w * q_data->h *
+						q_data->fmt->depth >> 3;
+	} else {
+		q_data->size = pix->sizeimage;
+	}
+
+	if (f_type == FMT_TYPE_OUTPUT) {
+		ctrl_subs = v4l2_ctrl_find(&ct->ctrl_handler,
+					V4L2_CID_JPEG_CHROMA_SUBSAMPLING);
+		if (ctrl_subs)
+			v4l2_ctrl_s_ctrl(ctrl_subs, q_data->fmt->subsampling);
+		ct->crop_altered = false;
+	}
+
+	/*
+	 * For decoding init crop_rect with capture buffer dimmensions which
+	 * contain aligned dimensions of the input JPEG image and do it only
+	 * if crop rectangle hasn't been altered by the user space e.g. with
+	 * S_SELECTION ioctl. For encoding assign output buffer dimensions.
+	 */
+	if (!ct->crop_altered &&
+	    ((ct->mode == S5P_JPEG_DECODE && f_type == FMT_TYPE_CAPTURE) ||
+	     (ct->mode == S5P_JPEG_ENCODE && f_type == FMT_TYPE_OUTPUT))) {
+		ct->crop_rect.width = pix->width;
+		ct->crop_rect.height = pix->height;
+	}
+
+	/*
+	 * Prevent downscaling to YUV420 format by more than 2
+	 * for Exynos3250/compatible SoC as it produces broken raw image
+	 * in such cases.
+	 */
+	if (ct->mode == S5P_JPEG_DECODE &&
+	    f_type == FMT_TYPE_CAPTURE &&
+	    ct->jpeg->variant->hw3250_compat &&
+	    pix->pixelformat == V4L2_PIX_FMT_YUV420 &&
+	    ct->scale_factor > 2) {
+		scale_rect.width = ct->out_q.w / 2;
+		scale_rect.height = ct->out_q.h / 2;
+		exynos3250_jpeg_try_downscale(ct, &scale_rect);
+	}
+
+	return 0;
+}
+
+static int s5p_jpeg_s_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	int ret;
+
+	ret = s5p_jpeg_try_fmt_vid_cap(file, priv, f);
+	if (ret)
+		return ret;
+
+	return s5p_jpeg_s_fmt(fh_to_ctx(priv), f);
+}
+
+static int s5p_jpeg_s_fmt_vid_out(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	int ret;
+
+	ret = s5p_jpeg_try_fmt_vid_out(file, priv, f);
+	if (ret)
+		return ret;
+
+	return s5p_jpeg_s_fmt(fh_to_ctx(priv), f);
+}
+
+static int exynos3250_jpeg_try_downscale(struct s5p_jpeg_ctx *ctx,
+				   struct v4l2_rect *r)
+{
+	int w_ratio, h_ratio, scale_factor, cur_ratio, i;
+
+	w_ratio = ctx->out_q.w / r->width;
+	h_ratio = ctx->out_q.h / r->height;
+
+	scale_factor = w_ratio > h_ratio ? w_ratio : h_ratio;
+	scale_factor = clamp_val(scale_factor, 1, 8);
+
+	/* Align scale ratio to the nearest power of 2 */
+	for (i = 0; i <= 3; ++i) {
+		cur_ratio = 1 << i;
+		if (scale_factor <= cur_ratio) {
+			ctx->scale_factor = cur_ratio;
+			break;
+		}
+	}
+
+	r->width = round_down(ctx->out_q.w / ctx->scale_factor, 2);
+	r->height = round_down(ctx->out_q.h / ctx->scale_factor, 2);
+
+	ctx->crop_rect.width = r->width;
+	ctx->crop_rect.height = r->height;
+	ctx->crop_rect.left = 0;
+	ctx->crop_rect.top = 0;
+
+	ctx->crop_altered = true;
+
+	return 0;
+}
+
+/* Return 1 if rectangle a is enclosed in rectangle b, or 0 otherwise. */
+static int enclosed_rectangle(struct v4l2_rect *a, struct v4l2_rect *b)
+{
+	if (a->left < b->left || a->top < b->top)
+		return 0;
+	if (a->left + a->width > b->left + b->width)
+		return 0;
+	if (a->top + a->height > b->top + b->height)
+		return 0;
+
+	return 1;
+}
+
+static int exynos3250_jpeg_try_crop(struct s5p_jpeg_ctx *ctx,
+				   struct v4l2_rect *r)
+{
+	struct v4l2_rect base_rect;
+	int w_step, h_step;
+
+	switch (ctx->cap_q.fmt->fourcc) {
+	case V4L2_PIX_FMT_NV12:
+	case V4L2_PIX_FMT_NV21:
+		w_step = 1;
+		h_step = 2;
+		break;
+	case V4L2_PIX_FMT_YUV420:
+		w_step = 2;
+		h_step = 2;
+		break;
+	default:
+		w_step = 1;
+		h_step = 1;
+		break;
+	}
+
+	base_rect.top = 0;
+	base_rect.left = 0;
+	base_rect.width = ctx->out_q.w;
+	base_rect.height = ctx->out_q.h;
+
+	r->width = round_down(r->width, w_step);
+	r->height = round_down(r->height, h_step);
+	r->left = round_down(r->left, 2);
+	r->top = round_down(r->top, 2);
+
+	if (!enclosed_rectangle(r, &base_rect))
+		return -EINVAL;
+
+	ctx->crop_rect.left = r->left;
+	ctx->crop_rect.top = r->top;
+	ctx->crop_rect.width = r->width;
+	ctx->crop_rect.height = r->height;
+
+	ctx->crop_altered = true;
+
+	return 0;
+}
+
+/*
+ * V4L2 controls
+ */
+
+static int s5p_jpeg_g_selection(struct file *file, void *priv,
+			 struct v4l2_selection *s)
+{
+	struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
+
+	if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT &&
+	    s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	/* For JPEG blob active == default == bounds */
+	switch (s->target) {
+	case V4L2_SEL_TGT_CROP:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+		s->r.width = ctx->out_q.w;
+		s->r.height = ctx->out_q.h;
+		s->r.left = 0;
+		s->r.top = 0;
+		break;
+	case V4L2_SEL_TGT_COMPOSE:
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+	case V4L2_SEL_TGT_COMPOSE_PADDED:
+		s->r.width = ctx->crop_rect.width;
+		s->r.height =  ctx->crop_rect.height;
+		s->r.left = ctx->crop_rect.left;
+		s->r.top = ctx->crop_rect.top;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/*
+ * V4L2 controls
+ */
+static int s5p_jpeg_s_selection(struct file *file, void *fh,
+				  struct v4l2_selection *s)
+{
+	struct s5p_jpeg_ctx *ctx = fh_to_ctx(file->private_data);
+	struct v4l2_rect *rect = &s->r;
+	int ret = -EINVAL;
+
+	if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	if (s->target == V4L2_SEL_TGT_COMPOSE) {
+		if (ctx->mode != S5P_JPEG_DECODE)
+			return -EINVAL;
+		if (ctx->jpeg->variant->hw3250_compat)
+			ret = exynos3250_jpeg_try_downscale(ctx, rect);
+	} else if (s->target == V4L2_SEL_TGT_CROP) {
+		if (ctx->mode != S5P_JPEG_ENCODE)
+			return -EINVAL;
+		if (ctx->jpeg->variant->hw3250_compat)
+			ret = exynos3250_jpeg_try_crop(ctx, rect);
+	}
+
+	return ret;
+}
+
+static int s5p_jpeg_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct s5p_jpeg_ctx *ctx = ctrl_to_ctx(ctrl);
+	struct s5p_jpeg *jpeg = ctx->jpeg;
+	unsigned long flags;
+
+	switch (ctrl->id) {
+	case V4L2_CID_JPEG_CHROMA_SUBSAMPLING:
+		spin_lock_irqsave(&jpeg->slock, flags);
+		ctrl->val = s5p_jpeg_to_user_subsampling(ctx);
+		spin_unlock_irqrestore(&jpeg->slock, flags);
+		break;
+	}
+
+	return 0;
+}
+
+static int s5p_jpeg_adjust_subs_ctrl(struct s5p_jpeg_ctx *ctx, int *ctrl_val)
+{
+	switch (ctx->jpeg->variant->version) {
+	case SJPEG_S5P:
+		return 0;
+	case SJPEG_EXYNOS3250:
+	case SJPEG_EXYNOS5420:
+		/*
+		 * The exynos3250/compatible device can produce JPEG image only
+		 * of 4:4:4 subsampling when given RGB32 source image.
+		 */
+		if (ctx->out_q.fmt->fourcc == V4L2_PIX_FMT_RGB32)
+			*ctrl_val = 0;
+		break;
+	case SJPEG_EXYNOS4:
+		/*
+		 * The exynos4x12 device requires input raw image fourcc
+		 * to be V4L2_PIX_FMT_GREY if gray jpeg format
+		 * is to be set.
+		 */
+		if (ctx->out_q.fmt->fourcc != V4L2_PIX_FMT_GREY &&
+		    *ctrl_val == V4L2_JPEG_CHROMA_SUBSAMPLING_GRAY)
+			return -EINVAL;
+		break;
+	}
+
+	/*
+	 * The exynos4x12 and exynos3250/compatible devices require resulting
+	 * jpeg subsampling not to be lower than the input raw image
+	 * subsampling.
+	 */
+	if (ctx->out_q.fmt->subsampling > *ctrl_val)
+		*ctrl_val = ctx->out_q.fmt->subsampling;
+
+	return 0;
+}
+
+static int s5p_jpeg_try_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct s5p_jpeg_ctx *ctx = ctrl_to_ctx(ctrl);
+	unsigned long flags;
+	int ret = 0;
+
+	spin_lock_irqsave(&ctx->jpeg->slock, flags);
+
+	if (ctrl->id == V4L2_CID_JPEG_CHROMA_SUBSAMPLING)
+		ret = s5p_jpeg_adjust_subs_ctrl(ctx, &ctrl->val);
+
+	spin_unlock_irqrestore(&ctx->jpeg->slock, flags);
+	return ret;
+}
+
+static int s5p_jpeg_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct s5p_jpeg_ctx *ctx = ctrl_to_ctx(ctrl);
+	unsigned long flags;
+
+	spin_lock_irqsave(&ctx->jpeg->slock, flags);
+
+	switch (ctrl->id) {
+	case V4L2_CID_JPEG_COMPRESSION_QUALITY:
+		ctx->compr_quality = ctrl->val;
+		break;
+	case V4L2_CID_JPEG_RESTART_INTERVAL:
+		ctx->restart_interval = ctrl->val;
+		break;
+	case V4L2_CID_JPEG_CHROMA_SUBSAMPLING:
+		ctx->subsampling = ctrl->val;
+		break;
+	}
+
+	spin_unlock_irqrestore(&ctx->jpeg->slock, flags);
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops s5p_jpeg_ctrl_ops = {
+	.g_volatile_ctrl	= s5p_jpeg_g_volatile_ctrl,
+	.try_ctrl		= s5p_jpeg_try_ctrl,
+	.s_ctrl			= s5p_jpeg_s_ctrl,
+};
+
+static int s5p_jpeg_controls_create(struct s5p_jpeg_ctx *ctx)
+{
+	unsigned int mask = ~0x27; /* 444, 422, 420, GRAY */
+	struct v4l2_ctrl *ctrl;
+	int ret;
+
+	v4l2_ctrl_handler_init(&ctx->ctrl_handler, 3);
+
+	if (ctx->mode == S5P_JPEG_ENCODE) {
+		v4l2_ctrl_new_std(&ctx->ctrl_handler, &s5p_jpeg_ctrl_ops,
+				  V4L2_CID_JPEG_COMPRESSION_QUALITY,
+				  0, 3, 1, S5P_JPEG_COMPR_QUAL_WORST);
+
+		v4l2_ctrl_new_std(&ctx->ctrl_handler, &s5p_jpeg_ctrl_ops,
+				  V4L2_CID_JPEG_RESTART_INTERVAL,
+				  0, 3, 0xffff, 0);
+		if (ctx->jpeg->variant->version == SJPEG_S5P)
+			mask = ~0x06; /* 422, 420 */
+	}
+
+	ctrl = v4l2_ctrl_new_std_menu(&ctx->ctrl_handler, &s5p_jpeg_ctrl_ops,
+				      V4L2_CID_JPEG_CHROMA_SUBSAMPLING,
+				      V4L2_JPEG_CHROMA_SUBSAMPLING_GRAY, mask,
+				      V4L2_JPEG_CHROMA_SUBSAMPLING_422);
+
+	if (ctx->ctrl_handler.error) {
+		ret = ctx->ctrl_handler.error;
+		goto error_free;
+	}
+
+	if (ctx->mode == S5P_JPEG_DECODE)
+		ctrl->flags |= V4L2_CTRL_FLAG_VOLATILE |
+			V4L2_CTRL_FLAG_READ_ONLY;
+
+	ret = v4l2_ctrl_handler_setup(&ctx->ctrl_handler);
+	if (ret < 0)
+		goto error_free;
+
+	return ret;
+
+error_free:
+	v4l2_ctrl_handler_free(&ctx->ctrl_handler);
+	return ret;
+}
+
+static const struct v4l2_ioctl_ops s5p_jpeg_ioctl_ops = {
+	.vidioc_querycap		= s5p_jpeg_querycap,
+
+	.vidioc_enum_fmt_vid_cap	= s5p_jpeg_enum_fmt_vid_cap,
+	.vidioc_enum_fmt_vid_out	= s5p_jpeg_enum_fmt_vid_out,
+
+	.vidioc_g_fmt_vid_cap		= s5p_jpeg_g_fmt,
+	.vidioc_g_fmt_vid_out		= s5p_jpeg_g_fmt,
+
+	.vidioc_try_fmt_vid_cap		= s5p_jpeg_try_fmt_vid_cap,
+	.vidioc_try_fmt_vid_out		= s5p_jpeg_try_fmt_vid_out,
+
+	.vidioc_s_fmt_vid_cap		= s5p_jpeg_s_fmt_vid_cap,
+	.vidioc_s_fmt_vid_out		= s5p_jpeg_s_fmt_vid_out,
+
+	.vidioc_reqbufs			= v4l2_m2m_ioctl_reqbufs,
+	.vidioc_querybuf		= v4l2_m2m_ioctl_querybuf,
+	.vidioc_qbuf			= v4l2_m2m_ioctl_qbuf,
+	.vidioc_dqbuf			= v4l2_m2m_ioctl_dqbuf,
+
+	.vidioc_streamon		= v4l2_m2m_ioctl_streamon,
+	.vidioc_streamoff		= v4l2_m2m_ioctl_streamoff,
+
+	.vidioc_g_selection		= s5p_jpeg_g_selection,
+	.vidioc_s_selection		= s5p_jpeg_s_selection,
+};
+
+/*
+ * ============================================================================
+ * mem2mem callbacks
+ * ============================================================================
+ */
+
+static void s5p_jpeg_device_run(void *priv)
+{
+	struct s5p_jpeg_ctx *ctx = priv;
+	struct s5p_jpeg *jpeg = ctx->jpeg;
+	struct vb2_buffer *src_buf, *dst_buf;
+	unsigned long src_addr, dst_addr, flags;
+
+	spin_lock_irqsave(&ctx->jpeg->slock, flags);
+
+	src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+	dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
+	src_addr = vb2_dma_contig_plane_dma_addr(src_buf, 0);
+	dst_addr = vb2_dma_contig_plane_dma_addr(dst_buf, 0);
+
+	s5p_jpeg_reset(jpeg->regs);
+	s5p_jpeg_poweron(jpeg->regs);
+	s5p_jpeg_proc_mode(jpeg->regs, ctx->mode);
+	if (ctx->mode == S5P_JPEG_ENCODE) {
+		if (ctx->out_q.fmt->fourcc == V4L2_PIX_FMT_RGB565)
+			s5p_jpeg_input_raw_mode(jpeg->regs,
+							S5P_JPEG_RAW_IN_565);
+		else
+			s5p_jpeg_input_raw_mode(jpeg->regs,
+							S5P_JPEG_RAW_IN_422);
+		s5p_jpeg_subsampling_mode(jpeg->regs, ctx->subsampling);
+		s5p_jpeg_dri(jpeg->regs, ctx->restart_interval);
+		s5p_jpeg_x(jpeg->regs, ctx->out_q.w);
+		s5p_jpeg_y(jpeg->regs, ctx->out_q.h);
+		s5p_jpeg_imgadr(jpeg->regs, src_addr);
+		s5p_jpeg_jpgadr(jpeg->regs, dst_addr);
+
+		/* ultimately comes from sizeimage from userspace */
+		s5p_jpeg_enc_stream_int(jpeg->regs, ctx->cap_q.size);
+
+		/* JPEG RGB to YCbCr conversion matrix */
+		s5p_jpeg_coef(jpeg->regs, 1, 1, S5P_JPEG_COEF11);
+		s5p_jpeg_coef(jpeg->regs, 1, 2, S5P_JPEG_COEF12);
+		s5p_jpeg_coef(jpeg->regs, 1, 3, S5P_JPEG_COEF13);
+		s5p_jpeg_coef(jpeg->regs, 2, 1, S5P_JPEG_COEF21);
+		s5p_jpeg_coef(jpeg->regs, 2, 2, S5P_JPEG_COEF22);
+		s5p_jpeg_coef(jpeg->regs, 2, 3, S5P_JPEG_COEF23);
+		s5p_jpeg_coef(jpeg->regs, 3, 1, S5P_JPEG_COEF31);
+		s5p_jpeg_coef(jpeg->regs, 3, 2, S5P_JPEG_COEF32);
+		s5p_jpeg_coef(jpeg->regs, 3, 3, S5P_JPEG_COEF33);
+
+		/*
+		 * JPEG IP allows storing 4 quantization tables
+		 * We fill table 0 for luma and table 1 for chroma
+		 */
+		s5p_jpeg_set_qtbl_lum(jpeg->regs, ctx->compr_quality);
+		s5p_jpeg_set_qtbl_chr(jpeg->regs, ctx->compr_quality);
+		/* use table 0 for Y */
+		s5p_jpeg_qtbl(jpeg->regs, 1, 0);
+		/* use table 1 for Cb and Cr*/
+		s5p_jpeg_qtbl(jpeg->regs, 2, 1);
+		s5p_jpeg_qtbl(jpeg->regs, 3, 1);
+
+		/* Y, Cb, Cr use Huffman table 0 */
+		s5p_jpeg_htbl_ac(jpeg->regs, 1);
+		s5p_jpeg_htbl_dc(jpeg->regs, 1);
+		s5p_jpeg_htbl_ac(jpeg->regs, 2);
+		s5p_jpeg_htbl_dc(jpeg->regs, 2);
+		s5p_jpeg_htbl_ac(jpeg->regs, 3);
+		s5p_jpeg_htbl_dc(jpeg->regs, 3);
+	} else { /* S5P_JPEG_DECODE */
+		s5p_jpeg_rst_int_enable(jpeg->regs, true);
+		s5p_jpeg_data_num_int_enable(jpeg->regs, true);
+		s5p_jpeg_final_mcu_num_int_enable(jpeg->regs, true);
+		if (ctx->cap_q.fmt->fourcc == V4L2_PIX_FMT_YUYV)
+			s5p_jpeg_outform_raw(jpeg->regs, S5P_JPEG_RAW_OUT_422);
+		else
+			s5p_jpeg_outform_raw(jpeg->regs, S5P_JPEG_RAW_OUT_420);
+		s5p_jpeg_jpgadr(jpeg->regs, src_addr);
+		s5p_jpeg_imgadr(jpeg->regs, dst_addr);
+	}
+
+	s5p_jpeg_start(jpeg->regs);
+
+	spin_unlock_irqrestore(&ctx->jpeg->slock, flags);
+}
+
+static void exynos4_jpeg_set_img_addr(struct s5p_jpeg_ctx *ctx)
+{
+	struct s5p_jpeg *jpeg = ctx->jpeg;
+	struct s5p_jpeg_fmt *fmt;
+	struct vb2_buffer *vb;
+	struct s5p_jpeg_addr jpeg_addr = {};
+	u32 pix_size, padding_bytes = 0;
+
+	jpeg_addr.cb = 0;
+	jpeg_addr.cr = 0;
+
+	pix_size = ctx->cap_q.w * ctx->cap_q.h;
+
+	if (ctx->mode == S5P_JPEG_ENCODE) {
+		vb = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+		fmt = ctx->out_q.fmt;
+		if (ctx->out_q.w % 2 && fmt->h_align > 0)
+			padding_bytes = ctx->out_q.h;
+	} else {
+		fmt = ctx->cap_q.fmt;
+		vb = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
+	}
+
+	jpeg_addr.y = vb2_dma_contig_plane_dma_addr(vb, 0);
+
+	if (fmt->colplanes == 2) {
+		jpeg_addr.cb = jpeg_addr.y + pix_size - padding_bytes;
+	} else if (fmt->colplanes == 3) {
+		jpeg_addr.cb = jpeg_addr.y + pix_size;
+		if (fmt->fourcc == V4L2_PIX_FMT_YUV420)
+			jpeg_addr.cr = jpeg_addr.cb + pix_size / 4;
+		else
+			jpeg_addr.cr = jpeg_addr.cb + pix_size / 2;
+	}
+
+	exynos4_jpeg_set_frame_buf_address(jpeg->regs, &jpeg_addr);
+}
+
+static void exynos4_jpeg_set_jpeg_addr(struct s5p_jpeg_ctx *ctx)
+{
+	struct s5p_jpeg *jpeg = ctx->jpeg;
+	struct vb2_buffer *vb;
+	unsigned int jpeg_addr = 0;
+
+	if (ctx->mode == S5P_JPEG_ENCODE)
+		vb = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
+	else
+		vb = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+
+	jpeg_addr = vb2_dma_contig_plane_dma_addr(vb, 0);
+	if (jpeg->variant->version == SJPEG_EXYNOS5433 &&
+	    ctx->mode == S5P_JPEG_DECODE)
+		jpeg_addr += ctx->out_q.sos;
+	exynos4_jpeg_set_stream_buf_address(jpeg->regs, jpeg_addr);
+}
+
+static inline void exynos4_jpeg_set_img_fmt(void __iomem *base,
+					    unsigned int img_fmt)
+{
+	__exynos4_jpeg_set_img_fmt(base, img_fmt, SJPEG_EXYNOS4);
+}
+
+static inline void exynos5433_jpeg_set_img_fmt(void __iomem *base,
+					       unsigned int img_fmt)
+{
+	__exynos4_jpeg_set_img_fmt(base, img_fmt, SJPEG_EXYNOS5433);
+}
+
+static inline void exynos4_jpeg_set_enc_out_fmt(void __iomem *base,
+						unsigned int out_fmt)
+{
+	__exynos4_jpeg_set_enc_out_fmt(base, out_fmt, SJPEG_EXYNOS4);
+}
+
+static inline void exynos5433_jpeg_set_enc_out_fmt(void __iomem *base,
+						   unsigned int out_fmt)
+{
+	__exynos4_jpeg_set_enc_out_fmt(base, out_fmt, SJPEG_EXYNOS5433);
+}
+
+static void exynos4_jpeg_device_run(void *priv)
+{
+	struct s5p_jpeg_ctx *ctx = priv;
+	struct s5p_jpeg *jpeg = ctx->jpeg;
+	unsigned int bitstream_size;
+	unsigned long flags;
+
+	spin_lock_irqsave(&jpeg->slock, flags);
+
+	if (ctx->mode == S5P_JPEG_ENCODE) {
+		exynos4_jpeg_sw_reset(jpeg->regs);
+		exynos4_jpeg_set_interrupt(jpeg->regs, jpeg->variant->version);
+		exynos4_jpeg_set_huf_table_enable(jpeg->regs, 1);
+
+		exynos4_jpeg_set_huff_tbl(jpeg->regs);
+
+		/*
+		 * JPEG IP allows storing 4 quantization tables
+		 * We fill table 0 for luma and table 1 for chroma
+		 */
+		exynos4_jpeg_set_qtbl_lum(jpeg->regs, ctx->compr_quality);
+		exynos4_jpeg_set_qtbl_chr(jpeg->regs, ctx->compr_quality);
+
+		exynos4_jpeg_set_encode_tbl_select(jpeg->regs,
+							ctx->compr_quality);
+		exynos4_jpeg_set_stream_size(jpeg->regs, ctx->cap_q.w,
+							ctx->cap_q.h);
+
+		if (ctx->jpeg->variant->version == SJPEG_EXYNOS4) {
+			exynos4_jpeg_set_enc_out_fmt(jpeg->regs,
+						     ctx->subsampling);
+			exynos4_jpeg_set_img_fmt(jpeg->regs,
+						 ctx->out_q.fmt->fourcc);
+		} else {
+			exynos5433_jpeg_set_enc_out_fmt(jpeg->regs,
+							ctx->subsampling);
+			exynos5433_jpeg_set_img_fmt(jpeg->regs,
+						    ctx->out_q.fmt->fourcc);
+		}
+		exynos4_jpeg_set_img_addr(ctx);
+		exynos4_jpeg_set_jpeg_addr(ctx);
+		exynos4_jpeg_set_encode_hoff_cnt(jpeg->regs,
+							ctx->out_q.fmt->fourcc);
+	} else {
+		exynos4_jpeg_sw_reset(jpeg->regs);
+		exynos4_jpeg_set_interrupt(jpeg->regs,
+					   jpeg->variant->version);
+		exynos4_jpeg_set_img_addr(ctx);
+		exynos4_jpeg_set_jpeg_addr(ctx);
+
+		if (jpeg->variant->version == SJPEG_EXYNOS5433) {
+			exynos4_jpeg_parse_huff_tbl(ctx);
+			exynos4_jpeg_parse_decode_h_tbl(ctx);
+
+			exynos4_jpeg_parse_q_tbl(ctx);
+			exynos4_jpeg_parse_decode_q_tbl(ctx);
+
+			exynos4_jpeg_set_huf_table_enable(jpeg->regs, 1);
+
+			exynos4_jpeg_set_stream_size(jpeg->regs, ctx->cap_q.w,
+					ctx->cap_q.h);
+			exynos5433_jpeg_set_enc_out_fmt(jpeg->regs,
+							ctx->subsampling);
+			exynos5433_jpeg_set_img_fmt(jpeg->regs,
+						    ctx->cap_q.fmt->fourcc);
+			bitstream_size = DIV_ROUND_UP(ctx->out_q.size, 16);
+		} else {
+			exynos4_jpeg_set_img_fmt(jpeg->regs,
+						 ctx->cap_q.fmt->fourcc);
+			bitstream_size = DIV_ROUND_UP(ctx->out_q.size, 32);
+		}
+
+		exynos4_jpeg_set_dec_bitstream_size(jpeg->regs, bitstream_size);
+	}
+
+	exynos4_jpeg_set_enc_dec_mode(jpeg->regs, ctx->mode);
+
+	spin_unlock_irqrestore(&jpeg->slock, flags);
+}
+
+static void exynos3250_jpeg_set_img_addr(struct s5p_jpeg_ctx *ctx)
+{
+	struct s5p_jpeg *jpeg = ctx->jpeg;
+	struct s5p_jpeg_fmt *fmt;
+	struct vb2_buffer *vb;
+	struct s5p_jpeg_addr jpeg_addr = {};
+	u32 pix_size;
+
+	pix_size = ctx->cap_q.w * ctx->cap_q.h;
+
+	if (ctx->mode == S5P_JPEG_ENCODE) {
+		vb = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+		fmt = ctx->out_q.fmt;
+	} else {
+		vb = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
+		fmt = ctx->cap_q.fmt;
+	}
+
+	jpeg_addr.y = vb2_dma_contig_plane_dma_addr(vb, 0);
+
+	if (fmt->colplanes == 2) {
+		jpeg_addr.cb = jpeg_addr.y + pix_size;
+	} else if (fmt->colplanes == 3) {
+		jpeg_addr.cb = jpeg_addr.y + pix_size;
+		if (fmt->fourcc == V4L2_PIX_FMT_YUV420)
+			jpeg_addr.cr = jpeg_addr.cb + pix_size / 4;
+		else
+			jpeg_addr.cr = jpeg_addr.cb + pix_size / 2;
+	}
+
+	exynos3250_jpeg_imgadr(jpeg->regs, &jpeg_addr);
+}
+
+static void exynos3250_jpeg_set_jpeg_addr(struct s5p_jpeg_ctx *ctx)
+{
+	struct s5p_jpeg *jpeg = ctx->jpeg;
+	struct vb2_buffer *vb;
+	unsigned int jpeg_addr = 0;
+
+	if (ctx->mode == S5P_JPEG_ENCODE)
+		vb = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
+	else
+		vb = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+
+	jpeg_addr = vb2_dma_contig_plane_dma_addr(vb, 0);
+	exynos3250_jpeg_jpgadr(jpeg->regs, jpeg_addr);
+}
+
+static void exynos3250_jpeg_device_run(void *priv)
+{
+	struct s5p_jpeg_ctx *ctx = priv;
+	struct s5p_jpeg *jpeg = ctx->jpeg;
+	unsigned long flags;
+
+	spin_lock_irqsave(&ctx->jpeg->slock, flags);
+
+	exynos3250_jpeg_reset(jpeg->regs);
+	exynos3250_jpeg_set_dma_num(jpeg->regs);
+	exynos3250_jpeg_poweron(jpeg->regs);
+	exynos3250_jpeg_clk_set(jpeg->regs);
+	exynos3250_jpeg_proc_mode(jpeg->regs, ctx->mode);
+
+	if (ctx->mode == S5P_JPEG_ENCODE) {
+		exynos3250_jpeg_input_raw_fmt(jpeg->regs,
+					      ctx->out_q.fmt->fourcc);
+		exynos3250_jpeg_dri(jpeg->regs, ctx->restart_interval);
+
+		/*
+		 * JPEG IP allows storing 4 quantization tables
+		 * We fill table 0 for luma and table 1 for chroma
+		 */
+		s5p_jpeg_set_qtbl_lum(jpeg->regs, ctx->compr_quality);
+		s5p_jpeg_set_qtbl_chr(jpeg->regs, ctx->compr_quality);
+		/* use table 0 for Y */
+		exynos3250_jpeg_qtbl(jpeg->regs, 1, 0);
+		/* use table 1 for Cb and Cr*/
+		exynos3250_jpeg_qtbl(jpeg->regs, 2, 1);
+		exynos3250_jpeg_qtbl(jpeg->regs, 3, 1);
+
+		/*
+		 * Some SoCs require setting Huffman tables before each run
+		 */
+		if (jpeg->variant->htbl_reinit) {
+			s5p_jpeg_set_hdctbl(jpeg->regs);
+			s5p_jpeg_set_hdctblg(jpeg->regs);
+			s5p_jpeg_set_hactbl(jpeg->regs);
+			s5p_jpeg_set_hactblg(jpeg->regs);
+		}
+
+		/* Y, Cb, Cr use Huffman table 0 */
+		exynos3250_jpeg_htbl_ac(jpeg->regs, 1);
+		exynos3250_jpeg_htbl_dc(jpeg->regs, 1);
+		exynos3250_jpeg_htbl_ac(jpeg->regs, 2);
+		exynos3250_jpeg_htbl_dc(jpeg->regs, 2);
+		exynos3250_jpeg_htbl_ac(jpeg->regs, 3);
+		exynos3250_jpeg_htbl_dc(jpeg->regs, 3);
+
+		exynos3250_jpeg_set_x(jpeg->regs, ctx->crop_rect.width);
+		exynos3250_jpeg_set_y(jpeg->regs, ctx->crop_rect.height);
+		exynos3250_jpeg_stride(jpeg->regs, ctx->out_q.fmt->fourcc,
+								ctx->out_q.w);
+		exynos3250_jpeg_offset(jpeg->regs, ctx->crop_rect.left,
+							ctx->crop_rect.top);
+		exynos3250_jpeg_set_img_addr(ctx);
+		exynos3250_jpeg_set_jpeg_addr(ctx);
+		exynos3250_jpeg_subsampling_mode(jpeg->regs, ctx->subsampling);
+
+		/* ultimately comes from sizeimage from userspace */
+		exynos3250_jpeg_enc_stream_bound(jpeg->regs, ctx->cap_q.size);
+
+		if (ctx->out_q.fmt->fourcc == V4L2_PIX_FMT_RGB565 ||
+		    ctx->out_q.fmt->fourcc == V4L2_PIX_FMT_RGB565X ||
+		    ctx->out_q.fmt->fourcc == V4L2_PIX_FMT_RGB32)
+			exynos3250_jpeg_set_y16(jpeg->regs, true);
+	} else {
+		exynos3250_jpeg_set_img_addr(ctx);
+		exynos3250_jpeg_set_jpeg_addr(ctx);
+		exynos3250_jpeg_stride(jpeg->regs, ctx->cap_q.fmt->fourcc,
+								ctx->cap_q.w);
+		exynos3250_jpeg_offset(jpeg->regs, 0, 0);
+		exynos3250_jpeg_dec_scaling_ratio(jpeg->regs,
+							ctx->scale_factor);
+		exynos3250_jpeg_dec_stream_size(jpeg->regs, ctx->out_q.size);
+		exynos3250_jpeg_output_raw_fmt(jpeg->regs,
+						ctx->cap_q.fmt->fourcc);
+	}
+
+	exynos3250_jpeg_interrupts_enable(jpeg->regs);
+
+	/* JPEG RGB to YCbCr conversion matrix */
+	exynos3250_jpeg_coef(jpeg->regs, ctx->mode);
+
+	exynos3250_jpeg_set_timer(jpeg->regs, EXYNOS3250_IRQ_TIMEOUT);
+	jpeg->irq_status = 0;
+	exynos3250_jpeg_start(jpeg->regs);
+
+	spin_unlock_irqrestore(&ctx->jpeg->slock, flags);
+}
+
+static int s5p_jpeg_job_ready(void *priv)
+{
+	struct s5p_jpeg_ctx *ctx = priv;
+
+	if (ctx->mode == S5P_JPEG_DECODE)
+		return ctx->hdr_parsed;
+	return 1;
+}
+
+static void s5p_jpeg_job_abort(void *priv)
+{
+}
+
+static struct v4l2_m2m_ops s5p_jpeg_m2m_ops = {
+	.device_run	= s5p_jpeg_device_run,
+	.job_ready	= s5p_jpeg_job_ready,
+	.job_abort	= s5p_jpeg_job_abort,
+};
+
+static struct v4l2_m2m_ops exynos3250_jpeg_m2m_ops = {
+	.device_run	= exynos3250_jpeg_device_run,
+	.job_ready	= s5p_jpeg_job_ready,
+	.job_abort	= s5p_jpeg_job_abort,
+};
+
+static struct v4l2_m2m_ops exynos4_jpeg_m2m_ops = {
+	.device_run	= exynos4_jpeg_device_run,
+	.job_ready	= s5p_jpeg_job_ready,
+	.job_abort	= s5p_jpeg_job_abort,
+};
+
+/*
+ * ============================================================================
+ * Queue operations
+ * ============================================================================
+ */
+
+static int s5p_jpeg_queue_setup(struct vb2_queue *vq,
+			   const void *parg,
+			   unsigned int *nbuffers, unsigned int *nplanes,
+			   unsigned int sizes[], void *alloc_ctxs[])
+{
+	struct s5p_jpeg_ctx *ctx = vb2_get_drv_priv(vq);
+	struct s5p_jpeg_q_data *q_data = NULL;
+	unsigned int size, count = *nbuffers;
+
+	q_data = get_q_data(ctx, vq->type);
+	BUG_ON(q_data == NULL);
+
+	size = q_data->size;
+
+	/*
+	 * header is parsed during decoding and parsed information stored
+	 * in the context so we do not allow another buffer to overwrite it
+	 */
+	if (ctx->mode == S5P_JPEG_DECODE)
+		count = 1;
+
+	*nbuffers = count;
+	*nplanes = 1;
+	sizes[0] = size;
+	alloc_ctxs[0] = ctx->jpeg->alloc_ctx;
+
+	return 0;
+}
+
+static int s5p_jpeg_buf_prepare(struct vb2_buffer *vb)
+{
+	struct s5p_jpeg_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct s5p_jpeg_q_data *q_data = NULL;
+
+	q_data = get_q_data(ctx, vb->vb2_queue->type);
+	BUG_ON(q_data == NULL);
+
+	if (vb2_plane_size(vb, 0) < q_data->size) {
+		pr_err("%s data will not fit into plane (%lu < %lu)\n",
+				__func__, vb2_plane_size(vb, 0),
+				(long)q_data->size);
+		return -EINVAL;
+	}
+
+	vb2_set_plane_payload(vb, 0, q_data->size);
+
+	return 0;
+}
+
+static void s5p_jpeg_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct s5p_jpeg_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+
+	if (ctx->mode == S5P_JPEG_DECODE &&
+	    vb->vb2_queue->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+		struct s5p_jpeg_q_data tmp, *q_data;
+		ctx->hdr_parsed = s5p_jpeg_parse_hdr(&tmp,
+		     (unsigned long)vb2_plane_vaddr(vb, 0),
+		     min((unsigned long)ctx->out_q.size,
+			 vb2_get_plane_payload(vb, 0)), ctx);
+		if (!ctx->hdr_parsed) {
+			vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
+			return;
+		}
+
+		q_data = &ctx->out_q;
+		q_data->w = tmp.w;
+		q_data->h = tmp.h;
+		q_data->sos = tmp.sos;
+		memcpy(q_data->dht.marker, tmp.dht.marker,
+		       sizeof(tmp.dht.marker));
+		memcpy(q_data->dht.len, tmp.dht.len, sizeof(tmp.dht.len));
+		q_data->dht.n = tmp.dht.n;
+		memcpy(q_data->dqt.marker, tmp.dqt.marker,
+		       sizeof(tmp.dqt.marker));
+		memcpy(q_data->dqt.len, tmp.dqt.len, sizeof(tmp.dqt.len));
+		q_data->dqt.n = tmp.dqt.n;
+		q_data->sof = tmp.sof;
+		q_data->sof_len = tmp.sof_len;
+
+		q_data = &ctx->cap_q;
+		q_data->w = tmp.w;
+		q_data->h = tmp.h;
+	}
+
+	v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
+}
+
+static int s5p_jpeg_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct s5p_jpeg_ctx *ctx = vb2_get_drv_priv(q);
+	int ret;
+
+	ret = pm_runtime_get_sync(ctx->jpeg->dev);
+
+	return ret > 0 ? 0 : ret;
+}
+
+static void s5p_jpeg_stop_streaming(struct vb2_queue *q)
+{
+	struct s5p_jpeg_ctx *ctx = vb2_get_drv_priv(q);
+
+	pm_runtime_put(ctx->jpeg->dev);
+}
+
+static struct vb2_ops s5p_jpeg_qops = {
+	.queue_setup		= s5p_jpeg_queue_setup,
+	.buf_prepare		= s5p_jpeg_buf_prepare,
+	.buf_queue		= s5p_jpeg_buf_queue,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
+	.start_streaming	= s5p_jpeg_start_streaming,
+	.stop_streaming		= s5p_jpeg_stop_streaming,
+};
+
+static int queue_init(void *priv, struct vb2_queue *src_vq,
+		      struct vb2_queue *dst_vq)
+{
+	struct s5p_jpeg_ctx *ctx = priv;
+	int ret;
+
+	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	src_vq->io_modes = VB2_MMAP | VB2_USERPTR;
+	src_vq->drv_priv = ctx;
+	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	src_vq->ops = &s5p_jpeg_qops;
+	src_vq->mem_ops = &vb2_dma_contig_memops;
+	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	src_vq->lock = &ctx->jpeg->lock;
+
+	ret = vb2_queue_init(src_vq);
+	if (ret)
+		return ret;
+
+	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	dst_vq->io_modes = VB2_MMAP | VB2_USERPTR;
+	dst_vq->drv_priv = ctx;
+	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	dst_vq->ops = &s5p_jpeg_qops;
+	dst_vq->mem_ops = &vb2_dma_contig_memops;
+	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	dst_vq->lock = &ctx->jpeg->lock;
+
+	return vb2_queue_init(dst_vq);
+}
+
+/*
+ * ============================================================================
+ * ISR
+ * ============================================================================
+ */
+
+static irqreturn_t s5p_jpeg_irq(int irq, void *dev_id)
+{
+	struct s5p_jpeg *jpeg = dev_id;
+	struct s5p_jpeg_ctx *curr_ctx;
+	struct vb2_v4l2_buffer *src_buf, *dst_buf;
+	unsigned long payload_size = 0;
+	enum vb2_buffer_state state = VB2_BUF_STATE_DONE;
+	bool enc_jpeg_too_large = false;
+	bool timer_elapsed = false;
+	bool op_completed = false;
+
+	spin_lock(&jpeg->slock);
+
+	curr_ctx = v4l2_m2m_get_curr_priv(jpeg->m2m_dev);
+
+	src_buf = v4l2_m2m_src_buf_remove(curr_ctx->fh.m2m_ctx);
+	dst_buf = v4l2_m2m_dst_buf_remove(curr_ctx->fh.m2m_ctx);
+
+	if (curr_ctx->mode == S5P_JPEG_ENCODE)
+		enc_jpeg_too_large = s5p_jpeg_enc_stream_stat(jpeg->regs);
+	timer_elapsed = s5p_jpeg_timer_stat(jpeg->regs);
+	op_completed = s5p_jpeg_result_stat_ok(jpeg->regs);
+	if (curr_ctx->mode == S5P_JPEG_DECODE)
+		op_completed = op_completed &&
+					s5p_jpeg_stream_stat_ok(jpeg->regs);
+
+	if (enc_jpeg_too_large) {
+		state = VB2_BUF_STATE_ERROR;
+		s5p_jpeg_clear_enc_stream_stat(jpeg->regs);
+	} else if (timer_elapsed) {
+		state = VB2_BUF_STATE_ERROR;
+		s5p_jpeg_clear_timer_stat(jpeg->regs);
+	} else if (!op_completed) {
+		state = VB2_BUF_STATE_ERROR;
+	} else {
+		payload_size = s5p_jpeg_compressed_size(jpeg->regs);
+	}
+
+	dst_buf->timecode = src_buf->timecode;
+	dst_buf->timestamp = src_buf->timestamp;
+	dst_buf->flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+	dst_buf->flags |=
+		src_buf->flags & V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+
+	v4l2_m2m_buf_done(src_buf, state);
+	if (curr_ctx->mode == S5P_JPEG_ENCODE)
+		vb2_set_plane_payload(&dst_buf->vb2_buf, 0, payload_size);
+	v4l2_m2m_buf_done(dst_buf, state);
+	v4l2_m2m_job_finish(jpeg->m2m_dev, curr_ctx->fh.m2m_ctx);
+
+	curr_ctx->subsampling = s5p_jpeg_get_subsampling_mode(jpeg->regs);
+	spin_unlock(&jpeg->slock);
+
+	s5p_jpeg_clear_int(jpeg->regs);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t exynos4_jpeg_irq(int irq, void *priv)
+{
+	unsigned int int_status;
+	struct vb2_v4l2_buffer *src_vb, *dst_vb;
+	struct s5p_jpeg *jpeg = priv;
+	struct s5p_jpeg_ctx *curr_ctx;
+	unsigned long payload_size = 0;
+
+	spin_lock(&jpeg->slock);
+
+	curr_ctx = v4l2_m2m_get_curr_priv(jpeg->m2m_dev);
+
+	src_vb = v4l2_m2m_src_buf_remove(curr_ctx->fh.m2m_ctx);
+	dst_vb = v4l2_m2m_dst_buf_remove(curr_ctx->fh.m2m_ctx);
+
+	int_status = exynos4_jpeg_get_int_status(jpeg->regs);
+
+	if (int_status) {
+		switch (int_status & 0x1f) {
+		case 0x1:
+			jpeg->irq_ret = ERR_PROT;
+			break;
+		case 0x2:
+			jpeg->irq_ret = OK_ENC_OR_DEC;
+			break;
+		case 0x4:
+			jpeg->irq_ret = ERR_DEC_INVALID_FORMAT;
+			break;
+		case 0x8:
+			jpeg->irq_ret = ERR_MULTI_SCAN;
+			break;
+		case 0x10:
+			jpeg->irq_ret = ERR_FRAME;
+			break;
+		default:
+			jpeg->irq_ret = ERR_UNKNOWN;
+			break;
+		}
+	} else {
+		jpeg->irq_ret = ERR_UNKNOWN;
+	}
+
+	if (jpeg->irq_ret == OK_ENC_OR_DEC) {
+		if (curr_ctx->mode == S5P_JPEG_ENCODE) {
+			payload_size = exynos4_jpeg_get_stream_size(jpeg->regs);
+			vb2_set_plane_payload(&dst_vb->vb2_buf,
+					0, payload_size);
+		}
+		v4l2_m2m_buf_done(src_vb, VB2_BUF_STATE_DONE);
+		v4l2_m2m_buf_done(dst_vb, VB2_BUF_STATE_DONE);
+	} else {
+		v4l2_m2m_buf_done(src_vb, VB2_BUF_STATE_ERROR);
+		v4l2_m2m_buf_done(dst_vb, VB2_BUF_STATE_ERROR);
+	}
+
+	v4l2_m2m_job_finish(jpeg->m2m_dev, curr_ctx->fh.m2m_ctx);
+	if (jpeg->variant->version == SJPEG_EXYNOS4)
+		curr_ctx->subsampling = exynos4_jpeg_get_frame_fmt(jpeg->regs);
+
+	spin_unlock(&jpeg->slock);
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t exynos3250_jpeg_irq(int irq, void *dev_id)
+{
+	struct s5p_jpeg *jpeg = dev_id;
+	struct s5p_jpeg_ctx *curr_ctx;
+	struct vb2_v4l2_buffer *src_buf, *dst_buf;
+	unsigned long payload_size = 0;
+	enum vb2_buffer_state state = VB2_BUF_STATE_DONE;
+	bool interrupt_timeout = false;
+	u32 irq_status;
+
+	spin_lock(&jpeg->slock);
+
+	irq_status = exynos3250_jpeg_get_timer_status(jpeg->regs);
+	if (irq_status & EXYNOS3250_TIMER_INT_STAT) {
+		exynos3250_jpeg_clear_timer_status(jpeg->regs);
+		interrupt_timeout = true;
+		dev_err(jpeg->dev, "Interrupt timeout occurred.\n");
+	}
+
+	irq_status = exynos3250_jpeg_get_int_status(jpeg->regs);
+	exynos3250_jpeg_clear_int_status(jpeg->regs, irq_status);
+
+	jpeg->irq_status |= irq_status;
+
+	curr_ctx = v4l2_m2m_get_curr_priv(jpeg->m2m_dev);
+
+	if (!curr_ctx)
+		goto exit_unlock;
+
+	if ((irq_status & EXYNOS3250_HEADER_STAT) &&
+	    (curr_ctx->mode == S5P_JPEG_DECODE)) {
+		exynos3250_jpeg_rstart(jpeg->regs);
+		goto exit_unlock;
+	}
+
+	if (jpeg->irq_status & (EXYNOS3250_JPEG_DONE |
+				EXYNOS3250_WDMA_DONE |
+				EXYNOS3250_RDMA_DONE |
+				EXYNOS3250_RESULT_STAT))
+		payload_size = exynos3250_jpeg_compressed_size(jpeg->regs);
+	else if (interrupt_timeout)
+		state = VB2_BUF_STATE_ERROR;
+	else
+		goto exit_unlock;
+
+	src_buf = v4l2_m2m_src_buf_remove(curr_ctx->fh.m2m_ctx);
+	dst_buf = v4l2_m2m_dst_buf_remove(curr_ctx->fh.m2m_ctx);
+
+	dst_buf->timecode = src_buf->timecode;
+	dst_buf->timestamp = src_buf->timestamp;
+
+	v4l2_m2m_buf_done(src_buf, state);
+	if (curr_ctx->mode == S5P_JPEG_ENCODE)
+		vb2_set_plane_payload(&dst_buf->vb2_buf, 0, payload_size);
+	v4l2_m2m_buf_done(dst_buf, state);
+	v4l2_m2m_job_finish(jpeg->m2m_dev, curr_ctx->fh.m2m_ctx);
+
+	curr_ctx->subsampling =
+			exynos3250_jpeg_get_subsampling_mode(jpeg->regs);
+exit_unlock:
+	spin_unlock(&jpeg->slock);
+	return IRQ_HANDLED;
+}
+
+static void *jpeg_get_drv_data(struct device *dev);
+
+/*
+ * ============================================================================
+ * Driver basic infrastructure
+ * ============================================================================
+ */
+
+static int s5p_jpeg_probe(struct platform_device *pdev)
+{
+	struct s5p_jpeg *jpeg;
+	struct resource *res;
+	int i, ret;
+
+	/* JPEG IP abstraction struct */
+	jpeg = devm_kzalloc(&pdev->dev, sizeof(struct s5p_jpeg), GFP_KERNEL);
+	if (!jpeg)
+		return -ENOMEM;
+
+	jpeg->variant = jpeg_get_drv_data(&pdev->dev);
+
+	mutex_init(&jpeg->lock);
+	spin_lock_init(&jpeg->slock);
+	jpeg->dev = &pdev->dev;
+
+	/* memory-mapped registers */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	jpeg->regs = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(jpeg->regs))
+		return PTR_ERR(jpeg->regs);
+
+	/* interrupt service routine registration */
+	jpeg->irq = ret = platform_get_irq(pdev, 0);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "cannot find IRQ\n");
+		return ret;
+	}
+
+	ret = devm_request_irq(&pdev->dev, jpeg->irq, jpeg->variant->jpeg_irq,
+				0, dev_name(&pdev->dev), jpeg);
+	if (ret) {
+		dev_err(&pdev->dev, "cannot claim IRQ %d\n", jpeg->irq);
+		return ret;
+	}
+
+	/* clocks */
+	for (i = 0; i < jpeg->variant->num_clocks; i++) {
+		jpeg->clocks[i] = devm_clk_get(&pdev->dev,
+					      jpeg->variant->clk_names[i]);
+		if (IS_ERR(jpeg->clocks[i])) {
+			dev_err(&pdev->dev, "failed to get clock: %s\n",
+				jpeg->variant->clk_names[i]);
+			return PTR_ERR(jpeg->clocks[i]);
+		}
+	}
+
+	/* v4l2 device */
+	ret = v4l2_device_register(&pdev->dev, &jpeg->v4l2_dev);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to register v4l2 device\n");
+		return ret;
+	}
+
+	/* mem2mem device */
+	jpeg->m2m_dev = v4l2_m2m_init(jpeg->variant->m2m_ops);
+	if (IS_ERR(jpeg->m2m_dev)) {
+		v4l2_err(&jpeg->v4l2_dev, "Failed to init mem2mem device\n");
+		ret = PTR_ERR(jpeg->m2m_dev);
+		goto device_register_rollback;
+	}
+
+	jpeg->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
+	if (IS_ERR(jpeg->alloc_ctx)) {
+		v4l2_err(&jpeg->v4l2_dev, "Failed to init memory allocator\n");
+		ret = PTR_ERR(jpeg->alloc_ctx);
+		goto m2m_init_rollback;
+	}
+
+	/* JPEG encoder /dev/videoX node */
+	jpeg->vfd_encoder = video_device_alloc();
+	if (!jpeg->vfd_encoder) {
+		v4l2_err(&jpeg->v4l2_dev, "Failed to allocate video device\n");
+		ret = -ENOMEM;
+		goto vb2_allocator_rollback;
+	}
+	snprintf(jpeg->vfd_encoder->name, sizeof(jpeg->vfd_encoder->name),
+				"%s-enc", S5P_JPEG_M2M_NAME);
+	jpeg->vfd_encoder->fops		= &s5p_jpeg_fops;
+	jpeg->vfd_encoder->ioctl_ops	= &s5p_jpeg_ioctl_ops;
+	jpeg->vfd_encoder->minor	= -1;
+	jpeg->vfd_encoder->release	= video_device_release;
+	jpeg->vfd_encoder->lock		= &jpeg->lock;
+	jpeg->vfd_encoder->v4l2_dev	= &jpeg->v4l2_dev;
+	jpeg->vfd_encoder->vfl_dir	= VFL_DIR_M2M;
+
+	ret = video_register_device(jpeg->vfd_encoder, VFL_TYPE_GRABBER, -1);
+	if (ret) {
+		v4l2_err(&jpeg->v4l2_dev, "Failed to register video device\n");
+		video_device_release(jpeg->vfd_encoder);
+		goto vb2_allocator_rollback;
+	}
+
+	video_set_drvdata(jpeg->vfd_encoder, jpeg);
+	v4l2_info(&jpeg->v4l2_dev,
+		  "encoder device registered as /dev/video%d\n",
+		  jpeg->vfd_encoder->num);
+
+	/* JPEG decoder /dev/videoX node */
+	jpeg->vfd_decoder = video_device_alloc();
+	if (!jpeg->vfd_decoder) {
+		v4l2_err(&jpeg->v4l2_dev, "Failed to allocate video device\n");
+		ret = -ENOMEM;
+		goto enc_vdev_register_rollback;
+	}
+	snprintf(jpeg->vfd_decoder->name, sizeof(jpeg->vfd_decoder->name),
+				"%s-dec", S5P_JPEG_M2M_NAME);
+	jpeg->vfd_decoder->fops		= &s5p_jpeg_fops;
+	jpeg->vfd_decoder->ioctl_ops	= &s5p_jpeg_ioctl_ops;
+	jpeg->vfd_decoder->minor	= -1;
+	jpeg->vfd_decoder->release	= video_device_release;
+	jpeg->vfd_decoder->lock		= &jpeg->lock;
+	jpeg->vfd_decoder->v4l2_dev	= &jpeg->v4l2_dev;
+	jpeg->vfd_decoder->vfl_dir	= VFL_DIR_M2M;
+
+	ret = video_register_device(jpeg->vfd_decoder, VFL_TYPE_GRABBER, -1);
+	if (ret) {
+		v4l2_err(&jpeg->v4l2_dev, "Failed to register video device\n");
+		video_device_release(jpeg->vfd_decoder);
+		goto enc_vdev_register_rollback;
+	}
+
+	video_set_drvdata(jpeg->vfd_decoder, jpeg);
+	v4l2_info(&jpeg->v4l2_dev,
+		  "decoder device registered as /dev/video%d\n",
+		  jpeg->vfd_decoder->num);
+
+	/* final statements & power management */
+	platform_set_drvdata(pdev, jpeg);
+
+	pm_runtime_enable(&pdev->dev);
+
+	v4l2_info(&jpeg->v4l2_dev, "Samsung S5P JPEG codec\n");
+
+	return 0;
+
+enc_vdev_register_rollback:
+	video_unregister_device(jpeg->vfd_encoder);
+
+vb2_allocator_rollback:
+	vb2_dma_contig_cleanup_ctx(jpeg->alloc_ctx);
+
+m2m_init_rollback:
+	v4l2_m2m_release(jpeg->m2m_dev);
+
+device_register_rollback:
+	v4l2_device_unregister(&jpeg->v4l2_dev);
+
+	return ret;
+}
+
+static int s5p_jpeg_remove(struct platform_device *pdev)
+{
+	struct s5p_jpeg *jpeg = platform_get_drvdata(pdev);
+	int i;
+
+	pm_runtime_disable(jpeg->dev);
+
+	video_unregister_device(jpeg->vfd_decoder);
+	video_unregister_device(jpeg->vfd_encoder);
+	vb2_dma_contig_cleanup_ctx(jpeg->alloc_ctx);
+	v4l2_m2m_release(jpeg->m2m_dev);
+	v4l2_device_unregister(&jpeg->v4l2_dev);
+
+	if (!pm_runtime_status_suspended(&pdev->dev)) {
+		for (i = jpeg->variant->num_clocks - 1; i >= 0; i--)
+			clk_disable_unprepare(jpeg->clocks[i]);
+	}
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int s5p_jpeg_runtime_suspend(struct device *dev)
+{
+	struct s5p_jpeg *jpeg = dev_get_drvdata(dev);
+	int i;
+
+	for (i = jpeg->variant->num_clocks - 1; i >= 0; i--)
+		clk_disable_unprepare(jpeg->clocks[i]);
+
+	return 0;
+}
+
+static int s5p_jpeg_runtime_resume(struct device *dev)
+{
+	struct s5p_jpeg *jpeg = dev_get_drvdata(dev);
+	unsigned long flags;
+	int i, ret;
+
+	for (i = 0; i < jpeg->variant->num_clocks; i++) {
+		ret = clk_prepare_enable(jpeg->clocks[i]);
+		if (ret) {
+			while (--i > 0)
+				clk_disable_unprepare(jpeg->clocks[i]);
+			return ret;
+		}
+	}
+
+	spin_lock_irqsave(&jpeg->slock, flags);
+
+	/*
+	 * JPEG IP allows storing two Huffman tables for each component.
+	 * We fill table 0 for each component and do this here only
+	 * for S5PC210 and Exynos3250 SoCs. Exynos4x12 and Exynos542x SoC
+	 * require programming their Huffman tables each time the encoding
+	 * process is initialized, and thus it is accomplished in the
+	 * device_run callback of m2m_ops.
+	 */
+	if (!jpeg->variant->htbl_reinit) {
+		s5p_jpeg_set_hdctbl(jpeg->regs);
+		s5p_jpeg_set_hdctblg(jpeg->regs);
+		s5p_jpeg_set_hactbl(jpeg->regs);
+		s5p_jpeg_set_hactblg(jpeg->regs);
+	}
+
+	spin_unlock_irqrestore(&jpeg->slock, flags);
+
+	return 0;
+}
+#endif /* CONFIG_PM */
+
+#ifdef CONFIG_PM_SLEEP
+static int s5p_jpeg_suspend(struct device *dev)
+{
+	if (pm_runtime_suspended(dev))
+		return 0;
+
+	return s5p_jpeg_runtime_suspend(dev);
+}
+
+static int s5p_jpeg_resume(struct device *dev)
+{
+	if (pm_runtime_suspended(dev))
+		return 0;
+
+	return s5p_jpeg_runtime_resume(dev);
+}
+#endif
+
+static const struct dev_pm_ops s5p_jpeg_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(s5p_jpeg_suspend, s5p_jpeg_resume)
+	SET_RUNTIME_PM_OPS(s5p_jpeg_runtime_suspend, s5p_jpeg_runtime_resume, NULL)
+};
+
+static struct s5p_jpeg_variant s5p_jpeg_drvdata = {
+	.version	= SJPEG_S5P,
+	.jpeg_irq	= s5p_jpeg_irq,
+	.m2m_ops	= &s5p_jpeg_m2m_ops,
+	.fmt_ver_flag	= SJPEG_FMT_FLAG_S5P,
+	.clk_names	= {"jpeg"},
+	.num_clocks	= 1,
+};
+
+static struct s5p_jpeg_variant exynos3250_jpeg_drvdata = {
+	.version	= SJPEG_EXYNOS3250,
+	.jpeg_irq	= exynos3250_jpeg_irq,
+	.m2m_ops	= &exynos3250_jpeg_m2m_ops,
+	.fmt_ver_flag	= SJPEG_FMT_FLAG_EXYNOS3250,
+	.hw3250_compat	= 1,
+	.clk_names	= {"jpeg", "sclk"},
+	.num_clocks	= 2,
+};
+
+static struct s5p_jpeg_variant exynos4_jpeg_drvdata = {
+	.version	= SJPEG_EXYNOS4,
+	.jpeg_irq	= exynos4_jpeg_irq,
+	.m2m_ops	= &exynos4_jpeg_m2m_ops,
+	.fmt_ver_flag	= SJPEG_FMT_FLAG_EXYNOS4,
+	.htbl_reinit	= 1,
+	.clk_names	= {"jpeg"},
+	.num_clocks	= 1,
+	.hw_ex4_compat	= 1,
+};
+
+static struct s5p_jpeg_variant exynos5420_jpeg_drvdata = {
+	.version	= SJPEG_EXYNOS5420,
+	.jpeg_irq	= exynos3250_jpeg_irq,		/* intentionally 3250 */
+	.m2m_ops	= &exynos3250_jpeg_m2m_ops,	/* intentionally 3250 */
+	.fmt_ver_flag	= SJPEG_FMT_FLAG_EXYNOS3250,	/* intentionally 3250 */
+	.hw3250_compat	= 1,
+	.htbl_reinit	= 1,
+	.clk_names	= {"jpeg"},
+	.num_clocks	= 1,
+};
+
+static struct s5p_jpeg_variant exynos5433_jpeg_drvdata = {
+	.version	= SJPEG_EXYNOS5433,
+	.jpeg_irq	= exynos4_jpeg_irq,
+	.m2m_ops	= &exynos4_jpeg_m2m_ops,
+	.fmt_ver_flag	= SJPEG_FMT_FLAG_EXYNOS4,
+	.htbl_reinit	= 1,
+	.clk_names	= {"pclk", "aclk", "aclk_xiu", "sclk"},
+	.num_clocks	= 4,
+	.hw_ex4_compat	= 1,
+};
+
+static const struct of_device_id samsung_jpeg_match[] = {
+	{
+		.compatible = "samsung,s5pv210-jpeg",
+		.data = &s5p_jpeg_drvdata,
+	}, {
+		.compatible = "samsung,exynos3250-jpeg",
+		.data = &exynos3250_jpeg_drvdata,
+	}, {
+		.compatible = "samsung,exynos4210-jpeg",
+		.data = &exynos4_jpeg_drvdata,
+	}, {
+		.compatible = "samsung,exynos4212-jpeg",
+		.data = &exynos4_jpeg_drvdata,
+	}, {
+		.compatible = "samsung,exynos5420-jpeg",
+		.data = &exynos5420_jpeg_drvdata,
+	}, {
+		.compatible = "samsung,exynos5433-jpeg",
+		.data = &exynos5433_jpeg_drvdata,
+	},
+	{},
+};
+
+MODULE_DEVICE_TABLE(of, samsung_jpeg_match);
+
+static void *jpeg_get_drv_data(struct device *dev)
+{
+	struct s5p_jpeg_variant *driver_data = NULL;
+	const struct of_device_id *match;
+
+	if (!IS_ENABLED(CONFIG_OF) || !dev->of_node)
+		return &s5p_jpeg_drvdata;
+
+	match = of_match_node(samsung_jpeg_match, dev->of_node);
+
+	if (match)
+		driver_data = (struct s5p_jpeg_variant *)match->data;
+
+	return driver_data;
+}
+
+static struct platform_driver s5p_jpeg_driver = {
+	.probe = s5p_jpeg_probe,
+	.remove = s5p_jpeg_remove,
+	.driver = {
+		.of_match_table	= of_match_ptr(samsung_jpeg_match),
+		.name		= S5P_JPEG_M2M_NAME,
+		.pm		= &s5p_jpeg_pm_ops,
+	},
+};
+
+module_platform_driver(s5p_jpeg_driver);
+
+MODULE_AUTHOR("Andrzej Pietrasiewicz <andrzej.p@samsung.com>");
+MODULE_AUTHOR("Jacek Anaszewski <j.anaszewski@samsung.com>");
+MODULE_DESCRIPTION("Samsung JPEG codec driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/s5p-jpeg/jpeg-core.h b/drivers/media/platform/s5p-jpeg/jpeg-core.h
new file mode 100644
index 0000000..9b1db09
--- /dev/null
+++ b/drivers/media/platform/s5p-jpeg/jpeg-core.h
@@ -0,0 +1,266 @@
+/* linux/drivers/media/platform/s5p-jpeg/jpeg-core.h
+ *
+ * Copyright (c) 2011 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ *
+ * Author: Andrzej Pietrasiewicz <andrzej.p@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef JPEG_CORE_H_
+#define JPEG_CORE_H_
+
+#include <linux/interrupt.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-ctrls.h>
+
+#define S5P_JPEG_M2M_NAME		"s5p-jpeg"
+
+#define JPEG_MAX_CLOCKS			4
+
+/* JPEG compression quality setting */
+#define S5P_JPEG_COMPR_QUAL_BEST	0
+#define S5P_JPEG_COMPR_QUAL_WORST	3
+
+/* JPEG RGB to YCbCr conversion matrix coefficients */
+#define S5P_JPEG_COEF11			0x4d
+#define S5P_JPEG_COEF12			0x97
+#define S5P_JPEG_COEF13			0x1e
+#define S5P_JPEG_COEF21			0x2c
+#define S5P_JPEG_COEF22			0x57
+#define S5P_JPEG_COEF23			0x83
+#define S5P_JPEG_COEF31			0x83
+#define S5P_JPEG_COEF32			0x6e
+#define S5P_JPEG_COEF33			0x13
+
+#define EXYNOS3250_IRQ_TIMEOUT		0x10000000
+
+/* a selection of JPEG markers */
+#define TEM				0x01
+#define SOF0				0xc0
+#define DHT				0xc4
+#define RST				0xd0
+#define SOI				0xd8
+#define EOI				0xd9
+#define	SOS				0xda
+#define DQT				0xdb
+#define DHP				0xde
+
+/* Flags that indicate a format can be used for capture/output */
+#define SJPEG_FMT_FLAG_ENC_CAPTURE	(1 << 0)
+#define SJPEG_FMT_FLAG_ENC_OUTPUT	(1 << 1)
+#define SJPEG_FMT_FLAG_DEC_CAPTURE	(1 << 2)
+#define SJPEG_FMT_FLAG_DEC_OUTPUT	(1 << 3)
+#define SJPEG_FMT_FLAG_S5P		(1 << 4)
+#define SJPEG_FMT_FLAG_EXYNOS3250	(1 << 5)
+#define SJPEG_FMT_FLAG_EXYNOS4		(1 << 6)
+#define SJPEG_FMT_RGB			(1 << 7)
+#define SJPEG_FMT_NON_RGB		(1 << 8)
+
+#define S5P_JPEG_ENCODE		0
+#define S5P_JPEG_DECODE		1
+
+#define FMT_TYPE_OUTPUT		0
+#define FMT_TYPE_CAPTURE	1
+
+#define SJPEG_SUBSAMPLING_444	0x11
+#define SJPEG_SUBSAMPLING_422	0x21
+#define SJPEG_SUBSAMPLING_420	0x22
+
+#define S5P_JPEG_MAX_MARKER	4
+
+/* Version numbers */
+enum sjpeg_version {
+	SJPEG_S5P,
+	SJPEG_EXYNOS3250,
+	SJPEG_EXYNOS4,
+	SJPEG_EXYNOS5420,
+	SJPEG_EXYNOS5433,
+};
+
+enum exynos4_jpeg_result {
+	OK_ENC_OR_DEC,
+	ERR_PROT,
+	ERR_DEC_INVALID_FORMAT,
+	ERR_MULTI_SCAN,
+	ERR_FRAME,
+	ERR_UNKNOWN,
+};
+
+enum  exynos4_jpeg_img_quality_level {
+	QUALITY_LEVEL_1 = 0,	/* high */
+	QUALITY_LEVEL_2,
+	QUALITY_LEVEL_3,
+	QUALITY_LEVEL_4,	/* low */
+};
+
+/**
+ * struct s5p_jpeg - JPEG IP abstraction
+ * @lock:		the mutex protecting this structure
+ * @slock:		spinlock protecting the device contexts
+ * @v4l2_dev:		v4l2 device for mem2mem mode
+ * @vfd_encoder:	video device node for encoder mem2mem mode
+ * @vfd_decoder:	video device node for decoder mem2mem mode
+ * @m2m_dev:		v4l2 mem2mem device data
+ * @regs:		JPEG IP registers mapping
+ * @irq:		JPEG IP irq
+ * @clocks:		JPEG IP clock(s)
+ * @dev:		JPEG IP struct device
+ * @alloc_ctx:		videobuf2 memory allocator's context
+ * @variant:		driver variant to be used
+ * @irq_status		interrupt flags set during single encode/decode
+			operation
+
+ */
+struct s5p_jpeg {
+	struct mutex		lock;
+	spinlock_t		slock;
+
+	struct v4l2_device	v4l2_dev;
+	struct video_device	*vfd_encoder;
+	struct video_device	*vfd_decoder;
+	struct v4l2_m2m_dev	*m2m_dev;
+
+	void __iomem		*regs;
+	unsigned int		irq;
+	enum exynos4_jpeg_result irq_ret;
+	struct clk		*clocks[JPEG_MAX_CLOCKS];
+	struct device		*dev;
+	void			*alloc_ctx;
+	struct s5p_jpeg_variant *variant;
+	u32			irq_status;
+};
+
+struct s5p_jpeg_variant {
+	unsigned int		version;
+	unsigned int		fmt_ver_flag;
+	unsigned int		hw3250_compat:1;
+	unsigned int		htbl_reinit:1;
+	unsigned int		hw_ex4_compat:1;
+	struct v4l2_m2m_ops	*m2m_ops;
+	irqreturn_t		(*jpeg_irq)(int irq, void *priv);
+	const char		*clk_names[JPEG_MAX_CLOCKS];
+	int			num_clocks;
+};
+
+/**
+ * struct jpeg_fmt - driver's internal color format data
+ * @name:	format descritpion
+ * @fourcc:	the fourcc code, 0 if not applicable
+ * @depth:	number of bits per pixel
+ * @colplanes:	number of color planes (1 for packed formats)
+ * @h_align:	horizontal alignment order (align to 2^h_align)
+ * @v_align:	vertical alignment order (align to 2^v_align)
+ * @flags:	flags describing format applicability
+ */
+struct s5p_jpeg_fmt {
+	char	*name;
+	u32	fourcc;
+	int	depth;
+	int	colplanes;
+	int	memplanes;
+	int	h_align;
+	int	v_align;
+	int	subsampling;
+	u32	flags;
+};
+
+/**
+ * s5p_jpeg_marker - collection of markers from jpeg header
+ * @marker:	markers' positions relative to the buffer beginning
+ * @len:	markers' payload lengths (without length field)
+ * @n:		number of markers in collection
+ */
+struct s5p_jpeg_marker {
+	u32	marker[S5P_JPEG_MAX_MARKER];
+	u32	len[S5P_JPEG_MAX_MARKER];
+	u32	n;
+};
+
+/**
+ * s5p_jpeg_q_data - parameters of one queue
+ * @fmt:	driver-specific format of this queue
+ * @w:		image width
+ * @h:		image height
+ * @sos:	SOS marker's position relative to the buffer beginning
+ * @dht:	DHT markers' positions relative to the buffer beginning
+ * @dqt:	DQT markers' positions relative to the buffer beginning
+ * @sof:	SOF0 marker's postition relative to the buffer beginning
+ * @sof_len:	SOF0 marker's payload length (without length field itself)
+ * @components:	number of image components
+ * @size:	image buffer size in bytes
+ */
+struct s5p_jpeg_q_data {
+	struct s5p_jpeg_fmt	*fmt;
+	u32			w;
+	u32			h;
+	u32			sos;
+	struct s5p_jpeg_marker	dht;
+	struct s5p_jpeg_marker	dqt;
+	u32			sof;
+	u32			sof_len;
+	u32			components;
+	u32			size;
+};
+
+/**
+ * s5p_jpeg_ctx - the device context data
+ * @jpeg:		JPEG IP device for this context
+ * @mode:		compression (encode) operation or decompression (decode)
+ * @compr_quality:	destination image quality in compression (encode) mode
+ * @restart_interval:	JPEG restart interval for JPEG encoding
+ * @subsampling:	subsampling of a raw format or a JPEG
+ * @out_q:		source (output) queue information
+ * @cap_q:		destination (capture) queue queue information
+ * @scale_factor:	scale factor for JPEG decoding
+ * @crop_rect:		a rectangle representing crop area of the output buffer
+ * @fh:			V4L2 file handle
+ * @hdr_parsed:		set if header has been parsed during decompression
+ * @crop_altered:	set if crop rectangle has been altered by the user space
+ * @ctrl_handler:	controls handler
+ */
+struct s5p_jpeg_ctx {
+	struct s5p_jpeg		*jpeg;
+	unsigned int		mode;
+	unsigned short		compr_quality;
+	unsigned short		restart_interval;
+	unsigned short		subsampling;
+	struct s5p_jpeg_q_data	out_q;
+	struct s5p_jpeg_q_data	cap_q;
+	unsigned int		scale_factor;
+	struct v4l2_rect	crop_rect;
+	struct v4l2_fh		fh;
+	bool			hdr_parsed;
+	bool			crop_altered;
+	struct v4l2_ctrl_handler ctrl_handler;
+};
+
+/**
+ * s5p_jpeg_buffer - description of memory containing input JPEG data
+ * @size:	buffer size
+ * @curr:	current position in the buffer
+ * @data:	pointer to the data
+ */
+struct s5p_jpeg_buffer {
+	unsigned long size;
+	unsigned long curr;
+	unsigned long data;
+};
+
+/**
+ * struct s5p_jpeg_addr - JPEG converter physical address set for DMA
+ * @y:   luminance plane physical address
+ * @cb:  Cb plane physical address
+ * @cr:  Cr plane physical address
+ */
+struct s5p_jpeg_addr {
+	u32     y;
+	u32     cb;
+	u32     cr;
+};
+
+#endif /* JPEG_CORE_H */
diff --git a/drivers/media/platform/s5p-jpeg/jpeg-hw-exynos3250.c b/drivers/media/platform/s5p-jpeg/jpeg-hw-exynos3250.c
new file mode 100644
index 0000000..0974b9a
--- /dev/null
+++ b/drivers/media/platform/s5p-jpeg/jpeg-hw-exynos3250.c
@@ -0,0 +1,489 @@
+/* linux/drivers/media/platform/exynos3250-jpeg/jpeg-hw.h
+ *
+ * Copyright (c) 2014 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ *
+ * Author: Jacek Anaszewski <j.anaszewski@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/io.h>
+#include <linux/videodev2.h>
+#include <linux/delay.h>
+
+#include "jpeg-core.h"
+#include "jpeg-regs.h"
+#include "jpeg-hw-exynos3250.h"
+
+void exynos3250_jpeg_reset(void __iomem *regs)
+{
+	u32 reg = 1;
+	int count = 1000;
+
+	writel(1, regs + EXYNOS3250_SW_RESET);
+	/* no other way but polling for when JPEG IP becomes operational */
+	while (reg != 0 && --count > 0) {
+		udelay(1);
+		cpu_relax();
+		reg = readl(regs + EXYNOS3250_SW_RESET);
+	}
+
+	reg = 0;
+	count = 1000;
+
+	while (reg != 1 && --count > 0) {
+		writel(1, regs + EXYNOS3250_JPGDRI);
+		udelay(1);
+		cpu_relax();
+		reg = readl(regs + EXYNOS3250_JPGDRI);
+	}
+
+	writel(0, regs + EXYNOS3250_JPGDRI);
+}
+
+void exynos3250_jpeg_poweron(void __iomem *regs)
+{
+	writel(EXYNOS3250_POWER_ON, regs + EXYNOS3250_JPGCLKCON);
+}
+
+void exynos3250_jpeg_set_dma_num(void __iomem *regs)
+{
+	writel(((EXYNOS3250_DMA_MO_COUNT << EXYNOS3250_WDMA_ISSUE_NUM_SHIFT) &
+			EXYNOS3250_WDMA_ISSUE_NUM_MASK) |
+	       ((EXYNOS3250_DMA_MO_COUNT << EXYNOS3250_RDMA_ISSUE_NUM_SHIFT) &
+			EXYNOS3250_RDMA_ISSUE_NUM_MASK) |
+	       ((EXYNOS3250_DMA_MO_COUNT << EXYNOS3250_ISSUE_GATHER_NUM_SHIFT) &
+			EXYNOS3250_ISSUE_GATHER_NUM_MASK),
+		regs + EXYNOS3250_DMA_ISSUE_NUM);
+}
+
+void exynos3250_jpeg_clk_set(void __iomem *base)
+{
+	u32 reg;
+
+	reg = readl(base + EXYNOS3250_JPGCMOD) & ~EXYNOS3250_HALF_EN_MASK;
+
+	writel(reg | EXYNOS3250_HALF_EN, base + EXYNOS3250_JPGCMOD);
+}
+
+void exynos3250_jpeg_input_raw_fmt(void __iomem *regs, unsigned int fmt)
+{
+	u32 reg;
+
+	reg = readl(regs + EXYNOS3250_JPGCMOD) &
+			EXYNOS3250_MODE_Y16_MASK;
+
+	switch (fmt) {
+	case V4L2_PIX_FMT_RGB32:
+		reg |= EXYNOS3250_MODE_SEL_ARGB8888;
+		break;
+	case V4L2_PIX_FMT_BGR32:
+		reg |= EXYNOS3250_MODE_SEL_ARGB8888 | EXYNOS3250_SRC_SWAP_RGB;
+		break;
+	case V4L2_PIX_FMT_RGB565:
+		reg |= EXYNOS3250_MODE_SEL_RGB565;
+		break;
+	case V4L2_PIX_FMT_RGB565X:
+		reg |= EXYNOS3250_MODE_SEL_RGB565 | EXYNOS3250_SRC_SWAP_RGB;
+		break;
+	case V4L2_PIX_FMT_YUYV:
+		reg |= EXYNOS3250_MODE_SEL_422_1P_LUM_CHR;
+		break;
+	case V4L2_PIX_FMT_YVYU:
+		reg |= EXYNOS3250_MODE_SEL_422_1P_LUM_CHR |
+			EXYNOS3250_SRC_SWAP_UV;
+		break;
+	case V4L2_PIX_FMT_UYVY:
+		reg |= EXYNOS3250_MODE_SEL_422_1P_CHR_LUM;
+		break;
+	case V4L2_PIX_FMT_VYUY:
+		reg |= EXYNOS3250_MODE_SEL_422_1P_CHR_LUM |
+			EXYNOS3250_SRC_SWAP_UV;
+		break;
+	case V4L2_PIX_FMT_NV12:
+		reg |= EXYNOS3250_MODE_SEL_420_2P | EXYNOS3250_SRC_NV12;
+		break;
+	case V4L2_PIX_FMT_NV21:
+		reg |= EXYNOS3250_MODE_SEL_420_2P | EXYNOS3250_SRC_NV21;
+		break;
+	case V4L2_PIX_FMT_YUV420:
+		reg |= EXYNOS3250_MODE_SEL_420_3P;
+		break;
+	default:
+		break;
+
+	}
+
+	writel(reg, regs + EXYNOS3250_JPGCMOD);
+}
+
+void exynos3250_jpeg_set_y16(void __iomem *regs, bool y16)
+{
+	u32 reg;
+
+	reg = readl(regs + EXYNOS3250_JPGCMOD);
+	if (y16)
+		reg |= EXYNOS3250_MODE_Y16;
+	else
+		reg &= ~EXYNOS3250_MODE_Y16_MASK;
+	writel(reg, regs + EXYNOS3250_JPGCMOD);
+}
+
+void exynos3250_jpeg_proc_mode(void __iomem *regs, unsigned int mode)
+{
+	u32 reg, m;
+
+	if (mode == S5P_JPEG_ENCODE)
+		m = EXYNOS3250_PROC_MODE_COMPR;
+	else
+		m = EXYNOS3250_PROC_MODE_DECOMPR;
+	reg = readl(regs + EXYNOS3250_JPGMOD);
+	reg &= ~EXYNOS3250_PROC_MODE_MASK;
+	reg |= m;
+	writel(reg, regs + EXYNOS3250_JPGMOD);
+}
+
+void exynos3250_jpeg_subsampling_mode(void __iomem *regs, unsigned int mode)
+{
+	u32 reg, m = 0;
+
+	switch (mode) {
+	case V4L2_JPEG_CHROMA_SUBSAMPLING_444:
+		m = EXYNOS3250_SUBSAMPLING_MODE_444;
+		break;
+	case V4L2_JPEG_CHROMA_SUBSAMPLING_422:
+		m = EXYNOS3250_SUBSAMPLING_MODE_422;
+		break;
+	case V4L2_JPEG_CHROMA_SUBSAMPLING_420:
+		m = EXYNOS3250_SUBSAMPLING_MODE_420;
+		break;
+	}
+
+	reg = readl(regs + EXYNOS3250_JPGMOD);
+	reg &= ~EXYNOS3250_SUBSAMPLING_MODE_MASK;
+	reg |= m;
+	writel(reg, regs + EXYNOS3250_JPGMOD);
+}
+
+unsigned int exynos3250_jpeg_get_subsampling_mode(void __iomem *regs)
+{
+	return readl(regs + EXYNOS3250_JPGMOD) &
+				EXYNOS3250_SUBSAMPLING_MODE_MASK;
+}
+
+void exynos3250_jpeg_dri(void __iomem *regs, unsigned int dri)
+{
+	u32 reg;
+
+	reg = dri & EXYNOS3250_JPGDRI_MASK;
+	writel(reg, regs + EXYNOS3250_JPGDRI);
+}
+
+void exynos3250_jpeg_qtbl(void __iomem *regs, unsigned int t, unsigned int n)
+{
+	unsigned long reg;
+
+	reg = readl(regs + EXYNOS3250_QHTBL);
+	reg &= ~EXYNOS3250_QT_NUM_MASK(t);
+	reg |= (n << EXYNOS3250_QT_NUM_SHIFT(t)) &
+					EXYNOS3250_QT_NUM_MASK(t);
+	writel(reg, regs + EXYNOS3250_QHTBL);
+}
+
+void exynos3250_jpeg_htbl_ac(void __iomem *regs, unsigned int t)
+{
+	unsigned long reg;
+
+	reg = readl(regs + EXYNOS3250_QHTBL);
+	reg &= ~EXYNOS3250_HT_NUM_AC_MASK(t);
+	/* this driver uses table 0 for all color components */
+	reg |= (0 << EXYNOS3250_HT_NUM_AC_SHIFT(t)) &
+					EXYNOS3250_HT_NUM_AC_MASK(t);
+	writel(reg, regs + EXYNOS3250_QHTBL);
+}
+
+void exynos3250_jpeg_htbl_dc(void __iomem *regs, unsigned int t)
+{
+	unsigned long reg;
+
+	reg = readl(regs + EXYNOS3250_QHTBL);
+	reg &= ~EXYNOS3250_HT_NUM_DC_MASK(t);
+	/* this driver uses table 0 for all color components */
+	reg |= (0 << EXYNOS3250_HT_NUM_DC_SHIFT(t)) &
+					EXYNOS3250_HT_NUM_DC_MASK(t);
+	writel(reg, regs + EXYNOS3250_QHTBL);
+}
+
+void exynos3250_jpeg_set_y(void __iomem *regs, unsigned int y)
+{
+	u32 reg;
+
+	reg = y & EXYNOS3250_JPGY_MASK;
+	writel(reg, regs + EXYNOS3250_JPGY);
+}
+
+void exynos3250_jpeg_set_x(void __iomem *regs, unsigned int x)
+{
+	u32 reg;
+
+	reg = x & EXYNOS3250_JPGX_MASK;
+	writel(reg, regs + EXYNOS3250_JPGX);
+}
+
+#if 0	/* Currently unused */
+unsigned int exynos3250_jpeg_get_y(void __iomem *regs)
+{
+	return readl(regs + EXYNOS3250_JPGY);
+}
+
+unsigned int exynos3250_jpeg_get_x(void __iomem *regs)
+{
+	return readl(regs + EXYNOS3250_JPGX);
+}
+#endif
+
+void exynos3250_jpeg_interrupts_enable(void __iomem *regs)
+{
+	u32 reg;
+
+	reg = readl(regs + EXYNOS3250_JPGINTSE);
+	reg |= (EXYNOS3250_JPEG_DONE_EN |
+		EXYNOS3250_WDMA_DONE_EN |
+		EXYNOS3250_RDMA_DONE_EN |
+		EXYNOS3250_ENC_STREAM_INT_EN |
+		EXYNOS3250_CORE_DONE_EN |
+		EXYNOS3250_ERR_INT_EN |
+		EXYNOS3250_HEAD_INT_EN);
+	writel(reg, regs + EXYNOS3250_JPGINTSE);
+}
+
+void exynos3250_jpeg_enc_stream_bound(void __iomem *regs, unsigned int size)
+{
+	u32 reg;
+
+	reg = size & EXYNOS3250_ENC_STREAM_BOUND_MASK;
+	writel(reg, regs + EXYNOS3250_ENC_STREAM_BOUND);
+}
+
+void exynos3250_jpeg_output_raw_fmt(void __iomem *regs, unsigned int fmt)
+{
+	u32 reg;
+
+	switch (fmt) {
+	case V4L2_PIX_FMT_RGB32:
+		reg = EXYNOS3250_OUT_FMT_ARGB8888;
+		break;
+	case V4L2_PIX_FMT_BGR32:
+		reg = EXYNOS3250_OUT_FMT_ARGB8888 | EXYNOS3250_OUT_SWAP_RGB;
+		break;
+	case V4L2_PIX_FMT_RGB565:
+		reg = EXYNOS3250_OUT_FMT_RGB565;
+		break;
+	case V4L2_PIX_FMT_RGB565X:
+		reg = EXYNOS3250_OUT_FMT_RGB565 | EXYNOS3250_OUT_SWAP_RGB;
+		break;
+	case V4L2_PIX_FMT_YUYV:
+		reg = EXYNOS3250_OUT_FMT_422_1P_LUM_CHR;
+		break;
+	case V4L2_PIX_FMT_YVYU:
+		reg = EXYNOS3250_OUT_FMT_422_1P_LUM_CHR |
+			EXYNOS3250_OUT_SWAP_UV;
+		break;
+	case V4L2_PIX_FMT_UYVY:
+		reg = EXYNOS3250_OUT_FMT_422_1P_CHR_LUM;
+		break;
+	case V4L2_PIX_FMT_VYUY:
+		reg = EXYNOS3250_OUT_FMT_422_1P_CHR_LUM |
+			EXYNOS3250_OUT_SWAP_UV;
+		break;
+	case V4L2_PIX_FMT_NV12:
+		reg = EXYNOS3250_OUT_FMT_420_2P | EXYNOS3250_OUT_NV12;
+		break;
+	case V4L2_PIX_FMT_NV21:
+		reg = EXYNOS3250_OUT_FMT_420_2P | EXYNOS3250_OUT_NV21;
+		break;
+	case V4L2_PIX_FMT_YUV420:
+		reg = EXYNOS3250_OUT_FMT_420_3P;
+		break;
+	default:
+		reg = 0;
+		break;
+	}
+
+	writel(reg, regs + EXYNOS3250_OUTFORM);
+}
+
+void exynos3250_jpeg_jpgadr(void __iomem *regs, unsigned int addr)
+{
+	writel(addr, regs + EXYNOS3250_JPG_JPGADR);
+}
+
+void exynos3250_jpeg_imgadr(void __iomem *regs, struct s5p_jpeg_addr *img_addr)
+{
+	writel(img_addr->y, regs + EXYNOS3250_LUMA_BASE);
+	writel(img_addr->cb, regs + EXYNOS3250_CHROMA_BASE);
+	writel(img_addr->cr, regs + EXYNOS3250_CHROMA_CR_BASE);
+}
+
+void exynos3250_jpeg_stride(void __iomem *regs, unsigned int img_fmt,
+			    unsigned int width)
+{
+	u32 reg_luma = 0, reg_cr = 0, reg_cb = 0;
+
+	switch (img_fmt) {
+	case V4L2_PIX_FMT_RGB32:
+		reg_luma = 4 * width;
+		break;
+	case V4L2_PIX_FMT_RGB565:
+	case V4L2_PIX_FMT_RGB565X:
+	case V4L2_PIX_FMT_YUYV:
+	case V4L2_PIX_FMT_YVYU:
+	case V4L2_PIX_FMT_UYVY:
+	case V4L2_PIX_FMT_VYUY:
+		reg_luma = 2 * width;
+		break;
+	case V4L2_PIX_FMT_NV12:
+	case V4L2_PIX_FMT_NV21:
+		reg_luma = width;
+		reg_cb = reg_luma;
+		break;
+	case V4L2_PIX_FMT_YUV420:
+		reg_luma = width;
+		reg_cb = reg_cr = reg_luma / 2;
+		break;
+	default:
+		break;
+	}
+
+	writel(reg_luma, regs + EXYNOS3250_LUMA_STRIDE);
+	writel(reg_cb, regs + EXYNOS3250_CHROMA_STRIDE);
+	writel(reg_cr, regs + EXYNOS3250_CHROMA_CR_STRIDE);
+}
+
+void exynos3250_jpeg_offset(void __iomem *regs, unsigned int x_offset,
+				unsigned int y_offset)
+{
+	u32 reg;
+
+	reg = (y_offset << EXYNOS3250_LUMA_YY_OFFSET_SHIFT) &
+			EXYNOS3250_LUMA_YY_OFFSET_MASK;
+	reg |= (x_offset << EXYNOS3250_LUMA_YX_OFFSET_SHIFT) &
+			EXYNOS3250_LUMA_YX_OFFSET_MASK;
+
+	writel(reg, regs + EXYNOS3250_LUMA_XY_OFFSET);
+
+	reg = (y_offset << EXYNOS3250_CHROMA_YY_OFFSET_SHIFT) &
+			EXYNOS3250_CHROMA_YY_OFFSET_MASK;
+	reg |= (x_offset << EXYNOS3250_CHROMA_YX_OFFSET_SHIFT) &
+			EXYNOS3250_CHROMA_YX_OFFSET_MASK;
+
+	writel(reg, regs + EXYNOS3250_CHROMA_XY_OFFSET);
+
+	reg = (y_offset << EXYNOS3250_CHROMA_CR_YY_OFFSET_SHIFT) &
+			EXYNOS3250_CHROMA_CR_YY_OFFSET_MASK;
+	reg |= (x_offset << EXYNOS3250_CHROMA_CR_YX_OFFSET_SHIFT) &
+			EXYNOS3250_CHROMA_CR_YX_OFFSET_MASK;
+
+	writel(reg, regs + EXYNOS3250_CHROMA_CR_XY_OFFSET);
+}
+
+void exynos3250_jpeg_coef(void __iomem *base, unsigned int mode)
+{
+	if (mode == S5P_JPEG_ENCODE) {
+		writel(EXYNOS3250_JPEG_ENC_COEF1,
+					base + EXYNOS3250_JPG_COEF(1));
+		writel(EXYNOS3250_JPEG_ENC_COEF2,
+					base + EXYNOS3250_JPG_COEF(2));
+		writel(EXYNOS3250_JPEG_ENC_COEF3,
+					base + EXYNOS3250_JPG_COEF(3));
+	} else {
+		writel(EXYNOS3250_JPEG_DEC_COEF1,
+					base + EXYNOS3250_JPG_COEF(1));
+		writel(EXYNOS3250_JPEG_DEC_COEF2,
+					base + EXYNOS3250_JPG_COEF(2));
+		writel(EXYNOS3250_JPEG_DEC_COEF3,
+					base + EXYNOS3250_JPG_COEF(3));
+	}
+}
+
+void exynos3250_jpeg_start(void __iomem *regs)
+{
+	writel(1, regs + EXYNOS3250_JSTART);
+}
+
+void exynos3250_jpeg_rstart(void __iomem *regs)
+{
+	writel(1, regs + EXYNOS3250_JRSTART);
+}
+
+unsigned int exynos3250_jpeg_get_int_status(void __iomem *regs)
+{
+	return readl(regs + EXYNOS3250_JPGINTST);
+}
+
+void exynos3250_jpeg_clear_int_status(void __iomem *regs,
+						unsigned int value)
+{
+	return writel(value, regs + EXYNOS3250_JPGINTST);
+}
+
+unsigned int exynos3250_jpeg_operating(void __iomem *regs)
+{
+	return readl(regs + S5P_JPGOPR) & EXYNOS3250_JPGOPR_MASK;
+}
+
+unsigned int exynos3250_jpeg_compressed_size(void __iomem *regs)
+{
+	return readl(regs + EXYNOS3250_JPGCNT) & EXYNOS3250_JPGCNT_MASK;
+}
+
+void exynos3250_jpeg_dec_stream_size(void __iomem *regs,
+						unsigned int size)
+{
+	writel(size & EXYNOS3250_DEC_STREAM_MASK,
+				regs + EXYNOS3250_DEC_STREAM_SIZE);
+}
+
+void exynos3250_jpeg_dec_scaling_ratio(void __iomem *regs,
+						unsigned int sratio)
+{
+	switch (sratio) {
+	case 1:
+	default:
+		sratio = EXYNOS3250_DEC_SCALE_FACTOR_8_8;
+		break;
+	case 2:
+		sratio = EXYNOS3250_DEC_SCALE_FACTOR_4_8;
+		break;
+	case 4:
+		sratio = EXYNOS3250_DEC_SCALE_FACTOR_2_8;
+		break;
+	case 8:
+		sratio = EXYNOS3250_DEC_SCALE_FACTOR_1_8;
+		break;
+	}
+
+	writel(sratio & EXYNOS3250_DEC_SCALE_FACTOR_MASK,
+				regs + EXYNOS3250_DEC_SCALING_RATIO);
+}
+
+void exynos3250_jpeg_set_timer(void __iomem *regs, unsigned int time_value)
+{
+	time_value &= EXYNOS3250_TIMER_INIT_MASK;
+
+	writel(EXYNOS3250_TIMER_INT_STAT | time_value,
+					regs + EXYNOS3250_TIMER_SE);
+}
+
+unsigned int exynos3250_jpeg_get_timer_status(void __iomem *regs)
+{
+	return readl(regs + EXYNOS3250_TIMER_ST);
+}
+
+void exynos3250_jpeg_clear_timer_status(void __iomem *regs)
+{
+	writel(EXYNOS3250_TIMER_INT_STAT, regs + EXYNOS3250_TIMER_ST);
+}
diff --git a/drivers/media/platform/s5p-jpeg/jpeg-hw-exynos3250.h b/drivers/media/platform/s5p-jpeg/jpeg-hw-exynos3250.h
new file mode 100644
index 0000000..b6e3be8
--- /dev/null
+++ b/drivers/media/platform/s5p-jpeg/jpeg-hw-exynos3250.h
@@ -0,0 +1,60 @@
+/* linux/drivers/media/platform/s5p-jpeg/jpeg-hw-exynos3250.h
+ *
+ * Copyright (c) 2014 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ *
+ * Author: Jacek Anaszewski <j.anaszewski@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#ifndef JPEG_HW_EXYNOS3250_H_
+#define JPEG_HW_EXYNOS3250_H_
+
+#include <linux/io.h>
+#include <linux/videodev2.h>
+
+#include "jpeg-regs.h"
+
+void exynos3250_jpeg_reset(void __iomem *regs);
+void exynos3250_jpeg_poweron(void __iomem *regs);
+void exynos3250_jpeg_set_dma_num(void __iomem *regs);
+void exynos3250_jpeg_clk_set(void __iomem *base);
+void exynos3250_jpeg_input_raw_fmt(void __iomem *regs, unsigned int fmt);
+void exynos3250_jpeg_output_raw_fmt(void __iomem *regs, unsigned int fmt);
+void exynos3250_jpeg_set_y16(void __iomem *regs, bool y16);
+void exynos3250_jpeg_proc_mode(void __iomem *regs, unsigned int mode);
+void exynos3250_jpeg_subsampling_mode(void __iomem *regs, unsigned int mode);
+unsigned int exynos3250_jpeg_get_subsampling_mode(void __iomem *regs);
+void exynos3250_jpeg_dri(void __iomem *regs, unsigned int dri);
+void exynos3250_jpeg_qtbl(void __iomem *regs, unsigned int t, unsigned int n);
+void exynos3250_jpeg_htbl_ac(void __iomem *regs, unsigned int t);
+void exynos3250_jpeg_htbl_dc(void __iomem *regs, unsigned int t);
+void exynos3250_jpeg_set_y(void __iomem *regs, unsigned int y);
+void exynos3250_jpeg_set_x(void __iomem *regs, unsigned int x);
+void exynos3250_jpeg_interrupts_enable(void __iomem *regs);
+void exynos3250_jpeg_enc_stream_bound(void __iomem *regs, unsigned int size);
+void exynos3250_jpeg_outform_raw(void __iomem *regs, unsigned long format);
+void exynos3250_jpeg_jpgadr(void __iomem *regs, unsigned int addr);
+void exynos3250_jpeg_imgadr(void __iomem *regs, struct s5p_jpeg_addr *img_addr);
+void exynos3250_jpeg_stride(void __iomem *regs, unsigned int img_fmt,
+			    unsigned int width);
+void exynos3250_jpeg_offset(void __iomem *regs, unsigned int x_offset,
+				unsigned int y_offset);
+void exynos3250_jpeg_coef(void __iomem *base, unsigned int mode);
+void exynos3250_jpeg_start(void __iomem *regs);
+void exynos3250_jpeg_rstart(void __iomem *regs);
+unsigned int exynos3250_jpeg_get_int_status(void __iomem *regs);
+void exynos3250_jpeg_clear_int_status(void __iomem *regs,
+						unsigned int value);
+unsigned int exynos3250_jpeg_operating(void __iomem *regs);
+unsigned int exynos3250_jpeg_compressed_size(void __iomem *regs);
+void exynos3250_jpeg_dec_stream_size(void __iomem *regs, unsigned int size);
+void exynos3250_jpeg_dec_scaling_ratio(void __iomem *regs, unsigned int sratio);
+void exynos3250_jpeg_set_timer(void __iomem *regs, unsigned int time_value);
+unsigned int exynos3250_jpeg_get_timer_status(void __iomem *regs);
+void exynos3250_jpeg_set_timer_status(void __iomem *regs);
+void exynos3250_jpeg_clear_timer_status(void __iomem *regs);
+
+#endif /* JPEG_HW_EXYNOS3250_H_ */
diff --git a/drivers/media/platform/s5p-jpeg/jpeg-hw-exynos4.c b/drivers/media/platform/s5p-jpeg/jpeg-hw-exynos4.c
new file mode 100644
index 0000000..0912d0a
--- /dev/null
+++ b/drivers/media/platform/s5p-jpeg/jpeg-hw-exynos4.c
@@ -0,0 +1,328 @@
+/* Copyright (c) 2013 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com/
+ *
+ * Author: Jacek Anaszewski <j.anaszewski@samsung.com>
+ *
+ * Register interface file for JPEG driver on Exynos4x12.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#include <linux/io.h>
+#include <linux/delay.h>
+
+#include "jpeg-core.h"
+#include "jpeg-hw-exynos4.h"
+#include "jpeg-regs.h"
+
+void exynos4_jpeg_sw_reset(void __iomem *base)
+{
+	unsigned int reg;
+
+	reg = readl(base + EXYNOS4_JPEG_CNTL_REG);
+	writel(reg & ~EXYNOS4_SOFT_RESET_HI, base + EXYNOS4_JPEG_CNTL_REG);
+
+	udelay(100);
+
+	writel(reg | EXYNOS4_SOFT_RESET_HI, base + EXYNOS4_JPEG_CNTL_REG);
+}
+
+void exynos4_jpeg_set_enc_dec_mode(void __iomem *base, unsigned int mode)
+{
+	unsigned int reg;
+
+	reg = readl(base + EXYNOS4_JPEG_CNTL_REG);
+	/* set exynos4_jpeg mod register */
+	if (mode == S5P_JPEG_DECODE) {
+		writel((reg & EXYNOS4_ENC_DEC_MODE_MASK) |
+					EXYNOS4_DEC_MODE,
+			base + EXYNOS4_JPEG_CNTL_REG);
+	} else {/* encode */
+		writel((reg & EXYNOS4_ENC_DEC_MODE_MASK) |
+					EXYNOS4_ENC_MODE,
+			base + EXYNOS4_JPEG_CNTL_REG);
+	}
+}
+
+void __exynos4_jpeg_set_img_fmt(void __iomem *base, unsigned int img_fmt,
+				unsigned int version)
+{
+	unsigned int reg;
+	unsigned int exynos4_swap_chroma_cbcr;
+	unsigned int exynos4_swap_chroma_crcb;
+
+	if (version == SJPEG_EXYNOS4) {
+		exynos4_swap_chroma_cbcr = EXYNOS4_SWAP_CHROMA_CBCR;
+		exynos4_swap_chroma_crcb = EXYNOS4_SWAP_CHROMA_CRCB;
+	} else {
+		exynos4_swap_chroma_cbcr = EXYNOS5433_SWAP_CHROMA_CBCR;
+		exynos4_swap_chroma_crcb = EXYNOS5433_SWAP_CHROMA_CRCB;
+	}
+
+	reg = readl(base + EXYNOS4_IMG_FMT_REG) &
+			EXYNOS4_ENC_IN_FMT_MASK; /* clear except enc format */
+
+	switch (img_fmt) {
+	case V4L2_PIX_FMT_GREY:
+		reg = reg | EXYNOS4_ENC_GRAY_IMG | EXYNOS4_GRAY_IMG_IP;
+		break;
+	case V4L2_PIX_FMT_RGB32:
+		reg = reg | EXYNOS4_ENC_RGB_IMG |
+				EXYNOS4_RGB_IP_RGB_32BIT_IMG;
+		break;
+	case V4L2_PIX_FMT_RGB565:
+		reg = reg | EXYNOS4_ENC_RGB_IMG |
+				EXYNOS4_RGB_IP_RGB_16BIT_IMG;
+		break;
+	case V4L2_PIX_FMT_NV24:
+		reg = reg | EXYNOS4_ENC_YUV_444_IMG |
+				EXYNOS4_YUV_444_IP_YUV_444_2P_IMG |
+				exynos4_swap_chroma_cbcr;
+		break;
+	case V4L2_PIX_FMT_NV42:
+		reg = reg | EXYNOS4_ENC_YUV_444_IMG |
+				EXYNOS4_YUV_444_IP_YUV_444_2P_IMG |
+				exynos4_swap_chroma_crcb;
+		break;
+	case V4L2_PIX_FMT_YUYV:
+		reg = reg | EXYNOS4_DEC_YUV_422_IMG |
+				EXYNOS4_YUV_422_IP_YUV_422_1P_IMG |
+				exynos4_swap_chroma_cbcr;
+		break;
+
+	case V4L2_PIX_FMT_YVYU:
+		reg = reg | EXYNOS4_DEC_YUV_422_IMG |
+				EXYNOS4_YUV_422_IP_YUV_422_1P_IMG |
+				exynos4_swap_chroma_crcb;
+		break;
+	case V4L2_PIX_FMT_NV16:
+		reg = reg | EXYNOS4_DEC_YUV_422_IMG |
+				EXYNOS4_YUV_422_IP_YUV_422_2P_IMG |
+				exynos4_swap_chroma_cbcr;
+		break;
+	case V4L2_PIX_FMT_NV61:
+		reg = reg | EXYNOS4_DEC_YUV_422_IMG |
+				EXYNOS4_YUV_422_IP_YUV_422_2P_IMG |
+				exynos4_swap_chroma_crcb;
+		break;
+	case V4L2_PIX_FMT_NV12:
+		reg = reg | EXYNOS4_DEC_YUV_420_IMG |
+				EXYNOS4_YUV_420_IP_YUV_420_2P_IMG |
+				exynos4_swap_chroma_cbcr;
+		break;
+	case V4L2_PIX_FMT_NV21:
+		reg = reg | EXYNOS4_DEC_YUV_420_IMG |
+				EXYNOS4_YUV_420_IP_YUV_420_2P_IMG |
+				exynos4_swap_chroma_crcb;
+		break;
+	case V4L2_PIX_FMT_YUV420:
+		reg = reg | EXYNOS4_DEC_YUV_420_IMG |
+				EXYNOS4_YUV_420_IP_YUV_420_3P_IMG |
+				exynos4_swap_chroma_cbcr;
+		break;
+	default:
+		break;
+
+	}
+
+	writel(reg, base + EXYNOS4_IMG_FMT_REG);
+}
+
+void __exynos4_jpeg_set_enc_out_fmt(void __iomem *base, unsigned int out_fmt,
+				    unsigned int version)
+{
+	unsigned int reg;
+
+	reg = readl(base + EXYNOS4_IMG_FMT_REG) &
+			~(version == SJPEG_EXYNOS4 ? EXYNOS4_ENC_FMT_MASK :
+			  EXYNOS5433_ENC_FMT_MASK); /* clear enc format */
+
+	switch (out_fmt) {
+	case V4L2_JPEG_CHROMA_SUBSAMPLING_GRAY:
+		reg = reg | EXYNOS4_ENC_FMT_GRAY;
+		break;
+
+	case V4L2_JPEG_CHROMA_SUBSAMPLING_444:
+		reg = reg | EXYNOS4_ENC_FMT_YUV_444;
+		break;
+
+	case V4L2_JPEG_CHROMA_SUBSAMPLING_422:
+		reg = reg | EXYNOS4_ENC_FMT_YUV_422;
+		break;
+
+	case V4L2_JPEG_CHROMA_SUBSAMPLING_420:
+		reg = reg | EXYNOS4_ENC_FMT_YUV_420;
+		break;
+
+	default:
+		break;
+	}
+
+	writel(reg, base + EXYNOS4_IMG_FMT_REG);
+}
+
+void exynos4_jpeg_set_interrupt(void __iomem *base, unsigned int version)
+{
+	unsigned int reg;
+
+	if (version == SJPEG_EXYNOS4) {
+		reg = readl(base + EXYNOS4_INT_EN_REG) & ~EXYNOS4_INT_EN_MASK;
+		writel(reg | EXYNOS4_INT_EN_ALL, base + EXYNOS4_INT_EN_REG);
+	} else {
+		reg = readl(base + EXYNOS4_INT_EN_REG) &
+							~EXYNOS5433_INT_EN_MASK;
+		writel(reg | EXYNOS5433_INT_EN_ALL, base + EXYNOS4_INT_EN_REG);
+	}
+}
+
+unsigned int exynos4_jpeg_get_int_status(void __iomem *base)
+{
+	unsigned int	int_status;
+
+	int_status = readl(base + EXYNOS4_INT_STATUS_REG);
+
+	return int_status;
+}
+
+unsigned int exynos4_jpeg_get_fifo_status(void __iomem *base)
+{
+	unsigned int fifo_status;
+
+	fifo_status = readl(base + EXYNOS4_FIFO_STATUS_REG);
+
+	return fifo_status;
+}
+
+void exynos4_jpeg_set_huf_table_enable(void __iomem *base, int value)
+{
+	unsigned int	reg;
+
+	reg = readl(base + EXYNOS4_JPEG_CNTL_REG) & ~EXYNOS4_HUF_TBL_EN;
+
+	if (value == 1)
+		writel(reg | EXYNOS4_HUF_TBL_EN,
+					base + EXYNOS4_JPEG_CNTL_REG);
+	else
+		writel(reg & ~EXYNOS4_HUF_TBL_EN,
+					base + EXYNOS4_JPEG_CNTL_REG);
+}
+
+void exynos4_jpeg_set_sys_int_enable(void __iomem *base, int value)
+{
+	unsigned int	reg;
+
+	reg = readl(base + EXYNOS4_JPEG_CNTL_REG) & ~(EXYNOS4_SYS_INT_EN);
+
+	if (value == 1)
+		writel(reg | EXYNOS4_SYS_INT_EN, base + EXYNOS4_JPEG_CNTL_REG);
+	else
+		writel(reg & ~EXYNOS4_SYS_INT_EN, base + EXYNOS4_JPEG_CNTL_REG);
+}
+
+void exynos4_jpeg_set_stream_buf_address(void __iomem *base,
+					 unsigned int address)
+{
+	writel(address, base + EXYNOS4_OUT_MEM_BASE_REG);
+}
+
+void exynos4_jpeg_set_stream_size(void __iomem *base,
+		unsigned int x_value, unsigned int y_value)
+{
+	writel(0x0, base + EXYNOS4_JPEG_IMG_SIZE_REG); /* clear */
+	writel(EXYNOS4_X_SIZE(x_value) | EXYNOS4_Y_SIZE(y_value),
+			base + EXYNOS4_JPEG_IMG_SIZE_REG);
+}
+
+void exynos4_jpeg_set_frame_buf_address(void __iomem *base,
+				struct s5p_jpeg_addr *exynos4_jpeg_addr)
+{
+	writel(exynos4_jpeg_addr->y, base + EXYNOS4_IMG_BA_PLANE_1_REG);
+	writel(exynos4_jpeg_addr->cb, base + EXYNOS4_IMG_BA_PLANE_2_REG);
+	writel(exynos4_jpeg_addr->cr, base + EXYNOS4_IMG_BA_PLANE_3_REG);
+}
+
+void exynos4_jpeg_set_encode_tbl_select(void __iomem *base,
+		enum exynos4_jpeg_img_quality_level level)
+{
+	unsigned int	reg;
+
+	reg = EXYNOS4_Q_TBL_COMP1_0 | EXYNOS4_Q_TBL_COMP2_1 |
+		EXYNOS4_Q_TBL_COMP3_1 |
+		EXYNOS4_HUFF_TBL_COMP1_AC_0_DC_1 |
+		EXYNOS4_HUFF_TBL_COMP2_AC_0_DC_0 |
+		EXYNOS4_HUFF_TBL_COMP3_AC_1_DC_1;
+
+	writel(reg, base + EXYNOS4_TBL_SEL_REG);
+}
+
+void exynos4_jpeg_set_dec_components(void __iomem *base, int n)
+{
+	unsigned int	reg;
+
+	reg = readl(base + EXYNOS4_TBL_SEL_REG);
+
+	reg |= EXYNOS4_NF(n);
+	writel(reg, base + EXYNOS4_TBL_SEL_REG);
+}
+
+void exynos4_jpeg_select_dec_q_tbl(void __iomem *base, char c, char x)
+{
+	unsigned int	reg;
+
+	reg = readl(base + EXYNOS4_TBL_SEL_REG);
+
+	reg |= EXYNOS4_Q_TBL_COMP(c, x);
+	writel(reg, base + EXYNOS4_TBL_SEL_REG);
+}
+
+void exynos4_jpeg_select_dec_h_tbl(void __iomem *base, char c, char x)
+{
+	unsigned int	reg;
+
+	reg = readl(base + EXYNOS4_TBL_SEL_REG);
+
+	reg |= EXYNOS4_HUFF_TBL_COMP(c, x);
+	writel(reg, base + EXYNOS4_TBL_SEL_REG);
+}
+
+void exynos4_jpeg_set_encode_hoff_cnt(void __iomem *base, unsigned int fmt)
+{
+	if (fmt == V4L2_PIX_FMT_GREY)
+		writel(0xd2, base + EXYNOS4_HUFF_CNT_REG);
+	else
+		writel(0x1a2, base + EXYNOS4_HUFF_CNT_REG);
+}
+
+unsigned int exynos4_jpeg_get_stream_size(void __iomem *base)
+{
+	unsigned int size;
+
+	size = readl(base + EXYNOS4_BITSTREAM_SIZE_REG);
+	return size;
+}
+
+void exynos4_jpeg_set_dec_bitstream_size(void __iomem *base, unsigned int size)
+{
+	writel(size, base + EXYNOS4_BITSTREAM_SIZE_REG);
+}
+
+void exynos4_jpeg_get_frame_size(void __iomem *base,
+			unsigned int *width, unsigned int *height)
+{
+	*width = (readl(base + EXYNOS4_DECODE_XY_SIZE_REG) &
+				EXYNOS4_DECODED_SIZE_MASK);
+	*height = (readl(base + EXYNOS4_DECODE_XY_SIZE_REG) >> 16) &
+				EXYNOS4_DECODED_SIZE_MASK;
+}
+
+unsigned int exynos4_jpeg_get_frame_fmt(void __iomem *base)
+{
+	return readl(base + EXYNOS4_DECODE_IMG_FMT_REG) &
+				EXYNOS4_JPEG_DECODED_IMG_FMT_MASK;
+}
+
+void exynos4_jpeg_set_timer_count(void __iomem *base, unsigned int size)
+{
+	writel(size, base + EXYNOS4_INT_TIMER_COUNT_REG);
+}
diff --git a/drivers/media/platform/s5p-jpeg/jpeg-hw-exynos4.h b/drivers/media/platform/s5p-jpeg/jpeg-hw-exynos4.h
new file mode 100644
index 0000000..cf6ec05
--- /dev/null
+++ b/drivers/media/platform/s5p-jpeg/jpeg-hw-exynos4.h
@@ -0,0 +1,47 @@
+/* Copyright (c) 2013 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com/
+ *
+ * Author: Jacek Anaszewski <j.anaszewski@samsung.com>
+ *
+ * Header file of the register interface for JPEG driver on Exynos4x12.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+*/
+
+#ifndef JPEG_HW_EXYNOS4_H_
+#define JPEG_HW_EXYNOS4_H_
+
+void exynos4_jpeg_sw_reset(void __iomem *base);
+void exynos4_jpeg_set_enc_dec_mode(void __iomem *base, unsigned int mode);
+void __exynos4_jpeg_set_img_fmt(void __iomem *base, unsigned int img_fmt,
+				unsigned int version);
+void __exynos4_jpeg_set_enc_out_fmt(void __iomem *base, unsigned int out_fmt,
+				    unsigned int version);
+void exynos4_jpeg_set_enc_tbl(void __iomem *base);
+void exynos4_jpeg_set_interrupt(void __iomem *base, unsigned int version);
+unsigned int exynos4_jpeg_get_int_status(void __iomem *base);
+void exynos4_jpeg_set_huf_table_enable(void __iomem *base, int value);
+void exynos4_jpeg_set_sys_int_enable(void __iomem *base, int value);
+void exynos4_jpeg_set_stream_buf_address(void __iomem *base,
+					 unsigned int address);
+void exynos4_jpeg_set_stream_size(void __iomem *base,
+		unsigned int x_value, unsigned int y_value);
+void exynos4_jpeg_set_frame_buf_address(void __iomem *base,
+				struct s5p_jpeg_addr *jpeg_addr);
+void exynos4_jpeg_set_encode_tbl_select(void __iomem *base,
+		enum exynos4_jpeg_img_quality_level level);
+void exynos4_jpeg_set_dec_components(void __iomem *base, int n);
+void exynos4_jpeg_select_dec_q_tbl(void __iomem *base, char c, char x);
+void exynos4_jpeg_select_dec_h_tbl(void __iomem *base, char c, char x);
+void exynos4_jpeg_set_encode_hoff_cnt(void __iomem *base, unsigned int fmt);
+void exynos4_jpeg_set_dec_bitstream_size(void __iomem *base, unsigned int size);
+unsigned int exynos4_jpeg_get_stream_size(void __iomem *base);
+void exynos4_jpeg_get_frame_size(void __iomem *base,
+			unsigned int *width, unsigned int *height);
+unsigned int exynos4_jpeg_get_frame_fmt(void __iomem *base);
+unsigned int exynos4_jpeg_get_fifo_status(void __iomem *base);
+void exynos4_jpeg_set_timer_count(void __iomem *base, unsigned int size);
+
+#endif /* JPEG_HW_EXYNOS4_H_ */
diff --git a/drivers/media/platform/s5p-jpeg/jpeg-hw-s5p.c b/drivers/media/platform/s5p-jpeg/jpeg-hw-s5p.c
new file mode 100644
index 0000000..b5f20e7
--- /dev/null
+++ b/drivers/media/platform/s5p-jpeg/jpeg-hw-s5p.c
@@ -0,0 +1,309 @@
+/* linux/drivers/media/platform/s5p-jpeg/jpeg-hw.h
+ *
+ * Copyright (c) 2011 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ *
+ * Author: Andrzej Pietrasiewicz <andrzej.p@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/io.h>
+#include <linux/videodev2.h>
+
+#include "jpeg-core.h"
+#include "jpeg-regs.h"
+#include "jpeg-hw-s5p.h"
+
+void s5p_jpeg_reset(void __iomem *regs)
+{
+	unsigned long reg;
+
+	writel(1, regs + S5P_JPG_SW_RESET);
+	reg = readl(regs + S5P_JPG_SW_RESET);
+	/* no other way but polling for when JPEG IP becomes operational */
+	while (reg != 0) {
+		cpu_relax();
+		reg = readl(regs + S5P_JPG_SW_RESET);
+	}
+}
+
+void s5p_jpeg_poweron(void __iomem *regs)
+{
+	writel(S5P_POWER_ON, regs + S5P_JPGCLKCON);
+}
+
+void s5p_jpeg_input_raw_mode(void __iomem *regs, unsigned long mode)
+{
+	unsigned long reg, m;
+
+	m = S5P_MOD_SEL_565;
+	if (mode == S5P_JPEG_RAW_IN_565)
+		m = S5P_MOD_SEL_565;
+	else if (mode == S5P_JPEG_RAW_IN_422)
+		m = S5P_MOD_SEL_422;
+
+	reg = readl(regs + S5P_JPGCMOD);
+	reg &= ~S5P_MOD_SEL_MASK;
+	reg |= m;
+	writel(reg, regs + S5P_JPGCMOD);
+}
+
+void s5p_jpeg_proc_mode(void __iomem *regs, unsigned long mode)
+{
+	unsigned long reg, m;
+
+	m = S5P_PROC_MODE_DECOMPR;
+	if (mode == S5P_JPEG_ENCODE)
+		m = S5P_PROC_MODE_COMPR;
+	else
+		m = S5P_PROC_MODE_DECOMPR;
+	reg = readl(regs + S5P_JPGMOD);
+	reg &= ~S5P_PROC_MODE_MASK;
+	reg |= m;
+	writel(reg, regs + S5P_JPGMOD);
+}
+
+void s5p_jpeg_subsampling_mode(void __iomem *regs, unsigned int mode)
+{
+	unsigned long reg, m;
+
+	if (mode == V4L2_JPEG_CHROMA_SUBSAMPLING_420)
+		m = S5P_SUBSAMPLING_MODE_420;
+	else
+		m = S5P_SUBSAMPLING_MODE_422;
+
+	reg = readl(regs + S5P_JPGMOD);
+	reg &= ~S5P_SUBSAMPLING_MODE_MASK;
+	reg |= m;
+	writel(reg, regs + S5P_JPGMOD);
+}
+
+unsigned int s5p_jpeg_get_subsampling_mode(void __iomem *regs)
+{
+	return readl(regs + S5P_JPGMOD) & S5P_SUBSAMPLING_MODE_MASK;
+}
+
+void s5p_jpeg_dri(void __iomem *regs, unsigned int dri)
+{
+	unsigned long reg;
+
+	reg = readl(regs + S5P_JPGDRI_U);
+	reg &= ~0xff;
+	reg |= (dri >> 8) & 0xff;
+	writel(reg, regs + S5P_JPGDRI_U);
+
+	reg = readl(regs + S5P_JPGDRI_L);
+	reg &= ~0xff;
+	reg |= dri & 0xff;
+	writel(reg, regs + S5P_JPGDRI_L);
+}
+
+void s5p_jpeg_qtbl(void __iomem *regs, unsigned int t, unsigned int n)
+{
+	unsigned long reg;
+
+	reg = readl(regs + S5P_JPG_QTBL);
+	reg &= ~S5P_QT_NUMt_MASK(t);
+	reg |= (n << S5P_QT_NUMt_SHIFT(t)) & S5P_QT_NUMt_MASK(t);
+	writel(reg, regs + S5P_JPG_QTBL);
+}
+
+void s5p_jpeg_htbl_ac(void __iomem *regs, unsigned int t)
+{
+	unsigned long reg;
+
+	reg = readl(regs + S5P_JPG_HTBL);
+	reg &= ~S5P_HT_NUMt_AC_MASK(t);
+	/* this driver uses table 0 for all color components */
+	reg |= (0 << S5P_HT_NUMt_AC_SHIFT(t)) & S5P_HT_NUMt_AC_MASK(t);
+	writel(reg, regs + S5P_JPG_HTBL);
+}
+
+void s5p_jpeg_htbl_dc(void __iomem *regs, unsigned int t)
+{
+	unsigned long reg;
+
+	reg = readl(regs + S5P_JPG_HTBL);
+	reg &= ~S5P_HT_NUMt_DC_MASK(t);
+	/* this driver uses table 0 for all color components */
+	reg |= (0 << S5P_HT_NUMt_DC_SHIFT(t)) & S5P_HT_NUMt_DC_MASK(t);
+	writel(reg, regs + S5P_JPG_HTBL);
+}
+
+void s5p_jpeg_y(void __iomem *regs, unsigned int y)
+{
+	unsigned long reg;
+
+	reg = readl(regs + S5P_JPGY_U);
+	reg &= ~0xff;
+	reg |= (y >> 8) & 0xff;
+	writel(reg, regs + S5P_JPGY_U);
+
+	reg = readl(regs + S5P_JPGY_L);
+	reg &= ~0xff;
+	reg |= y & 0xff;
+	writel(reg, regs + S5P_JPGY_L);
+}
+
+void s5p_jpeg_x(void __iomem *regs, unsigned int x)
+{
+	unsigned long reg;
+
+	reg = readl(regs + S5P_JPGX_U);
+	reg &= ~0xff;
+	reg |= (x >> 8) & 0xff;
+	writel(reg, regs + S5P_JPGX_U);
+
+	reg = readl(regs + S5P_JPGX_L);
+	reg &= ~0xff;
+	reg |= x & 0xff;
+	writel(reg, regs + S5P_JPGX_L);
+}
+
+void s5p_jpeg_rst_int_enable(void __iomem *regs, bool enable)
+{
+	unsigned long reg;
+
+	reg = readl(regs + S5P_JPGINTSE);
+	reg &= ~S5P_RSTm_INT_EN_MASK;
+	if (enable)
+		reg |= S5P_RSTm_INT_EN;
+	writel(reg, regs + S5P_JPGINTSE);
+}
+
+void s5p_jpeg_data_num_int_enable(void __iomem *regs, bool enable)
+{
+	unsigned long reg;
+
+	reg = readl(regs + S5P_JPGINTSE);
+	reg &= ~S5P_DATA_NUM_INT_EN_MASK;
+	if (enable)
+		reg |= S5P_DATA_NUM_INT_EN;
+	writel(reg, regs + S5P_JPGINTSE);
+}
+
+void s5p_jpeg_final_mcu_num_int_enable(void __iomem *regs, bool enbl)
+{
+	unsigned long reg;
+
+	reg = readl(regs + S5P_JPGINTSE);
+	reg &= ~S5P_FINAL_MCU_NUM_INT_EN_MASK;
+	if (enbl)
+		reg |= S5P_FINAL_MCU_NUM_INT_EN;
+	writel(reg, regs + S5P_JPGINTSE);
+}
+
+int s5p_jpeg_timer_stat(void __iomem *regs)
+{
+	return (int)((readl(regs + S5P_JPG_TIMER_ST) & S5P_TIMER_INT_STAT_MASK)
+		     >> S5P_TIMER_INT_STAT_SHIFT);
+}
+
+void s5p_jpeg_clear_timer_stat(void __iomem *regs)
+{
+	unsigned long reg;
+
+	reg = readl(regs + S5P_JPG_TIMER_SE);
+	reg &= ~S5P_TIMER_INT_STAT_MASK;
+	writel(reg, regs + S5P_JPG_TIMER_SE);
+}
+
+void s5p_jpeg_enc_stream_int(void __iomem *regs, unsigned long size)
+{
+	unsigned long reg;
+
+	reg = readl(regs + S5P_JPG_ENC_STREAM_INTSE);
+	reg &= ~S5P_ENC_STREAM_BOUND_MASK;
+	reg |= S5P_ENC_STREAM_INT_EN;
+	reg |= size & S5P_ENC_STREAM_BOUND_MASK;
+	writel(reg, regs + S5P_JPG_ENC_STREAM_INTSE);
+}
+
+int s5p_jpeg_enc_stream_stat(void __iomem *regs)
+{
+	return (int)(readl(regs + S5P_JPG_ENC_STREAM_INTST) &
+		     S5P_ENC_STREAM_INT_STAT_MASK);
+}
+
+void s5p_jpeg_clear_enc_stream_stat(void __iomem *regs)
+{
+	unsigned long reg;
+
+	reg = readl(regs + S5P_JPG_ENC_STREAM_INTSE);
+	reg &= ~S5P_ENC_STREAM_INT_MASK;
+	writel(reg, regs + S5P_JPG_ENC_STREAM_INTSE);
+}
+
+void s5p_jpeg_outform_raw(void __iomem *regs, unsigned long format)
+{
+	unsigned long reg, f;
+
+	f = S5P_DEC_OUT_FORMAT_422;
+	if (format == S5P_JPEG_RAW_OUT_422)
+		f = S5P_DEC_OUT_FORMAT_422;
+	else if (format == S5P_JPEG_RAW_OUT_420)
+		f = S5P_DEC_OUT_FORMAT_420;
+	reg = readl(regs + S5P_JPG_OUTFORM);
+	reg &= ~S5P_DEC_OUT_FORMAT_MASK;
+	reg |= f;
+	writel(reg, regs + S5P_JPG_OUTFORM);
+}
+
+void s5p_jpeg_jpgadr(void __iomem *regs, unsigned long addr)
+{
+	writel(addr, regs + S5P_JPG_JPGADR);
+}
+
+void s5p_jpeg_imgadr(void __iomem *regs, unsigned long addr)
+{
+	writel(addr, regs + S5P_JPG_IMGADR);
+}
+
+void s5p_jpeg_coef(void __iomem *regs, unsigned int i,
+			     unsigned int j, unsigned int coef)
+{
+	unsigned long reg;
+
+	reg = readl(regs + S5P_JPG_COEF(i));
+	reg &= ~S5P_COEFn_MASK(j);
+	reg |= (coef << S5P_COEFn_SHIFT(j)) & S5P_COEFn_MASK(j);
+	writel(reg, regs + S5P_JPG_COEF(i));
+}
+
+void s5p_jpeg_start(void __iomem *regs)
+{
+	writel(1, regs + S5P_JSTART);
+}
+
+int s5p_jpeg_result_stat_ok(void __iomem *regs)
+{
+	return (int)((readl(regs + S5P_JPGINTST) & S5P_RESULT_STAT_MASK)
+		     >> S5P_RESULT_STAT_SHIFT);
+}
+
+int s5p_jpeg_stream_stat_ok(void __iomem *regs)
+{
+	return !(int)((readl(regs + S5P_JPGINTST) & S5P_STREAM_STAT_MASK)
+		      >> S5P_STREAM_STAT_SHIFT);
+}
+
+void s5p_jpeg_clear_int(void __iomem *regs)
+{
+	readl(regs + S5P_JPGINTST);
+	writel(S5P_INT_RELEASE, regs + S5P_JPGCOM);
+	readl(regs + S5P_JPGOPR);
+}
+
+unsigned int s5p_jpeg_compressed_size(void __iomem *regs)
+{
+	unsigned long jpeg_size = 0;
+
+	jpeg_size |= (readl(regs + S5P_JPGCNT_U) & 0xff) << 16;
+	jpeg_size |= (readl(regs + S5P_JPGCNT_M) & 0xff) << 8;
+	jpeg_size |= (readl(regs + S5P_JPGCNT_L) & 0xff);
+
+	return (unsigned int)jpeg_size;
+}
diff --git a/drivers/media/platform/s5p-jpeg/jpeg-hw-s5p.h b/drivers/media/platform/s5p-jpeg/jpeg-hw-s5p.h
new file mode 100644
index 0000000..f208fa3
--- /dev/null
+++ b/drivers/media/platform/s5p-jpeg/jpeg-hw-s5p.h
@@ -0,0 +1,60 @@
+/* linux/drivers/media/platform/s5p-jpeg/jpeg-hw.h
+ *
+ * Copyright (c) 2011 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ *
+ * Author: Andrzej Pietrasiewicz <andrzej.p@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#ifndef JPEG_HW_S5P_H_
+#define JPEG_HW_S5P_H_
+
+#include <linux/io.h>
+#include <linux/videodev2.h>
+
+#include "jpeg-regs.h"
+
+#define S5P_JPEG_MIN_WIDTH		32
+#define S5P_JPEG_MIN_HEIGHT		32
+#define S5P_JPEG_MAX_WIDTH		8192
+#define S5P_JPEG_MAX_HEIGHT		8192
+#define S5P_JPEG_RAW_IN_565		0
+#define S5P_JPEG_RAW_IN_422		1
+#define S5P_JPEG_RAW_OUT_422		0
+#define S5P_JPEG_RAW_OUT_420		1
+
+void s5p_jpeg_reset(void __iomem *regs);
+void s5p_jpeg_poweron(void __iomem *regs);
+void s5p_jpeg_input_raw_mode(void __iomem *regs, unsigned long mode);
+void s5p_jpeg_proc_mode(void __iomem *regs, unsigned long mode);
+void s5p_jpeg_subsampling_mode(void __iomem *regs, unsigned int mode);
+unsigned int s5p_jpeg_get_subsampling_mode(void __iomem *regs);
+void s5p_jpeg_dri(void __iomem *regs, unsigned int dri);
+void s5p_jpeg_qtbl(void __iomem *regs, unsigned int t, unsigned int n);
+void s5p_jpeg_htbl_ac(void __iomem *regs, unsigned int t);
+void s5p_jpeg_htbl_dc(void __iomem *regs, unsigned int t);
+void s5p_jpeg_y(void __iomem *regs, unsigned int y);
+void s5p_jpeg_x(void __iomem *regs, unsigned int x);
+void s5p_jpeg_rst_int_enable(void __iomem *regs, bool enable);
+void s5p_jpeg_data_num_int_enable(void __iomem *regs, bool enable);
+void s5p_jpeg_final_mcu_num_int_enable(void __iomem *regs, bool enbl);
+int s5p_jpeg_timer_stat(void __iomem *regs);
+void s5p_jpeg_clear_timer_stat(void __iomem *regs);
+void s5p_jpeg_enc_stream_int(void __iomem *regs, unsigned long size);
+int s5p_jpeg_enc_stream_stat(void __iomem *regs);
+void s5p_jpeg_clear_enc_stream_stat(void __iomem *regs);
+void s5p_jpeg_outform_raw(void __iomem *regs, unsigned long format);
+void s5p_jpeg_jpgadr(void __iomem *regs, unsigned long addr);
+void s5p_jpeg_imgadr(void __iomem *regs, unsigned long addr);
+void s5p_jpeg_coef(void __iomem *regs, unsigned int i,
+			     unsigned int j, unsigned int coef);
+void s5p_jpeg_start(void __iomem *regs);
+int s5p_jpeg_result_stat_ok(void __iomem *regs);
+int s5p_jpeg_stream_stat_ok(void __iomem *regs);
+void s5p_jpeg_clear_int(void __iomem *regs);
+unsigned int s5p_jpeg_compressed_size(void __iomem *regs);
+
+#endif /* JPEG_HW_S5P_H_ */
diff --git a/drivers/media/platform/s5p-jpeg/jpeg-regs.h b/drivers/media/platform/s5p-jpeg/jpeg-regs.h
new file mode 100644
index 0000000..1870400
--- /dev/null
+++ b/drivers/media/platform/s5p-jpeg/jpeg-regs.h
@@ -0,0 +1,649 @@
+/* linux/drivers/media/platform/s5p-jpeg/jpeg-regs.h
+ *
+ * Register definition file for Samsung JPEG codec driver
+ *
+ * Copyright (c) 2011-2014 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ *
+ * Author: Andrzej Pietrasiewicz <andrzej.p@samsung.com>
+ * Author: Jacek Anaszewski <j.anaszewski@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef JPEG_REGS_H_
+#define JPEG_REGS_H_
+
+/* Register and bit definitions for S5PC210 */
+
+/* JPEG mode register */
+#define S5P_JPGMOD			0x00
+#define S5P_PROC_MODE_MASK		(0x1 << 3)
+#define S5P_PROC_MODE_DECOMPR		(0x1 << 3)
+#define S5P_PROC_MODE_COMPR		(0x0 << 3)
+#define S5P_SUBSAMPLING_MODE_MASK	0x7
+#define S5P_SUBSAMPLING_MODE_444	(0x0 << 0)
+#define S5P_SUBSAMPLING_MODE_422	(0x1 << 0)
+#define S5P_SUBSAMPLING_MODE_420	(0x2 << 0)
+#define S5P_SUBSAMPLING_MODE_GRAY	(0x3 << 0)
+
+/* JPEG operation status register */
+#define S5P_JPGOPR			0x04
+
+/* Quantization tables*/
+#define S5P_JPG_QTBL			0x08
+#define S5P_QT_NUMt_SHIFT(t)		(((t) - 1) << 1)
+#define S5P_QT_NUMt_MASK(t)		(0x3 << S5P_QT_NUMt_SHIFT(t))
+
+/* Huffman tables */
+#define S5P_JPG_HTBL			0x0c
+#define S5P_HT_NUMt_AC_SHIFT(t)		(((t) << 1) - 1)
+#define S5P_HT_NUMt_AC_MASK(t)		(0x1 << S5P_HT_NUMt_AC_SHIFT(t))
+
+#define S5P_HT_NUMt_DC_SHIFT(t)		(((t) - 1) << 1)
+#define S5P_HT_NUMt_DC_MASK(t)		(0x1 << S5P_HT_NUMt_DC_SHIFT(t))
+
+/* JPEG restart interval register upper byte */
+#define S5P_JPGDRI_U			0x10
+
+/* JPEG restart interval register lower byte */
+#define S5P_JPGDRI_L			0x14
+
+/* JPEG vertical resolution register upper byte */
+#define S5P_JPGY_U			0x18
+
+/* JPEG vertical resolution register lower byte */
+#define S5P_JPGY_L			0x1c
+
+/* JPEG horizontal resolution register upper byte */
+#define S5P_JPGX_U			0x20
+
+/* JPEG horizontal resolution register lower byte */
+#define S5P_JPGX_L			0x24
+
+/* JPEG byte count register upper byte */
+#define S5P_JPGCNT_U			0x28
+
+/* JPEG byte count register middle byte */
+#define S5P_JPGCNT_M			0x2c
+
+/* JPEG byte count register lower byte */
+#define S5P_JPGCNT_L			0x30
+
+/* JPEG interrupt setting register */
+#define S5P_JPGINTSE			0x34
+#define S5P_RSTm_INT_EN_MASK		(0x1 << 7)
+#define S5P_RSTm_INT_EN			(0x1 << 7)
+#define S5P_DATA_NUM_INT_EN_MASK	(0x1 << 6)
+#define S5P_DATA_NUM_INT_EN		(0x1 << 6)
+#define S5P_FINAL_MCU_NUM_INT_EN_MASK	(0x1 << 5)
+#define S5P_FINAL_MCU_NUM_INT_EN	(0x1 << 5)
+
+/* JPEG interrupt status register */
+#define S5P_JPGINTST			0x38
+#define S5P_RESULT_STAT_SHIFT		6
+#define S5P_RESULT_STAT_MASK		(0x1 << S5P_RESULT_STAT_SHIFT)
+#define S5P_STREAM_STAT_SHIFT		5
+#define S5P_STREAM_STAT_MASK		(0x1 << S5P_STREAM_STAT_SHIFT)
+
+/* JPEG command register */
+#define S5P_JPGCOM			0x4c
+#define S5P_INT_RELEASE			(0x1 << 2)
+
+/* Raw image data r/w address register */
+#define S5P_JPG_IMGADR			0x50
+
+/* JPEG file r/w address register */
+#define S5P_JPG_JPGADR			0x58
+
+/* Coefficient for RGB-to-YCbCr converter register */
+#define S5P_JPG_COEF(n)			(0x5c + (((n) - 1) << 2))
+#define S5P_COEFn_SHIFT(j)		((3 - (j)) << 3)
+#define S5P_COEFn_MASK(j)		(0xff << S5P_COEFn_SHIFT(j))
+
+/* JPEG color mode register */
+#define S5P_JPGCMOD			0x68
+#define S5P_MOD_SEL_MASK		(0x7 << 5)
+#define S5P_MOD_SEL_422			(0x1 << 5)
+#define S5P_MOD_SEL_565			(0x2 << 5)
+#define S5P_MODE_Y16_MASK		(0x1 << 1)
+#define S5P_MODE_Y16			(0x1 << 1)
+
+/* JPEG clock control register */
+#define S5P_JPGCLKCON			0x6c
+#define S5P_CLK_DOWN_READY		(0x1 << 1)
+#define S5P_POWER_ON			(0x1 << 0)
+
+/* JPEG start register */
+#define S5P_JSTART			0x70
+
+/* JPEG SW reset register */
+#define S5P_JPG_SW_RESET		0x78
+
+/* JPEG timer setting register */
+#define S5P_JPG_TIMER_SE		0x7c
+#define S5P_TIMER_INT_EN_MASK		(0x1 << 31)
+#define S5P_TIMER_INT_EN		(0x1 << 31)
+#define S5P_TIMER_INIT_MASK		0x7fffffff
+
+/* JPEG timer status register */
+#define S5P_JPG_TIMER_ST		0x80
+#define S5P_TIMER_INT_STAT_SHIFT	31
+#define S5P_TIMER_INT_STAT_MASK		(0x1 << S5P_TIMER_INT_STAT_SHIFT)
+#define S5P_TIMER_CNT_SHIFT		0
+#define S5P_TIMER_CNT_MASK		0x7fffffff
+
+/* JPEG decompression output format register */
+#define S5P_JPG_OUTFORM			0x88
+#define S5P_DEC_OUT_FORMAT_MASK		(0x1 << 0)
+#define S5P_DEC_OUT_FORMAT_422		(0x0 << 0)
+#define S5P_DEC_OUT_FORMAT_420		(0x1 << 0)
+
+/* JPEG version register */
+#define S5P_JPG_VERSION			0x8c
+
+/* JPEG compressed stream size interrupt setting register */
+#define S5P_JPG_ENC_STREAM_INTSE	0x98
+#define S5P_ENC_STREAM_INT_MASK		(0x1 << 24)
+#define S5P_ENC_STREAM_INT_EN		(0x1 << 24)
+#define S5P_ENC_STREAM_BOUND_MASK	0xffffff
+
+/* JPEG compressed stream size interrupt status register */
+#define S5P_JPG_ENC_STREAM_INTST	0x9c
+#define S5P_ENC_STREAM_INT_STAT_MASK	0x1
+
+/* JPEG quantizer table register */
+#define S5P_JPG_QTBL_CONTENT(n)		(0x400 + (n) * 0x100)
+
+/* JPEG DC Huffman table register */
+#define S5P_JPG_HDCTBL(n)		(0x800 + (n) * 0x400)
+
+/* JPEG DC Huffman table register */
+#define S5P_JPG_HDCTBLG(n)		(0x840 + (n) * 0x400)
+
+/* JPEG AC Huffman table register */
+#define S5P_JPG_HACTBL(n)		(0x880 + (n) * 0x400)
+
+/* JPEG AC Huffman table register */
+#define S5P_JPG_HACTBLG(n)		(0x8c0 + (n) * 0x400)
+
+
+/* Register and bit definitions for Exynos 4x12 */
+
+/* JPEG Codec Control Registers */
+#define EXYNOS4_JPEG_CNTL_REG		0x00
+#define EXYNOS4_INT_EN_REG		0x04
+#define EXYNOS4_INT_TIMER_COUNT_REG	0x08
+#define EXYNOS4_INT_STATUS_REG		0x0c
+#define EXYNOS4_OUT_MEM_BASE_REG		0x10
+#define EXYNOS4_JPEG_IMG_SIZE_REG	0x14
+#define EXYNOS4_IMG_BA_PLANE_1_REG	0x18
+#define EXYNOS4_IMG_SO_PLANE_1_REG	0x1c
+#define EXYNOS4_IMG_PO_PLANE_1_REG	0x20
+#define EXYNOS4_IMG_BA_PLANE_2_REG	0x24
+#define EXYNOS4_IMG_SO_PLANE_2_REG	0x28
+#define EXYNOS4_IMG_PO_PLANE_2_REG	0x2c
+#define EXYNOS4_IMG_BA_PLANE_3_REG	0x30
+#define EXYNOS4_IMG_SO_PLANE_3_REG	0x34
+#define EXYNOS4_IMG_PO_PLANE_3_REG	0x38
+
+#define EXYNOS4_TBL_SEL_REG		0x3c
+
+#define EXYNOS4_IMG_FMT_REG		0x40
+
+#define EXYNOS4_BITSTREAM_SIZE_REG	0x44
+#define EXYNOS4_PADDING_REG		0x48
+#define EXYNOS4_HUFF_CNT_REG		0x4c
+#define EXYNOS4_FIFO_STATUS_REG	0x50
+#define EXYNOS4_DECODE_XY_SIZE_REG	0x54
+#define EXYNOS4_DECODE_IMG_FMT_REG	0x58
+
+#define EXYNOS4_QUAN_TBL_ENTRY_REG	0x100
+#define EXYNOS4_HUFF_TBL_ENTRY_REG	0x200
+
+
+/****************************************************************/
+/* Bit definition part						*/
+/****************************************************************/
+
+/* JPEG CNTL Register bit */
+#define EXYNOS4_ENC_DEC_MODE_MASK	(0xfffffffc << 0)
+#define EXYNOS4_DEC_MODE		(1 << 0)
+#define EXYNOS4_ENC_MODE		(1 << 1)
+#define EXYNOS4_AUTO_RST_MARKER		(1 << 2)
+#define EXYNOS4_RST_INTERVAL_SHIFT	3
+#define EXYNOS4_RST_INTERVAL(x)		(((x) & 0xffff) \
+						<< EXYNOS4_RST_INTERVAL_SHIFT)
+#define EXYNOS4_HUF_TBL_EN		(1 << 19)
+#define EXYNOS4_HOR_SCALING_SHIFT	20
+#define EXYNOS4_HOR_SCALING_MASK	(3 << EXYNOS4_HOR_SCALING_SHIFT)
+#define EXYNOS4_HOR_SCALING(x)		(((x) & 0x3) \
+						<< EXYNOS4_HOR_SCALING_SHIFT)
+#define EXYNOS4_VER_SCALING_SHIFT	22
+#define EXYNOS4_VER_SCALING_MASK	(3 << EXYNOS4_VER_SCALING_SHIFT)
+#define EXYNOS4_VER_SCALING(x)		(((x) & 0x3) \
+						<< EXYNOS4_VER_SCALING_SHIFT)
+#define EXYNOS4_PADDING			(1 << 27)
+#define EXYNOS4_SYS_INT_EN		(1 << 28)
+#define EXYNOS4_SOFT_RESET_HI		(1 << 29)
+
+/* JPEG INT Register bit */
+#define EXYNOS4_INT_EN_MASK		(0x1f << 0)
+#define EXYNOS5433_INT_EN_MASK		(0x1ff << 0)
+#define EXYNOS4_PROT_ERR_INT_EN		(1 << 0)
+#define EXYNOS4_IMG_COMPLETION_INT_EN	(1 << 1)
+#define EXYNOS4_DEC_INVALID_FORMAT_EN	(1 << 2)
+#define EXYNOS4_MULTI_SCAN_ERROR_EN	(1 << 3)
+#define EXYNOS4_FRAME_ERR_EN		(1 << 4)
+#define EXYNOS4_INT_EN_ALL		(0x1f << 0)
+#define EXYNOS5433_INT_EN_ALL		(0x1b6 << 0)
+
+#define EXYNOS4_MOD_REG_PROC_ENC	(0 << 3)
+#define EXYNOS4_MOD_REG_PROC_DEC	(1 << 3)
+
+#define EXYNOS4_MOD_REG_SUBSAMPLE_444	(0 << 0)
+#define EXYNOS4_MOD_REG_SUBSAMPLE_422	(1 << 0)
+#define EXYNOS4_MOD_REG_SUBSAMPLE_420	(2 << 0)
+#define EXYNOS4_MOD_REG_SUBSAMPLE_GRAY	(3 << 0)
+
+
+/* JPEG IMAGE SIZE Register bit */
+#define EXYNOS4_X_SIZE_SHIFT		0
+#define EXYNOS4_X_SIZE_MASK		(0xffff << EXYNOS4_X_SIZE_SHIFT)
+#define EXYNOS4_X_SIZE(x)		(((x) & 0xffff) << EXYNOS4_X_SIZE_SHIFT)
+#define EXYNOS4_Y_SIZE_SHIFT		16
+#define EXYNOS4_Y_SIZE_MASK		(0xffff << EXYNOS4_Y_SIZE_SHIFT)
+#define EXYNOS4_Y_SIZE(x)		(((x) & 0xffff) << EXYNOS4_Y_SIZE_SHIFT)
+
+/* JPEG IMAGE FORMAT Register bit */
+#define EXYNOS4_ENC_IN_FMT_MASK		0xffff0000
+#define EXYNOS4_ENC_GRAY_IMG		(0 << 0)
+#define EXYNOS4_ENC_RGB_IMG		(1 << 0)
+#define EXYNOS4_ENC_YUV_444_IMG		(2 << 0)
+#define EXYNOS4_ENC_YUV_422_IMG		(3 << 0)
+#define EXYNOS4_ENC_YUV_440_IMG		(4 << 0)
+
+#define EXYNOS4_DEC_GRAY_IMG		(0 << 0)
+#define EXYNOS4_DEC_RGB_IMG		(1 << 0)
+#define EXYNOS4_DEC_YUV_444_IMG		(2 << 0)
+#define EXYNOS4_DEC_YUV_422_IMG		(3 << 0)
+#define EXYNOS4_DEC_YUV_420_IMG		(4 << 0)
+
+#define EXYNOS4_GRAY_IMG_IP_SHIFT	3
+#define EXYNOS4_GRAY_IMG_IP_MASK	(7 << EXYNOS4_GRAY_IMG_IP_SHIFT)
+#define EXYNOS4_GRAY_IMG_IP		(4 << EXYNOS4_GRAY_IMG_IP_SHIFT)
+
+#define EXYNOS4_RGB_IP_SHIFT		6
+#define EXYNOS4_RGB_IP_MASK		(7 << EXYNOS4_RGB_IP_SHIFT)
+#define EXYNOS4_RGB_IP_RGB_16BIT_IMG	(4 << EXYNOS4_RGB_IP_SHIFT)
+#define EXYNOS4_RGB_IP_RGB_32BIT_IMG	(5 << EXYNOS4_RGB_IP_SHIFT)
+
+#define EXYNOS4_YUV_444_IP_SHIFT		9
+#define EXYNOS4_YUV_444_IP_MASK			(7 << EXYNOS4_YUV_444_IP_SHIFT)
+#define EXYNOS4_YUV_444_IP_YUV_444_2P_IMG	(4 << EXYNOS4_YUV_444_IP_SHIFT)
+#define EXYNOS4_YUV_444_IP_YUV_444_3P_IMG	(5 << EXYNOS4_YUV_444_IP_SHIFT)
+
+#define EXYNOS4_YUV_422_IP_SHIFT		12
+#define EXYNOS4_YUV_422_IP_MASK			(7 << EXYNOS4_YUV_422_IP_SHIFT)
+#define EXYNOS4_YUV_422_IP_YUV_422_1P_IMG	(4 << EXYNOS4_YUV_422_IP_SHIFT)
+#define EXYNOS4_YUV_422_IP_YUV_422_2P_IMG	(5 << EXYNOS4_YUV_422_IP_SHIFT)
+#define EXYNOS4_YUV_422_IP_YUV_422_3P_IMG	(6 << EXYNOS4_YUV_422_IP_SHIFT)
+
+#define EXYNOS4_YUV_420_IP_SHIFT		15
+#define EXYNOS4_YUV_420_IP_MASK			(7 << EXYNOS4_YUV_420_IP_SHIFT)
+#define EXYNOS4_YUV_420_IP_YUV_420_2P_IMG	(4 << EXYNOS4_YUV_420_IP_SHIFT)
+#define EXYNOS4_YUV_420_IP_YUV_420_3P_IMG	(5 << EXYNOS4_YUV_420_IP_SHIFT)
+
+#define EXYNOS4_ENC_FMT_SHIFT			24
+#define EXYNOS4_ENC_FMT_MASK			(3 << EXYNOS4_ENC_FMT_SHIFT)
+#define EXYNOS5433_ENC_FMT_MASK			(7 << EXYNOS4_ENC_FMT_SHIFT)
+
+#define EXYNOS4_ENC_FMT_GRAY			(0 << EXYNOS4_ENC_FMT_SHIFT)
+#define EXYNOS4_ENC_FMT_YUV_444			(1 << EXYNOS4_ENC_FMT_SHIFT)
+#define EXYNOS4_ENC_FMT_YUV_422			(2 << EXYNOS4_ENC_FMT_SHIFT)
+#define EXYNOS4_ENC_FMT_YUV_420			(3 << EXYNOS4_ENC_FMT_SHIFT)
+
+#define EXYNOS4_JPEG_DECODED_IMG_FMT_MASK	0x03
+
+#define EXYNOS4_SWAP_CHROMA_CRCB		(1 << 26)
+#define EXYNOS4_SWAP_CHROMA_CBCR		(0 << 26)
+#define EXYNOS5433_SWAP_CHROMA_CRCB		(1 << 27)
+#define EXYNOS5433_SWAP_CHROMA_CBCR		(0 << 27)
+
+/* JPEG HUFF count Register bit */
+#define EXYNOS4_HUFF_COUNT_MASK			0xffff
+
+/* JPEG Decoded_img_x_y_size Register bit */
+#define EXYNOS4_DECODED_SIZE_MASK		0x0000ffff
+
+/* JPEG Decoded image format Register bit */
+#define EXYNOS4_DECODED_IMG_FMT_MASK		0x3
+
+/* JPEG TBL SEL Register bit */
+#define EXYNOS4_Q_TBL_COMP(c, n)	((n) << (((c) - 1) << 1))
+
+#define EXYNOS4_Q_TBL_COMP1_0		EXYNOS4_Q_TBL_COMP(1, 0)
+#define EXYNOS4_Q_TBL_COMP1_1		EXYNOS4_Q_TBL_COMP(1, 1)
+#define EXYNOS4_Q_TBL_COMP1_2		EXYNOS4_Q_TBL_COMP(1, 2)
+#define EXYNOS4_Q_TBL_COMP1_3		EXYNOS4_Q_TBL_COMP(1, 3)
+
+#define EXYNOS4_Q_TBL_COMP2_0		EXYNOS4_Q_TBL_COMP(2, 0)
+#define EXYNOS4_Q_TBL_COMP2_1		EXYNOS4_Q_TBL_COMP(2, 1)
+#define EXYNOS4_Q_TBL_COMP2_2		EXYNOS4_Q_TBL_COMP(2, 2)
+#define EXYNOS4_Q_TBL_COMP2_3		EXYNOS4_Q_TBL_COMP(2, 3)
+
+#define EXYNOS4_Q_TBL_COMP3_0		EXYNOS4_Q_TBL_COMP(3, 0)
+#define EXYNOS4_Q_TBL_COMP3_1		EXYNOS4_Q_TBL_COMP(3, 1)
+#define EXYNOS4_Q_TBL_COMP3_2		EXYNOS4_Q_TBL_COMP(3, 2)
+#define EXYNOS4_Q_TBL_COMP3_3		EXYNOS4_Q_TBL_COMP(3, 3)
+
+#define EXYNOS4_HUFF_TBL_COMP(c, n)	((n) << ((((c) - 1) << 1) + 6))
+
+#define EXYNOS4_HUFF_TBL_COMP1_AC_0_DC_0	\
+	EXYNOS4_HUFF_TBL_COMP(1, 0)
+#define EXYNOS4_HUFF_TBL_COMP1_AC_0_DC_1	\
+	EXYNOS4_HUFF_TBL_COMP(1, 1)
+#define EXYNOS4_HUFF_TBL_COMP1_AC_1_DC_0	\
+	EXYNOS4_HUFF_TBL_COMP(1, 2)
+#define EXYNOS4_HUFF_TBL_COMP1_AC_1_DC_1	\
+	EXYNOS4_HUFF_TBL_COMP(1, 3)
+
+#define EXYNOS4_HUFF_TBL_COMP2_AC_0_DC_0	\
+	EXYNOS4_HUFF_TBL_COMP(2, 0)
+#define EXYNOS4_HUFF_TBL_COMP2_AC_0_DC_1	\
+	EXYNOS4_HUFF_TBL_COMP(2, 1)
+#define EXYNOS4_HUFF_TBL_COMP2_AC_1_DC_0	\
+	EXYNOS4_HUFF_TBL_COMP(2, 2)
+#define EXYNOS4_HUFF_TBL_COMP2_AC_1_DC_1	\
+	EXYNOS4_HUFF_TBL_COMP(2, 3)
+
+#define EXYNOS4_HUFF_TBL_COMP3_AC_0_DC_0	\
+	EXYNOS4_HUFF_TBL_COMP(3, 0)
+#define EXYNOS4_HUFF_TBL_COMP3_AC_0_DC_1	\
+	EXYNOS4_HUFF_TBL_COMP(3, 1)
+#define EXYNOS4_HUFF_TBL_COMP3_AC_1_DC_0	\
+	EXYNOS4_HUFF_TBL_COMP(3, 2)
+#define EXYNOS4_HUFF_TBL_COMP3_AC_1_DC_1	\
+	EXYNOS4_HUFF_TBL_COMP(3, 3)
+
+#define EXYNOS4_NF_SHIFT			16
+#define EXYNOS4_NF_MASK				0xff
+#define EXYNOS4_NF(x)				\
+	(((x) << EXYNOS4_NF_SHIFT) & EXYNOS4_NF_MASK)
+
+/* JPEG quantizer table register */
+#define EXYNOS4_QTBL_CONTENT(n)	(0x100 + (n) * 0x40)
+
+/* JPEG DC luminance (code length) Huffman table register */
+#define EXYNOS4_HUFF_TBL_HDCLL	0x200
+
+/* JPEG DC luminance (values) Huffman table register */
+#define EXYNOS4_HUFF_TBL_HDCLV	0x210
+
+/* JPEG DC chrominance (code length) Huffman table register */
+#define EXYNOS4_HUFF_TBL_HDCCL	0x220
+
+/* JPEG DC chrominance (values) Huffman table register */
+#define EXYNOS4_HUFF_TBL_HDCCV	0x230
+
+/* JPEG AC luminance (code length) Huffman table register */
+#define EXYNOS4_HUFF_TBL_HACLL	0x240
+
+/* JPEG AC luminance (values) Huffman table register */
+#define EXYNOS4_HUFF_TBL_HACLV	0x250
+
+/* JPEG AC chrominance (code length) Huffman table register */
+#define EXYNOS4_HUFF_TBL_HACCL	0x300
+
+/* JPEG AC chrominance (values) Huffman table register */
+#define EXYNOS4_HUFF_TBL_HACCV	0x310
+
+/* Register and bit definitions for Exynos 3250 */
+
+/* JPEG mode register */
+#define EXYNOS3250_JPGMOD			0x00
+#define EXYNOS3250_PROC_MODE_MASK		(0x1 << 3)
+#define EXYNOS3250_PROC_MODE_DECOMPR		(0x1 << 3)
+#define EXYNOS3250_PROC_MODE_COMPR		(0x0 << 3)
+#define EXYNOS3250_SUBSAMPLING_MODE_MASK	(0x7 << 0)
+#define EXYNOS3250_SUBSAMPLING_MODE_444		(0x0 << 0)
+#define EXYNOS3250_SUBSAMPLING_MODE_422		(0x1 << 0)
+#define EXYNOS3250_SUBSAMPLING_MODE_420		(0x2 << 0)
+#define EXYNOS3250_SUBSAMPLING_MODE_411		(0x6 << 0)
+#define EXYNOS3250_SUBSAMPLING_MODE_GRAY	(0x3 << 0)
+
+/* JPEG operation status register */
+#define EXYNOS3250_JPGOPR			0x04
+#define EXYNOS3250_JPGOPR_MASK			0x01
+
+/* Quantization and Huffman tables register */
+#define EXYNOS3250_QHTBL			0x08
+#define EXYNOS3250_QT_NUM_SHIFT(t)		((((t) - 1) << 1) + 8)
+#define EXYNOS3250_QT_NUM_MASK(t)		(0x3 << EXYNOS3250_QT_NUM_SHIFT(t))
+
+/* Huffman tables */
+#define EXYNOS3250_HT_NUM_AC_SHIFT(t)		(((t) << 1) - 1)
+#define EXYNOS3250_HT_NUM_AC_MASK(t)		(0x1 << EXYNOS3250_HT_NUM_AC_SHIFT(t))
+
+#define EXYNOS3250_HT_NUM_DC_SHIFT(t)		(((t) - 1) << 1)
+#define EXYNOS3250_HT_NUM_DC_MASK(t)		(0x1 << EXYNOS3250_HT_NUM_DC_SHIFT(t))
+
+/* JPEG restart interval register */
+#define EXYNOS3250_JPGDRI			0x0c
+#define EXYNOS3250_JPGDRI_MASK			0xffff
+
+/* JPEG vertical resolution register */
+#define EXYNOS3250_JPGY				0x10
+#define EXYNOS3250_JPGY_MASK			0xffff
+
+/* JPEG horizontal resolution register */
+#define EXYNOS3250_JPGX				0x14
+#define EXYNOS3250_JPGX_MASK			0xffff
+
+/* JPEG byte count register */
+#define EXYNOS3250_JPGCNT			0x18
+#define EXYNOS3250_JPGCNT_MASK			0xffffff
+
+/* JPEG interrupt mask register */
+#define EXYNOS3250_JPGINTSE			0x1c
+#define EXYNOS3250_JPEG_DONE_EN			(1 << 11)
+#define EXYNOS3250_WDMA_DONE_EN			(1 << 10)
+#define EXYNOS3250_RDMA_DONE_EN			(1 << 9)
+#define EXYNOS3250_ENC_STREAM_INT_EN		(1 << 8)
+#define EXYNOS3250_CORE_DONE_EN			(1 << 5)
+#define EXYNOS3250_ERR_INT_EN			(1 << 4)
+#define EXYNOS3250_HEAD_INT_EN			(1 << 3)
+
+/* JPEG interrupt status register */
+#define EXYNOS3250_JPGINTST			0x20
+#define EXYNOS3250_JPEG_DONE			(1 << 11)
+#define EXYNOS3250_WDMA_DONE			(1 << 10)
+#define EXYNOS3250_RDMA_DONE			(1 << 9)
+#define EXYNOS3250_ENC_STREAM_STAT		(1 << 8)
+#define EXYNOS3250_RESULT_STAT			(1 << 5)
+#define EXYNOS3250_STREAM_STAT			(1 << 4)
+#define EXYNOS3250_HEADER_STAT			(1 << 3)
+
+/*
+ * Base address of the luma component DMA buffer
+ * of the raw input or output image.
+ */
+#define EXYNOS3250_LUMA_BASE			0x100
+#define EXYNOS3250_SRC_TILE_EN_MASK		0x100
+
+/* Stride of source or destination luma raw image buffer */
+#define EXYNOS3250_LUMA_STRIDE			0x104
+
+/* Horizontal/vertical offset of active region in luma raw image buffer */
+#define EXYNOS3250_LUMA_XY_OFFSET		0x108
+#define EXYNOS3250_LUMA_YY_OFFSET_SHIFT		18
+#define EXYNOS3250_LUMA_YY_OFFSET_MASK		(0x1fff << EXYNOS3250_LUMA_YY_OFFSET_SHIFT)
+#define EXYNOS3250_LUMA_YX_OFFSET_SHIFT		2
+#define EXYNOS3250_LUMA_YX_OFFSET_MASK		(0x1fff << EXYNOS3250_LUMA_YX_OFFSET_SHIFT)
+
+/*
+ * Base address of the chroma(Cb) component DMA buffer
+ * of the raw input or output image.
+ */
+#define EXYNOS3250_CHROMA_BASE			0x10c
+
+/* Stride of source or destination chroma(Cb) raw image buffer */
+#define EXYNOS3250_CHROMA_STRIDE		0x110
+
+/* Horizontal/vertical offset of active region in chroma(Cb) raw image buffer */
+#define EXYNOS3250_CHROMA_XY_OFFSET		0x114
+#define EXYNOS3250_CHROMA_YY_OFFSET_SHIFT	18
+#define EXYNOS3250_CHROMA_YY_OFFSET_MASK	(0x1fff << EXYNOS3250_CHROMA_YY_OFFSET_SHIFT)
+#define EXYNOS3250_CHROMA_YX_OFFSET_SHIFT	2
+#define EXYNOS3250_CHROMA_YX_OFFSET_MASK	(0x1fff << EXYNOS3250_CHROMA_YX_OFFSET_SHIFT)
+
+/*
+ * Base address of the chroma(Cr) component DMA buffer
+ * of the raw input or output image.
+ */
+#define EXYNOS3250_CHROMA_CR_BASE		0x118
+
+/* Stride of source or destination chroma(Cr) raw image buffer */
+#define EXYNOS3250_CHROMA_CR_STRIDE		0x11c
+
+/* Horizontal/vertical offset of active region in chroma(Cb) raw image buffer */
+#define EXYNOS3250_CHROMA_CR_XY_OFFSET		0x120
+#define EXYNOS3250_CHROMA_CR_YY_OFFSET_SHIFT	18
+#define EXYNOS3250_CHROMA_CR_YY_OFFSET_MASK	(0x1fff << EXYNOS3250_CHROMA_CR_YY_OFFSET_SHIFT)
+#define EXYNOS3250_CHROMA_CR_YX_OFFSET_SHIFT	2
+#define EXYNOS3250_CHROMA_CR_YX_OFFSET_MASK	(0x1fff << EXYNOS3250_CHROMA_CR_YX_OFFSET_SHIFT)
+
+/* Raw image data r/w address register */
+#define EXYNOS3250_JPG_IMGADR			0x50
+
+/* Source or destination JPEG file DMA buffer address */
+#define EXYNOS3250_JPG_JPGADR			0x124
+
+/* Coefficients for RGB-to-YCbCr converter register */
+#define EXYNOS3250_JPG_COEF(n)			(0x128 + (((n) - 1) << 2))
+#define EXYNOS3250_COEF_SHIFT(j)		((3 - (j)) << 3)
+#define EXYNOS3250_COEF_MASK(j)			(0xff << EXYNOS3250_COEF_SHIFT(j))
+
+/* Raw input format setting */
+#define EXYNOS3250_JPGCMOD			0x134
+#define EXYNOS3250_SRC_TILE_EN			(0x1 << 10)
+#define EXYNOS3250_SRC_NV_MASK			(0x1 << 9)
+#define EXYNOS3250_SRC_NV12			(0x0 << 9)
+#define EXYNOS3250_SRC_NV21			(0x1 << 9)
+#define EXYNOS3250_SRC_BIG_ENDIAN_MASK		(0x1 << 8)
+#define EXYNOS3250_SRC_BIG_ENDIAN		(0x1 << 8)
+#define EXYNOS3250_MODE_SEL_MASK		(0x7 << 5)
+#define EXYNOS3250_MODE_SEL_420_2P		(0x0 << 5)
+#define EXYNOS3250_MODE_SEL_422_1P_LUM_CHR	(0x1 << 5)
+#define EXYNOS3250_MODE_SEL_RGB565		(0x2 << 5)
+#define EXYNOS3250_MODE_SEL_422_1P_CHR_LUM	(0x3 << 5)
+#define EXYNOS3250_MODE_SEL_ARGB8888		(0x4 << 5)
+#define EXYNOS3250_MODE_SEL_420_3P		(0x5 << 5)
+#define EXYNOS3250_SRC_SWAP_RGB			(0x1 << 3)
+#define EXYNOS3250_SRC_SWAP_UV			(0x1 << 2)
+#define EXYNOS3250_MODE_Y16_MASK		(0x1 << 1)
+#define EXYNOS3250_MODE_Y16			(0x1 << 1)
+#define EXYNOS3250_HALF_EN_MASK			(0x1 << 0)
+#define EXYNOS3250_HALF_EN			(0x1 << 0)
+
+/* Power on/off and clock down control */
+#define EXYNOS3250_JPGCLKCON			0x138
+#define EXYNOS3250_CLK_DOWN_READY		(0x1 << 1)
+#define EXYNOS3250_POWER_ON			(0x1 << 0)
+
+/* Start compression or decompression */
+#define EXYNOS3250_JSTART			0x13c
+
+/* Restart decompression after header analysis */
+#define EXYNOS3250_JRSTART			0x140
+
+/* JPEG SW reset register */
+#define EXYNOS3250_SW_RESET			0x144
+
+/* JPEG timer setting register */
+#define EXYNOS3250_TIMER_SE			0x148
+#define EXYNOS3250_TIMER_INT_EN_SHIFT		31
+#define EXYNOS3250_TIMER_INT_EN			(1 << EXYNOS3250_TIMER_INT_EN_SHIFT)
+#define EXYNOS3250_TIMER_INIT_MASK		0x7fffffff
+
+/* JPEG timer status register */
+#define EXYNOS3250_TIMER_ST			0x14c
+#define EXYNOS3250_TIMER_INT_STAT_SHIFT		31
+#define EXYNOS3250_TIMER_INT_STAT		(1 << EXYNOS3250_TIMER_INT_STAT_SHIFT)
+#define EXYNOS3250_TIMER_CNT_SHIFT		0
+#define EXYNOS3250_TIMER_CNT_MASK		0x7fffffff
+
+/* Command status register */
+#define EXYNOS3250_COMSTAT			0x150
+#define EXYNOS3250_CUR_PROC_MODE		(0x1 << 1)
+#define EXYNOS3250_CUR_COM_MODE			(0x1 << 0)
+
+/* JPEG decompression output format register */
+#define EXYNOS3250_OUTFORM			0x154
+#define EXYNOS3250_OUT_ALPHA_MASK		(0xff << 24)
+#define EXYNOS3250_OUT_TILE_EN			(0x1 << 10)
+#define EXYNOS3250_OUT_NV_MASK			(0x1 << 9)
+#define EXYNOS3250_OUT_NV12			(0x0 << 9)
+#define EXYNOS3250_OUT_NV21			(0x1 << 9)
+#define EXYNOS3250_OUT_BIG_ENDIAN_MASK		(0x1 << 8)
+#define EXYNOS3250_OUT_BIG_ENDIAN		(0x1 << 8)
+#define EXYNOS3250_OUT_SWAP_RGB			(0x1 << 7)
+#define EXYNOS3250_OUT_SWAP_UV			(0x1 << 6)
+#define EXYNOS3250_OUT_FMT_MASK			(0x7 << 0)
+#define EXYNOS3250_OUT_FMT_420_2P		(0x0 << 0)
+#define EXYNOS3250_OUT_FMT_422_1P_LUM_CHR	(0x1 << 0)
+#define EXYNOS3250_OUT_FMT_422_1P_CHR_LUM	(0x3 << 0)
+#define EXYNOS3250_OUT_FMT_420_3P		(0x4 << 0)
+#define EXYNOS3250_OUT_FMT_RGB565		(0x5 << 0)
+#define EXYNOS3250_OUT_FMT_ARGB8888		(0x6 << 0)
+
+/* Input JPEG stream byte size for decompression */
+#define EXYNOS3250_DEC_STREAM_SIZE		0x158
+#define EXYNOS3250_DEC_STREAM_MASK		0x1fffffff
+
+/* The upper bound of the byte size of output compressed stream */
+#define EXYNOS3250_ENC_STREAM_BOUND		0x15c
+#define EXYNOS3250_ENC_STREAM_BOUND_MASK	0xffffc0
+
+/* Scale-down ratio when decoding */
+#define EXYNOS3250_DEC_SCALING_RATIO		0x160
+#define EXYNOS3250_DEC_SCALE_FACTOR_MASK	0x3
+#define EXYNOS3250_DEC_SCALE_FACTOR_8_8		0x0
+#define EXYNOS3250_DEC_SCALE_FACTOR_4_8		0x1
+#define EXYNOS3250_DEC_SCALE_FACTOR_2_8		0x2
+#define EXYNOS3250_DEC_SCALE_FACTOR_1_8		0x3
+
+/* Error check */
+#define EXYNOS3250_CRC_RESULT			0x164
+
+/* RDMA and WDMA operation status register */
+#define EXYNOS3250_DMA_OPER_STATUS		0x168
+#define EXYNOS3250_WDMA_OPER_STATUS		(0x1 << 1)
+#define EXYNOS3250_RDMA_OPER_STATUS		(0x1 << 0)
+
+/* DMA issue gathering number and issue number settings */
+#define EXYNOS3250_DMA_ISSUE_NUM		0x16c
+#define EXYNOS3250_WDMA_ISSUE_NUM_SHIFT		16
+#define EXYNOS3250_WDMA_ISSUE_NUM_MASK		(0x7 << EXYNOS3250_WDMA_ISSUE_NUM_SHIFT)
+#define EXYNOS3250_RDMA_ISSUE_NUM_SHIFT		8
+#define EXYNOS3250_RDMA_ISSUE_NUM_MASK		(0x7 << EXYNOS3250_RDMA_ISSUE_NUM_SHIFT)
+#define EXYNOS3250_ISSUE_GATHER_NUM_SHIFT	0
+#define EXYNOS3250_ISSUE_GATHER_NUM_MASK	(0x7 << EXYNOS3250_ISSUE_GATHER_NUM_SHIFT)
+#define EXYNOS3250_DMA_MO_COUNT			0x7
+
+/* Version register */
+#define EXYNOS3250_VERSION			0x1fc
+
+/* RGB <-> YUV conversion coefficients */
+#define EXYNOS3250_JPEG_ENC_COEF1		0x01352e1e
+#define EXYNOS3250_JPEG_ENC_COEF2		0x00b0ae83
+#define EXYNOS3250_JPEG_ENC_COEF3		0x020cdc13
+
+#define EXYNOS3250_JPEG_DEC_COEF1		0x04a80199
+#define EXYNOS3250_JPEG_DEC_COEF2		0x04a9a064
+#define EXYNOS3250_JPEG_DEC_COEF3		0x04a80102
+
+#endif /* JPEG_REGS_H_ */
+
diff --git a/drivers/media/platform/s5p-mfc/Makefile b/drivers/media/platform/s5p-mfc/Makefile
new file mode 100644
index 0000000..15f59b3
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/Makefile
@@ -0,0 +1,6 @@
+obj-$(CONFIG_VIDEO_SAMSUNG_S5P_MFC) += s5p-mfc.o
+s5p-mfc-y += s5p_mfc.o s5p_mfc_intr.o
+s5p-mfc-y += s5p_mfc_dec.o s5p_mfc_enc.o
+s5p-mfc-y += s5p_mfc_ctrl.o s5p_mfc_pm.o
+s5p-mfc-y += s5p_mfc_opr.o s5p_mfc_opr_v5.o s5p_mfc_opr_v6.o
+s5p-mfc-y += s5p_mfc_cmd.o s5p_mfc_cmd_v5.o s5p_mfc_cmd_v6.o
diff --git a/drivers/media/platform/s5p-mfc/regs-mfc-v6.h b/drivers/media/platform/s5p-mfc/regs-mfc-v6.h
new file mode 100644
index 0000000..83e01f3
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/regs-mfc-v6.h
@@ -0,0 +1,410 @@
+/*
+ * Register definition file for Samsung MFC V6.x Interface (FIMV) driver
+ *
+ * Copyright (c) 2012 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef _REGS_FIMV_V6_H
+#define _REGS_FIMV_V6_H
+
+#include <linux/kernel.h>
+#include <linux/sizes.h>
+
+#define S5P_FIMV_REG_SIZE_V6	(S5P_FIMV_END_ADDR - S5P_FIMV_START_ADDR)
+#define S5P_FIMV_REG_COUNT_V6	((S5P_FIMV_END_ADDR - S5P_FIMV_START_ADDR) / 4)
+
+/* Number of bits that the buffer address should be shifted for particular
+ * MFC buffers.  */
+#define S5P_FIMV_MEM_OFFSET_V6		0
+
+#define S5P_FIMV_START_ADDR_V6		0x0000
+#define S5P_FIMV_END_ADDR_V6		0xfd80
+
+#define S5P_FIMV_REG_CLEAR_BEGIN_V6	0xf000
+#define S5P_FIMV_REG_CLEAR_COUNT_V6	1024
+
+/* Codec Common Registers */
+#define S5P_FIMV_RISC_ON_V6			0x0000
+#define S5P_FIMV_RISC2HOST_INT_V6		0x003C
+#define S5P_FIMV_HOST2RISC_INT_V6		0x0044
+#define S5P_FIMV_RISC_BASE_ADDRESS_V6		0x0054
+
+#define S5P_FIMV_MFC_RESET_V6			0x1070
+
+#define S5P_FIMV_HOST2RISC_CMD_V6		0x1100
+#define S5P_FIMV_H2R_CMD_EMPTY_V6		0
+#define S5P_FIMV_H2R_CMD_SYS_INIT_V6		1
+#define S5P_FIMV_H2R_CMD_OPEN_INSTANCE_V6	2
+#define S5P_FIMV_CH_SEQ_HEADER_V6		3
+#define S5P_FIMV_CH_INIT_BUFS_V6		4
+#define S5P_FIMV_CH_FRAME_START_V6		5
+#define S5P_FIMV_H2R_CMD_CLOSE_INSTANCE_V6	6
+#define S5P_FIMV_H2R_CMD_SLEEP_V6		7
+#define S5P_FIMV_H2R_CMD_WAKEUP_V6		8
+#define S5P_FIMV_CH_LAST_FRAME_V6		9
+#define S5P_FIMV_H2R_CMD_FLUSH_V6		10
+/* RMVME: REALLOC used? */
+#define S5P_FIMV_CH_FRAME_START_REALLOC_V6	5
+
+#define S5P_FIMV_RISC2HOST_CMD_V6		0x1104
+#define S5P_FIMV_R2H_CMD_EMPTY_V6		0
+#define S5P_FIMV_R2H_CMD_SYS_INIT_RET_V6	1
+#define S5P_FIMV_R2H_CMD_OPEN_INSTANCE_RET_V6	2
+#define S5P_FIMV_R2H_CMD_SEQ_DONE_RET_V6	3
+#define S5P_FIMV_R2H_CMD_INIT_BUFFERS_RET_V6	4
+
+#define S5P_FIMV_R2H_CMD_CLOSE_INSTANCE_RET_V6	6
+#define S5P_FIMV_R2H_CMD_SLEEP_RET_V6		7
+#define S5P_FIMV_R2H_CMD_WAKEUP_RET_V6		8
+#define S5P_FIMV_R2H_CMD_COMPLETE_SEQ_RET_V6	9
+#define S5P_FIMV_R2H_CMD_DPB_FLUSH_RET_V6	10
+#define S5P_FIMV_R2H_CMD_NAL_ABORT_RET_V6	11
+#define S5P_FIMV_R2H_CMD_FW_STATUS_RET_V6	12
+#define S5P_FIMV_R2H_CMD_FRAME_DONE_RET_V6	13
+#define S5P_FIMV_R2H_CMD_FIELD_DONE_RET_V6	14
+#define S5P_FIMV_R2H_CMD_SLICE_DONE_RET_V6	15
+#define S5P_FIMV_R2H_CMD_ENC_BUFFER_FUL_RET_V6	16
+#define S5P_FIMV_R2H_CMD_ERR_RET_V6		32
+
+#define S5P_FIMV_MFC_BUS_RESET_CTRL            0x7110
+#define S5P_FIMV_FW_VERSION_V6			0xf000
+
+#define S5P_FIMV_INSTANCE_ID_V6			0xf008
+#define S5P_FIMV_CODEC_TYPE_V6			0xf00c
+#define S5P_FIMV_CONTEXT_MEM_ADDR_V6		0xf014
+#define S5P_FIMV_CONTEXT_MEM_SIZE_V6		0xf018
+#define S5P_FIMV_PIXEL_FORMAT_V6		0xf020
+
+#define S5P_FIMV_METADATA_ENABLE_V6		0xf024
+#define S5P_FIMV_DBG_BUFFER_ADDR_V6		0xf030
+#define S5P_FIMV_DBG_BUFFER_SIZE_V6		0xf034
+#define S5P_FIMV_RET_INSTANCE_ID_V6		0xf070
+
+#define S5P_FIMV_ERROR_CODE_V6			0xf074
+#define S5P_FIMV_ERR_WARNINGS_START_V6		160
+#define S5P_FIMV_ERR_DEC_MASK_V6		0xffff
+#define S5P_FIMV_ERR_DEC_SHIFT_V6		0
+#define S5P_FIMV_ERR_DSPL_MASK_V6		0xffff0000
+#define S5P_FIMV_ERR_DSPL_SHIFT_V6		16
+
+#define S5P_FIMV_DBG_BUFFER_OUTPUT_SIZE_V6	0xf078
+#define S5P_FIMV_METADATA_STATUS_V6		0xf07C
+#define S5P_FIMV_METADATA_ADDR_MB_INFO_V6	0xf080
+#define S5P_FIMV_METADATA_SIZE_MB_INFO_V6	0xf084
+
+/* Decoder Registers */
+#define S5P_FIMV_D_CRC_CTRL_V6			0xf0b0
+#define S5P_FIMV_D_DEC_OPTIONS_V6		0xf0b4
+#define S5P_FIMV_D_OPT_FMO_ASO_CTRL_MASK_V6	4
+#define S5P_FIMV_D_OPT_DDELAY_EN_SHIFT_V6	3
+#define S5P_FIMV_D_OPT_LF_CTRL_SHIFT_V6		1
+#define S5P_FIMV_D_OPT_LF_CTRL_MASK_V6		0x3
+#define S5P_FIMV_D_OPT_TILE_MODE_SHIFT_V6	0
+
+#define S5P_FIMV_D_DISPLAY_DELAY_V6		0xf0b8
+
+#define S5P_FIMV_D_SET_FRAME_WIDTH_V6		0xf0bc
+#define S5P_FIMV_D_SET_FRAME_HEIGHT_V6		0xf0c0
+
+#define S5P_FIMV_D_SEI_ENABLE_V6		0xf0c4
+
+/* Buffer setting registers */
+#define S5P_FIMV_D_MIN_NUM_DPB_V6		0xf0f0
+#define S5P_FIMV_D_MIN_LUMA_DPB_SIZE_V6		0xf0f4
+#define S5P_FIMV_D_MIN_CHROMA_DPB_SIZE_V6	0xf0f8
+#define S5P_FIMV_D_MVC_NUM_VIEWS_V6		0xf0fc
+#define S5P_FIMV_D_MIN_NUM_MV_V6		0xf100
+#define S5P_FIMV_D_NUM_DPB_V6			0xf130
+#define S5P_FIMV_D_LUMA_DPB_SIZE_V6		0xf134
+#define S5P_FIMV_D_CHROMA_DPB_SIZE_V6		0xf138
+#define S5P_FIMV_D_MV_BUFFER_SIZE_V6		0xf13c
+
+#define S5P_FIMV_D_LUMA_DPB_V6			0xf140
+#define S5P_FIMV_D_CHROMA_DPB_V6		0xf240
+#define S5P_FIMV_D_MV_BUFFER_V6			0xf340
+
+#define S5P_FIMV_D_SCRATCH_BUFFER_ADDR_V6	0xf440
+#define S5P_FIMV_D_SCRATCH_BUFFER_SIZE_V6	0xf444
+#define S5P_FIMV_D_METADATA_BUFFER_ADDR_V6	0xf448
+#define S5P_FIMV_D_METADATA_BUFFER_SIZE_V6	0xf44c
+#define S5P_FIMV_D_NUM_MV_V6			0xf478
+#define S5P_FIMV_D_CPB_BUFFER_ADDR_V6		0xf4b0
+#define S5P_FIMV_D_CPB_BUFFER_SIZE_V6		0xf4b4
+
+#define S5P_FIMV_D_AVAILABLE_DPB_FLAG_UPPER_V6	0xf4b8
+#define S5P_FIMV_D_AVAILABLE_DPB_FLAG_LOWER_V6	0xf4bc
+#define S5P_FIMV_D_CPB_BUFFER_OFFSET_V6		0xf4c0
+#define S5P_FIMV_D_SLICE_IF_ENABLE_V6		0xf4c4
+#define S5P_FIMV_D_PICTURE_TAG_V6		0xf4c8
+#define S5P_FIMV_D_STREAM_DATA_SIZE_V6		0xf4d0
+#define S5P_FIMV_D_INIT_BUFFER_OPTIONS_V6	0xf47c
+
+/* Display information register */
+#define S5P_FIMV_D_DISPLAY_FRAME_WIDTH_V6	0xf500
+#define S5P_FIMV_D_DISPLAY_FRAME_HEIGHT_V6	0xf504
+
+/* Display status */
+#define S5P_FIMV_D_DISPLAY_STATUS_V6		0xf508
+
+#define S5P_FIMV_D_DISPLAY_LUMA_ADDR_V6		0xf50c
+#define S5P_FIMV_D_DISPLAY_CHROMA_ADDR_V6	0xf510
+
+#define S5P_FIMV_D_DISPLAY_FRAME_TYPE_V6	0xf514
+
+#define S5P_FIMV_D_DISPLAY_CROP_INFO1_V6	0xf518
+#define S5P_FIMV_D_DISPLAY_CROP_INFO2_V6	0xf51c
+#define S5P_FIMV_D_DISPLAY_PICTURE_PROFILE_V6	0xf520
+#define S5P_FIMV_D_DISPLAY_LUMA_CRC_TOP_V6	0xf524
+#define S5P_FIMV_D_DISPLAY_CHROMA_CRC_TOP_V6	0xf528
+#define S5P_FIMV_D_DISPLAY_LUMA_CRC_BOT_V6	0xf52c
+#define S5P_FIMV_D_DISPLAY_CHROMA_CRC_BOT_V6	0xf530
+#define S5P_FIMV_D_DISPLAY_ASPECT_RATIO_V6	0xf534
+#define S5P_FIMV_D_DISPLAY_EXTENDED_AR_V6	0xf538
+
+/* Decoded picture information register */
+#define S5P_FIMV_D_DECODED_FRAME_WIDTH_V6	0xf53c
+#define S5P_FIMV_D_DECODED_FRAME_HEIGHT_V6	0xf540
+#define S5P_FIMV_D_DECODED_STATUS_V6		0xf544
+#define S5P_FIMV_DEC_CRC_GEN_MASK_V6		0x1
+#define S5P_FIMV_DEC_CRC_GEN_SHIFT_V6		6
+
+#define S5P_FIMV_D_DECODED_LUMA_ADDR_V6		0xf548
+#define S5P_FIMV_D_DECODED_CHROMA_ADDR_V6	0xf54c
+
+#define S5P_FIMV_D_DECODED_FRAME_TYPE_V6	0xf550
+#define S5P_FIMV_DECODE_FRAME_MASK_V6		7
+
+#define S5P_FIMV_D_DECODED_CROP_INFO1_V6	0xf554
+#define S5P_FIMV_D_DECODED_CROP_INFO2_V6	0xf558
+#define S5P_FIMV_D_DECODED_PICTURE_PROFILE_V6	0xf55c
+#define S5P_FIMV_D_DECODED_NAL_SIZE_V6		0xf560
+#define S5P_FIMV_D_DECODED_LUMA_CRC_TOP_V6	0xf564
+#define S5P_FIMV_D_DECODED_CHROMA_CRC_TOP_V6	0xf568
+#define S5P_FIMV_D_DECODED_LUMA_CRC_BOT_V6	0xf56c
+#define S5P_FIMV_D_DECODED_CHROMA_CRC_BOT_V6	0xf570
+
+/* Returned value register for specific setting */
+#define S5P_FIMV_D_RET_PICTURE_TAG_TOP_V6		0xf574
+#define S5P_FIMV_D_RET_PICTURE_TAG_BOT_V6		0xf578
+#define S5P_FIMV_D_RET_PICTURE_TIME_TOP_V6		0xf57c
+#define S5P_FIMV_D_RET_PICTURE_TIME_BOT_V6		0xf580
+#define S5P_FIMV_D_CHROMA_FORMAT_V6			0xf588
+#define S5P_FIMV_D_MPEG4_INFO_V6			0xf58c
+#define S5P_FIMV_D_H264_INFO_V6				0xf590
+
+#define S5P_FIMV_D_METADATA_ADDR_CONCEALED_MB_V6	0xf594
+#define S5P_FIMV_D_METADATA_SIZE_CONCEALED_MB_V6	0xf598
+#define S5P_FIMV_D_METADATA_ADDR_VC1_PARAM_V6		0xf59c
+#define S5P_FIMV_D_METADATA_SIZE_VC1_PARAM_V6		0xf5a0
+#define S5P_FIMV_D_METADATA_ADDR_SEI_NAL_V6		0xf5a4
+#define S5P_FIMV_D_METADATA_SIZE_SEI_NAL_V6		0xf5a8
+#define S5P_FIMV_D_METADATA_ADDR_VUI_V6			0xf5ac
+#define S5P_FIMV_D_METADATA_SIZE_VUI_V6			0xf5b0
+
+#define S5P_FIMV_D_MVC_VIEW_ID_V6		0xf5b4
+
+/* SEI related information */
+#define S5P_FIMV_D_FRAME_PACK_SEI_AVAIL_V6	0xf5f0
+#define S5P_FIMV_D_FRAME_PACK_ARRGMENT_ID_V6	0xf5f4
+#define S5P_FIMV_D_FRAME_PACK_SEI_INFO_V6	0xf5f8
+#define S5P_FIMV_D_FRAME_PACK_GRID_POS_V6	0xf5fc
+
+/* Encoder Registers */
+#define S5P_FIMV_E_FRAME_WIDTH_V6		0xf770
+#define S5P_FIMV_E_FRAME_HEIGHT_V6		0xf774
+#define S5P_FIMV_E_CROPPED_FRAME_WIDTH_V6	0xf778
+#define S5P_FIMV_E_CROPPED_FRAME_HEIGHT_V6	0xf77c
+#define S5P_FIMV_E_FRAME_CROP_OFFSET_V6		0xf780
+#define S5P_FIMV_E_ENC_OPTIONS_V6		0xf784
+#define S5P_FIMV_E_PICTURE_PROFILE_V6		0xf788
+#define S5P_FIMV_E_FIXED_PICTURE_QP_V6		0xf790
+
+#define S5P_FIMV_E_RC_CONFIG_V6			0xf794
+#define S5P_FIMV_E_RC_QP_BOUND_V6		0xf798
+#define S5P_FIMV_E_RC_RPARAM_V6			0xf79c
+#define S5P_FIMV_E_MB_RC_CONFIG_V6		0xf7a0
+#define S5P_FIMV_E_PADDING_CTRL_V6		0xf7a4
+#define S5P_FIMV_E_MV_HOR_RANGE_V6		0xf7ac
+#define S5P_FIMV_E_MV_VER_RANGE_V6		0xf7b0
+#define S5P_FIMV_E_MV_RANGE_V6_MASK		0x3fff
+
+#define S5P_FIMV_E_VBV_BUFFER_SIZE_V6		0xf84c
+#define S5P_FIMV_E_VBV_INIT_DELAY_V6		0xf850
+#define S5P_FIMV_E_NUM_DPB_V6			0xf890
+#define S5P_FIMV_E_LUMA_DPB_V6			0xf8c0
+#define S5P_FIMV_E_CHROMA_DPB_V6		0xf904
+#define S5P_FIMV_E_ME_BUFFER_V6			0xf948
+
+#define S5P_FIMV_E_SCRATCH_BUFFER_ADDR_V6	0xf98c
+#define S5P_FIMV_E_SCRATCH_BUFFER_SIZE_V6	0xf990
+#define S5P_FIMV_E_TMV_BUFFER0_V6		0xf994
+#define S5P_FIMV_E_TMV_BUFFER1_V6		0xf998
+#define S5P_FIMV_E_SOURCE_LUMA_ADDR_V6		0xf9f0
+#define S5P_FIMV_E_SOURCE_CHROMA_ADDR_V6	0xf9f4
+#define S5P_FIMV_E_STREAM_BUFFER_ADDR_V6	0xf9f8
+#define S5P_FIMV_E_STREAM_BUFFER_SIZE_V6	0xf9fc
+#define S5P_FIMV_E_ROI_BUFFER_ADDR_V6		0xfA00
+
+#define S5P_FIMV_E_PARAM_CHANGE_V6		0xfa04
+#define S5P_FIMV_E_IR_SIZE_V6			0xfa08
+#define S5P_FIMV_E_GOP_CONFIG_V6		0xfa0c
+#define S5P_FIMV_E_MSLICE_MODE_V6		0xfa10
+#define S5P_FIMV_E_MSLICE_SIZE_MB_V6		0xfa14
+#define S5P_FIMV_E_MSLICE_SIZE_BITS_V6		0xfa18
+#define S5P_FIMV_E_FRAME_INSERTION_V6		0xfa1c
+
+#define S5P_FIMV_E_RC_FRAME_RATE_V6		0xfa20
+#define S5P_FIMV_E_RC_BIT_RATE_V6		0xfa24
+#define S5P_FIMV_E_RC_QP_OFFSET_V6		0xfa28
+#define S5P_FIMV_E_RC_ROI_CTRL_V6		0xfa2c
+#define S5P_FIMV_E_PICTURE_TAG_V6		0xfa30
+#define S5P_FIMV_E_BIT_COUNT_ENABLE_V6		0xfa34
+#define S5P_FIMV_E_MAX_BIT_COUNT_V6		0xfa38
+#define S5P_FIMV_E_MIN_BIT_COUNT_V6		0xfa3c
+
+#define S5P_FIMV_E_METADATA_BUFFER_ADDR_V6		0xfa40
+#define S5P_FIMV_E_METADATA_BUFFER_SIZE_V6		0xfa44
+#define S5P_FIMV_E_STREAM_SIZE_V6			0xfa80
+#define S5P_FIMV_E_SLICE_TYPE_V6			0xfa84
+#define S5P_FIMV_E_PICTURE_COUNT_V6			0xfa88
+#define S5P_FIMV_E_RET_PICTURE_TAG_V6			0xfa8c
+#define S5P_FIMV_E_STREAM_BUFFER_WRITE_POINTER_V6	0xfa90
+
+#define S5P_FIMV_E_ENCODED_SOURCE_LUMA_ADDR_V6		0xfa94
+#define S5P_FIMV_E_ENCODED_SOURCE_CHROMA_ADDR_V6	0xfa98
+#define S5P_FIMV_E_RECON_LUMA_DPB_ADDR_V6		0xfa9c
+#define S5P_FIMV_E_RECON_CHROMA_DPB_ADDR_V6		0xfaa0
+#define S5P_FIMV_E_METADATA_ADDR_ENC_SLICE_V6		0xfaa4
+#define S5P_FIMV_E_METADATA_SIZE_ENC_SLICE_V6		0xfaa8
+
+#define S5P_FIMV_E_MPEG4_OPTIONS_V6		0xfb10
+#define S5P_FIMV_E_MPEG4_HEC_PERIOD_V6		0xfb14
+#define S5P_FIMV_E_ASPECT_RATIO_V6		0xfb50
+#define S5P_FIMV_E_EXTENDED_SAR_V6		0xfb54
+
+#define S5P_FIMV_E_H264_OPTIONS_V6		0xfb58
+#define S5P_FIMV_E_H264_LF_ALPHA_OFFSET_V6	0xfb5c
+#define S5P_FIMV_E_H264_LF_BETA_OFFSET_V6	0xfb60
+#define S5P_FIMV_E_H264_I_PERIOD_V6		0xfb64
+
+#define S5P_FIMV_E_H264_FMO_SLICE_GRP_MAP_TYPE_V6		0xfb68
+#define S5P_FIMV_E_H264_FMO_NUM_SLICE_GRP_MINUS1_V6		0xfb6c
+#define S5P_FIMV_E_H264_FMO_SLICE_GRP_CHANGE_DIR_V6		0xfb70
+#define S5P_FIMV_E_H264_FMO_SLICE_GRP_CHANGE_RATE_MINUS1_V6	0xfb74
+#define S5P_FIMV_E_H264_FMO_RUN_LENGTH_MINUS1_0_V6		0xfb78
+#define S5P_FIMV_E_H264_FMO_RUN_LENGTH_MINUS1_1_V6		0xfb7c
+#define S5P_FIMV_E_H264_FMO_RUN_LENGTH_MINUS1_2_V6		0xfb80
+#define S5P_FIMV_E_H264_FMO_RUN_LENGTH_MINUS1_3_V6		0xfb84
+
+#define S5P_FIMV_E_H264_ASO_SLICE_ORDER_0_V6	0xfb88
+#define S5P_FIMV_E_H264_ASO_SLICE_ORDER_1_V6	0xfb8c
+#define S5P_FIMV_E_H264_ASO_SLICE_ORDER_2_V6	0xfb90
+#define S5P_FIMV_E_H264_ASO_SLICE_ORDER_3_V6	0xfb94
+#define S5P_FIMV_E_H264_ASO_SLICE_ORDER_4_V6	0xfb98
+#define S5P_FIMV_E_H264_ASO_SLICE_ORDER_5_V6	0xfb9c
+#define S5P_FIMV_E_H264_ASO_SLICE_ORDER_6_V6	0xfba0
+#define S5P_FIMV_E_H264_ASO_SLICE_ORDER_7_V6	0xfba4
+
+#define S5P_FIMV_E_H264_CHROMA_QP_OFFSET_V6	0xfba8
+#define S5P_FIMV_E_H264_NUM_T_LAYER_V6		0xfbac
+
+#define S5P_FIMV_E_H264_HIERARCHICAL_QP_LAYER0_V6	0xfbb0
+#define S5P_FIMV_E_H264_HIERARCHICAL_QP_LAYER1_V6	0xfbb4
+#define S5P_FIMV_E_H264_HIERARCHICAL_QP_LAYER2_V6	0xfbb8
+#define S5P_FIMV_E_H264_HIERARCHICAL_QP_LAYER3_V6	0xfbbc
+#define S5P_FIMV_E_H264_HIERARCHICAL_QP_LAYER4_V6	0xfbc0
+#define S5P_FIMV_E_H264_HIERARCHICAL_QP_LAYER5_V6	0xfbc4
+#define S5P_FIMV_E_H264_HIERARCHICAL_QP_LAYER6_V6	0xfbc8
+
+#define S5P_FIMV_E_H264_FRAME_PACKING_SEI_INFO_V6		0xfc4c
+#define S5P_FIMV_ENC_FP_ARRANGEMENT_TYPE_SIDE_BY_SIDE_V6	0
+#define S5P_FIMV_ENC_FP_ARRANGEMENT_TYPE_TOP_BOTTOM_V6		1
+#define S5P_FIMV_ENC_FP_ARRANGEMENT_TYPE_TEMPORAL_V6		2
+
+#define S5P_FIMV_E_MVC_FRAME_QP_VIEW1_V6		0xfd40
+#define S5P_FIMV_E_MVC_RC_FRAME_RATE_VIEW1_V6		0xfd44
+#define S5P_FIMV_E_MVC_RC_BIT_RATE_VIEW1_V6		0xfd48
+#define S5P_FIMV_E_MVC_RC_QBOUND_VIEW1_V6		0xfd4c
+#define S5P_FIMV_E_MVC_RC_RPARA_VIEW1_V6		0xfd50
+#define S5P_FIMV_E_MVC_INTER_VIEW_PREDICTION_ON_V6	0xfd80
+
+/* Codec numbers  */
+#define S5P_FIMV_CODEC_NONE_V6		-1
+
+
+#define S5P_FIMV_CODEC_H264_DEC_V6	0
+#define S5P_FIMV_CODEC_H264_MVC_DEC_V6	1
+
+#define S5P_FIMV_CODEC_MPEG4_DEC_V6	3
+#define S5P_FIMV_CODEC_FIMV1_DEC_V6	4
+#define S5P_FIMV_CODEC_FIMV2_DEC_V6	5
+#define S5P_FIMV_CODEC_FIMV3_DEC_V6	6
+#define S5P_FIMV_CODEC_FIMV4_DEC_V6	7
+#define S5P_FIMV_CODEC_H263_DEC_V6	8
+#define S5P_FIMV_CODEC_VC1RCV_DEC_V6	9
+#define S5P_FIMV_CODEC_VC1_DEC_V6	10
+/* FIXME: Add 11~12 */
+#define S5P_FIMV_CODEC_MPEG2_DEC_V6	13
+#define S5P_FIMV_CODEC_VP8_DEC_V6	14
+/* FIXME: Add 15~16 */
+#define S5P_FIMV_CODEC_H264_ENC_V6	20
+#define S5P_FIMV_CODEC_H264_MVC_ENC_V6	21
+
+#define S5P_FIMV_CODEC_MPEG4_ENC_V6	23
+#define S5P_FIMV_CODEC_H263_ENC_V6	24
+
+#define S5P_FIMV_NV12M_HALIGN_V6		16
+#define S5P_FIMV_NV12MT_HALIGN_V6		16
+#define S5P_FIMV_NV12MT_VALIGN_V6		16
+
+#define S5P_FIMV_TMV_BUFFER_ALIGN_V6		16
+#define S5P_FIMV_LUMA_DPB_BUFFER_ALIGN_V6	256
+#define S5P_FIMV_CHROMA_DPB_BUFFER_ALIGN_V6	256
+#define S5P_FIMV_ME_BUFFER_ALIGN_V6		256
+#define S5P_FIMV_SCRATCH_BUFFER_ALIGN_V6	256
+
+#define S5P_FIMV_LUMA_MB_TO_PIXEL_V6		256
+#define S5P_FIMV_CHROMA_MB_TO_PIXEL_V6		128
+#define S5P_FIMV_NUM_TMV_BUFFERS_V6		2
+
+#define S5P_FIMV_MAX_FRAME_SIZE_V6		(2 * SZ_1M)
+#define S5P_FIMV_NUM_PIXELS_IN_MB_ROW_V6	16
+#define S5P_FIMV_NUM_PIXELS_IN_MB_COL_V6	16
+
+/* Buffer size requirements defined by hardware */
+#define S5P_FIMV_TMV_BUFFER_SIZE_V6(w, h)	(((w) + 1) * ((h) + 3) * 8)
+#define S5P_FIMV_ME_BUFFER_SIZE_V6(imw, imh, mbw, mbh) \
+	(((((imw + 127) / 64) * 16) *  DIV_ROUND_UP(imh, 64) * 256) + \
+	 (DIV_ROUND_UP((mbw) * (mbh), 32) * 16))
+#define S5P_FIMV_SCRATCH_BUF_SIZE_H264_DEC_V6(w, h)	(((w) * 192) + 64)
+#define S5P_FIMV_SCRATCH_BUF_SIZE_MPEG4_DEC_V6(w, h) \
+			((w) * 144 + 8192 * (h) + 49216 + 1048576)
+#define S5P_FIMV_SCRATCH_BUF_SIZE_VC1_DEC_V6(w, h) \
+						(2096 * ((w) + (h) + 1))
+#define S5P_FIMV_SCRATCH_BUF_SIZE_H263_DEC_V6(w, h)	((w) * 400)
+#define S5P_FIMV_SCRATCH_BUF_SIZE_VP8_DEC_V6(w, h) \
+			((w) * 32 + (h) * 128 + (((w) + 1) / 2) * 64 + 2112)
+#define S5P_FIMV_SCRATCH_BUF_SIZE_H264_ENC_V6(w, h) \
+			(((w) * 64) + (((w) + 1) * 16) + (4096 * 16))
+#define S5P_FIMV_SCRATCH_BUF_SIZE_MPEG4_ENC_V6(w, h) \
+			(((w) * 16) + (((w) + 1) * 16))
+
+/* MFC Context buffer sizes */
+#define MFC_CTX_BUF_SIZE_V6		(28 * SZ_1K)	/*  28KB */
+#define MFC_H264_DEC_CTX_BUF_SIZE_V6	(2 * SZ_1M)	/*  2MB */
+#define MFC_OTHER_DEC_CTX_BUF_SIZE_V6	(20 * SZ_1K)	/*  20KB */
+#define MFC_H264_ENC_CTX_BUF_SIZE_V6	(100 * SZ_1K)	/* 100KB */
+#define MFC_OTHER_ENC_CTX_BUF_SIZE_V6	(12 * SZ_1K)	/*  12KB */
+
+/* MFCv6 variant defines */
+#define MAX_FW_SIZE_V6			(SZ_1M)		/* 1MB */
+#define MAX_CPB_SIZE_V6			(3 * SZ_1M)	/* 3MB */
+#define MFC_VERSION_V6			0x61
+#define MFC_NUM_PORTS_V6		1
+
+#endif /* _REGS_FIMV_V6_H */
diff --git a/drivers/media/platform/s5p-mfc/regs-mfc-v7.h b/drivers/media/platform/s5p-mfc/regs-mfc-v7.h
new file mode 100644
index 0000000..1a5c6fd
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/regs-mfc-v7.h
@@ -0,0 +1,60 @@
+/*
+ * Register definition file for Samsung MFC V7.x Interface (FIMV) driver
+ *
+ * Copyright (c) 2013 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef _REGS_MFC_V7_H
+#define _REGS_MFC_V7_H
+
+#include "regs-mfc-v6.h"
+
+/* Additional features of v7 */
+#define S5P_FIMV_CODEC_VP8_ENC_V7	25
+
+/* Additional registers for v7 */
+#define S5P_FIMV_E_SOURCE_FIRST_ADDR_V7			0xf9e0
+#define S5P_FIMV_E_SOURCE_SECOND_ADDR_V7		0xf9e4
+#define S5P_FIMV_E_SOURCE_THIRD_ADDR_V7			0xf9e8
+#define S5P_FIMV_E_SOURCE_FIRST_STRIDE_V7		0xf9ec
+#define S5P_FIMV_E_SOURCE_SECOND_STRIDE_V7		0xf9f0
+#define S5P_FIMV_E_SOURCE_THIRD_STRIDE_V7		0xf9f4
+
+#define S5P_FIMV_E_ENCODED_SOURCE_FIRST_ADDR_V7		0xfa70
+#define S5P_FIMV_E_ENCODED_SOURCE_SECOND_ADDR_V7	0xfa74
+
+#define S5P_FIMV_E_VP8_OPTIONS_V7			0xfdb0
+#define S5P_FIMV_E_VP8_FILTER_OPTIONS_V7		0xfdb4
+#define S5P_FIMV_E_VP8_GOLDEN_FRAME_OPTION_V7		0xfdb8
+#define S5P_FIMV_E_VP8_NUM_T_LAYER_V7			0xfdc4
+
+/* MFCv7 variant defines */
+#define MAX_FW_SIZE_V7			(SZ_1M)		/* 1MB */
+#define MAX_CPB_SIZE_V7			(3 * SZ_1M)	/* 3MB */
+#define MFC_VERSION_V7			0x72
+#define MFC_NUM_PORTS_V7		1
+
+#define MFC_LUMA_PAD_BYTES_V7		256
+#define MFC_CHROMA_PAD_BYTES_V7		128
+
+/* MFCv7 Context buffer sizes */
+#define MFC_CTX_BUF_SIZE_V7		(30 * SZ_1K)	/*  30KB */
+#define MFC_H264_DEC_CTX_BUF_SIZE_V7	(2 * SZ_1M)	/*  2MB */
+#define MFC_OTHER_DEC_CTX_BUF_SIZE_V7	(20 * SZ_1K)	/*  20KB */
+#define MFC_H264_ENC_CTX_BUF_SIZE_V7	(100 * SZ_1K)	/* 100KB */
+#define MFC_OTHER_ENC_CTX_BUF_SIZE_V7	(10 * SZ_1K)	/*  10KB */
+
+/* Buffer size defines */
+#define S5P_FIMV_SCRATCH_BUF_SIZE_MPEG4_DEC_V7(w, h) \
+			(SZ_1M + ((w) * 144) + (8192 * (h)) + 49216)
+
+#define S5P_FIMV_SCRATCH_BUF_SIZE_VP8_ENC_V7(w, h) \
+			(((w) * 48) + 8192 + ((((w) + 1) / 2) * 128) + 144 + \
+			((((((w) * 16) * ((h) * 16)) * 3) / 2) * 4))
+
+#endif /*_REGS_MFC_V7_H*/
diff --git a/drivers/media/platform/s5p-mfc/regs-mfc-v8.h b/drivers/media/platform/s5p-mfc/regs-mfc-v8.h
new file mode 100644
index 0000000..cc7cbec
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/regs-mfc-v8.h
@@ -0,0 +1,124 @@
+/*
+ * Register definition file for Samsung MFC V8.x Interface (FIMV) driver
+ *
+ * Copyright (c) 2014 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef _REGS_MFC_V8_H
+#define _REGS_MFC_V8_H
+
+#include <linux/sizes.h>
+#include "regs-mfc-v7.h"
+
+/* Additional registers for v8 */
+#define S5P_FIMV_D_MVC_NUM_VIEWS_V8		0xf104
+#define S5P_FIMV_D_FIRST_PLANE_DPB_SIZE_V8	0xf144
+#define S5P_FIMV_D_SECOND_PLANE_DPB_SIZE_V8	0xf148
+#define S5P_FIMV_D_MV_BUFFER_SIZE_V8		0xf150
+
+#define S5P_FIMV_D_FIRST_PLANE_DPB_STRIDE_SIZE_V8	0xf138
+#define S5P_FIMV_D_SECOND_PLANE_DPB_STRIDE_SIZE_V8	0xf13c
+
+#define S5P_FIMV_D_FIRST_PLANE_DPB_V8		0xf160
+#define S5P_FIMV_D_SECOND_PLANE_DPB_V8		0xf260
+#define S5P_FIMV_D_MV_BUFFER_V8			0xf460
+
+#define S5P_FIMV_D_NUM_MV_V8			0xf134
+#define S5P_FIMV_D_INIT_BUFFER_OPTIONS_V8	0xf154
+
+#define S5P_FIMV_D_SCRATCH_BUFFER_ADDR_V8	0xf560
+#define S5P_FIMV_D_SCRATCH_BUFFER_SIZE_V8	0xf564
+
+#define S5P_FIMV_D_CPB_BUFFER_ADDR_V8		0xf5b0
+#define S5P_FIMV_D_CPB_BUFFER_SIZE_V8		0xf5b4
+#define S5P_FIMV_D_AVAILABLE_DPB_FLAG_LOWER_V8	0xf5bc
+#define S5P_FIMV_D_CPB_BUFFER_OFFSET_V8		0xf5c0
+#define S5P_FIMV_D_SLICE_IF_ENABLE_V8		0xf5c4
+#define S5P_FIMV_D_STREAM_DATA_SIZE_V8		0xf5d0
+
+/* Display information register */
+#define S5P_FIMV_D_DISPLAY_FRAME_WIDTH_V8	0xf600
+#define S5P_FIMV_D_DISPLAY_FRAME_HEIGHT_V8	0xf604
+
+/* Display status */
+#define S5P_FIMV_D_DISPLAY_STATUS_V8		0xf608
+
+#define S5P_FIMV_D_DISPLAY_FIRST_PLANE_ADDR_V8	0xf60c
+#define S5P_FIMV_D_DISPLAY_SECOND_PLANE_ADDR_V8	0xf610
+
+#define S5P_FIMV_D_DISPLAY_FRAME_TYPE_V8	0xf618
+#define S5P_FIMV_D_DISPLAY_CROP_INFO1_V8	0xf61c
+#define S5P_FIMV_D_DISPLAY_CROP_INFO2_V8	0xf620
+#define S5P_FIMV_D_DISPLAY_PICTURE_PROFILE_V8	0xf624
+
+/* Decoded picture information register */
+#define S5P_FIMV_D_DECODED_STATUS_V8		0xf644
+#define S5P_FIMV_D_DECODED_FIRST_PLANE_ADDR_V8	0xf648
+#define S5P_FIMV_D_DECODED_SECOND_PLANE_ADDR_V8	0xf64c
+#define S5P_FIMV_D_DECODED_THIRD_PLANE_ADDR_V8	0xf650
+#define S5P_FIMV_D_DECODED_FRAME_TYPE_V8	0xf654
+#define S5P_FIMV_D_DECODED_NAL_SIZE_V8          0xf664
+
+/* Returned value register for specific setting */
+#define S5P_FIMV_D_RET_PICTURE_TAG_TOP_V8	0xf674
+#define S5P_FIMV_D_RET_PICTURE_TAG_BOT_V8	0xf678
+#define S5P_FIMV_D_MVC_VIEW_ID_V8		0xf6d8
+
+/* SEI related information */
+#define S5P_FIMV_D_FRAME_PACK_SEI_AVAIL_V8	0xf6dc
+
+/* Encoder Registers */
+#define S5P_FIMV_E_FIXED_PICTURE_QP_V8		0xf794
+#define S5P_FIMV_E_RC_CONFIG_V8			0xf798
+#define S5P_FIMV_E_RC_QP_BOUND_V8		0xf79c
+#define S5P_FIMV_E_RC_RPARAM_V8			0xf7a4
+#define S5P_FIMV_E_MB_RC_CONFIG_V8		0xf7a8
+#define S5P_FIMV_E_PADDING_CTRL_V8		0xf7ac
+#define S5P_FIMV_E_MV_HOR_RANGE_V8		0xf7b4
+#define S5P_FIMV_E_MV_VER_RANGE_V8		0xf7b8
+
+#define S5P_FIMV_E_VBV_BUFFER_SIZE_V8		0xf78c
+#define S5P_FIMV_E_VBV_INIT_DELAY_V8		0xf790
+
+#define S5P_FIMV_E_ASPECT_RATIO_V8		0xfb4c
+#define S5P_FIMV_E_EXTENDED_SAR_V8		0xfb50
+#define S5P_FIMV_E_H264_OPTIONS_V8		0xfb54
+
+/* MFCv8 Context buffer sizes */
+#define MFC_CTX_BUF_SIZE_V8		(30 * SZ_1K)	/*  30KB */
+#define MFC_H264_DEC_CTX_BUF_SIZE_V8	(2 * SZ_1M)	/*  2MB */
+#define MFC_OTHER_DEC_CTX_BUF_SIZE_V8	(20 * SZ_1K)	/*  20KB */
+#define MFC_H264_ENC_CTX_BUF_SIZE_V8	(100 * SZ_1K)	/* 100KB */
+#define MFC_OTHER_ENC_CTX_BUF_SIZE_V8	(10 * SZ_1K)	/*  10KB */
+
+/* Buffer size defines */
+#define S5P_FIMV_TMV_BUFFER_SIZE_V8(w, h)	(((w) + 1) * ((h) + 1) * 8)
+
+#define S5P_FIMV_SCRATCH_BUF_SIZE_H264_DEC_V8(w, h)	(((w) * 704) + 2176)
+#define S5P_FIMV_SCRATCH_BUF_SIZE_VP8_DEC_V8(w, h) \
+		(((w) * 576 + (h) * 128)  + 4128)
+
+#define S5P_FIMV_SCRATCH_BUF_SIZE_H264_ENC_V8(w, h) \
+			(((w) * 592) + 2336)
+#define S5P_FIMV_SCRATCH_BUF_SIZE_VP8_ENC_V8(w, h) \
+			(((w) * 576) + 10512 + \
+			((((((w) * 16) * ((h) * 16)) * 3) / 2) * 4))
+#define S5P_FIMV_ME_BUFFER_SIZE_V8(imw, imh, mbw, mbh) \
+	((DIV_ROUND_UP((mbw * 16), 64) *  DIV_ROUND_UP((mbh * 16), 64) * 256) \
+	 + (DIV_ROUND_UP((mbw) * (mbh), 32) * 16))
+
+/* BUffer alignment defines */
+#define S5P_FIMV_D_ALIGN_PLANE_SIZE_V8	64
+
+/* MFCv8 variant defines */
+#define MAX_FW_SIZE_V8			(SZ_1M)		/* 1MB */
+#define MAX_CPB_SIZE_V8			(3 * SZ_1M)	/* 3MB */
+#define MFC_VERSION_V8			0x80
+#define MFC_NUM_PORTS_V8		1
+
+#endif /*_REGS_MFC_V8_H*/
diff --git a/drivers/media/platform/s5p-mfc/regs-mfc.h b/drivers/media/platform/s5p-mfc/regs-mfc.h
new file mode 100644
index 0000000..6ccc3f8
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/regs-mfc.h
@@ -0,0 +1,459 @@
+/*
+ * Register definition file for Samsung MFC V5.1 Interface (FIMV) driver
+ *
+ * Kamil Debski, Copyright (c) 2010 Samsung Electronics
+ * http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+*/
+
+#ifndef _REGS_FIMV_H
+#define _REGS_FIMV_H
+
+#include <linux/kernel.h>
+#include <linux/sizes.h>
+
+#define S5P_FIMV_REG_SIZE	(S5P_FIMV_END_ADDR - S5P_FIMV_START_ADDR)
+#define S5P_FIMV_REG_COUNT	((S5P_FIMV_END_ADDR - S5P_FIMV_START_ADDR) / 4)
+
+/* Number of bits that the buffer address should be shifted for particular
+ * MFC buffers.  */
+#define S5P_FIMV_START_ADDR		0x0000
+#define S5P_FIMV_END_ADDR		0xe008
+
+#define S5P_FIMV_SW_RESET		0x0000
+#define S5P_FIMV_RISC_HOST_INT		0x0008
+
+/* Command from HOST to RISC */
+#define S5P_FIMV_HOST2RISC_CMD		0x0030
+#define S5P_FIMV_HOST2RISC_ARG1		0x0034
+#define S5P_FIMV_HOST2RISC_ARG2		0x0038
+#define S5P_FIMV_HOST2RISC_ARG3		0x003c
+#define S5P_FIMV_HOST2RISC_ARG4		0x0040
+
+/* Command from RISC to HOST */
+#define S5P_FIMV_RISC2HOST_CMD		0x0044
+#define S5P_FIMV_RISC2HOST_CMD_MASK	0x1FFFF
+#define S5P_FIMV_RISC2HOST_ARG1		0x0048
+#define S5P_FIMV_RISC2HOST_ARG2		0x004c
+#define S5P_FIMV_RISC2HOST_ARG3		0x0050
+#define S5P_FIMV_RISC2HOST_ARG4		0x0054
+
+#define S5P_FIMV_FW_VERSION		0x0058
+#define S5P_FIMV_SYS_MEM_SZ		0x005c
+#define S5P_FIMV_FW_STATUS		0x0080
+
+/* Memory controller register */
+#define S5P_FIMV_MC_DRAMBASE_ADR_A	0x0508
+#define S5P_FIMV_MC_DRAMBASE_ADR_B	0x050c
+#define S5P_FIMV_MC_STATUS		0x0510
+
+/* Common register */
+#define S5P_FIMV_COMMON_BASE_A		0x0600
+#define S5P_FIMV_COMMON_BASE_B		0x0700
+
+/* Decoder */
+#define S5P_FIMV_DEC_CHROMA_ADR		(S5P_FIMV_COMMON_BASE_A)
+#define S5P_FIMV_DEC_LUMA_ADR		(S5P_FIMV_COMMON_BASE_B)
+
+/* H.264 decoding */
+#define S5P_FIMV_H264_VERT_NB_MV_ADR	(S5P_FIMV_COMMON_BASE_A + 0x8c)
+					/* vertical neighbor motion vector */
+#define S5P_FIMV_H264_NB_IP_ADR		(S5P_FIMV_COMMON_BASE_A + 0x90)
+					/* neighbor pixels for intra pred */
+#define S5P_FIMV_H264_MV_ADR		(S5P_FIMV_COMMON_BASE_B + 0x80)
+					/* H264 motion vector */
+
+/* MPEG4 decoding */
+#define S5P_FIMV_MPEG4_NB_DCAC_ADR	(S5P_FIMV_COMMON_BASE_A + 0x8c)
+					/* neighbor AC/DC coeff. */
+#define S5P_FIMV_MPEG4_UP_NB_MV_ADR	(S5P_FIMV_COMMON_BASE_A + 0x90)
+					/* upper neighbor motion vector */
+#define S5P_FIMV_MPEG4_SA_MV_ADR	(S5P_FIMV_COMMON_BASE_A + 0x94)
+					/* subseq. anchor motion vector */
+#define S5P_FIMV_MPEG4_OT_LINE_ADR	(S5P_FIMV_COMMON_BASE_A + 0x98)
+					/* overlap transform line */
+#define S5P_FIMV_MPEG4_SP_ADR		(S5P_FIMV_COMMON_BASE_A + 0xa8)
+					/* syntax parser */
+
+/* H.263 decoding */
+#define S5P_FIMV_H263_NB_DCAC_ADR	(S5P_FIMV_COMMON_BASE_A + 0x8c)
+#define S5P_FIMV_H263_UP_NB_MV_ADR	(S5P_FIMV_COMMON_BASE_A + 0x90)
+#define S5P_FIMV_H263_SA_MV_ADR		(S5P_FIMV_COMMON_BASE_A + 0x94)
+#define S5P_FIMV_H263_OT_LINE_ADR	(S5P_FIMV_COMMON_BASE_A + 0x98)
+
+/* VC-1 decoding */
+#define S5P_FIMV_VC1_NB_DCAC_ADR	(S5P_FIMV_COMMON_BASE_A + 0x8c)
+#define S5P_FIMV_VC1_UP_NB_MV_ADR	(S5P_FIMV_COMMON_BASE_A + 0x90)
+#define S5P_FIMV_VC1_SA_MV_ADR		(S5P_FIMV_COMMON_BASE_A + 0x94)
+#define S5P_FIMV_VC1_OT_LINE_ADR	(S5P_FIMV_COMMON_BASE_A + 0x98)
+#define S5P_FIMV_VC1_BITPLANE3_ADR	(S5P_FIMV_COMMON_BASE_A + 0x9c)
+					/* bitplane3 */
+#define S5P_FIMV_VC1_BITPLANE2_ADR	(S5P_FIMV_COMMON_BASE_A + 0xa0)
+					/* bitplane2 */
+#define S5P_FIMV_VC1_BITPLANE1_ADR	(S5P_FIMV_COMMON_BASE_A + 0xa4)
+					/* bitplane1 */
+
+/* Encoder */
+#define S5P_FIMV_ENC_REF0_LUMA_ADR	(S5P_FIMV_COMMON_BASE_A + 0x1c)
+#define S5P_FIMV_ENC_REF1_LUMA_ADR	(S5P_FIMV_COMMON_BASE_A + 0x20)
+					/* reconstructed luma */
+#define S5P_FIMV_ENC_REF0_CHROMA_ADR	(S5P_FIMV_COMMON_BASE_B)
+#define S5P_FIMV_ENC_REF1_CHROMA_ADR	(S5P_FIMV_COMMON_BASE_B + 0x04)
+					/* reconstructed chroma */
+#define S5P_FIMV_ENC_REF2_LUMA_ADR	(S5P_FIMV_COMMON_BASE_B + 0x10)
+#define S5P_FIMV_ENC_REF2_CHROMA_ADR	(S5P_FIMV_COMMON_BASE_B + 0x08)
+#define S5P_FIMV_ENC_REF3_LUMA_ADR	(S5P_FIMV_COMMON_BASE_B + 0x14)
+#define S5P_FIMV_ENC_REF3_CHROMA_ADR	(S5P_FIMV_COMMON_BASE_B + 0x0c)
+
+/* H.264 encoding */
+#define S5P_FIMV_H264_UP_MV_ADR		(S5P_FIMV_COMMON_BASE_A)
+					/* upper motion vector */
+#define S5P_FIMV_H264_NBOR_INFO_ADR	(S5P_FIMV_COMMON_BASE_A + 0x04)
+					/* entropy engine's neighbor info. */
+#define S5P_FIMV_H264_UP_INTRA_MD_ADR	(S5P_FIMV_COMMON_BASE_A + 0x08)
+					/* upper intra MD */
+#define S5P_FIMV_H264_COZERO_FLAG_ADR	(S5P_FIMV_COMMON_BASE_A + 0x10)
+					/* direct cozero flag */
+#define S5P_FIMV_H264_UP_INTRA_PRED_ADR	(S5P_FIMV_COMMON_BASE_B + 0x40)
+					/* upper intra PRED */
+
+/* H.263 encoding */
+#define S5P_FIMV_H263_UP_MV_ADR		(S5P_FIMV_COMMON_BASE_A)
+					/* upper motion vector */
+#define S5P_FIMV_H263_ACDC_COEF_ADR	(S5P_FIMV_COMMON_BASE_A + 0x04)
+					/* upper Q coeff. */
+
+/* MPEG4 encoding */
+#define S5P_FIMV_MPEG4_UP_MV_ADR	(S5P_FIMV_COMMON_BASE_A)
+					/* upper motion vector */
+#define S5P_FIMV_MPEG4_ACDC_COEF_ADR	(S5P_FIMV_COMMON_BASE_A + 0x04)
+					/* upper Q coeff. */
+#define S5P_FIMV_MPEG4_COZERO_FLAG_ADR	(S5P_FIMV_COMMON_BASE_A + 0x10)
+					/* direct cozero flag */
+
+#define S5P_FIMV_ENC_REF_B_LUMA_ADR     0x062c /* ref B Luma addr */
+#define S5P_FIMV_ENC_REF_B_CHROMA_ADR   0x0630 /* ref B Chroma addr */
+
+#define S5P_FIMV_ENC_CUR_LUMA_ADR	0x0718 /* current Luma addr */
+#define S5P_FIMV_ENC_CUR_CHROMA_ADR	0x071C /* current Chroma addr */
+
+/* Codec common register */
+#define S5P_FIMV_ENC_HSIZE_PX		0x0818 /* frame width at encoder */
+#define S5P_FIMV_ENC_VSIZE_PX		0x081c /* frame height at encoder */
+#define S5P_FIMV_ENC_PROFILE		0x0830 /* profile register */
+#define S5P_FIMV_ENC_PROFILE_H264_MAIN			0
+#define S5P_FIMV_ENC_PROFILE_H264_HIGH			1
+#define S5P_FIMV_ENC_PROFILE_H264_BASELINE		2
+#define S5P_FIMV_ENC_PROFILE_H264_CONSTRAINED_BASELINE	3
+#define S5P_FIMV_ENC_PROFILE_MPEG4_SIMPLE		0
+#define S5P_FIMV_ENC_PROFILE_MPEG4_ADVANCED_SIMPLE	1
+#define S5P_FIMV_ENC_PIC_STRUCT		0x083c /* picture field/frame flag */
+#define S5P_FIMV_ENC_LF_CTRL		0x0848 /* loop filter control */
+#define S5P_FIMV_ENC_ALPHA_OFF		0x084c /* loop filter alpha offset */
+#define S5P_FIMV_ENC_BETA_OFF		0x0850 /* loop filter beta offset */
+#define S5P_FIMV_MR_BUSIF_CTRL		0x0854 /* hidden, bus interface ctrl */
+#define S5P_FIMV_ENC_PXL_CACHE_CTRL	0x0a00 /* pixel cache control */
+
+/* Channel & stream interface register */
+#define S5P_FIMV_SI_RTN_CHID		0x2000 /* Return CH inst ID register */
+#define S5P_FIMV_SI_CH0_INST_ID		0x2040 /* codec instance ID */
+#define S5P_FIMV_SI_CH1_INST_ID		0x2080 /* codec instance ID */
+/* Decoder */
+#define S5P_FIMV_SI_VRESOL		0x2004 /* vertical res of decoder */
+#define S5P_FIMV_SI_HRESOL		0x2008 /* horizontal res of decoder */
+#define S5P_FIMV_SI_BUF_NUMBER		0x200c /* number of frames in the
+								decoded pic */
+#define S5P_FIMV_SI_DISPLAY_Y_ADR	0x2010 /* luma addr of displayed pic */
+#define S5P_FIMV_SI_DISPLAY_C_ADR	0x2014 /* chroma addrof displayed pic */
+
+#define S5P_FIMV_SI_CONSUMED_BYTES	0x2018 /* Consumed number of bytes to
+							decode a frame */
+#define S5P_FIMV_SI_DISPLAY_STATUS	0x201c /* status of decoded picture */
+
+#define S5P_FIMV_SI_DECODE_Y_ADR	0x2024 /* luma addr of decoded pic */
+#define S5P_FIMV_SI_DECODE_C_ADR	0x2028 /* chroma addrof decoded pic */
+#define S5P_FIMV_SI_DECODE_STATUS	0x202c /* status of decoded picture */
+
+#define S5P_FIMV_SI_CH0_SB_ST_ADR	0x2044 /* start addr of stream buf */
+#define S5P_FIMV_SI_CH0_SB_FRM_SIZE	0x2048 /* size of stream buf */
+#define S5P_FIMV_SI_CH0_DESC_ADR	0x204c /* addr of descriptor buf */
+#define S5P_FIMV_SI_CH0_CPB_SIZE	0x2058 /* max size of coded pic. buf */
+#define S5P_FIMV_SI_CH0_DESC_SIZE	0x205c /* max size of descriptor buf */
+
+#define S5P_FIMV_SI_CH1_SB_ST_ADR	0x2084 /* start addr of stream buf */
+#define S5P_FIMV_SI_CH1_SB_FRM_SIZE	0x2088 /* size of stream buf */
+#define S5P_FIMV_SI_CH1_DESC_ADR	0x208c /* addr of descriptor buf */
+#define S5P_FIMV_SI_CH1_CPB_SIZE	0x2098 /* max size of coded pic. buf */
+#define S5P_FIMV_SI_CH1_DESC_SIZE	0x209c /* max size of descriptor buf */
+
+#define S5P_FIMV_CRC_LUMA0		0x2030 /* luma crc data per frame
+								(top field) */
+#define S5P_FIMV_CRC_CHROMA0		0x2034 /* chroma crc data per frame
+								(top field) */
+#define S5P_FIMV_CRC_LUMA1		0x2038 /* luma crc data per bottom
+								field */
+#define S5P_FIMV_CRC_CHROMA1		0x203c /* chroma crc data per bottom
+								field */
+
+/* Display status */
+#define S5P_FIMV_DEC_STATUS_DECODING_ONLY		0
+#define S5P_FIMV_DEC_STATUS_DECODING_DISPLAY		1
+#define S5P_FIMV_DEC_STATUS_DISPLAY_ONLY		2
+#define S5P_FIMV_DEC_STATUS_DECODING_EMPTY		3
+#define S5P_FIMV_DEC_STATUS_DECODING_STATUS_MASK	7
+#define S5P_FIMV_DEC_STATUS_PROGRESSIVE			(0<<3)
+#define S5P_FIMV_DEC_STATUS_INTERLACE			(1<<3)
+#define S5P_FIMV_DEC_STATUS_INTERLACE_MASK		(1<<3)
+#define S5P_FIMV_DEC_STATUS_CRC_NUMBER_TWO		(0<<4)
+#define S5P_FIMV_DEC_STATUS_CRC_NUMBER_FOUR		(1<<4)
+#define S5P_FIMV_DEC_STATUS_CRC_NUMBER_MASK		(1<<4)
+#define S5P_FIMV_DEC_STATUS_CRC_GENERATED		(1<<5)
+#define S5P_FIMV_DEC_STATUS_CRC_NOT_GENERATED		(0<<5)
+#define S5P_FIMV_DEC_STATUS_CRC_MASK			(1<<5)
+
+#define S5P_FIMV_DEC_STATUS_RESOLUTION_MASK		(3<<4)
+#define S5P_FIMV_DEC_STATUS_RESOLUTION_INC		(1<<4)
+#define S5P_FIMV_DEC_STATUS_RESOLUTION_DEC		(2<<4)
+#define S5P_FIMV_DEC_STATUS_RESOLUTION_SHIFT		4
+
+/* Decode frame address */
+#define S5P_FIMV_DECODE_Y_ADR			0x2024
+#define S5P_FIMV_DECODE_C_ADR			0x2028
+
+/* Decoded frame tpe */
+#define S5P_FIMV_DECODE_FRAME_TYPE		0x2020
+#define S5P_FIMV_DECODE_FRAME_MASK		7
+
+#define S5P_FIMV_DECODE_FRAME_SKIPPED		0
+#define S5P_FIMV_DECODE_FRAME_I_FRAME		1
+#define S5P_FIMV_DECODE_FRAME_P_FRAME		2
+#define S5P_FIMV_DECODE_FRAME_B_FRAME		3
+#define S5P_FIMV_DECODE_FRAME_OTHER_FRAME	4
+
+/* Sizes of buffers required for decoding */
+#define S5P_FIMV_DEC_NB_IP_SIZE			(32 * 1024)
+#define S5P_FIMV_DEC_VERT_NB_MV_SIZE		(16 * 1024)
+#define S5P_FIMV_DEC_NB_DCAC_SIZE		(16 * 1024)
+#define S5P_FIMV_DEC_UPNB_MV_SIZE		(68 * 1024)
+#define S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE		(136 * 1024)
+#define S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE     (32 * 1024)
+#define S5P_FIMV_DEC_VC1_BITPLANE_SIZE		(2 * 1024)
+#define S5P_FIMV_DEC_STX_PARSER_SIZE		(68 * 1024)
+
+#define S5P_FIMV_DEC_BUF_ALIGN			(8 * 1024)
+#define S5P_FIMV_ENC_BUF_ALIGN			(8 * 1024)
+#define S5P_FIMV_NV12M_HALIGN			16
+#define S5P_FIMV_NV12M_LVALIGN			16
+#define S5P_FIMV_NV12M_CVALIGN			8
+#define S5P_FIMV_NV12MT_HALIGN			128
+#define S5P_FIMV_NV12MT_VALIGN			32
+#define S5P_FIMV_NV12M_SALIGN			2048
+#define S5P_FIMV_NV12MT_SALIGN			8192
+
+/* Sizes of buffers required for encoding */
+#define S5P_FIMV_ENC_UPMV_SIZE		0x10000
+#define S5P_FIMV_ENC_COLFLG_SIZE	0x10000
+#define S5P_FIMV_ENC_INTRAMD_SIZE	0x10000
+#define S5P_FIMV_ENC_INTRAPRED_SIZE	0x4000
+#define S5P_FIMV_ENC_NBORINFO_SIZE	0x10000
+#define S5P_FIMV_ENC_ACDCCOEF_SIZE	0x10000
+
+/* Encoder */
+#define S5P_FIMV_ENC_SI_STRM_SIZE	0x2004 /* stream size */
+#define S5P_FIMV_ENC_SI_PIC_CNT		0x2008 /* picture count */
+#define S5P_FIMV_ENC_SI_WRITE_PTR	0x200c /* write pointer */
+#define S5P_FIMV_ENC_SI_SLICE_TYPE	0x2010 /* slice type(I/P/B/IDR) */
+#define S5P_FIMV_ENC_SI_SLICE_TYPE_NON_CODED	0
+#define S5P_FIMV_ENC_SI_SLICE_TYPE_I		1
+#define S5P_FIMV_ENC_SI_SLICE_TYPE_P		2
+#define S5P_FIMV_ENC_SI_SLICE_TYPE_B		3
+#define S5P_FIMV_ENC_SI_SLICE_TYPE_SKIPPED	4
+#define S5P_FIMV_ENC_SI_SLICE_TYPE_OTHERS	5
+#define S5P_FIMV_ENCODED_Y_ADDR         0x2014 /* the addr of the encoded
+								luma pic */
+#define S5P_FIMV_ENCODED_C_ADDR         0x2018 /* the addr of the encoded
+								chroma pic */
+
+#define S5P_FIMV_ENC_SI_CH0_SB_ADR	0x2044 /* addr of stream buf */
+#define S5P_FIMV_ENC_SI_CH0_SB_SIZE	0x204c /* size of stream buf */
+#define S5P_FIMV_ENC_SI_CH0_CUR_Y_ADR	0x2050 /* current Luma addr */
+#define S5P_FIMV_ENC_SI_CH0_CUR_C_ADR	0x2054 /* current Chroma addr */
+#define S5P_FIMV_ENC_SI_CH0_FRAME_INS	0x2058 /* frame insertion */
+
+#define S5P_FIMV_ENC_SI_CH1_SB_ADR	0x2084 /* addr of stream buf */
+#define S5P_FIMV_ENC_SI_CH1_SB_SIZE	0x208c /* size of stream buf */
+#define S5P_FIMV_ENC_SI_CH1_CUR_Y_ADR	0x2090 /* current Luma addr */
+#define S5P_FIMV_ENC_SI_CH1_CUR_C_ADR	0x2094 /* current Chroma addr */
+#define S5P_FIMV_ENC_SI_CH1_FRAME_INS	0x2098 /* frame insertion */
+
+#define S5P_FIMV_ENC_PIC_TYPE_CTRL	0xc504 /* pic type level control */
+#define S5P_FIMV_ENC_B_RECON_WRITE_ON	0xc508 /* B frame recon write ctrl */
+#define S5P_FIMV_ENC_MSLICE_CTRL	0xc50c /* multi slice control */
+#define S5P_FIMV_ENC_MSLICE_MB		0xc510 /* MB number in the one slice */
+#define S5P_FIMV_ENC_MSLICE_BIT		0xc514 /* bit count for one slice */
+#define S5P_FIMV_ENC_CIR_CTRL		0xc518 /* number of intra refresh MB */
+#define S5P_FIMV_ENC_MAP_FOR_CUR	0xc51c /* linear or tiled mode */
+#define S5P_FIMV_ENC_PADDING_CTRL	0xc520 /* padding control */
+
+#define S5P_FIMV_ENC_RC_CONFIG		0xc5a0 /* RC config */
+#define S5P_FIMV_ENC_RC_BIT_RATE	0xc5a8 /* bit rate */
+#define S5P_FIMV_ENC_RC_QBOUND		0xc5ac /* max/min QP */
+#define S5P_FIMV_ENC_RC_RPARA		0xc5b0 /* rate control reaction coeff */
+#define S5P_FIMV_ENC_RC_MB_CTRL		0xc5b4 /* MB adaptive scaling */
+
+/* Encoder for H264 only */
+#define S5P_FIMV_ENC_H264_ENTROPY_MODE	0xd004 /* CAVLC or CABAC */
+#define S5P_FIMV_ENC_H264_ALPHA_OFF	0xd008 /* loop filter alpha offset */
+#define S5P_FIMV_ENC_H264_BETA_OFF	0xd00c /* loop filter beta offset */
+#define S5P_FIMV_ENC_H264_NUM_OF_REF	0xd010 /* number of reference for P/B */
+#define S5P_FIMV_ENC_H264_TRANS_FLAG	0xd034 /* 8x8 transform flag in PPS &
+								high profile */
+
+#define S5P_FIMV_ENC_RC_FRAME_RATE	0xd0d0 /* frame rate */
+
+/* Encoder for MPEG4 only */
+#define S5P_FIMV_ENC_MPEG4_QUART_PXL	0xe008 /* qpel interpolation ctrl */
+
+/* Additional */
+#define S5P_FIMV_SI_CH0_DPB_CONF_CTRL   0x2068 /* DPB Config Control Register */
+#define S5P_FIMV_SLICE_INT_MASK		1
+#define S5P_FIMV_SLICE_INT_SHIFT	31
+#define S5P_FIMV_DDELAY_ENA_SHIFT	30
+#define S5P_FIMV_DDELAY_VAL_MASK	0xff
+#define S5P_FIMV_DDELAY_VAL_SHIFT	16
+#define S5P_FIMV_DPB_COUNT_MASK		0xffff
+#define S5P_FIMV_DPB_FLUSH_MASK		1
+#define S5P_FIMV_DPB_FLUSH_SHIFT	14
+
+
+#define S5P_FIMV_SI_CH0_RELEASE_BUF     0x2060 /* DPB release buffer register */
+#define S5P_FIMV_SI_CH0_HOST_WR_ADR	0x2064 /* address of shared memory */
+
+/* Codec numbers  */
+#define S5P_FIMV_CODEC_NONE		-1
+
+#define S5P_FIMV_CODEC_H264_DEC		0
+#define S5P_FIMV_CODEC_VC1_DEC		1
+#define S5P_FIMV_CODEC_MPEG4_DEC	2
+#define S5P_FIMV_CODEC_MPEG2_DEC	3
+#define S5P_FIMV_CODEC_H263_DEC		4
+#define S5P_FIMV_CODEC_VC1RCV_DEC	5
+
+#define S5P_FIMV_CODEC_H264_ENC		16
+#define S5P_FIMV_CODEC_MPEG4_ENC	17
+#define S5P_FIMV_CODEC_H263_ENC		18
+
+/* Channel Control Register */
+#define S5P_FIMV_CH_SEQ_HEADER		1
+#define S5P_FIMV_CH_FRAME_START		2
+#define S5P_FIMV_CH_LAST_FRAME		3
+#define S5P_FIMV_CH_INIT_BUFS		4
+#define S5P_FIMV_CH_FRAME_START_REALLOC	5
+#define S5P_FIMV_CH_MASK		7
+#define S5P_FIMV_CH_SHIFT		16
+
+
+/* Host to RISC command */
+#define S5P_FIMV_H2R_CMD_EMPTY		0
+#define S5P_FIMV_H2R_CMD_OPEN_INSTANCE	1
+#define S5P_FIMV_H2R_CMD_CLOSE_INSTANCE	2
+#define S5P_FIMV_H2R_CMD_SYS_INIT	3
+#define S5P_FIMV_H2R_CMD_FLUSH		4
+#define S5P_FIMV_H2R_CMD_SLEEP		5
+#define S5P_FIMV_H2R_CMD_WAKEUP		6
+
+#define S5P_FIMV_R2H_CMD_EMPTY			0
+#define S5P_FIMV_R2H_CMD_OPEN_INSTANCE_RET	1
+#define S5P_FIMV_R2H_CMD_CLOSE_INSTANCE_RET	2
+#define S5P_FIMV_R2H_CMD_RSV_RET		3
+#define S5P_FIMV_R2H_CMD_SEQ_DONE_RET		4
+#define S5P_FIMV_R2H_CMD_FRAME_DONE_RET		5
+#define S5P_FIMV_R2H_CMD_SLICE_DONE_RET		6
+#define S5P_FIMV_R2H_CMD_ENC_COMPLETE_RET	7
+#define S5P_FIMV_R2H_CMD_SYS_INIT_RET		8
+#define S5P_FIMV_R2H_CMD_FW_STATUS_RET		9
+#define S5P_FIMV_R2H_CMD_SLEEP_RET		10
+#define S5P_FIMV_R2H_CMD_WAKEUP_RET		11
+#define S5P_FIMV_R2H_CMD_FLUSH_RET		12
+#define S5P_FIMV_R2H_CMD_INIT_BUFFERS_RET	15
+#define S5P_FIMV_R2H_CMD_EDFU_INIT_RET		16
+#define S5P_FIMV_R2H_CMD_ERR_RET		32
+
+/* Dummy definition for MFCv6 compatibility */
+#define S5P_FIMV_CODEC_H264_MVC_DEC		-1
+#define S5P_FIMV_R2H_CMD_FIELD_DONE_RET		-1
+#define S5P_FIMV_MFC_RESET			-1
+#define S5P_FIMV_RISC_ON			-1
+#define S5P_FIMV_RISC_BASE_ADDRESS		-1
+#define S5P_FIMV_CODEC_VP8_DEC			-1
+#define S5P_FIMV_REG_CLEAR_BEGIN		0
+#define S5P_FIMV_REG_CLEAR_COUNT		0
+
+/* Error handling defines */
+#define S5P_FIMV_ERR_WARNINGS_START		145
+#define S5P_FIMV_ERR_DEC_MASK			0xFFFF
+#define S5P_FIMV_ERR_DEC_SHIFT			0
+#define S5P_FIMV_ERR_DSPL_MASK			0xFFFF0000
+#define S5P_FIMV_ERR_DSPL_SHIFT			16
+
+/* Shared memory registers' offsets */
+
+/* An offset of the start position in the stream when
+ * the start position is not aligned */
+#define S5P_FIMV_SHARED_CROP_INFO_H		0x0020
+#define S5P_FIMV_SHARED_CROP_LEFT_MASK		0xFFFF
+#define S5P_FIMV_SHARED_CROP_LEFT_SHIFT		0
+#define S5P_FIMV_SHARED_CROP_RIGHT_MASK		0xFFFF0000
+#define S5P_FIMV_SHARED_CROP_RIGHT_SHIFT	16
+#define S5P_FIMV_SHARED_CROP_INFO_V		0x0024
+#define S5P_FIMV_SHARED_CROP_TOP_MASK		0xFFFF
+#define S5P_FIMV_SHARED_CROP_TOP_SHIFT		0
+#define S5P_FIMV_SHARED_CROP_BOTTOM_MASK	0xFFFF0000
+#define S5P_FIMV_SHARED_CROP_BOTTOM_SHIFT	16
+#define S5P_FIMV_SHARED_SET_FRAME_TAG		0x0004
+#define S5P_FIMV_SHARED_GET_FRAME_TAG_TOP	0x0008
+#define S5P_FIMV_SHARED_GET_FRAME_TAG_BOT	0x000C
+#define S5P_FIMV_SHARED_START_BYTE_NUM		0x0018
+#define S5P_FIMV_SHARED_RC_VOP_TIMING		0x0030
+#define S5P_FIMV_SHARED_LUMA_DPB_SIZE		0x0064
+#define S5P_FIMV_SHARED_CHROMA_DPB_SIZE		0x0068
+#define S5P_FIMV_SHARED_MV_SIZE			0x006C
+#define S5P_FIMV_SHARED_PIC_TIME_TOP		0x0010
+#define S5P_FIMV_SHARED_PIC_TIME_BOTTOM		0x0014
+#define S5P_FIMV_SHARED_EXT_ENC_CONTROL		0x0028
+#define S5P_FIMV_SHARED_P_B_FRAME_QP		0x0070
+#define S5P_FIMV_SHARED_ASPECT_RATIO_IDC	0x0074
+#define S5P_FIMV_SHARED_EXTENDED_SAR		0x0078
+#define S5P_FIMV_SHARED_H264_I_PERIOD		0x009C
+#define S5P_FIMV_SHARED_RC_CONTROL_CONFIG	0x00A0
+#define S5P_FIMV_SHARED_DISP_FRAME_TYPE_SHIFT	2
+
+/* Offset used by the hardware to store addresses */
+#define MFC_OFFSET_SHIFT	11
+
+#define FIRMWARE_ALIGN		(128 * SZ_1K)	/* 128KB */
+#define MFC_H264_CTX_BUF_SIZE	(600 * SZ_1K)	/* 600KB per H264 instance */
+#define MFC_CTX_BUF_SIZE	(10 * SZ_1K)	/* 10KB per instance */
+#define DESC_BUF_SIZE		(128 * SZ_1K)	/* 128KB for DESC buffer */
+#define SHARED_BUF_SIZE		(8 * SZ_1K)	/* 8KB for shared buffer */
+
+#define DEF_CPB_SIZE		(256 * SZ_1K)	/* 256KB */
+#define MAX_CPB_SIZE		(4 * SZ_1M)	/* 4MB */
+#define MAX_FW_SIZE		(384 * SZ_1K)
+
+#define MFC_VERSION		0x51
+#define MFC_NUM_PORTS		2
+
+#define S5P_FIMV_SHARED_FRAME_PACK_SEI_AVAIL    0x16C
+#define S5P_FIMV_SHARED_FRAME_PACK_ARRGMENT_ID  0x170
+#define S5P_FIMV_SHARED_FRAME_PACK_SEI_INFO     0x174
+#define S5P_FIMV_SHARED_FRAME_PACK_GRID_POS     0x178
+
+/* Values for resolution change in display status */
+#define S5P_FIMV_RES_INCREASE	1
+#define S5P_FIMV_RES_DECREASE	2
+
+#endif /* _REGS_FIMV_H */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc.c b/drivers/media/platform/s5p-mfc/s5p_mfc.c
new file mode 100644
index 0000000..7727789
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc.c
@@ -0,0 +1,1555 @@
+/*
+ * Samsung S5P Multi Format Codec v 5.1
+ *
+ * Copyright (c) 2011 Samsung Electronics Co., Ltd.
+ * Kamil Debski, <k.debski@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-event.h>
+#include <linux/workqueue.h>
+#include <linux/of.h>
+#include <media/videobuf2-v4l2.h>
+#include "s5p_mfc_common.h"
+#include "s5p_mfc_ctrl.h"
+#include "s5p_mfc_debug.h"
+#include "s5p_mfc_dec.h"
+#include "s5p_mfc_enc.h"
+#include "s5p_mfc_intr.h"
+#include "s5p_mfc_opr.h"
+#include "s5p_mfc_cmd.h"
+#include "s5p_mfc_pm.h"
+
+#define S5P_MFC_NAME		"s5p-mfc"
+#define S5P_MFC_DEC_NAME	"s5p-mfc-dec"
+#define S5P_MFC_ENC_NAME	"s5p-mfc-enc"
+
+int mfc_debug_level;
+module_param_named(debug, mfc_debug_level, int, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(debug, "Debug level - higher value produces more verbose messages");
+
+/* Helper functions for interrupt processing */
+
+/* Remove from hw execution round robin */
+void clear_work_bit(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+
+	spin_lock(&dev->condlock);
+	__clear_bit(ctx->num, &dev->ctx_work_bits);
+	spin_unlock(&dev->condlock);
+}
+
+/* Add to hw execution round robin */
+void set_work_bit(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+
+	spin_lock(&dev->condlock);
+	__set_bit(ctx->num, &dev->ctx_work_bits);
+	spin_unlock(&dev->condlock);
+}
+
+/* Remove from hw execution round robin */
+void clear_work_bit_irqsave(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev->condlock, flags);
+	__clear_bit(ctx->num, &dev->ctx_work_bits);
+	spin_unlock_irqrestore(&dev->condlock, flags);
+}
+
+/* Add to hw execution round robin */
+void set_work_bit_irqsave(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev->condlock, flags);
+	__set_bit(ctx->num, &dev->ctx_work_bits);
+	spin_unlock_irqrestore(&dev->condlock, flags);
+}
+
+/* Wake up context wait_queue */
+static void wake_up_ctx(struct s5p_mfc_ctx *ctx, unsigned int reason,
+			unsigned int err)
+{
+	ctx->int_cond = 1;
+	ctx->int_type = reason;
+	ctx->int_err = err;
+	wake_up(&ctx->queue);
+}
+
+/* Wake up device wait_queue */
+static void wake_up_dev(struct s5p_mfc_dev *dev, unsigned int reason,
+			unsigned int err)
+{
+	dev->int_cond = 1;
+	dev->int_type = reason;
+	dev->int_err = err;
+	wake_up(&dev->queue);
+}
+
+static void s5p_mfc_watchdog(unsigned long arg)
+{
+	struct s5p_mfc_dev *dev = (struct s5p_mfc_dev *)arg;
+
+	if (test_bit(0, &dev->hw_lock))
+		atomic_inc(&dev->watchdog_cnt);
+	if (atomic_read(&dev->watchdog_cnt) >= MFC_WATCHDOG_CNT) {
+		/* This means that hw is busy and no interrupts were
+		 * generated by hw for the Nth time of running this
+		 * watchdog timer. This usually means a serious hw
+		 * error. Now it is time to kill all instances and
+		 * reset the MFC. */
+		mfc_err("Time out during waiting for HW\n");
+		queue_work(dev->watchdog_workqueue, &dev->watchdog_work);
+	}
+	dev->watchdog_timer.expires = jiffies +
+					msecs_to_jiffies(MFC_WATCHDOG_INTERVAL);
+	add_timer(&dev->watchdog_timer);
+}
+
+static void s5p_mfc_watchdog_worker(struct work_struct *work)
+{
+	struct s5p_mfc_dev *dev;
+	struct s5p_mfc_ctx *ctx;
+	unsigned long flags;
+	int mutex_locked;
+	int i, ret;
+
+	dev = container_of(work, struct s5p_mfc_dev, watchdog_work);
+
+	mfc_err("Driver timeout error handling\n");
+	/* Lock the mutex that protects open and release.
+	 * This is necessary as they may load and unload firmware. */
+	mutex_locked = mutex_trylock(&dev->mfc_mutex);
+	if (!mutex_locked)
+		mfc_err("Error: some instance may be closing/opening\n");
+	spin_lock_irqsave(&dev->irqlock, flags);
+
+	s5p_mfc_clock_off();
+
+	for (i = 0; i < MFC_NUM_CONTEXTS; i++) {
+		ctx = dev->ctx[i];
+		if (!ctx)
+			continue;
+		ctx->state = MFCINST_ERROR;
+		s5p_mfc_hw_call_void(dev->mfc_ops, cleanup_queue,
+						&ctx->dst_queue, &ctx->vq_dst);
+		s5p_mfc_hw_call_void(dev->mfc_ops, cleanup_queue,
+						&ctx->src_queue, &ctx->vq_src);
+		clear_work_bit(ctx);
+		wake_up_ctx(ctx, S5P_MFC_R2H_CMD_ERR_RET, 0);
+	}
+	clear_bit(0, &dev->hw_lock);
+	spin_unlock_irqrestore(&dev->irqlock, flags);
+
+	/* De-init MFC */
+	s5p_mfc_deinit_hw(dev);
+
+	/* Double check if there is at least one instance running.
+	 * If no instance is in memory than no firmware should be present */
+	if (dev->num_inst > 0) {
+		ret = s5p_mfc_load_firmware(dev);
+		if (ret) {
+			mfc_err("Failed to reload FW\n");
+			goto unlock;
+		}
+		s5p_mfc_clock_on();
+		ret = s5p_mfc_init_hw(dev);
+		s5p_mfc_clock_off();
+		if (ret)
+			mfc_err("Failed to reinit FW\n");
+	}
+unlock:
+	if (mutex_locked)
+		mutex_unlock(&dev->mfc_mutex);
+}
+
+static void s5p_mfc_handle_frame_all_extracted(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_buf *dst_buf;
+	struct s5p_mfc_dev *dev = ctx->dev;
+
+	ctx->state = MFCINST_FINISHED;
+	ctx->sequence++;
+	while (!list_empty(&ctx->dst_queue)) {
+		dst_buf = list_entry(ctx->dst_queue.next,
+				     struct s5p_mfc_buf, list);
+		mfc_debug(2, "Cleaning up buffer: %d\n",
+					  dst_buf->b->vb2_buf.index);
+		vb2_set_plane_payload(&dst_buf->b->vb2_buf, 0, 0);
+		vb2_set_plane_payload(&dst_buf->b->vb2_buf, 1, 0);
+		list_del(&dst_buf->list);
+		dst_buf->flags |= MFC_BUF_FLAG_EOS;
+		ctx->dst_queue_cnt--;
+		dst_buf->b->sequence = (ctx->sequence++);
+
+		if (s5p_mfc_hw_call(dev->mfc_ops, get_pic_type_top, ctx) ==
+			s5p_mfc_hw_call(dev->mfc_ops, get_pic_type_bot, ctx))
+			dst_buf->b->field = V4L2_FIELD_NONE;
+		else
+			dst_buf->b->field = V4L2_FIELD_INTERLACED;
+		dst_buf->b->flags |= V4L2_BUF_FLAG_LAST;
+
+		ctx->dec_dst_flag &= ~(1 << dst_buf->b->vb2_buf.index);
+		vb2_buffer_done(&dst_buf->b->vb2_buf, VB2_BUF_STATE_DONE);
+	}
+}
+
+static void s5p_mfc_handle_frame_copy_time(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_buf  *dst_buf, *src_buf;
+	size_t dec_y_addr;
+	unsigned int frame_type;
+
+	/* Make sure we actually have a new frame before continuing. */
+	frame_type = s5p_mfc_hw_call(dev->mfc_ops, get_dec_frame_type, dev);
+	if (frame_type == S5P_FIMV_DECODE_FRAME_SKIPPED)
+		return;
+	dec_y_addr = s5p_mfc_hw_call(dev->mfc_ops, get_dec_y_adr, dev);
+
+	/* Copy timestamp / timecode from decoded src to dst and set
+	   appropriate flags. */
+	src_buf = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
+	list_for_each_entry(dst_buf, &ctx->dst_queue, list) {
+		if (vb2_dma_contig_plane_dma_addr(&dst_buf->b->vb2_buf, 0)
+				== dec_y_addr) {
+			dst_buf->b->timecode =
+						src_buf->b->timecode;
+			dst_buf->b->timestamp =
+						src_buf->b->timestamp;
+			dst_buf->b->flags &=
+				~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+			dst_buf->b->flags |=
+				src_buf->b->flags
+				& V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+			switch (frame_type) {
+			case S5P_FIMV_DECODE_FRAME_I_FRAME:
+				dst_buf->b->flags |=
+						V4L2_BUF_FLAG_KEYFRAME;
+				break;
+			case S5P_FIMV_DECODE_FRAME_P_FRAME:
+				dst_buf->b->flags |=
+						V4L2_BUF_FLAG_PFRAME;
+				break;
+			case S5P_FIMV_DECODE_FRAME_B_FRAME:
+				dst_buf->b->flags |=
+						V4L2_BUF_FLAG_BFRAME;
+				break;
+			default:
+				/* Don't know how to handle
+				   S5P_FIMV_DECODE_FRAME_OTHER_FRAME. */
+				mfc_debug(2, "Unexpected frame type: %d\n",
+						frame_type);
+			}
+			break;
+		}
+	}
+}
+
+static void s5p_mfc_handle_frame_new(struct s5p_mfc_ctx *ctx, unsigned int err)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_buf  *dst_buf;
+	size_t dspl_y_addr;
+	unsigned int frame_type;
+
+	dspl_y_addr = s5p_mfc_hw_call(dev->mfc_ops, get_dspl_y_adr, dev);
+	if (IS_MFCV6_PLUS(dev))
+		frame_type = s5p_mfc_hw_call(dev->mfc_ops,
+			get_disp_frame_type, ctx);
+	else
+		frame_type = s5p_mfc_hw_call(dev->mfc_ops,
+			get_dec_frame_type, dev);
+
+	/* If frame is same as previous then skip and do not dequeue */
+	if (frame_type == S5P_FIMV_DECODE_FRAME_SKIPPED) {
+		if (!ctx->after_packed_pb)
+			ctx->sequence++;
+		ctx->after_packed_pb = 0;
+		return;
+	}
+	ctx->sequence++;
+	/* The MFC returns address of the buffer, now we have to
+	 * check which videobuf does it correspond to */
+	list_for_each_entry(dst_buf, &ctx->dst_queue, list) {
+		/* Check if this is the buffer we're looking for */
+		if (vb2_dma_contig_plane_dma_addr(&dst_buf->b->vb2_buf, 0)
+				== dspl_y_addr) {
+			list_del(&dst_buf->list);
+			ctx->dst_queue_cnt--;
+			dst_buf->b->sequence = ctx->sequence;
+			if (s5p_mfc_hw_call(dev->mfc_ops,
+					get_pic_type_top, ctx) ==
+				s5p_mfc_hw_call(dev->mfc_ops,
+					get_pic_type_bot, ctx))
+				dst_buf->b->field = V4L2_FIELD_NONE;
+			else
+				dst_buf->b->field =
+							V4L2_FIELD_INTERLACED;
+			vb2_set_plane_payload(&dst_buf->b->vb2_buf, 0,
+						ctx->luma_size);
+			vb2_set_plane_payload(&dst_buf->b->vb2_buf, 1,
+						ctx->chroma_size);
+			clear_bit(dst_buf->b->vb2_buf.index,
+							&ctx->dec_dst_flag);
+
+			vb2_buffer_done(&dst_buf->b->vb2_buf, err ?
+				VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+
+			break;
+		}
+	}
+}
+
+/* Handle frame decoding interrupt */
+static void s5p_mfc_handle_frame(struct s5p_mfc_ctx *ctx,
+					unsigned int reason, unsigned int err)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	unsigned int dst_frame_status;
+	unsigned int dec_frame_status;
+	struct s5p_mfc_buf *src_buf;
+	unsigned long flags;
+	unsigned int res_change;
+
+	dst_frame_status = s5p_mfc_hw_call(dev->mfc_ops, get_dspl_status, dev)
+				& S5P_FIMV_DEC_STATUS_DECODING_STATUS_MASK;
+	dec_frame_status = s5p_mfc_hw_call(dev->mfc_ops, get_dec_status, dev)
+				& S5P_FIMV_DEC_STATUS_DECODING_STATUS_MASK;
+	res_change = (s5p_mfc_hw_call(dev->mfc_ops, get_dspl_status, dev)
+				& S5P_FIMV_DEC_STATUS_RESOLUTION_MASK)
+				>> S5P_FIMV_DEC_STATUS_RESOLUTION_SHIFT;
+	mfc_debug(2, "Frame Status: %x\n", dst_frame_status);
+	if (ctx->state == MFCINST_RES_CHANGE_INIT)
+		ctx->state = MFCINST_RES_CHANGE_FLUSH;
+	if (res_change == S5P_FIMV_RES_INCREASE ||
+		res_change == S5P_FIMV_RES_DECREASE) {
+		ctx->state = MFCINST_RES_CHANGE_INIT;
+		s5p_mfc_hw_call_void(dev->mfc_ops, clear_int_flags, dev);
+		wake_up_ctx(ctx, reason, err);
+		WARN_ON(test_and_clear_bit(0, &dev->hw_lock) == 0);
+		s5p_mfc_clock_off();
+		s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
+		return;
+	}
+	if (ctx->dpb_flush_flag)
+		ctx->dpb_flush_flag = 0;
+
+	spin_lock_irqsave(&dev->irqlock, flags);
+	/* All frames remaining in the buffer have been extracted  */
+	if (dst_frame_status == S5P_FIMV_DEC_STATUS_DECODING_EMPTY) {
+		if (ctx->state == MFCINST_RES_CHANGE_FLUSH) {
+			static const struct v4l2_event ev_src_ch = {
+				.type = V4L2_EVENT_SOURCE_CHANGE,
+				.u.src_change.changes =
+					V4L2_EVENT_SRC_CH_RESOLUTION,
+			};
+
+			s5p_mfc_handle_frame_all_extracted(ctx);
+			ctx->state = MFCINST_RES_CHANGE_END;
+			v4l2_event_queue_fh(&ctx->fh, &ev_src_ch);
+
+			goto leave_handle_frame;
+		} else {
+			s5p_mfc_handle_frame_all_extracted(ctx);
+		}
+	}
+
+	if (dec_frame_status == S5P_FIMV_DEC_STATUS_DECODING_DISPLAY)
+		s5p_mfc_handle_frame_copy_time(ctx);
+
+	/* A frame has been decoded and is in the buffer  */
+	if (dst_frame_status == S5P_FIMV_DEC_STATUS_DISPLAY_ONLY ||
+	    dst_frame_status == S5P_FIMV_DEC_STATUS_DECODING_DISPLAY) {
+		s5p_mfc_handle_frame_new(ctx, err);
+	} else {
+		mfc_debug(2, "No frame decode\n");
+	}
+	/* Mark source buffer as complete */
+	if (dst_frame_status != S5P_FIMV_DEC_STATUS_DISPLAY_ONLY
+		&& !list_empty(&ctx->src_queue)) {
+		src_buf = list_entry(ctx->src_queue.next, struct s5p_mfc_buf,
+								list);
+		ctx->consumed_stream += s5p_mfc_hw_call(dev->mfc_ops,
+						get_consumed_stream, dev);
+		if (ctx->codec_mode != S5P_MFC_CODEC_H264_DEC &&
+			ctx->codec_mode != S5P_MFC_CODEC_VP8_DEC &&
+			ctx->consumed_stream + STUFF_BYTE <
+			src_buf->b->vb2_buf.planes[0].bytesused) {
+			/* Run MFC again on the same buffer */
+			mfc_debug(2, "Running again the same buffer\n");
+			ctx->after_packed_pb = 1;
+		} else {
+			mfc_debug(2, "MFC needs next buffer\n");
+			ctx->consumed_stream = 0;
+			if (src_buf->flags & MFC_BUF_FLAG_EOS)
+				ctx->state = MFCINST_FINISHING;
+			list_del(&src_buf->list);
+			ctx->src_queue_cnt--;
+			if (s5p_mfc_hw_call(dev->mfc_ops, err_dec, err) > 0)
+				vb2_buffer_done(&src_buf->b->vb2_buf,
+						VB2_BUF_STATE_ERROR);
+			else
+				vb2_buffer_done(&src_buf->b->vb2_buf,
+						VB2_BUF_STATE_DONE);
+		}
+	}
+leave_handle_frame:
+	spin_unlock_irqrestore(&dev->irqlock, flags);
+	if ((ctx->src_queue_cnt == 0 && ctx->state != MFCINST_FINISHING)
+				    || ctx->dst_queue_cnt < ctx->pb_count)
+		clear_work_bit(ctx);
+	s5p_mfc_hw_call_void(dev->mfc_ops, clear_int_flags, dev);
+	wake_up_ctx(ctx, reason, err);
+	WARN_ON(test_and_clear_bit(0, &dev->hw_lock) == 0);
+	s5p_mfc_clock_off();
+	/* if suspending, wake up device and do not try_run again*/
+	if (test_bit(0, &dev->enter_suspend))
+		wake_up_dev(dev, reason, err);
+	else
+		s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
+}
+
+/* Error handling for interrupt */
+static void s5p_mfc_handle_error(struct s5p_mfc_dev *dev,
+		struct s5p_mfc_ctx *ctx, unsigned int reason, unsigned int err)
+{
+	unsigned long flags;
+
+	mfc_err("Interrupt Error: %08x\n", err);
+
+	if (ctx != NULL) {
+		/* Error recovery is dependent on the state of context */
+		switch (ctx->state) {
+		case MFCINST_RES_CHANGE_INIT:
+		case MFCINST_RES_CHANGE_FLUSH:
+		case MFCINST_RES_CHANGE_END:
+		case MFCINST_FINISHING:
+		case MFCINST_FINISHED:
+		case MFCINST_RUNNING:
+			/* It is highly probable that an error occurred
+			 * while decoding a frame */
+			clear_work_bit(ctx);
+			ctx->state = MFCINST_ERROR;
+			/* Mark all dst buffers as having an error */
+			spin_lock_irqsave(&dev->irqlock, flags);
+			s5p_mfc_hw_call_void(dev->mfc_ops, cleanup_queue,
+						&ctx->dst_queue, &ctx->vq_dst);
+			/* Mark all src buffers as having an error */
+			s5p_mfc_hw_call_void(dev->mfc_ops, cleanup_queue,
+						&ctx->src_queue, &ctx->vq_src);
+			spin_unlock_irqrestore(&dev->irqlock, flags);
+			wake_up_ctx(ctx, reason, err);
+			break;
+		default:
+			clear_work_bit(ctx);
+			ctx->state = MFCINST_ERROR;
+			wake_up_ctx(ctx, reason, err);
+			break;
+		}
+	}
+	WARN_ON(test_and_clear_bit(0, &dev->hw_lock) == 0);
+	s5p_mfc_hw_call_void(dev->mfc_ops, clear_int_flags, dev);
+	s5p_mfc_clock_off();
+	wake_up_dev(dev, reason, err);
+	return;
+}
+
+/* Header parsing interrupt handling */
+static void s5p_mfc_handle_seq_done(struct s5p_mfc_ctx *ctx,
+				 unsigned int reason, unsigned int err)
+{
+	struct s5p_mfc_dev *dev;
+
+	if (ctx == NULL)
+		return;
+	dev = ctx->dev;
+	if (ctx->c_ops->post_seq_start) {
+		if (ctx->c_ops->post_seq_start(ctx))
+			mfc_err("post_seq_start() failed\n");
+	} else {
+		ctx->img_width = s5p_mfc_hw_call(dev->mfc_ops, get_img_width,
+				dev);
+		ctx->img_height = s5p_mfc_hw_call(dev->mfc_ops, get_img_height,
+				dev);
+
+		s5p_mfc_hw_call_void(dev->mfc_ops, dec_calc_dpb_size, ctx);
+
+		ctx->pb_count = s5p_mfc_hw_call(dev->mfc_ops, get_dpb_count,
+				dev);
+		ctx->mv_count = s5p_mfc_hw_call(dev->mfc_ops, get_mv_count,
+				dev);
+		if (ctx->img_width == 0 || ctx->img_height == 0)
+			ctx->state = MFCINST_ERROR;
+		else
+			ctx->state = MFCINST_HEAD_PARSED;
+
+		if ((ctx->codec_mode == S5P_MFC_CODEC_H264_DEC ||
+			ctx->codec_mode == S5P_MFC_CODEC_H264_MVC_DEC) &&
+				!list_empty(&ctx->src_queue)) {
+			struct s5p_mfc_buf *src_buf;
+			src_buf = list_entry(ctx->src_queue.next,
+					struct s5p_mfc_buf, list);
+			if (s5p_mfc_hw_call(dev->mfc_ops, get_consumed_stream,
+						dev) <
+					src_buf->b->vb2_buf.planes[0].bytesused)
+				ctx->head_processed = 0;
+			else
+				ctx->head_processed = 1;
+		} else {
+			ctx->head_processed = 1;
+		}
+	}
+	s5p_mfc_hw_call_void(dev->mfc_ops, clear_int_flags, dev);
+	clear_work_bit(ctx);
+	WARN_ON(test_and_clear_bit(0, &dev->hw_lock) == 0);
+	s5p_mfc_clock_off();
+	s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
+	wake_up_ctx(ctx, reason, err);
+}
+
+/* Header parsing interrupt handling */
+static void s5p_mfc_handle_init_buffers(struct s5p_mfc_ctx *ctx,
+				 unsigned int reason, unsigned int err)
+{
+	struct s5p_mfc_buf *src_buf;
+	struct s5p_mfc_dev *dev;
+	unsigned long flags;
+
+	if (ctx == NULL)
+		return;
+	dev = ctx->dev;
+	s5p_mfc_hw_call_void(dev->mfc_ops, clear_int_flags, dev);
+	ctx->int_type = reason;
+	ctx->int_err = err;
+	ctx->int_cond = 1;
+	clear_work_bit(ctx);
+	if (err == 0) {
+		ctx->state = MFCINST_RUNNING;
+		if (!ctx->dpb_flush_flag && ctx->head_processed) {
+			spin_lock_irqsave(&dev->irqlock, flags);
+			if (!list_empty(&ctx->src_queue)) {
+				src_buf = list_entry(ctx->src_queue.next,
+					     struct s5p_mfc_buf, list);
+				list_del(&src_buf->list);
+				ctx->src_queue_cnt--;
+				vb2_buffer_done(&src_buf->b->vb2_buf,
+						VB2_BUF_STATE_DONE);
+			}
+			spin_unlock_irqrestore(&dev->irqlock, flags);
+		} else {
+			ctx->dpb_flush_flag = 0;
+		}
+		WARN_ON(test_and_clear_bit(0, &dev->hw_lock) == 0);
+
+		s5p_mfc_clock_off();
+
+		wake_up(&ctx->queue);
+		s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
+	} else {
+		WARN_ON(test_and_clear_bit(0, &dev->hw_lock) == 0);
+
+		s5p_mfc_clock_off();
+
+		wake_up(&ctx->queue);
+	}
+}
+
+static void s5p_mfc_handle_stream_complete(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_buf *mb_entry;
+
+	mfc_debug(2, "Stream completed\n");
+
+	ctx->state = MFCINST_FINISHED;
+
+	spin_lock(&dev->irqlock);
+	if (!list_empty(&ctx->dst_queue)) {
+		mb_entry = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf,
+									list);
+		list_del(&mb_entry->list);
+		ctx->dst_queue_cnt--;
+		vb2_set_plane_payload(&mb_entry->b->vb2_buf, 0, 0);
+		vb2_buffer_done(&mb_entry->b->vb2_buf, VB2_BUF_STATE_DONE);
+	}
+	spin_unlock(&dev->irqlock);
+
+	clear_work_bit(ctx);
+
+	WARN_ON(test_and_clear_bit(0, &dev->hw_lock) == 0);
+
+	s5p_mfc_clock_off();
+	wake_up(&ctx->queue);
+	s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
+}
+
+/* Interrupt processing */
+static irqreturn_t s5p_mfc_irq(int irq, void *priv)
+{
+	struct s5p_mfc_dev *dev = priv;
+	struct s5p_mfc_ctx *ctx;
+	unsigned int reason;
+	unsigned int err;
+
+	mfc_debug_enter();
+	/* Reset the timeout watchdog */
+	atomic_set(&dev->watchdog_cnt, 0);
+	ctx = dev->ctx[dev->curr_ctx];
+	/* Get the reason of interrupt and the error code */
+	reason = s5p_mfc_hw_call(dev->mfc_ops, get_int_reason, dev);
+	err = s5p_mfc_hw_call(dev->mfc_ops, get_int_err, dev);
+	mfc_debug(1, "Int reason: %d (err: %08x)\n", reason, err);
+	switch (reason) {
+	case S5P_MFC_R2H_CMD_ERR_RET:
+		/* An error has occurred */
+		if (ctx->state == MFCINST_RUNNING &&
+			s5p_mfc_hw_call(dev->mfc_ops, err_dec, err) >=
+				dev->warn_start)
+			s5p_mfc_handle_frame(ctx, reason, err);
+		else
+			s5p_mfc_handle_error(dev, ctx, reason, err);
+		clear_bit(0, &dev->enter_suspend);
+		break;
+
+	case S5P_MFC_R2H_CMD_SLICE_DONE_RET:
+	case S5P_MFC_R2H_CMD_FIELD_DONE_RET:
+	case S5P_MFC_R2H_CMD_FRAME_DONE_RET:
+		if (ctx->c_ops->post_frame_start) {
+			if (ctx->c_ops->post_frame_start(ctx))
+				mfc_err("post_frame_start() failed\n");
+
+			if (ctx->state == MFCINST_FINISHING &&
+						list_empty(&ctx->ref_queue)) {
+				s5p_mfc_hw_call_void(dev->mfc_ops, clear_int_flags, dev);
+				s5p_mfc_handle_stream_complete(ctx);
+				break;
+			}
+			s5p_mfc_hw_call_void(dev->mfc_ops, clear_int_flags, dev);
+			wake_up_ctx(ctx, reason, err);
+			WARN_ON(test_and_clear_bit(0, &dev->hw_lock) == 0);
+			s5p_mfc_clock_off();
+			s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
+		} else {
+			s5p_mfc_handle_frame(ctx, reason, err);
+		}
+		break;
+
+	case S5P_MFC_R2H_CMD_SEQ_DONE_RET:
+		s5p_mfc_handle_seq_done(ctx, reason, err);
+		break;
+
+	case S5P_MFC_R2H_CMD_OPEN_INSTANCE_RET:
+		ctx->inst_no = s5p_mfc_hw_call(dev->mfc_ops, get_inst_no, dev);
+		ctx->state = MFCINST_GOT_INST;
+		clear_work_bit(ctx);
+		wake_up(&ctx->queue);
+		goto irq_cleanup_hw;
+
+	case S5P_MFC_R2H_CMD_CLOSE_INSTANCE_RET:
+		clear_work_bit(ctx);
+		ctx->inst_no = MFC_NO_INSTANCE_SET;
+		ctx->state = MFCINST_FREE;
+		wake_up(&ctx->queue);
+		goto irq_cleanup_hw;
+
+	case S5P_MFC_R2H_CMD_SYS_INIT_RET:
+	case S5P_MFC_R2H_CMD_FW_STATUS_RET:
+	case S5P_MFC_R2H_CMD_SLEEP_RET:
+	case S5P_MFC_R2H_CMD_WAKEUP_RET:
+		if (ctx)
+			clear_work_bit(ctx);
+		s5p_mfc_hw_call_void(dev->mfc_ops, clear_int_flags, dev);
+		wake_up_dev(dev, reason, err);
+		clear_bit(0, &dev->hw_lock);
+		clear_bit(0, &dev->enter_suspend);
+		break;
+
+	case S5P_MFC_R2H_CMD_INIT_BUFFERS_RET:
+		s5p_mfc_handle_init_buffers(ctx, reason, err);
+		break;
+
+	case S5P_MFC_R2H_CMD_COMPLETE_SEQ_RET:
+		s5p_mfc_hw_call_void(dev->mfc_ops, clear_int_flags, dev);
+		ctx->int_type = reason;
+		ctx->int_err = err;
+		s5p_mfc_handle_stream_complete(ctx);
+		break;
+
+	case S5P_MFC_R2H_CMD_DPB_FLUSH_RET:
+		clear_work_bit(ctx);
+		ctx->state = MFCINST_RUNNING;
+		wake_up(&ctx->queue);
+		goto irq_cleanup_hw;
+
+	default:
+		mfc_debug(2, "Unknown int reason\n");
+		s5p_mfc_hw_call_void(dev->mfc_ops, clear_int_flags, dev);
+	}
+	mfc_debug_leave();
+	return IRQ_HANDLED;
+irq_cleanup_hw:
+	s5p_mfc_hw_call_void(dev->mfc_ops, clear_int_flags, dev);
+	ctx->int_type = reason;
+	ctx->int_err = err;
+	ctx->int_cond = 1;
+	if (test_and_clear_bit(0, &dev->hw_lock) == 0)
+		mfc_err("Failed to unlock hw\n");
+
+	s5p_mfc_clock_off();
+
+	s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
+	mfc_debug(2, "Exit via irq_cleanup_hw\n");
+	return IRQ_HANDLED;
+}
+
+/* Open an MFC node */
+static int s5p_mfc_open(struct file *file)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct s5p_mfc_dev *dev = video_drvdata(file);
+	struct s5p_mfc_ctx *ctx = NULL;
+	struct vb2_queue *q;
+	int ret = 0;
+
+	mfc_debug_enter();
+	if (mutex_lock_interruptible(&dev->mfc_mutex))
+		return -ERESTARTSYS;
+	dev->num_inst++;	/* It is guarded by mfc_mutex in vfd */
+	/* Allocate memory for context */
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx) {
+		mfc_err("Not enough memory\n");
+		ret = -ENOMEM;
+		goto err_alloc;
+	}
+	v4l2_fh_init(&ctx->fh, vdev);
+	file->private_data = &ctx->fh;
+	v4l2_fh_add(&ctx->fh);
+	ctx->dev = dev;
+	INIT_LIST_HEAD(&ctx->src_queue);
+	INIT_LIST_HEAD(&ctx->dst_queue);
+	ctx->src_queue_cnt = 0;
+	ctx->dst_queue_cnt = 0;
+	/* Get context number */
+	ctx->num = 0;
+	while (dev->ctx[ctx->num]) {
+		ctx->num++;
+		if (ctx->num >= MFC_NUM_CONTEXTS) {
+			mfc_err("Too many open contexts\n");
+			ret = -EBUSY;
+			goto err_no_ctx;
+		}
+	}
+	/* Mark context as idle */
+	clear_work_bit_irqsave(ctx);
+	dev->ctx[ctx->num] = ctx;
+	if (vdev == dev->vfd_dec) {
+		ctx->type = MFCINST_DECODER;
+		ctx->c_ops = get_dec_codec_ops();
+		s5p_mfc_dec_init(ctx);
+		/* Setup ctrl handler */
+		ret = s5p_mfc_dec_ctrls_setup(ctx);
+		if (ret) {
+			mfc_err("Failed to setup mfc controls\n");
+			goto err_ctrls_setup;
+		}
+	} else if (vdev == dev->vfd_enc) {
+		ctx->type = MFCINST_ENCODER;
+		ctx->c_ops = get_enc_codec_ops();
+		/* only for encoder */
+		INIT_LIST_HEAD(&ctx->ref_queue);
+		ctx->ref_queue_cnt = 0;
+		s5p_mfc_enc_init(ctx);
+		/* Setup ctrl handler */
+		ret = s5p_mfc_enc_ctrls_setup(ctx);
+		if (ret) {
+			mfc_err("Failed to setup mfc controls\n");
+			goto err_ctrls_setup;
+		}
+	} else {
+		ret = -ENOENT;
+		goto err_bad_node;
+	}
+	ctx->fh.ctrl_handler = &ctx->ctrl_handler;
+	ctx->inst_no = MFC_NO_INSTANCE_SET;
+	/* Load firmware if this is the first instance */
+	if (dev->num_inst == 1) {
+		dev->watchdog_timer.expires = jiffies +
+					msecs_to_jiffies(MFC_WATCHDOG_INTERVAL);
+		add_timer(&dev->watchdog_timer);
+		ret = s5p_mfc_power_on();
+		if (ret < 0) {
+			mfc_err("power on failed\n");
+			goto err_pwr_enable;
+		}
+		s5p_mfc_clock_on();
+		ret = s5p_mfc_load_firmware(dev);
+		if (ret) {
+			s5p_mfc_clock_off();
+			goto err_load_fw;
+		}
+		/* Init the FW */
+		ret = s5p_mfc_init_hw(dev);
+		s5p_mfc_clock_off();
+		if (ret)
+			goto err_init_hw;
+	}
+	/* Init videobuf2 queue for CAPTURE */
+	q = &ctx->vq_dst;
+	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	q->drv_priv = &ctx->fh;
+	q->lock = &dev->mfc_mutex;
+	if (vdev == dev->vfd_dec) {
+		q->io_modes = VB2_MMAP;
+		q->ops = get_dec_queue_ops();
+	} else if (vdev == dev->vfd_enc) {
+		q->io_modes = VB2_MMAP | VB2_USERPTR;
+		q->ops = get_enc_queue_ops();
+	} else {
+		ret = -ENOENT;
+		goto err_queue_init;
+	}
+	q->mem_ops = &vb2_dma_contig_memops;
+	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	ret = vb2_queue_init(q);
+	if (ret) {
+		mfc_err("Failed to initialize videobuf2 queue(capture)\n");
+		goto err_queue_init;
+	}
+	/* Init videobuf2 queue for OUTPUT */
+	q = &ctx->vq_src;
+	q->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+	q->io_modes = VB2_MMAP;
+	q->drv_priv = &ctx->fh;
+	q->lock = &dev->mfc_mutex;
+	if (vdev == dev->vfd_dec) {
+		q->io_modes = VB2_MMAP;
+		q->ops = get_dec_queue_ops();
+	} else if (vdev == dev->vfd_enc) {
+		q->io_modes = VB2_MMAP | VB2_USERPTR;
+		q->ops = get_enc_queue_ops();
+	} else {
+		ret = -ENOENT;
+		goto err_queue_init;
+	}
+	/* One way to indicate end-of-stream for MFC is to set the
+	 * bytesused == 0. However by default videobuf2 handles bytesused
+	 * equal to 0 as a special case and changes its value to the size
+	 * of the buffer. Set the allow_zero_bytesused flag so that videobuf2
+	 * will keep the value of bytesused intact.
+	 */
+	q->allow_zero_bytesused = 1;
+	q->mem_ops = &vb2_dma_contig_memops;
+	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	ret = vb2_queue_init(q);
+	if (ret) {
+		mfc_err("Failed to initialize videobuf2 queue(output)\n");
+		goto err_queue_init;
+	}
+	init_waitqueue_head(&ctx->queue);
+	mutex_unlock(&dev->mfc_mutex);
+	mfc_debug_leave();
+	return ret;
+	/* Deinit when failure occurred */
+err_queue_init:
+	if (dev->num_inst == 1)
+		s5p_mfc_deinit_hw(dev);
+err_init_hw:
+err_load_fw:
+err_pwr_enable:
+	if (dev->num_inst == 1) {
+		if (s5p_mfc_power_off() < 0)
+			mfc_err("power off failed\n");
+		del_timer_sync(&dev->watchdog_timer);
+	}
+err_ctrls_setup:
+	s5p_mfc_dec_ctrls_delete(ctx);
+err_bad_node:
+	dev->ctx[ctx->num] = NULL;
+err_no_ctx:
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	kfree(ctx);
+err_alloc:
+	dev->num_inst--;
+	mutex_unlock(&dev->mfc_mutex);
+	mfc_debug_leave();
+	return ret;
+}
+
+/* Release MFC context */
+static int s5p_mfc_release(struct file *file)
+{
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(file->private_data);
+	struct s5p_mfc_dev *dev = ctx->dev;
+
+	mfc_debug_enter();
+	mutex_lock(&dev->mfc_mutex);
+	s5p_mfc_clock_on();
+	vb2_queue_release(&ctx->vq_src);
+	vb2_queue_release(&ctx->vq_dst);
+	/* Mark context as idle */
+	clear_work_bit_irqsave(ctx);
+	/* If instance was initialised and not yet freed,
+	 * return instance and free resources */
+	if (ctx->state != MFCINST_FREE && ctx->state != MFCINST_INIT) {
+		mfc_debug(2, "Has to free instance\n");
+		s5p_mfc_close_mfc_inst(dev, ctx);
+	}
+	/* hardware locking scheme */
+	if (dev->curr_ctx == ctx->num)
+		clear_bit(0, &dev->hw_lock);
+	dev->num_inst--;
+	if (dev->num_inst == 0) {
+		mfc_debug(2, "Last instance\n");
+		s5p_mfc_deinit_hw(dev);
+		del_timer_sync(&dev->watchdog_timer);
+		if (s5p_mfc_power_off() < 0)
+			mfc_err("Power off failed\n");
+	}
+	mfc_debug(2, "Shutting down clock\n");
+	s5p_mfc_clock_off();
+	dev->ctx[ctx->num] = NULL;
+	s5p_mfc_dec_ctrls_delete(ctx);
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	kfree(ctx);
+	mfc_debug_leave();
+	mutex_unlock(&dev->mfc_mutex);
+	return 0;
+}
+
+/* Poll */
+static unsigned int s5p_mfc_poll(struct file *file,
+				 struct poll_table_struct *wait)
+{
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(file->private_data);
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct vb2_queue *src_q, *dst_q;
+	struct vb2_buffer *src_vb = NULL, *dst_vb = NULL;
+	unsigned int rc = 0;
+	unsigned long flags;
+
+	mutex_lock(&dev->mfc_mutex);
+	src_q = &ctx->vq_src;
+	dst_q = &ctx->vq_dst;
+	/*
+	 * There has to be at least one buffer queued on each queued_list, which
+	 * means either in driver already or waiting for driver to claim it
+	 * and start processing.
+	 */
+	if ((!src_q->streaming || list_empty(&src_q->queued_list))
+		&& (!dst_q->streaming || list_empty(&dst_q->queued_list))) {
+		rc = POLLERR;
+		goto end;
+	}
+	mutex_unlock(&dev->mfc_mutex);
+	poll_wait(file, &ctx->fh.wait, wait);
+	poll_wait(file, &src_q->done_wq, wait);
+	poll_wait(file, &dst_q->done_wq, wait);
+	mutex_lock(&dev->mfc_mutex);
+	if (v4l2_event_pending(&ctx->fh))
+		rc |= POLLPRI;
+	spin_lock_irqsave(&src_q->done_lock, flags);
+	if (!list_empty(&src_q->done_list))
+		src_vb = list_first_entry(&src_q->done_list, struct vb2_buffer,
+								done_entry);
+	if (src_vb && (src_vb->state == VB2_BUF_STATE_DONE
+				|| src_vb->state == VB2_BUF_STATE_ERROR))
+		rc |= POLLOUT | POLLWRNORM;
+	spin_unlock_irqrestore(&src_q->done_lock, flags);
+	spin_lock_irqsave(&dst_q->done_lock, flags);
+	if (!list_empty(&dst_q->done_list))
+		dst_vb = list_first_entry(&dst_q->done_list, struct vb2_buffer,
+								done_entry);
+	if (dst_vb && (dst_vb->state == VB2_BUF_STATE_DONE
+				|| dst_vb->state == VB2_BUF_STATE_ERROR))
+		rc |= POLLIN | POLLRDNORM;
+	spin_unlock_irqrestore(&dst_q->done_lock, flags);
+end:
+	mutex_unlock(&dev->mfc_mutex);
+	return rc;
+}
+
+/* Mmap */
+static int s5p_mfc_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(file->private_data);
+	struct s5p_mfc_dev *dev = ctx->dev;
+	unsigned long offset = vma->vm_pgoff << PAGE_SHIFT;
+	int ret;
+
+	if (mutex_lock_interruptible(&dev->mfc_mutex))
+		return -ERESTARTSYS;
+	if (offset < DST_QUEUE_OFF_BASE) {
+		mfc_debug(2, "mmaping source\n");
+		ret = vb2_mmap(&ctx->vq_src, vma);
+	} else {		/* capture */
+		mfc_debug(2, "mmaping destination\n");
+		vma->vm_pgoff -= (DST_QUEUE_OFF_BASE >> PAGE_SHIFT);
+		ret = vb2_mmap(&ctx->vq_dst, vma);
+	}
+	mutex_unlock(&dev->mfc_mutex);
+	return ret;
+}
+
+/* v4l2 ops */
+static const struct v4l2_file_operations s5p_mfc_fops = {
+	.owner = THIS_MODULE,
+	.open = s5p_mfc_open,
+	.release = s5p_mfc_release,
+	.poll = s5p_mfc_poll,
+	.unlocked_ioctl = video_ioctl2,
+	.mmap = s5p_mfc_mmap,
+};
+
+static int match_child(struct device *dev, void *data)
+{
+	if (!dev_name(dev))
+		return 0;
+	return !strcmp(dev_name(dev), (char *)data);
+}
+
+static void s5p_mfc_memdev_release(struct device *dev)
+{
+	dma_release_declared_memory(dev);
+}
+
+static void *mfc_get_drv_data(struct platform_device *pdev);
+
+static int s5p_mfc_alloc_memdevs(struct s5p_mfc_dev *dev)
+{
+	unsigned int mem_info[2] = { };
+
+	dev->mem_dev_l = devm_kzalloc(&dev->plat_dev->dev,
+			sizeof(struct device), GFP_KERNEL);
+	if (!dev->mem_dev_l) {
+		mfc_err("Not enough memory\n");
+		return -ENOMEM;
+	}
+
+	dev_set_name(dev->mem_dev_l, "%s", "s5p-mfc-l");
+	dev->mem_dev_l->release = s5p_mfc_memdev_release;
+	device_initialize(dev->mem_dev_l);
+	of_property_read_u32_array(dev->plat_dev->dev.of_node,
+			"samsung,mfc-l", mem_info, 2);
+	if (dma_declare_coherent_memory(dev->mem_dev_l, mem_info[0],
+				mem_info[0], mem_info[1],
+				DMA_MEMORY_MAP | DMA_MEMORY_EXCLUSIVE) == 0) {
+		mfc_err("Failed to declare coherent memory for\n"
+		"MFC device\n");
+		return -ENOMEM;
+	}
+
+	dev->mem_dev_r = devm_kzalloc(&dev->plat_dev->dev,
+			sizeof(struct device), GFP_KERNEL);
+	if (!dev->mem_dev_r) {
+		mfc_err("Not enough memory\n");
+		return -ENOMEM;
+	}
+
+	dev_set_name(dev->mem_dev_r, "%s", "s5p-mfc-r");
+	dev->mem_dev_r->release = s5p_mfc_memdev_release;
+	device_initialize(dev->mem_dev_r);
+	of_property_read_u32_array(dev->plat_dev->dev.of_node,
+			"samsung,mfc-r", mem_info, 2);
+	if (dma_declare_coherent_memory(dev->mem_dev_r, mem_info[0],
+				mem_info[0], mem_info[1],
+				DMA_MEMORY_MAP | DMA_MEMORY_EXCLUSIVE) == 0) {
+		pr_err("Failed to declare coherent memory for\n"
+		"MFC device\n");
+		return -ENOMEM;
+	}
+	return 0;
+}
+
+/* MFC probe function */
+static int s5p_mfc_probe(struct platform_device *pdev)
+{
+	struct s5p_mfc_dev *dev;
+	struct video_device *vfd;
+	struct resource *res;
+	int ret;
+
+	pr_debug("%s++\n", __func__);
+	dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
+	if (!dev) {
+		dev_err(&pdev->dev, "Not enough memory for MFC device\n");
+		return -ENOMEM;
+	}
+
+	spin_lock_init(&dev->irqlock);
+	spin_lock_init(&dev->condlock);
+	dev->plat_dev = pdev;
+	if (!dev->plat_dev) {
+		dev_err(&pdev->dev, "No platform data specified\n");
+		return -ENODEV;
+	}
+
+	dev->variant = mfc_get_drv_data(pdev);
+
+	ret = s5p_mfc_init_pm(dev);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to get mfc clock source\n");
+		return ret;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	dev->regs_base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(dev->regs_base))
+		return PTR_ERR(dev->regs_base);
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (res == NULL) {
+		dev_err(&pdev->dev, "failed to get irq resource\n");
+		ret = -ENOENT;
+		goto err_res;
+	}
+	dev->irq = res->start;
+	ret = devm_request_irq(&pdev->dev, dev->irq, s5p_mfc_irq,
+					0, pdev->name, dev);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to install irq (%d)\n", ret);
+		goto err_res;
+	}
+
+	if (pdev->dev.of_node) {
+		ret = s5p_mfc_alloc_memdevs(dev);
+		if (ret < 0)
+			goto err_res;
+	} else {
+		dev->mem_dev_l = device_find_child(&dev->plat_dev->dev,
+				"s5p-mfc-l", match_child);
+		if (!dev->mem_dev_l) {
+			mfc_err("Mem child (L) device get failed\n");
+			ret = -ENODEV;
+			goto err_res;
+		}
+		dev->mem_dev_r = device_find_child(&dev->plat_dev->dev,
+				"s5p-mfc-r", match_child);
+		if (!dev->mem_dev_r) {
+			mfc_err("Mem child (R) device get failed\n");
+			ret = -ENODEV;
+			goto err_res;
+		}
+	}
+
+	dev->alloc_ctx[0] = vb2_dma_contig_init_ctx(dev->mem_dev_l);
+	if (IS_ERR(dev->alloc_ctx[0])) {
+		ret = PTR_ERR(dev->alloc_ctx[0]);
+		goto err_res;
+	}
+	dev->alloc_ctx[1] = vb2_dma_contig_init_ctx(dev->mem_dev_r);
+	if (IS_ERR(dev->alloc_ctx[1])) {
+		ret = PTR_ERR(dev->alloc_ctx[1]);
+		goto err_mem_init_ctx_1;
+	}
+
+	mutex_init(&dev->mfc_mutex);
+
+	ret = s5p_mfc_alloc_firmware(dev);
+	if (ret)
+		goto err_alloc_fw;
+
+	ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
+	if (ret)
+		goto err_v4l2_dev_reg;
+	init_waitqueue_head(&dev->queue);
+
+	/* decoder */
+	vfd = video_device_alloc();
+	if (!vfd) {
+		v4l2_err(&dev->v4l2_dev, "Failed to allocate video device\n");
+		ret = -ENOMEM;
+		goto err_dec_alloc;
+	}
+	vfd->fops	= &s5p_mfc_fops;
+	vfd->ioctl_ops	= get_dec_v4l2_ioctl_ops();
+	vfd->release	= video_device_release;
+	vfd->lock	= &dev->mfc_mutex;
+	vfd->v4l2_dev	= &dev->v4l2_dev;
+	vfd->vfl_dir	= VFL_DIR_M2M;
+	snprintf(vfd->name, sizeof(vfd->name), "%s", S5P_MFC_DEC_NAME);
+	dev->vfd_dec	= vfd;
+	ret = video_register_device(vfd, VFL_TYPE_GRABBER, 0);
+	if (ret) {
+		v4l2_err(&dev->v4l2_dev, "Failed to register video device\n");
+		video_device_release(vfd);
+		goto err_dec_reg;
+	}
+	v4l2_info(&dev->v4l2_dev,
+		  "decoder registered as /dev/video%d\n", vfd->num);
+	video_set_drvdata(vfd, dev);
+
+	/* encoder */
+	vfd = video_device_alloc();
+	if (!vfd) {
+		v4l2_err(&dev->v4l2_dev, "Failed to allocate video device\n");
+		ret = -ENOMEM;
+		goto err_enc_alloc;
+	}
+	vfd->fops	= &s5p_mfc_fops;
+	vfd->ioctl_ops	= get_enc_v4l2_ioctl_ops();
+	vfd->release	= video_device_release;
+	vfd->lock	= &dev->mfc_mutex;
+	vfd->v4l2_dev	= &dev->v4l2_dev;
+	vfd->vfl_dir	= VFL_DIR_M2M;
+	snprintf(vfd->name, sizeof(vfd->name), "%s", S5P_MFC_ENC_NAME);
+	dev->vfd_enc	= vfd;
+	ret = video_register_device(vfd, VFL_TYPE_GRABBER, 0);
+	if (ret) {
+		v4l2_err(&dev->v4l2_dev, "Failed to register video device\n");
+		video_device_release(vfd);
+		goto err_enc_reg;
+	}
+	v4l2_info(&dev->v4l2_dev,
+		  "encoder registered as /dev/video%d\n", vfd->num);
+	video_set_drvdata(vfd, dev);
+	platform_set_drvdata(pdev, dev);
+
+	dev->hw_lock = 0;
+	dev->watchdog_workqueue = create_singlethread_workqueue(S5P_MFC_NAME);
+	INIT_WORK(&dev->watchdog_work, s5p_mfc_watchdog_worker);
+	atomic_set(&dev->watchdog_cnt, 0);
+	init_timer(&dev->watchdog_timer);
+	dev->watchdog_timer.data = (unsigned long)dev;
+	dev->watchdog_timer.function = s5p_mfc_watchdog;
+
+	/* Initialize HW ops and commands based on MFC version */
+	s5p_mfc_init_hw_ops(dev);
+	s5p_mfc_init_hw_cmds(dev);
+	s5p_mfc_init_regs(dev);
+
+	pr_debug("%s--\n", __func__);
+	return 0;
+
+/* Deinit MFC if probe had failed */
+err_enc_reg:
+	video_device_release(dev->vfd_enc);
+err_enc_alloc:
+	video_unregister_device(dev->vfd_dec);
+err_dec_reg:
+	video_device_release(dev->vfd_dec);
+err_dec_alloc:
+	v4l2_device_unregister(&dev->v4l2_dev);
+err_v4l2_dev_reg:
+	s5p_mfc_release_firmware(dev);
+err_alloc_fw:
+	vb2_dma_contig_cleanup_ctx(dev->alloc_ctx[1]);
+err_mem_init_ctx_1:
+	vb2_dma_contig_cleanup_ctx(dev->alloc_ctx[0]);
+err_res:
+	s5p_mfc_final_pm(dev);
+
+	pr_debug("%s-- with error\n", __func__);
+	return ret;
+
+}
+
+/* Remove the driver */
+static int s5p_mfc_remove(struct platform_device *pdev)
+{
+	struct s5p_mfc_dev *dev = platform_get_drvdata(pdev);
+
+	v4l2_info(&dev->v4l2_dev, "Removing %s\n", pdev->name);
+
+	del_timer_sync(&dev->watchdog_timer);
+	flush_workqueue(dev->watchdog_workqueue);
+	destroy_workqueue(dev->watchdog_workqueue);
+
+	video_unregister_device(dev->vfd_enc);
+	video_unregister_device(dev->vfd_dec);
+	v4l2_device_unregister(&dev->v4l2_dev);
+	s5p_mfc_release_firmware(dev);
+	vb2_dma_contig_cleanup_ctx(dev->alloc_ctx[0]);
+	vb2_dma_contig_cleanup_ctx(dev->alloc_ctx[1]);
+	if (pdev->dev.of_node) {
+		put_device(dev->mem_dev_l);
+		put_device(dev->mem_dev_r);
+	}
+
+	s5p_mfc_final_pm(dev);
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+
+static int s5p_mfc_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct s5p_mfc_dev *m_dev = platform_get_drvdata(pdev);
+	int ret;
+
+	if (m_dev->num_inst == 0)
+		return 0;
+
+	if (test_and_set_bit(0, &m_dev->enter_suspend) != 0) {
+		mfc_err("Error: going to suspend for a second time\n");
+		return -EIO;
+	}
+
+	/* Check if we're processing then wait if it necessary. */
+	while (test_and_set_bit(0, &m_dev->hw_lock) != 0) {
+		/* Try and lock the HW */
+		/* Wait on the interrupt waitqueue */
+		ret = wait_event_interruptible_timeout(m_dev->queue,
+			m_dev->int_cond, msecs_to_jiffies(MFC_INT_TIMEOUT));
+		if (ret == 0) {
+			mfc_err("Waiting for hardware to finish timed out\n");
+			clear_bit(0, &m_dev->enter_suspend);
+			return -EIO;
+		}
+	}
+
+	ret = s5p_mfc_sleep(m_dev);
+	if (ret) {
+		clear_bit(0, &m_dev->enter_suspend);
+		clear_bit(0, &m_dev->hw_lock);
+	}
+	return ret;
+}
+
+static int s5p_mfc_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct s5p_mfc_dev *m_dev = platform_get_drvdata(pdev);
+
+	if (m_dev->num_inst == 0)
+		return 0;
+	return s5p_mfc_wakeup(m_dev);
+}
+#endif
+
+#ifdef CONFIG_PM
+static int s5p_mfc_runtime_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct s5p_mfc_dev *m_dev = platform_get_drvdata(pdev);
+
+	atomic_set(&m_dev->pm.power, 0);
+	return 0;
+}
+
+static int s5p_mfc_runtime_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct s5p_mfc_dev *m_dev = platform_get_drvdata(pdev);
+
+	atomic_set(&m_dev->pm.power, 1);
+	return 0;
+}
+#endif
+
+/* Power management */
+static const struct dev_pm_ops s5p_mfc_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(s5p_mfc_suspend, s5p_mfc_resume)
+	SET_RUNTIME_PM_OPS(s5p_mfc_runtime_suspend, s5p_mfc_runtime_resume,
+			   NULL)
+};
+
+static struct s5p_mfc_buf_size_v5 mfc_buf_size_v5 = {
+	.h264_ctx	= MFC_H264_CTX_BUF_SIZE,
+	.non_h264_ctx	= MFC_CTX_BUF_SIZE,
+	.dsc		= DESC_BUF_SIZE,
+	.shm		= SHARED_BUF_SIZE,
+};
+
+static struct s5p_mfc_buf_size buf_size_v5 = {
+	.fw	= MAX_FW_SIZE,
+	.cpb	= MAX_CPB_SIZE,
+	.priv	= &mfc_buf_size_v5,
+};
+
+static struct s5p_mfc_buf_align mfc_buf_align_v5 = {
+	.base = MFC_BASE_ALIGN_ORDER,
+};
+
+static struct s5p_mfc_variant mfc_drvdata_v5 = {
+	.version	= MFC_VERSION,
+	.version_bit	= MFC_V5_BIT,
+	.port_num	= MFC_NUM_PORTS,
+	.buf_size	= &buf_size_v5,
+	.buf_align	= &mfc_buf_align_v5,
+	.fw_name[0]	= "s5p-mfc.fw",
+};
+
+static struct s5p_mfc_buf_size_v6 mfc_buf_size_v6 = {
+	.dev_ctx	= MFC_CTX_BUF_SIZE_V6,
+	.h264_dec_ctx	= MFC_H264_DEC_CTX_BUF_SIZE_V6,
+	.other_dec_ctx	= MFC_OTHER_DEC_CTX_BUF_SIZE_V6,
+	.h264_enc_ctx	= MFC_H264_ENC_CTX_BUF_SIZE_V6,
+	.other_enc_ctx	= MFC_OTHER_ENC_CTX_BUF_SIZE_V6,
+};
+
+static struct s5p_mfc_buf_size buf_size_v6 = {
+	.fw	= MAX_FW_SIZE_V6,
+	.cpb	= MAX_CPB_SIZE_V6,
+	.priv	= &mfc_buf_size_v6,
+};
+
+static struct s5p_mfc_buf_align mfc_buf_align_v6 = {
+	.base = 0,
+};
+
+static struct s5p_mfc_variant mfc_drvdata_v6 = {
+	.version	= MFC_VERSION_V6,
+	.version_bit	= MFC_V6_BIT,
+	.port_num	= MFC_NUM_PORTS_V6,
+	.buf_size	= &buf_size_v6,
+	.buf_align	= &mfc_buf_align_v6,
+	.fw_name[0]     = "s5p-mfc-v6.fw",
+	/*
+	 * v6-v2 firmware contains bug fixes and interface change
+	 * for init buffer command
+	 */
+	.fw_name[1]     = "s5p-mfc-v6-v2.fw",
+};
+
+static struct s5p_mfc_buf_size_v6 mfc_buf_size_v7 = {
+	.dev_ctx	= MFC_CTX_BUF_SIZE_V7,
+	.h264_dec_ctx	= MFC_H264_DEC_CTX_BUF_SIZE_V7,
+	.other_dec_ctx	= MFC_OTHER_DEC_CTX_BUF_SIZE_V7,
+	.h264_enc_ctx	= MFC_H264_ENC_CTX_BUF_SIZE_V7,
+	.other_enc_ctx	= MFC_OTHER_ENC_CTX_BUF_SIZE_V7,
+};
+
+static struct s5p_mfc_buf_size buf_size_v7 = {
+	.fw	= MAX_FW_SIZE_V7,
+	.cpb	= MAX_CPB_SIZE_V7,
+	.priv	= &mfc_buf_size_v7,
+};
+
+static struct s5p_mfc_buf_align mfc_buf_align_v7 = {
+	.base = 0,
+};
+
+static struct s5p_mfc_variant mfc_drvdata_v7 = {
+	.version	= MFC_VERSION_V7,
+	.version_bit	= MFC_V7_BIT,
+	.port_num	= MFC_NUM_PORTS_V7,
+	.buf_size	= &buf_size_v7,
+	.buf_align	= &mfc_buf_align_v7,
+	.fw_name[0]     = "s5p-mfc-v7.fw",
+};
+
+static struct s5p_mfc_buf_size_v6 mfc_buf_size_v8 = {
+	.dev_ctx	= MFC_CTX_BUF_SIZE_V8,
+	.h264_dec_ctx	= MFC_H264_DEC_CTX_BUF_SIZE_V8,
+	.other_dec_ctx	= MFC_OTHER_DEC_CTX_BUF_SIZE_V8,
+	.h264_enc_ctx	= MFC_H264_ENC_CTX_BUF_SIZE_V8,
+	.other_enc_ctx	= MFC_OTHER_ENC_CTX_BUF_SIZE_V8,
+};
+
+static struct s5p_mfc_buf_size buf_size_v8 = {
+	.fw	= MAX_FW_SIZE_V8,
+	.cpb	= MAX_CPB_SIZE_V8,
+	.priv	= &mfc_buf_size_v8,
+};
+
+static struct s5p_mfc_buf_align mfc_buf_align_v8 = {
+	.base = 0,
+};
+
+static struct s5p_mfc_variant mfc_drvdata_v8 = {
+	.version	= MFC_VERSION_V8,
+	.version_bit	= MFC_V8_BIT,
+	.port_num	= MFC_NUM_PORTS_V8,
+	.buf_size	= &buf_size_v8,
+	.buf_align	= &mfc_buf_align_v8,
+	.fw_name[0]     = "s5p-mfc-v8.fw",
+};
+
+static const struct platform_device_id mfc_driver_ids[] = {
+	{
+		.name = "s5p-mfc",
+		.driver_data = (unsigned long)&mfc_drvdata_v5,
+	}, {
+		.name = "s5p-mfc-v5",
+		.driver_data = (unsigned long)&mfc_drvdata_v5,
+	}, {
+		.name = "s5p-mfc-v6",
+		.driver_data = (unsigned long)&mfc_drvdata_v6,
+	}, {
+		.name = "s5p-mfc-v7",
+		.driver_data = (unsigned long)&mfc_drvdata_v7,
+	}, {
+		.name = "s5p-mfc-v8",
+		.driver_data = (unsigned long)&mfc_drvdata_v8,
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(platform, mfc_driver_ids);
+
+static const struct of_device_id exynos_mfc_match[] = {
+	{
+		.compatible = "samsung,mfc-v5",
+		.data = &mfc_drvdata_v5,
+	}, {
+		.compatible = "samsung,mfc-v6",
+		.data = &mfc_drvdata_v6,
+	}, {
+		.compatible = "samsung,mfc-v7",
+		.data = &mfc_drvdata_v7,
+	}, {
+		.compatible = "samsung,mfc-v8",
+		.data = &mfc_drvdata_v8,
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(of, exynos_mfc_match);
+
+static void *mfc_get_drv_data(struct platform_device *pdev)
+{
+	struct s5p_mfc_variant *driver_data = NULL;
+
+	if (pdev->dev.of_node) {
+		const struct of_device_id *match;
+		match = of_match_node(exynos_mfc_match,
+				pdev->dev.of_node);
+		if (match)
+			driver_data = (struct s5p_mfc_variant *)match->data;
+	} else {
+		driver_data = (struct s5p_mfc_variant *)
+			platform_get_device_id(pdev)->driver_data;
+	}
+	return driver_data;
+}
+
+static struct platform_driver s5p_mfc_driver = {
+	.probe		= s5p_mfc_probe,
+	.remove		= s5p_mfc_remove,
+	.id_table	= mfc_driver_ids,
+	.driver	= {
+		.name	= S5P_MFC_NAME,
+		.pm	= &s5p_mfc_pm_ops,
+		.of_match_table = exynos_mfc_match,
+	},
+};
+
+module_platform_driver(s5p_mfc_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Kamil Debski <k.debski@samsung.com>");
+MODULE_DESCRIPTION("Samsung S5P Multi Format Codec V4L2 driver");
+
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.c b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.c
new file mode 100644
index 0000000..242c033
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.c
@@ -0,0 +1,29 @@
+/*
+ * linux/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.c
+ *
+ * Copyright (C) 2012 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include "s5p_mfc_cmd.h"
+#include "s5p_mfc_common.h"
+#include "s5p_mfc_debug.h"
+#include "s5p_mfc_cmd_v5.h"
+#include "s5p_mfc_cmd_v6.h"
+
+static struct s5p_mfc_hw_cmds *s5p_mfc_cmds;
+
+void s5p_mfc_init_hw_cmds(struct s5p_mfc_dev *dev)
+{
+	if (IS_MFCV6_PLUS(dev))
+		s5p_mfc_cmds = s5p_mfc_init_hw_cmds_v6();
+	else
+		s5p_mfc_cmds = s5p_mfc_init_hw_cmds_v5();
+
+	dev->mfc_cmds = s5p_mfc_cmds;
+}
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.h b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.h
new file mode 100644
index 0000000..282e6c7
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.h
@@ -0,0 +1,35 @@
+/*
+ * linux/drivers/media/platform/s5p-mfc/s5p_mfc_cmd.h
+ *
+ * Copyright (C) 2012 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#ifndef S5P_MFC_CMD_H_
+#define S5P_MFC_CMD_H_
+
+#include "s5p_mfc_common.h"
+
+#define MAX_H2R_ARG	4
+
+struct s5p_mfc_cmd_args {
+	unsigned int	arg[MAX_H2R_ARG];
+};
+
+struct s5p_mfc_hw_cmds {
+	int (*cmd_host2risc)(struct s5p_mfc_dev *dev, int cmd,
+				struct s5p_mfc_cmd_args *args);
+	int (*sys_init_cmd)(struct s5p_mfc_dev *dev);
+	int (*sleep_cmd)(struct s5p_mfc_dev *dev);
+	int (*wakeup_cmd)(struct s5p_mfc_dev *dev);
+	int (*open_inst_cmd)(struct s5p_mfc_ctx *ctx);
+	int (*close_inst_cmd)(struct s5p_mfc_ctx *ctx);
+};
+
+void s5p_mfc_init_hw_cmds(struct s5p_mfc_dev *dev);
+#endif /* S5P_MFC_CMD_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.c b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.c
new file mode 100644
index 0000000..8c4739c
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.c
@@ -0,0 +1,167 @@
+/*
+ * linux/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.c
+ *
+ * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include "regs-mfc.h"
+#include "s5p_mfc_cmd.h"
+#include "s5p_mfc_common.h"
+#include "s5p_mfc_debug.h"
+#include "s5p_mfc_cmd_v5.h"
+
+/* This function is used to send a command to the MFC */
+static int s5p_mfc_cmd_host2risc_v5(struct s5p_mfc_dev *dev, int cmd,
+				struct s5p_mfc_cmd_args *args)
+{
+	int cur_cmd;
+	unsigned long timeout;
+
+	timeout = jiffies + msecs_to_jiffies(MFC_BW_TIMEOUT);
+	/* wait until host to risc command register becomes 'H2R_CMD_EMPTY' */
+	do {
+		if (time_after(jiffies, timeout)) {
+			mfc_err("Timeout while waiting for hardware\n");
+			return -EIO;
+		}
+		cur_cmd = mfc_read(dev, S5P_FIMV_HOST2RISC_CMD);
+	} while (cur_cmd != S5P_FIMV_H2R_CMD_EMPTY);
+	mfc_write(dev, args->arg[0], S5P_FIMV_HOST2RISC_ARG1);
+	mfc_write(dev, args->arg[1], S5P_FIMV_HOST2RISC_ARG2);
+	mfc_write(dev, args->arg[2], S5P_FIMV_HOST2RISC_ARG3);
+	mfc_write(dev, args->arg[3], S5P_FIMV_HOST2RISC_ARG4);
+	/* Issue the command */
+	mfc_write(dev, cmd, S5P_FIMV_HOST2RISC_CMD);
+	return 0;
+}
+
+/* Initialize the MFC */
+static int s5p_mfc_sys_init_cmd_v5(struct s5p_mfc_dev *dev)
+{
+	struct s5p_mfc_cmd_args h2r_args;
+
+	memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
+	h2r_args.arg[0] = dev->fw_size;
+	return s5p_mfc_cmd_host2risc_v5(dev, S5P_FIMV_H2R_CMD_SYS_INIT,
+			&h2r_args);
+}
+
+/* Suspend the MFC hardware */
+static int s5p_mfc_sleep_cmd_v5(struct s5p_mfc_dev *dev)
+{
+	struct s5p_mfc_cmd_args h2r_args;
+
+	memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
+	return s5p_mfc_cmd_host2risc_v5(dev, S5P_FIMV_H2R_CMD_SLEEP, &h2r_args);
+}
+
+/* Wake up the MFC hardware */
+static int s5p_mfc_wakeup_cmd_v5(struct s5p_mfc_dev *dev)
+{
+	struct s5p_mfc_cmd_args h2r_args;
+
+	memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
+	return s5p_mfc_cmd_host2risc_v5(dev, S5P_FIMV_H2R_CMD_WAKEUP,
+			&h2r_args);
+}
+
+
+static int s5p_mfc_open_inst_cmd_v5(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_cmd_args h2r_args;
+	int ret;
+
+	/* Preparing decoding - getting instance number */
+	mfc_debug(2, "Getting instance number (codec: %d)\n", ctx->codec_mode);
+	dev->curr_ctx = ctx->num;
+	memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
+	switch (ctx->codec_mode) {
+	case S5P_MFC_CODEC_H264_DEC:
+		h2r_args.arg[0] = S5P_FIMV_CODEC_H264_DEC;
+		break;
+	case S5P_MFC_CODEC_VC1_DEC:
+		h2r_args.arg[0] = S5P_FIMV_CODEC_VC1_DEC;
+		break;
+	case S5P_MFC_CODEC_MPEG4_DEC:
+		h2r_args.arg[0] = S5P_FIMV_CODEC_MPEG4_DEC;
+		break;
+	case S5P_MFC_CODEC_MPEG2_DEC:
+		h2r_args.arg[0] = S5P_FIMV_CODEC_MPEG2_DEC;
+		break;
+	case S5P_MFC_CODEC_H263_DEC:
+		h2r_args.arg[0] = S5P_FIMV_CODEC_H263_DEC;
+		break;
+	case S5P_MFC_CODEC_VC1RCV_DEC:
+		h2r_args.arg[0] = S5P_FIMV_CODEC_VC1RCV_DEC;
+		break;
+	case S5P_MFC_CODEC_H264_ENC:
+		h2r_args.arg[0] = S5P_FIMV_CODEC_H264_ENC;
+		break;
+	case S5P_MFC_CODEC_MPEG4_ENC:
+		h2r_args.arg[0] = S5P_FIMV_CODEC_MPEG4_ENC;
+		break;
+	case S5P_MFC_CODEC_H263_ENC:
+		h2r_args.arg[0] = S5P_FIMV_CODEC_H263_ENC;
+		break;
+	default:
+		h2r_args.arg[0] = S5P_FIMV_CODEC_NONE;
+	}
+	h2r_args.arg[1] = 0; /* no crc & no pixelcache */
+	h2r_args.arg[2] = ctx->ctx.ofs;
+	h2r_args.arg[3] = ctx->ctx.size;
+	ret = s5p_mfc_cmd_host2risc_v5(dev, S5P_FIMV_H2R_CMD_OPEN_INSTANCE,
+								&h2r_args);
+	if (ret) {
+		mfc_err("Failed to create a new instance\n");
+		ctx->state = MFCINST_ERROR;
+	}
+	return ret;
+}
+
+static int s5p_mfc_close_inst_cmd_v5(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_cmd_args h2r_args;
+	int ret;
+
+	if (ctx->state == MFCINST_FREE) {
+		mfc_err("Instance already returned\n");
+		ctx->state = MFCINST_ERROR;
+		return -EINVAL;
+	}
+	/* Closing decoding instance  */
+	mfc_debug(2, "Returning instance number %d\n", ctx->inst_no);
+	dev->curr_ctx = ctx->num;
+	memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
+	h2r_args.arg[0] = ctx->inst_no;
+	ret = s5p_mfc_cmd_host2risc_v5(dev, S5P_FIMV_H2R_CMD_CLOSE_INSTANCE,
+								&h2r_args);
+	if (ret) {
+		mfc_err("Failed to return an instance\n");
+		ctx->state = MFCINST_ERROR;
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/* Initialize cmd function pointers for MFC v5 */
+static struct s5p_mfc_hw_cmds s5p_mfc_cmds_v5 = {
+	.cmd_host2risc = s5p_mfc_cmd_host2risc_v5,
+	.sys_init_cmd = s5p_mfc_sys_init_cmd_v5,
+	.sleep_cmd = s5p_mfc_sleep_cmd_v5,
+	.wakeup_cmd = s5p_mfc_wakeup_cmd_v5,
+	.open_inst_cmd = s5p_mfc_open_inst_cmd_v5,
+	.close_inst_cmd = s5p_mfc_close_inst_cmd_v5,
+};
+
+struct s5p_mfc_hw_cmds *s5p_mfc_init_hw_cmds_v5(void)
+{
+	return &s5p_mfc_cmds_v5;
+}
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.h b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.h
new file mode 100644
index 0000000..6928a55
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.h
@@ -0,0 +1,20 @@
+/*
+ * linux/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.h
+ *
+ * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#ifndef S5P_MFC_CMD_V5_H_
+#define S5P_MFC_CMD_V5_H_
+
+#include "s5p_mfc_common.h"
+
+struct s5p_mfc_hw_cmds *s5p_mfc_init_hw_cmds_v5(void);
+
+#endif /* S5P_MFC_CMD_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.c b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.c
new file mode 100644
index 0000000..b1b1491
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.c
@@ -0,0 +1,164 @@
+/*
+ * linux/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.c
+ *
+ * Copyright (c) 2012 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include "s5p_mfc_common.h"
+
+#include "s5p_mfc_cmd.h"
+#include "s5p_mfc_debug.h"
+#include "s5p_mfc_intr.h"
+#include "s5p_mfc_opr.h"
+#include "s5p_mfc_cmd_v6.h"
+
+static int s5p_mfc_cmd_host2risc_v6(struct s5p_mfc_dev *dev, int cmd,
+				struct s5p_mfc_cmd_args *args)
+{
+	mfc_debug(2, "Issue the command: %d\n", cmd);
+
+	/* Reset RISC2HOST command */
+	mfc_write(dev, 0x0, S5P_FIMV_RISC2HOST_CMD_V6);
+
+	/* Issue the command */
+	mfc_write(dev, cmd, S5P_FIMV_HOST2RISC_CMD_V6);
+	mfc_write(dev, 0x1, S5P_FIMV_HOST2RISC_INT_V6);
+
+	return 0;
+}
+
+static int s5p_mfc_sys_init_cmd_v6(struct s5p_mfc_dev *dev)
+{
+	struct s5p_mfc_cmd_args h2r_args;
+	struct s5p_mfc_buf_size_v6 *buf_size = dev->variant->buf_size->priv;
+	int ret;
+
+	ret = s5p_mfc_hw_call(dev->mfc_ops, alloc_dev_context_buffer, dev);
+	if (ret)
+		return ret;
+
+	mfc_write(dev, dev->ctx_buf.dma, S5P_FIMV_CONTEXT_MEM_ADDR_V6);
+	mfc_write(dev, buf_size->dev_ctx, S5P_FIMV_CONTEXT_MEM_SIZE_V6);
+	return s5p_mfc_cmd_host2risc_v6(dev, S5P_FIMV_H2R_CMD_SYS_INIT_V6,
+					&h2r_args);
+}
+
+static int s5p_mfc_sleep_cmd_v6(struct s5p_mfc_dev *dev)
+{
+	struct s5p_mfc_cmd_args h2r_args;
+
+	memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
+	return s5p_mfc_cmd_host2risc_v6(dev, S5P_FIMV_H2R_CMD_SLEEP_V6,
+			&h2r_args);
+}
+
+static int s5p_mfc_wakeup_cmd_v6(struct s5p_mfc_dev *dev)
+{
+	struct s5p_mfc_cmd_args h2r_args;
+
+	memset(&h2r_args, 0, sizeof(struct s5p_mfc_cmd_args));
+	return s5p_mfc_cmd_host2risc_v6(dev, S5P_FIMV_H2R_CMD_WAKEUP_V6,
+					&h2r_args);
+}
+
+/* Open a new instance and get its number */
+static int s5p_mfc_open_inst_cmd_v6(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_cmd_args h2r_args;
+	int codec_type;
+
+	mfc_debug(2, "Requested codec mode: %d\n", ctx->codec_mode);
+	dev->curr_ctx = ctx->num;
+	switch (ctx->codec_mode) {
+	case S5P_MFC_CODEC_H264_DEC:
+		codec_type = S5P_FIMV_CODEC_H264_DEC_V6;
+		break;
+	case S5P_MFC_CODEC_H264_MVC_DEC:
+		codec_type = S5P_FIMV_CODEC_H264_MVC_DEC_V6;
+		break;
+	case S5P_MFC_CODEC_VC1_DEC:
+		codec_type = S5P_FIMV_CODEC_VC1_DEC_V6;
+		break;
+	case S5P_MFC_CODEC_MPEG4_DEC:
+		codec_type = S5P_FIMV_CODEC_MPEG4_DEC_V6;
+		break;
+	case S5P_MFC_CODEC_MPEG2_DEC:
+		codec_type = S5P_FIMV_CODEC_MPEG2_DEC_V6;
+		break;
+	case S5P_MFC_CODEC_H263_DEC:
+		codec_type = S5P_FIMV_CODEC_H263_DEC_V6;
+		break;
+	case S5P_MFC_CODEC_VC1RCV_DEC:
+		codec_type = S5P_FIMV_CODEC_VC1RCV_DEC_V6;
+		break;
+	case S5P_MFC_CODEC_VP8_DEC:
+		codec_type = S5P_FIMV_CODEC_VP8_DEC_V6;
+		break;
+	case S5P_MFC_CODEC_H264_ENC:
+		codec_type = S5P_FIMV_CODEC_H264_ENC_V6;
+		break;
+	case S5P_MFC_CODEC_H264_MVC_ENC:
+		codec_type = S5P_FIMV_CODEC_H264_MVC_ENC_V6;
+		break;
+	case S5P_MFC_CODEC_MPEG4_ENC:
+		codec_type = S5P_FIMV_CODEC_MPEG4_ENC_V6;
+		break;
+	case S5P_MFC_CODEC_H263_ENC:
+		codec_type = S5P_FIMV_CODEC_H263_ENC_V6;
+		break;
+	case S5P_MFC_CODEC_VP8_ENC:
+		codec_type = S5P_FIMV_CODEC_VP8_ENC_V7;
+		break;
+	default:
+		codec_type = S5P_FIMV_CODEC_NONE_V6;
+	}
+	mfc_write(dev, codec_type, S5P_FIMV_CODEC_TYPE_V6);
+	mfc_write(dev, ctx->ctx.dma, S5P_FIMV_CONTEXT_MEM_ADDR_V6);
+	mfc_write(dev, ctx->ctx.size, S5P_FIMV_CONTEXT_MEM_SIZE_V6);
+	mfc_write(dev, 0, S5P_FIMV_D_CRC_CTRL_V6); /* no crc */
+
+	return s5p_mfc_cmd_host2risc_v6(dev, S5P_FIMV_H2R_CMD_OPEN_INSTANCE_V6,
+					&h2r_args);
+}
+
+/* Close instance */
+static int s5p_mfc_close_inst_cmd_v6(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_cmd_args h2r_args;
+	int ret = 0;
+
+	dev->curr_ctx = ctx->num;
+	if (ctx->state != MFCINST_FREE) {
+		mfc_write(dev, ctx->inst_no, S5P_FIMV_INSTANCE_ID_V6);
+		ret = s5p_mfc_cmd_host2risc_v6(dev,
+					S5P_FIMV_H2R_CMD_CLOSE_INSTANCE_V6,
+					&h2r_args);
+	} else {
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+/* Initialize cmd function pointers for MFC v6 */
+static struct s5p_mfc_hw_cmds s5p_mfc_cmds_v6 = {
+	.cmd_host2risc = s5p_mfc_cmd_host2risc_v6,
+	.sys_init_cmd = s5p_mfc_sys_init_cmd_v6,
+	.sleep_cmd = s5p_mfc_sleep_cmd_v6,
+	.wakeup_cmd = s5p_mfc_wakeup_cmd_v6,
+	.open_inst_cmd = s5p_mfc_open_inst_cmd_v6,
+	.close_inst_cmd = s5p_mfc_close_inst_cmd_v6,
+};
+
+struct s5p_mfc_hw_cmds *s5p_mfc_init_hw_cmds_v6(void)
+{
+	return &s5p_mfc_cmds_v6;
+}
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.h b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.h
new file mode 100644
index 0000000..b7a8e57
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.h
@@ -0,0 +1,20 @@
+/*
+ * linux/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.h
+ *
+ * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#ifndef S5P_MFC_CMD_V6_H_
+#define S5P_MFC_CMD_V6_H_
+
+#include "s5p_mfc_common.h"
+
+struct s5p_mfc_hw_cmds *s5p_mfc_init_hw_cmds_v6(void);
+
+#endif /* S5P_MFC_CMD_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_common.h b/drivers/media/platform/s5p-mfc/s5p_mfc_common.h
new file mode 100644
index 0000000..d1a3f9b
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_common.h
@@ -0,0 +1,727 @@
+/*
+ * Samsung S5P Multi Format Codec v 5.0
+ *
+ * This file contains definitions of enums and structs used by the codec
+ * driver.
+ *
+ * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ * Kamil Debski, <k.debski@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation; either version 2 of the
+ * License, or (at your option) any later version
+ */
+
+#ifndef S5P_MFC_COMMON_H_
+#define S5P_MFC_COMMON_H_
+
+#include <linux/platform_device.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-v4l2.h>
+#include "regs-mfc.h"
+#include "regs-mfc-v8.h"
+
+/* Definitions related to MFC memory */
+
+/* Offset base used to differentiate between CAPTURE and OUTPUT
+*  while mmaping */
+#define DST_QUEUE_OFF_BASE	(1 << 30)
+
+#define MFC_BANK1_ALLOC_CTX	0
+#define MFC_BANK2_ALLOC_CTX	1
+
+#define MFC_BANK1_ALIGN_ORDER	13
+#define MFC_BANK2_ALIGN_ORDER	13
+#define MFC_BASE_ALIGN_ORDER	17
+
+#define MFC_FW_MAX_VERSIONS	2
+
+#include <media/videobuf2-dma-contig.h>
+
+static inline dma_addr_t s5p_mfc_mem_cookie(void *a, void *b)
+{
+	/* Same functionality as the vb2_dma_contig_plane_paddr */
+	dma_addr_t *paddr = vb2_dma_contig_memops.cookie(b);
+
+	return *paddr;
+}
+
+/* MFC definitions */
+#define MFC_MAX_EXTRA_DPB       5
+#define MFC_MAX_BUFFERS		32
+#define MFC_NUM_CONTEXTS	4
+/* Interrupt timeout */
+#define MFC_INT_TIMEOUT		2000
+/* Busy wait timeout */
+#define MFC_BW_TIMEOUT		500
+/* Watchdog interval */
+#define MFC_WATCHDOG_INTERVAL   1000
+/* After how many executions watchdog should assume lock up */
+#define MFC_WATCHDOG_CNT        10
+#define MFC_NO_INSTANCE_SET	-1
+#define MFC_ENC_CAP_PLANE_COUNT	1
+#define MFC_ENC_OUT_PLANE_COUNT	2
+#define STUFF_BYTE		4
+#define MFC_MAX_CTRLS		77
+
+#define S5P_MFC_CODEC_NONE		-1
+#define S5P_MFC_CODEC_H264_DEC		0
+#define S5P_MFC_CODEC_H264_MVC_DEC	1
+#define S5P_MFC_CODEC_VC1_DEC		2
+#define S5P_MFC_CODEC_MPEG4_DEC		3
+#define S5P_MFC_CODEC_MPEG2_DEC		4
+#define S5P_MFC_CODEC_H263_DEC		5
+#define S5P_MFC_CODEC_VC1RCV_DEC	6
+#define S5P_MFC_CODEC_VP8_DEC		7
+
+#define S5P_MFC_CODEC_H264_ENC		20
+#define S5P_MFC_CODEC_H264_MVC_ENC	21
+#define S5P_MFC_CODEC_MPEG4_ENC		22
+#define S5P_MFC_CODEC_H263_ENC		23
+#define S5P_MFC_CODEC_VP8_ENC		24
+
+#define S5P_MFC_R2H_CMD_EMPTY			0
+#define S5P_MFC_R2H_CMD_SYS_INIT_RET		1
+#define S5P_MFC_R2H_CMD_OPEN_INSTANCE_RET	2
+#define S5P_MFC_R2H_CMD_SEQ_DONE_RET		3
+#define S5P_MFC_R2H_CMD_INIT_BUFFERS_RET	4
+#define S5P_MFC_R2H_CMD_CLOSE_INSTANCE_RET	6
+#define S5P_MFC_R2H_CMD_SLEEP_RET		7
+#define S5P_MFC_R2H_CMD_WAKEUP_RET		8
+#define S5P_MFC_R2H_CMD_COMPLETE_SEQ_RET	9
+#define S5P_MFC_R2H_CMD_DPB_FLUSH_RET		10
+#define S5P_MFC_R2H_CMD_NAL_ABORT_RET		11
+#define S5P_MFC_R2H_CMD_FW_STATUS_RET		12
+#define S5P_MFC_R2H_CMD_FRAME_DONE_RET		13
+#define S5P_MFC_R2H_CMD_FIELD_DONE_RET		14
+#define S5P_MFC_R2H_CMD_SLICE_DONE_RET		15
+#define S5P_MFC_R2H_CMD_ENC_BUFFER_FUL_RET	16
+#define S5P_MFC_R2H_CMD_ERR_RET			32
+
+#define mfc_read(dev, offset)		readl(dev->regs_base + (offset))
+#define mfc_write(dev, data, offset)	writel((data), dev->regs_base + \
+								(offset))
+
+/**
+ * enum s5p_mfc_fmt_type - type of the pixelformat
+ */
+enum s5p_mfc_fmt_type {
+	MFC_FMT_DEC,
+	MFC_FMT_ENC,
+	MFC_FMT_RAW,
+};
+
+/**
+ * enum s5p_mfc_inst_type - The type of an MFC instance.
+ */
+enum s5p_mfc_inst_type {
+	MFCINST_INVALID,
+	MFCINST_DECODER,
+	MFCINST_ENCODER,
+};
+
+/**
+ * enum s5p_mfc_inst_state - The state of an MFC instance.
+ */
+enum s5p_mfc_inst_state {
+	MFCINST_FREE = 0,
+	MFCINST_INIT = 100,
+	MFCINST_GOT_INST,
+	MFCINST_HEAD_PARSED,
+	MFCINST_HEAD_PRODUCED,
+	MFCINST_BUFS_SET,
+	MFCINST_RUNNING,
+	MFCINST_FINISHING,
+	MFCINST_FINISHED,
+	MFCINST_RETURN_INST,
+	MFCINST_ERROR,
+	MFCINST_ABORT,
+	MFCINST_FLUSH,
+	MFCINST_RES_CHANGE_INIT,
+	MFCINST_RES_CHANGE_FLUSH,
+	MFCINST_RES_CHANGE_END,
+};
+
+/**
+ * enum s5p_mfc_queue_state - The state of buffer queue.
+ */
+enum s5p_mfc_queue_state {
+	QUEUE_FREE,
+	QUEUE_BUFS_REQUESTED,
+	QUEUE_BUFS_QUERIED,
+	QUEUE_BUFS_MMAPED,
+};
+
+/**
+ * enum s5p_mfc_decode_arg - type of frame decoding
+ */
+enum s5p_mfc_decode_arg {
+	MFC_DEC_FRAME,
+	MFC_DEC_LAST_FRAME,
+	MFC_DEC_RES_CHANGE,
+};
+
+enum s5p_mfc_fw_ver {
+	MFC_FW_V1,
+	MFC_FW_V2,
+};
+
+#define MFC_BUF_FLAG_USED	(1 << 0)
+#define MFC_BUF_FLAG_EOS	(1 << 1)
+
+struct s5p_mfc_ctx;
+
+/**
+ * struct s5p_mfc_buf - MFC buffer
+ */
+struct s5p_mfc_buf {
+	struct vb2_v4l2_buffer *b;
+	struct list_head list;
+	union {
+		struct {
+			size_t luma;
+			size_t chroma;
+		} raw;
+		size_t stream;
+	} cookie;
+	int flags;
+};
+
+/**
+ * struct s5p_mfc_pm - power management data structure
+ */
+struct s5p_mfc_pm {
+	struct clk	*clock;
+	struct clk	*clock_gate;
+	atomic_t	power;
+	struct device	*device;
+};
+
+struct s5p_mfc_buf_size_v5 {
+	unsigned int h264_ctx;
+	unsigned int non_h264_ctx;
+	unsigned int dsc;
+	unsigned int shm;
+};
+
+struct s5p_mfc_buf_size_v6 {
+	unsigned int dev_ctx;
+	unsigned int h264_dec_ctx;
+	unsigned int other_dec_ctx;
+	unsigned int h264_enc_ctx;
+	unsigned int other_enc_ctx;
+};
+
+struct s5p_mfc_buf_size {
+	unsigned int fw;
+	unsigned int cpb;
+	void *priv;
+};
+
+struct s5p_mfc_buf_align {
+	unsigned int base;
+};
+
+struct s5p_mfc_variant {
+	unsigned int version;
+	unsigned int port_num;
+	u32 version_bit;
+	struct s5p_mfc_buf_size *buf_size;
+	struct s5p_mfc_buf_align *buf_align;
+	char	*fw_name[MFC_FW_MAX_VERSIONS];
+};
+
+/**
+ * struct s5p_mfc_priv_buf - represents internal used buffer
+ * @ofs:		offset of each buffer, will be used for MFC
+ * @virt:		kernel virtual address, only valid when the
+ *			buffer accessed by driver
+ * @dma:		DMA address, only valid when kernel DMA API used
+ * @size:		size of the buffer
+ */
+struct s5p_mfc_priv_buf {
+	unsigned long	ofs;
+	void		*virt;
+	dma_addr_t	dma;
+	size_t		size;
+};
+
+/**
+ * struct s5p_mfc_dev - The struct containing driver internal parameters.
+ *
+ * @v4l2_dev:		v4l2_device
+ * @vfd_dec:		video device for decoding
+ * @vfd_enc:		video device for encoding
+ * @plat_dev:		platform device
+ * @mem_dev_l:		child device of the left memory bank (0)
+ * @mem_dev_r:		child device of the right memory bank (1)
+ * @regs_base:		base address of the MFC hw registers
+ * @irq:		irq resource
+ * @dec_ctrl_handler:	control framework handler for decoding
+ * @enc_ctrl_handler:	control framework handler for encoding
+ * @pm:			power management control
+ * @variant:		MFC hardware variant information
+ * @num_inst:		couter of active MFC instances
+ * @irqlock:		lock for operations on videobuf2 queues
+ * @condlock:		lock for changing/checking if a context is ready to be
+ *			processed
+ * @mfc_mutex:		lock for video_device
+ * @int_cond:		variable used by the waitqueue
+ * @int_type:		type of last interrupt
+ * @int_err:		error number for last interrupt
+ * @queue:		waitqueue for waiting for completion of device commands
+ * @fw_size:		size of firmware
+ * @fw_virt_addr:	virtual firmware address
+ * @bank1:		address of the beginning of bank 1 memory
+ * @bank2:		address of the beginning of bank 2 memory
+ * @hw_lock:		used for hardware locking
+ * @ctx:		array of driver contexts
+ * @curr_ctx:		number of the currently running context
+ * @ctx_work_bits:	used to mark which contexts are waiting for hardware
+ * @watchdog_cnt:	counter for the watchdog
+ * @watchdog_workqueue:	workqueue for the watchdog
+ * @watchdog_work:	worker for the watchdog
+ * @alloc_ctx:		videobuf2 allocator contexts for two memory banks
+ * @enter_suspend:	flag set when entering suspend
+ * @ctx_buf:		common context memory (MFCv6)
+ * @warn_start:		hardware error code from which warnings start
+ * @mfc_ops:		ops structure holding HW operation function pointers
+ * @mfc_cmds:		cmd structure holding HW commands function pointers
+ * @fw_ver:		loaded firmware sub-version
+ *
+ */
+struct s5p_mfc_dev {
+	struct v4l2_device	v4l2_dev;
+	struct video_device	*vfd_dec;
+	struct video_device	*vfd_enc;
+	struct platform_device	*plat_dev;
+	struct device		*mem_dev_l;
+	struct device		*mem_dev_r;
+	void __iomem		*regs_base;
+	int			irq;
+	struct v4l2_ctrl_handler dec_ctrl_handler;
+	struct v4l2_ctrl_handler enc_ctrl_handler;
+	struct s5p_mfc_pm	pm;
+	struct s5p_mfc_variant	*variant;
+	int num_inst;
+	spinlock_t irqlock;	/* lock when operating on videobuf2 queues */
+	spinlock_t condlock;	/* lock when changing/checking if a context is
+					ready to be processed */
+	struct mutex mfc_mutex; /* video_device lock */
+	int int_cond;
+	int int_type;
+	unsigned int int_err;
+	wait_queue_head_t queue;
+	size_t fw_size;
+	void *fw_virt_addr;
+	dma_addr_t bank1;
+	dma_addr_t bank2;
+	unsigned long hw_lock;
+	struct s5p_mfc_ctx *ctx[MFC_NUM_CONTEXTS];
+	int curr_ctx;
+	unsigned long ctx_work_bits;
+	atomic_t watchdog_cnt;
+	struct timer_list watchdog_timer;
+	struct workqueue_struct *watchdog_workqueue;
+	struct work_struct watchdog_work;
+	void *alloc_ctx[2];
+	unsigned long enter_suspend;
+
+	struct s5p_mfc_priv_buf ctx_buf;
+	int warn_start;
+	struct s5p_mfc_hw_ops *mfc_ops;
+	struct s5p_mfc_hw_cmds *mfc_cmds;
+	const struct s5p_mfc_regs *mfc_regs;
+	enum s5p_mfc_fw_ver fw_ver;
+	bool risc_on; /* indicates if RISC is on or off */
+};
+
+/**
+ * struct s5p_mfc_h264_enc_params - encoding parameters for h264
+ */
+struct s5p_mfc_h264_enc_params {
+	enum v4l2_mpeg_video_h264_profile profile;
+	enum v4l2_mpeg_video_h264_loop_filter_mode loop_filter_mode;
+	s8 loop_filter_alpha;
+	s8 loop_filter_beta;
+	enum v4l2_mpeg_video_h264_entropy_mode entropy_mode;
+	u8 max_ref_pic;
+	u8 num_ref_pic_4p;
+	int _8x8_transform;
+	int rc_mb_dark;
+	int rc_mb_smooth;
+	int rc_mb_static;
+	int rc_mb_activity;
+	int vui_sar;
+	u8 vui_sar_idc;
+	u16 vui_ext_sar_width;
+	u16 vui_ext_sar_height;
+	int open_gop;
+	u16 open_gop_size;
+	u8 rc_frame_qp;
+	u8 rc_min_qp;
+	u8 rc_max_qp;
+	u8 rc_p_frame_qp;
+	u8 rc_b_frame_qp;
+	enum v4l2_mpeg_video_h264_level level_v4l2;
+	int level;
+	u16 cpb_size;
+	int interlace;
+	u8 hier_qp;
+	u8 hier_qp_type;
+	u8 hier_qp_layer;
+	u8 hier_qp_layer_qp[7];
+	u8 sei_frame_packing;
+	u8 sei_fp_curr_frame_0;
+	u8 sei_fp_arrangement_type;
+
+	u8 fmo;
+	u8 fmo_map_type;
+	u8 fmo_slice_grp;
+	u8 fmo_chg_dir;
+	u32 fmo_chg_rate;
+	u32 fmo_run_len[4];
+	u8 aso;
+	u32 aso_slice_order[8];
+};
+
+/**
+ * struct s5p_mfc_mpeg4_enc_params - encoding parameters for h263 and mpeg4
+ */
+struct s5p_mfc_mpeg4_enc_params {
+	/* MPEG4 Only */
+	enum v4l2_mpeg_video_mpeg4_profile profile;
+	int quarter_pixel;
+	/* Common for MPEG4, H263 */
+	u16 vop_time_res;
+	u16 vop_frm_delta;
+	u8 rc_frame_qp;
+	u8 rc_min_qp;
+	u8 rc_max_qp;
+	u8 rc_p_frame_qp;
+	u8 rc_b_frame_qp;
+	enum v4l2_mpeg_video_mpeg4_level level_v4l2;
+	int level;
+};
+
+/**
+ * struct s5p_mfc_vp8_enc_params - encoding parameters for vp8
+ */
+struct s5p_mfc_vp8_enc_params {
+	u8 imd_4x4;
+	enum v4l2_vp8_num_partitions num_partitions;
+	enum v4l2_vp8_num_ref_frames num_ref;
+	u8 filter_level;
+	u8 filter_sharpness;
+	u32 golden_frame_ref_period;
+	enum v4l2_vp8_golden_frame_sel golden_frame_sel;
+	u8 hier_layer;
+	u8 hier_layer_qp[3];
+	u8 rc_min_qp;
+	u8 rc_max_qp;
+	u8 rc_frame_qp;
+	u8 rc_p_frame_qp;
+	u8 profile;
+};
+
+/**
+ * struct s5p_mfc_enc_params - general encoding parameters
+ */
+struct s5p_mfc_enc_params {
+	u16 width;
+	u16 height;
+	u32 mv_h_range;
+	u32 mv_v_range;
+
+	u16 gop_size;
+	enum v4l2_mpeg_video_multi_slice_mode slice_mode;
+	u16 slice_mb;
+	u32 slice_bit;
+	u16 intra_refresh_mb;
+	int pad;
+	u8 pad_luma;
+	u8 pad_cb;
+	u8 pad_cr;
+	int rc_frame;
+	int rc_mb;
+	u32 rc_bitrate;
+	u16 rc_reaction_coeff;
+	u16 vbv_size;
+	u32 vbv_delay;
+
+	enum v4l2_mpeg_video_header_mode seq_hdr_mode;
+	enum v4l2_mpeg_mfc51_video_frame_skip_mode frame_skip_mode;
+	int fixed_target_bit;
+
+	u8 num_b_frame;
+	u32 rc_framerate_num;
+	u32 rc_framerate_denom;
+
+	struct {
+		struct s5p_mfc_h264_enc_params h264;
+		struct s5p_mfc_mpeg4_enc_params mpeg4;
+		struct s5p_mfc_vp8_enc_params vp8;
+	} codec;
+
+};
+
+/**
+ * struct s5p_mfc_codec_ops - codec ops, used by encoding
+ */
+struct s5p_mfc_codec_ops {
+	/* initialization routines */
+	int (*pre_seq_start) (struct s5p_mfc_ctx *ctx);
+	int (*post_seq_start) (struct s5p_mfc_ctx *ctx);
+	/* execution routines */
+	int (*pre_frame_start) (struct s5p_mfc_ctx *ctx);
+	int (*post_frame_start) (struct s5p_mfc_ctx *ctx);
+};
+
+#define call_cop(c, op, args...)				\
+	(((c)->c_ops->op) ?					\
+		((c)->c_ops->op(args)) : 0)
+
+/**
+ * struct s5p_mfc_ctx - This struct contains the instance context
+ *
+ * @dev:		pointer to the s5p_mfc_dev of the device
+ * @fh:			struct v4l2_fh
+ * @num:		number of the context that this structure describes
+ * @int_cond:		variable used by the waitqueue
+ * @int_type:		type of the last interrupt
+ * @int_err:		error number received from MFC hw in the interrupt
+ * @queue:		waitqueue that can be used to wait for this context to
+ *			finish
+ * @src_fmt:		source pixelformat information
+ * @dst_fmt:		destination pixelformat information
+ * @vq_src:		vb2 queue for source buffers
+ * @vq_dst:		vb2 queue for destination buffers
+ * @src_queue:		driver internal queue for source buffers
+ * @dst_queue:		driver internal queue for destination buffers
+ * @src_queue_cnt:	number of buffers queued on the source internal queue
+ * @dst_queue_cnt:	number of buffers queued on the dest internal queue
+ * @type:		type of the instance - decoder or encoder
+ * @state:		state of the context
+ * @inst_no:		number of hw instance associated with the context
+ * @img_width:		width of the image that is decoded or encoded
+ * @img_height:		height of the image that is decoded or encoded
+ * @buf_width:		width of the buffer for processed image
+ * @buf_height:		height of the buffer for processed image
+ * @luma_size:		size of a luma plane
+ * @chroma_size:	size of a chroma plane
+ * @mv_size:		size of a motion vectors buffer
+ * @consumed_stream:	number of bytes that have been used so far from the
+ *			decoding buffer
+ * @dpb_flush_flag:	flag used to indicate that a DPB buffers are being
+ *			flushed
+ * @head_processed:	flag mentioning whether the header data is processed
+ *			completely or not
+ * @bank1:		handle to memory allocated for temporary buffers from
+ *			memory bank 1
+ * @bank2:		handle to memory allocated for temporary buffers from
+ *			memory bank 2
+ * @capture_state:	state of the capture buffers queue
+ * @output_state:	state of the output buffers queue
+ * @src_bufs:		information on allocated source buffers
+ * @dst_bufs:		information on allocated destination buffers
+ * @sequence:		counter for the sequence number for v4l2
+ * @dec_dst_flag:	flags for buffers queued in the hardware
+ * @dec_src_buf_size:	size of the buffer for source buffers in decoding
+ * @codec_mode:		number of codec mode used by MFC hw
+ * @slice_interface:	slice interface flag
+ * @loop_filter_mpeg4:	loop filter for MPEG4 flag
+ * @display_delay:	value of the display delay for H264
+ * @display_delay_enable:	display delay for H264 enable flag
+ * @after_packed_pb:	flag used to track buffer when stream is in
+ *			Packed PB format
+ * @sei_fp_parse:	enable/disable parsing of frame packing SEI information
+ * @dpb_count:		count of the DPB buffers required by MFC hw
+ * @total_dpb_count:	count of DPB buffers with additional buffers
+ *			requested by the application
+ * @ctx:		context buffer information
+ * @dsc:		descriptor buffer information
+ * @shm:		shared memory buffer information
+ * @mv_count:		number of MV buffers allocated for decoding
+ * @enc_params:		encoding parameters for MFC
+ * @enc_dst_buf_size:	size of the buffers for encoder output
+ * @luma_dpb_size:	dpb buffer size for luma
+ * @chroma_dpb_size:	dpb buffer size for chroma
+ * @me_buffer_size:	size of the motion estimation buffer
+ * @tmv_buffer_size:	size of temporal predictor motion vector buffer
+ * @frame_type:		used to force the type of the next encoded frame
+ * @ref_queue:		list of the reference buffers for encoding
+ * @ref_queue_cnt:	number of the buffers in the reference list
+ * @c_ops:		ops for encoding
+ * @ctrls:		array of controls, used when adding controls to the
+ *			v4l2 control framework
+ * @ctrl_handler:	handler for v4l2 framework
+ */
+struct s5p_mfc_ctx {
+	struct s5p_mfc_dev *dev;
+	struct v4l2_fh fh;
+
+	int num;
+
+	int int_cond;
+	int int_type;
+	unsigned int int_err;
+	wait_queue_head_t queue;
+
+	struct s5p_mfc_fmt *src_fmt;
+	struct s5p_mfc_fmt *dst_fmt;
+
+	struct vb2_queue vq_src;
+	struct vb2_queue vq_dst;
+
+	struct list_head src_queue;
+	struct list_head dst_queue;
+
+	unsigned int src_queue_cnt;
+	unsigned int dst_queue_cnt;
+
+	enum s5p_mfc_inst_type type;
+	enum s5p_mfc_inst_state state;
+	int inst_no;
+
+	/* Image parameters */
+	int img_width;
+	int img_height;
+	int buf_width;
+	int buf_height;
+
+	int luma_size;
+	int chroma_size;
+	int mv_size;
+
+	unsigned long consumed_stream;
+
+	unsigned int dpb_flush_flag;
+	unsigned int head_processed;
+
+	struct s5p_mfc_priv_buf bank1;
+	struct s5p_mfc_priv_buf bank2;
+
+	enum s5p_mfc_queue_state capture_state;
+	enum s5p_mfc_queue_state output_state;
+
+	struct s5p_mfc_buf src_bufs[MFC_MAX_BUFFERS];
+	int src_bufs_cnt;
+	struct s5p_mfc_buf dst_bufs[MFC_MAX_BUFFERS];
+	int dst_bufs_cnt;
+
+	unsigned int sequence;
+	unsigned long dec_dst_flag;
+	size_t dec_src_buf_size;
+
+	/* Control values */
+	int codec_mode;
+	int slice_interface;
+	int loop_filter_mpeg4;
+	int display_delay;
+	int display_delay_enable;
+	int after_packed_pb;
+	int sei_fp_parse;
+
+	int pb_count;
+	int total_dpb_count;
+	int mv_count;
+	/* Buffers */
+	struct s5p_mfc_priv_buf ctx;
+	struct s5p_mfc_priv_buf dsc;
+	struct s5p_mfc_priv_buf shm;
+
+	struct s5p_mfc_enc_params enc_params;
+
+	size_t enc_dst_buf_size;
+	size_t luma_dpb_size;
+	size_t chroma_dpb_size;
+	size_t me_buffer_size;
+	size_t tmv_buffer_size;
+
+	enum v4l2_mpeg_mfc51_video_force_frame_type force_frame_type;
+
+	struct list_head ref_queue;
+	unsigned int ref_queue_cnt;
+
+	enum v4l2_mpeg_video_multi_slice_mode slice_mode;
+	union {
+		unsigned int mb;
+		unsigned int bits;
+	} slice_size;
+
+	struct s5p_mfc_codec_ops *c_ops;
+
+	struct v4l2_ctrl *ctrls[MFC_MAX_CTRLS];
+	struct v4l2_ctrl_handler ctrl_handler;
+	unsigned int frame_tag;
+	size_t scratch_buf_size;
+};
+
+/*
+ * struct s5p_mfc_fmt -	structure used to store information about pixelformats
+ *			used by the MFC
+ */
+struct s5p_mfc_fmt {
+	char *name;
+	u32 fourcc;
+	u32 codec_mode;
+	enum s5p_mfc_fmt_type type;
+	u32 num_planes;
+	u32 versions;
+};
+
+/**
+ * struct mfc_control -	structure used to store information about MFC controls
+ *			it is used to initialize the control framework.
+ */
+struct mfc_control {
+	__u32			id;
+	enum v4l2_ctrl_type	type;
+	__u8			name[32];  /* Whatever */
+	__s32			minimum;   /* Note signedness */
+	__s32			maximum;
+	__s32			step;
+	__u32			menu_skip_mask;
+	__s32			default_value;
+	__u32			flags;
+	__u32			reserved[2];
+	__u8			is_volatile;
+};
+
+/* Macro for making hardware specific calls */
+#define s5p_mfc_hw_call(f, op, args...) \
+	((f && f->op) ? f->op(args) : -ENODEV)
+
+#define s5p_mfc_hw_call_void(f, op, args...) \
+do { \
+	if (f && f->op) \
+		f->op(args); \
+} while (0)
+
+#define fh_to_ctx(__fh) container_of(__fh, struct s5p_mfc_ctx, fh)
+#define ctrl_to_ctx(__ctrl) \
+	container_of((__ctrl)->handler, struct s5p_mfc_ctx, ctrl_handler)
+
+void clear_work_bit(struct s5p_mfc_ctx *ctx);
+void set_work_bit(struct s5p_mfc_ctx *ctx);
+void clear_work_bit_irqsave(struct s5p_mfc_ctx *ctx);
+void set_work_bit_irqsave(struct s5p_mfc_ctx *ctx);
+
+#define HAS_PORTNUM(dev)	(dev ? (dev->variant ? \
+				(dev->variant->port_num ? 1 : 0) : 0) : 0)
+#define IS_TWOPORT(dev)		(dev->variant->port_num == 2 ? 1 : 0)
+#define IS_MFCV6_PLUS(dev)	(dev->variant->version >= 0x60 ? 1 : 0)
+#define IS_MFCV7_PLUS(dev)	(dev->variant->version >= 0x70 ? 1 : 0)
+#define IS_MFCV8(dev)		(dev->variant->version >= 0x80 ? 1 : 0)
+
+#define MFC_V5_BIT	BIT(0)
+#define MFC_V6_BIT	BIT(1)
+#define MFC_V7_BIT	BIT(2)
+#define MFC_V8_BIT	BIT(3)
+
+
+#endif /* S5P_MFC_COMMON_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.c b/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.c
new file mode 100644
index 0000000..40d8a03
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.c
@@ -0,0 +1,510 @@
+/*
+ * linux/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.c
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/firmware.h>
+#include <linux/jiffies.h>
+#include <linux/sched.h>
+#include "s5p_mfc_cmd.h"
+#include "s5p_mfc_common.h"
+#include "s5p_mfc_debug.h"
+#include "s5p_mfc_intr.h"
+#include "s5p_mfc_opr.h"
+#include "s5p_mfc_pm.h"
+#include "s5p_mfc_ctrl.h"
+
+/* Allocate memory for firmware */
+int s5p_mfc_alloc_firmware(struct s5p_mfc_dev *dev)
+{
+	void *bank2_virt;
+	dma_addr_t bank2_dma_addr;
+
+	dev->fw_size = dev->variant->buf_size->fw;
+
+	if (dev->fw_virt_addr) {
+		mfc_err("Attempting to allocate firmware when it seems that it is already loaded\n");
+		return -ENOMEM;
+	}
+
+	dev->fw_virt_addr = dma_alloc_coherent(dev->mem_dev_l, dev->fw_size,
+					&dev->bank1, GFP_KERNEL);
+
+	if (!dev->fw_virt_addr) {
+		mfc_err("Allocating bitprocessor buffer failed\n");
+		return -ENOMEM;
+	}
+
+	if (HAS_PORTNUM(dev) && IS_TWOPORT(dev)) {
+		bank2_virt = dma_alloc_coherent(dev->mem_dev_r, 1 << MFC_BASE_ALIGN_ORDER,
+					&bank2_dma_addr, GFP_KERNEL);
+
+		if (!bank2_virt) {
+			mfc_err("Allocating bank2 base failed\n");
+			dma_free_coherent(dev->mem_dev_l, dev->fw_size,
+				dev->fw_virt_addr, dev->bank1);
+			dev->fw_virt_addr = NULL;
+			return -ENOMEM;
+		}
+
+		/* Valid buffers passed to MFC encoder with LAST_FRAME command
+		 * should not have address of bank2 - MFC will treat it as a null frame.
+		 * To avoid such situation we set bank2 address below the pool address.
+		 */
+		dev->bank2 = bank2_dma_addr - (1 << MFC_BASE_ALIGN_ORDER);
+
+		dma_free_coherent(dev->mem_dev_r, 1 << MFC_BASE_ALIGN_ORDER,
+			bank2_virt, bank2_dma_addr);
+
+	} else {
+		/* In this case bank2 can point to the same address as bank1.
+		 * Firmware will always occupy the beginning of this area so it is
+		 * impossible having a video frame buffer with zero address. */
+		dev->bank2 = dev->bank1;
+	}
+	return 0;
+}
+
+/* Load firmware */
+int s5p_mfc_load_firmware(struct s5p_mfc_dev *dev)
+{
+	struct firmware *fw_blob;
+	int i, err = -EINVAL;
+
+	/* Firmare has to be present as a separate file or compiled
+	 * into kernel. */
+	mfc_debug_enter();
+
+	for (i = MFC_FW_MAX_VERSIONS - 1; i >= 0; i--) {
+		if (!dev->variant->fw_name[i])
+			continue;
+		err = request_firmware((const struct firmware **)&fw_blob,
+				dev->variant->fw_name[i], dev->v4l2_dev.dev);
+		if (!err) {
+			dev->fw_ver = (enum s5p_mfc_fw_ver) i;
+			break;
+		}
+	}
+
+	if (err != 0) {
+		mfc_err("Firmware is not present in the /lib/firmware directory nor compiled in kernel\n");
+		return -EINVAL;
+	}
+	if (fw_blob->size > dev->fw_size) {
+		mfc_err("MFC firmware is too big to be loaded\n");
+		release_firmware(fw_blob);
+		return -ENOMEM;
+	}
+	if (!dev->fw_virt_addr) {
+		mfc_err("MFC firmware is not allocated\n");
+		release_firmware(fw_blob);
+		return -EINVAL;
+	}
+	memcpy(dev->fw_virt_addr, fw_blob->data, fw_blob->size);
+	wmb();
+	release_firmware(fw_blob);
+	mfc_debug_leave();
+	return 0;
+}
+
+/* Release firmware memory */
+int s5p_mfc_release_firmware(struct s5p_mfc_dev *dev)
+{
+	/* Before calling this function one has to make sure
+	 * that MFC is no longer processing */
+	if (!dev->fw_virt_addr)
+		return -EINVAL;
+	dma_free_coherent(dev->mem_dev_l, dev->fw_size, dev->fw_virt_addr,
+						dev->bank1);
+	dev->fw_virt_addr = NULL;
+	return 0;
+}
+
+static int s5p_mfc_bus_reset(struct s5p_mfc_dev *dev)
+{
+	unsigned int status;
+	unsigned long timeout;
+
+	/* Reset */
+	mfc_write(dev, 0x1, S5P_FIMV_MFC_BUS_RESET_CTRL);
+	timeout = jiffies + msecs_to_jiffies(MFC_BW_TIMEOUT);
+	/* Check bus status */
+	do {
+		if (time_after(jiffies, timeout)) {
+			mfc_err("Timeout while resetting MFC.\n");
+			return -EIO;
+		}
+		status = mfc_read(dev, S5P_FIMV_MFC_BUS_RESET_CTRL);
+	} while ((status & 0x2) == 0);
+	return 0;
+}
+
+/* Reset the device */
+int s5p_mfc_reset(struct s5p_mfc_dev *dev)
+{
+	unsigned int mc_status;
+	unsigned long timeout;
+	int i;
+
+	mfc_debug_enter();
+
+	if (IS_MFCV6_PLUS(dev)) {
+		/* Zero Initialization of MFC registers */
+		mfc_write(dev, 0, S5P_FIMV_RISC2HOST_CMD_V6);
+		mfc_write(dev, 0, S5P_FIMV_HOST2RISC_CMD_V6);
+		mfc_write(dev, 0, S5P_FIMV_FW_VERSION_V6);
+
+		for (i = 0; i < S5P_FIMV_REG_CLEAR_COUNT_V6; i++)
+			mfc_write(dev, 0, S5P_FIMV_REG_CLEAR_BEGIN_V6 + (i*4));
+
+		/* check bus reset control before reset */
+		if (dev->risc_on)
+			if (s5p_mfc_bus_reset(dev))
+				return -EIO;
+		/* Reset
+		 * set RISC_ON to 0 during power_on & wake_up.
+		 * V6 needs RISC_ON set to 0 during reset also.
+		 */
+		if ((!dev->risc_on) || (!IS_MFCV7_PLUS(dev)))
+			mfc_write(dev, 0, S5P_FIMV_RISC_ON_V6);
+
+		mfc_write(dev, 0x1FFF, S5P_FIMV_MFC_RESET_V6);
+		mfc_write(dev, 0, S5P_FIMV_MFC_RESET_V6);
+	} else {
+		/* Stop procedure */
+		/*  reset RISC */
+		mfc_write(dev, 0x3f6, S5P_FIMV_SW_RESET);
+		/*  All reset except for MC */
+		mfc_write(dev, 0x3e2, S5P_FIMV_SW_RESET);
+		mdelay(10);
+
+		timeout = jiffies + msecs_to_jiffies(MFC_BW_TIMEOUT);
+		/* Check MC status */
+		do {
+			if (time_after(jiffies, timeout)) {
+				mfc_err("Timeout while resetting MFC\n");
+				return -EIO;
+			}
+
+			mc_status = mfc_read(dev, S5P_FIMV_MC_STATUS);
+
+		} while (mc_status & 0x3);
+
+		mfc_write(dev, 0x0, S5P_FIMV_SW_RESET);
+		mfc_write(dev, 0x3fe, S5P_FIMV_SW_RESET);
+	}
+
+	mfc_debug_leave();
+	return 0;
+}
+
+static inline void s5p_mfc_init_memctrl(struct s5p_mfc_dev *dev)
+{
+	if (IS_MFCV6_PLUS(dev)) {
+		mfc_write(dev, dev->bank1, S5P_FIMV_RISC_BASE_ADDRESS_V6);
+		mfc_debug(2, "Base Address : %pad\n", &dev->bank1);
+	} else {
+		mfc_write(dev, dev->bank1, S5P_FIMV_MC_DRAMBASE_ADR_A);
+		mfc_write(dev, dev->bank2, S5P_FIMV_MC_DRAMBASE_ADR_B);
+		mfc_debug(2, "Bank1: %pad, Bank2: %pad\n",
+				&dev->bank1, &dev->bank2);
+	}
+}
+
+static inline void s5p_mfc_clear_cmds(struct s5p_mfc_dev *dev)
+{
+	if (IS_MFCV6_PLUS(dev)) {
+		/* Zero initialization should be done before RESET.
+		 * Nothing to do here. */
+	} else {
+		mfc_write(dev, 0xffffffff, S5P_FIMV_SI_CH0_INST_ID);
+		mfc_write(dev, 0xffffffff, S5P_FIMV_SI_CH1_INST_ID);
+		mfc_write(dev, 0, S5P_FIMV_RISC2HOST_CMD);
+		mfc_write(dev, 0, S5P_FIMV_HOST2RISC_CMD);
+	}
+}
+
+/* Initialize hardware */
+int s5p_mfc_init_hw(struct s5p_mfc_dev *dev)
+{
+	unsigned int ver;
+	int ret;
+
+	mfc_debug_enter();
+	if (!dev->fw_virt_addr) {
+		mfc_err("Firmware memory is not allocated.\n");
+		return -EINVAL;
+	}
+
+	/* 0. MFC reset */
+	mfc_debug(2, "MFC reset..\n");
+	s5p_mfc_clock_on();
+	dev->risc_on = 0;
+	ret = s5p_mfc_reset(dev);
+	if (ret) {
+		mfc_err("Failed to reset MFC - timeout\n");
+		return ret;
+	}
+	mfc_debug(2, "Done MFC reset..\n");
+	/* 1. Set DRAM base Addr */
+	s5p_mfc_init_memctrl(dev);
+	/* 2. Initialize registers of channel I/F */
+	s5p_mfc_clear_cmds(dev);
+	/* 3. Release reset signal to the RISC */
+	s5p_mfc_clean_dev_int_flags(dev);
+	if (IS_MFCV6_PLUS(dev)) {
+		dev->risc_on = 1;
+		mfc_write(dev, 0x1, S5P_FIMV_RISC_ON_V6);
+	}
+	else
+		mfc_write(dev, 0x3ff, S5P_FIMV_SW_RESET);
+	mfc_debug(2, "Will now wait for completion of firmware transfer\n");
+	if (s5p_mfc_wait_for_done_dev(dev, S5P_MFC_R2H_CMD_FW_STATUS_RET)) {
+		mfc_err("Failed to load firmware\n");
+		s5p_mfc_reset(dev);
+		s5p_mfc_clock_off();
+		return -EIO;
+	}
+	s5p_mfc_clean_dev_int_flags(dev);
+	/* 4. Initialize firmware */
+	ret = s5p_mfc_hw_call(dev->mfc_cmds, sys_init_cmd, dev);
+	if (ret) {
+		mfc_err("Failed to send command to MFC - timeout\n");
+		s5p_mfc_reset(dev);
+		s5p_mfc_clock_off();
+		return ret;
+	}
+	mfc_debug(2, "Ok, now will wait for completion of hardware init\n");
+	if (s5p_mfc_wait_for_done_dev(dev, S5P_MFC_R2H_CMD_SYS_INIT_RET)) {
+		mfc_err("Failed to init hardware\n");
+		s5p_mfc_reset(dev);
+		s5p_mfc_clock_off();
+		return -EIO;
+	}
+	dev->int_cond = 0;
+	if (dev->int_err != 0 || dev->int_type !=
+					S5P_MFC_R2H_CMD_SYS_INIT_RET) {
+		/* Failure. */
+		mfc_err("Failed to init firmware - error: %d int: %d\n",
+						dev->int_err, dev->int_type);
+		s5p_mfc_reset(dev);
+		s5p_mfc_clock_off();
+		return -EIO;
+	}
+	if (IS_MFCV6_PLUS(dev))
+		ver = mfc_read(dev, S5P_FIMV_FW_VERSION_V6);
+	else
+		ver = mfc_read(dev, S5P_FIMV_FW_VERSION);
+
+	mfc_debug(2, "MFC F/W version : %02xyy, %02xmm, %02xdd\n",
+		(ver >> 16) & 0xFF, (ver >> 8) & 0xFF, ver & 0xFF);
+	s5p_mfc_clock_off();
+	mfc_debug_leave();
+	return 0;
+}
+
+
+/* Deinitialize hardware */
+void s5p_mfc_deinit_hw(struct s5p_mfc_dev *dev)
+{
+	s5p_mfc_clock_on();
+
+	s5p_mfc_reset(dev);
+	s5p_mfc_hw_call_void(dev->mfc_ops, release_dev_context_buffer, dev);
+
+	s5p_mfc_clock_off();
+}
+
+int s5p_mfc_sleep(struct s5p_mfc_dev *dev)
+{
+	int ret;
+
+	mfc_debug_enter();
+	s5p_mfc_clock_on();
+	s5p_mfc_clean_dev_int_flags(dev);
+	ret = s5p_mfc_hw_call(dev->mfc_cmds, sleep_cmd, dev);
+	if (ret) {
+		mfc_err("Failed to send command to MFC - timeout\n");
+		return ret;
+	}
+	if (s5p_mfc_wait_for_done_dev(dev, S5P_MFC_R2H_CMD_SLEEP_RET)) {
+		mfc_err("Failed to sleep\n");
+		return -EIO;
+	}
+	s5p_mfc_clock_off();
+	dev->int_cond = 0;
+	if (dev->int_err != 0 || dev->int_type !=
+						S5P_MFC_R2H_CMD_SLEEP_RET) {
+		/* Failure. */
+		mfc_err("Failed to sleep - error: %d int: %d\n", dev->int_err,
+								dev->int_type);
+		return -EIO;
+	}
+	mfc_debug_leave();
+	return ret;
+}
+
+static int s5p_mfc_v8_wait_wakeup(struct s5p_mfc_dev *dev)
+{
+	int ret;
+
+	/* Release reset signal to the RISC */
+	dev->risc_on = 1;
+	mfc_write(dev, 0x1, S5P_FIMV_RISC_ON_V6);
+
+	if (s5p_mfc_wait_for_done_dev(dev, S5P_MFC_R2H_CMD_FW_STATUS_RET)) {
+		mfc_err("Failed to reset MFCV8\n");
+		return -EIO;
+	}
+	mfc_debug(2, "Write command to wakeup MFCV8\n");
+	ret = s5p_mfc_hw_call(dev->mfc_cmds, wakeup_cmd, dev);
+	if (ret) {
+		mfc_err("Failed to send command to MFCV8 - timeout\n");
+		return ret;
+	}
+
+	if (s5p_mfc_wait_for_done_dev(dev, S5P_MFC_R2H_CMD_WAKEUP_RET)) {
+		mfc_err("Failed to wakeup MFC\n");
+		return -EIO;
+	}
+	return ret;
+}
+
+static int s5p_mfc_wait_wakeup(struct s5p_mfc_dev *dev)
+{
+	int ret;
+
+	/* Send MFC wakeup command */
+	ret = s5p_mfc_hw_call(dev->mfc_cmds, wakeup_cmd, dev);
+	if (ret) {
+		mfc_err("Failed to send command to MFC - timeout\n");
+		return ret;
+	}
+
+	/* Release reset signal to the RISC */
+	if (IS_MFCV6_PLUS(dev)) {
+		dev->risc_on = 1;
+		mfc_write(dev, 0x1, S5P_FIMV_RISC_ON_V6);
+	} else {
+		mfc_write(dev, 0x3ff, S5P_FIMV_SW_RESET);
+	}
+
+	if (s5p_mfc_wait_for_done_dev(dev, S5P_MFC_R2H_CMD_WAKEUP_RET)) {
+		mfc_err("Failed to wakeup MFC\n");
+		return -EIO;
+	}
+	return ret;
+}
+
+int s5p_mfc_wakeup(struct s5p_mfc_dev *dev)
+{
+	int ret;
+
+	mfc_debug_enter();
+	/* 0. MFC reset */
+	mfc_debug(2, "MFC reset..\n");
+	s5p_mfc_clock_on();
+	dev->risc_on = 0;
+	ret = s5p_mfc_reset(dev);
+	if (ret) {
+		mfc_err("Failed to reset MFC - timeout\n");
+		s5p_mfc_clock_off();
+		return ret;
+	}
+	mfc_debug(2, "Done MFC reset..\n");
+	/* 1. Set DRAM base Addr */
+	s5p_mfc_init_memctrl(dev);
+	/* 2. Initialize registers of channel I/F */
+	s5p_mfc_clear_cmds(dev);
+	s5p_mfc_clean_dev_int_flags(dev);
+	/* 3. Send MFC wakeup command and wait for completion*/
+	if (IS_MFCV8(dev))
+		ret = s5p_mfc_v8_wait_wakeup(dev);
+	else
+		ret = s5p_mfc_wait_wakeup(dev);
+
+	s5p_mfc_clock_off();
+	if (ret)
+		return ret;
+
+	dev->int_cond = 0;
+	if (dev->int_err != 0 || dev->int_type !=
+						S5P_MFC_R2H_CMD_WAKEUP_RET) {
+		/* Failure. */
+		mfc_err("Failed to wakeup - error: %d int: %d\n", dev->int_err,
+								dev->int_type);
+		return -EIO;
+	}
+	mfc_debug_leave();
+	return 0;
+}
+
+int s5p_mfc_open_mfc_inst(struct s5p_mfc_dev *dev, struct s5p_mfc_ctx *ctx)
+{
+	int ret = 0;
+
+	ret = s5p_mfc_hw_call(dev->mfc_ops, alloc_instance_buffer, ctx);
+	if (ret) {
+		mfc_err("Failed allocating instance buffer\n");
+		goto err;
+	}
+
+	if (ctx->type == MFCINST_DECODER) {
+		ret = s5p_mfc_hw_call(dev->mfc_ops,
+					alloc_dec_temp_buffers, ctx);
+		if (ret) {
+			mfc_err("Failed allocating temporary buffers\n");
+			goto err_free_inst_buf;
+		}
+	}
+
+	set_work_bit_irqsave(ctx);
+	s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
+	if (s5p_mfc_wait_for_done_ctx(ctx,
+		S5P_MFC_R2H_CMD_OPEN_INSTANCE_RET, 0)) {
+		/* Error or timeout */
+		mfc_err("Error getting instance from hardware\n");
+		ret = -EIO;
+		goto err_free_desc_buf;
+	}
+
+	mfc_debug(2, "Got instance number: %d\n", ctx->inst_no);
+	return ret;
+
+err_free_desc_buf:
+	if (ctx->type == MFCINST_DECODER)
+		s5p_mfc_hw_call_void(dev->mfc_ops, release_dec_desc_buffer, ctx);
+err_free_inst_buf:
+	s5p_mfc_hw_call_void(dev->mfc_ops, release_instance_buffer, ctx);
+err:
+	return ret;
+}
+
+void s5p_mfc_close_mfc_inst(struct s5p_mfc_dev *dev, struct s5p_mfc_ctx *ctx)
+{
+	ctx->state = MFCINST_RETURN_INST;
+	set_work_bit_irqsave(ctx);
+	s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
+	/* Wait until instance is returned or timeout occurred */
+	if (s5p_mfc_wait_for_done_ctx(ctx,
+				S5P_MFC_R2H_CMD_CLOSE_INSTANCE_RET, 0))
+		mfc_err("Err returning instance\n");
+
+	/* Free resources */
+	s5p_mfc_hw_call_void(dev->mfc_ops, release_codec_buffers, ctx);
+	s5p_mfc_hw_call_void(dev->mfc_ops, release_instance_buffer, ctx);
+	if (ctx->type == MFCINST_DECODER)
+		s5p_mfc_hw_call_void(dev->mfc_ops, release_dec_desc_buffer, ctx);
+
+	ctx->inst_no = MFC_NO_INSTANCE_SET;
+	ctx->state = MFCINST_FREE;
+}
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.h b/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.h
new file mode 100644
index 0000000..8e5df04
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.h
@@ -0,0 +1,34 @@
+/*
+ * linux/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.h
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#ifndef S5P_MFC_CTRL_H
+#define S5P_MFC_CTRL_H
+
+#include "s5p_mfc_common.h"
+
+int s5p_mfc_release_firmware(struct s5p_mfc_dev *dev);
+int s5p_mfc_alloc_firmware(struct s5p_mfc_dev *dev);
+int s5p_mfc_load_firmware(struct s5p_mfc_dev *dev);
+int s5p_mfc_reload_firmware(struct s5p_mfc_dev *dev);
+
+int s5p_mfc_init_hw(struct s5p_mfc_dev *dev);
+void s5p_mfc_deinit_hw(struct s5p_mfc_dev *dev);
+
+int s5p_mfc_sleep(struct s5p_mfc_dev *dev);
+int s5p_mfc_wakeup(struct s5p_mfc_dev *dev);
+
+int s5p_mfc_reset(struct s5p_mfc_dev *dev);
+
+int s5p_mfc_open_mfc_inst(struct s5p_mfc_dev *dev, struct s5p_mfc_ctx *ctx);
+void s5p_mfc_close_mfc_inst(struct s5p_mfc_dev *dev, struct s5p_mfc_ctx *ctx);
+
+#endif /* S5P_MFC_CTRL_H */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_debug.h b/drivers/media/platform/s5p-mfc/s5p_mfc_debug.h
new file mode 100644
index 0000000..5936923
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_debug.h
@@ -0,0 +1,48 @@
+/*
+ * drivers/media/platform/s5p-mfc/s5p_mfc_debug.h
+ *
+ * Header file for Samsung MFC (Multi Function Codec - FIMV) driver
+ * This file contains debug macros
+ *
+ * Kamil Debski, Copyright (c) 2011 Samsung Electronics
+ * http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef S5P_MFC_DEBUG_H_
+#define S5P_MFC_DEBUG_H_
+
+#define DEBUG
+
+#ifdef DEBUG
+extern int mfc_debug_level;
+
+#define mfc_debug(level, fmt, args...)				\
+	do {							\
+		if (mfc_debug_level >= level)			\
+			printk(KERN_DEBUG "%s:%d: " fmt,	\
+				__func__, __LINE__, ##args);	\
+	} while (0)
+#else
+#define mfc_debug(level, fmt, args...)
+#endif
+
+#define mfc_debug_enter() mfc_debug(5, "enter\n")
+#define mfc_debug_leave() mfc_debug(5, "leave\n")
+
+#define mfc_err(fmt, args...)				\
+	do {						\
+		printk(KERN_ERR "%s:%d: " fmt,		\
+		       __func__, __LINE__, ##args);	\
+	} while (0)
+
+#define mfc_info(fmt, args...)				\
+	do {						\
+		printk(KERN_INFO "%s:%d: " fmt,		\
+		       __func__, __LINE__, ##args);	\
+	} while (0)
+
+#endif /* S5P_MFC_DEBUG_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c b/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c
new file mode 100644
index 0000000..8c5060a
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c
@@ -0,0 +1,1193 @@
+/*
+ * linux/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c
+ *
+ * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com/
+ * Kamil Debski, <k.debski@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/clk.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <linux/workqueue.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
+#include <media/videobuf2-v4l2.h>
+#include "s5p_mfc_common.h"
+#include "s5p_mfc_ctrl.h"
+#include "s5p_mfc_debug.h"
+#include "s5p_mfc_dec.h"
+#include "s5p_mfc_intr.h"
+#include "s5p_mfc_opr.h"
+#include "s5p_mfc_pm.h"
+
+static struct s5p_mfc_fmt formats[] = {
+	{
+		.name		= "4:2:0 2 Planes 16x16 Tiles",
+		.fourcc		= V4L2_PIX_FMT_NV12MT_16X16,
+		.codec_mode	= S5P_MFC_CODEC_NONE,
+		.type		= MFC_FMT_RAW,
+		.num_planes	= 2,
+		.versions	= MFC_V6_BIT | MFC_V7_BIT,
+	},
+	{
+		.name		= "4:2:0 2 Planes 64x32 Tiles",
+		.fourcc		= V4L2_PIX_FMT_NV12MT,
+		.codec_mode	= S5P_MFC_CODEC_NONE,
+		.type		= MFC_FMT_RAW,
+		.num_planes	= 2,
+		.versions	= MFC_V5_BIT,
+	},
+	{
+		.name		= "4:2:0 2 Planes Y/CbCr",
+		.fourcc		= V4L2_PIX_FMT_NV12M,
+		.codec_mode	= S5P_MFC_CODEC_NONE,
+		.type		= MFC_FMT_RAW,
+		.num_planes	= 2,
+		.versions	= MFC_V6_BIT | MFC_V7_BIT | MFC_V8_BIT,
+	},
+	{
+		.name		= "4:2:0 2 Planes Y/CrCb",
+		.fourcc		= V4L2_PIX_FMT_NV21M,
+		.codec_mode	= S5P_MFC_CODEC_NONE,
+		.type		= MFC_FMT_RAW,
+		.num_planes	= 2,
+		.versions	= MFC_V6_BIT | MFC_V7_BIT | MFC_V8_BIT,
+	},
+	{
+		.name		= "H264 Encoded Stream",
+		.fourcc		= V4L2_PIX_FMT_H264,
+		.codec_mode	= S5P_MFC_CODEC_H264_DEC,
+		.type		= MFC_FMT_DEC,
+		.num_planes	= 1,
+		.versions	= MFC_V5_BIT | MFC_V6_BIT | MFC_V7_BIT |
+								MFC_V8_BIT,
+	},
+	{
+		.name		= "H264/MVC Encoded Stream",
+		.fourcc		= V4L2_PIX_FMT_H264_MVC,
+		.codec_mode	= S5P_MFC_CODEC_H264_MVC_DEC,
+		.type		= MFC_FMT_DEC,
+		.num_planes	= 1,
+		.versions	= MFC_V6_BIT | MFC_V7_BIT | MFC_V8_BIT,
+	},
+	{
+		.name		= "H263 Encoded Stream",
+		.fourcc		= V4L2_PIX_FMT_H263,
+		.codec_mode	= S5P_MFC_CODEC_H263_DEC,
+		.type		= MFC_FMT_DEC,
+		.num_planes	= 1,
+		.versions	= MFC_V5_BIT | MFC_V6_BIT | MFC_V7_BIT |
+								MFC_V8_BIT,
+	},
+	{
+		.name		= "MPEG1 Encoded Stream",
+		.fourcc		= V4L2_PIX_FMT_MPEG1,
+		.codec_mode	= S5P_MFC_CODEC_MPEG2_DEC,
+		.type		= MFC_FMT_DEC,
+		.num_planes	= 1,
+		.versions	= MFC_V5_BIT | MFC_V6_BIT | MFC_V7_BIT |
+								MFC_V8_BIT,
+	},
+	{
+		.name		= "MPEG2 Encoded Stream",
+		.fourcc		= V4L2_PIX_FMT_MPEG2,
+		.codec_mode	= S5P_MFC_CODEC_MPEG2_DEC,
+		.type		= MFC_FMT_DEC,
+		.num_planes	= 1,
+		.versions	= MFC_V5_BIT | MFC_V6_BIT | MFC_V7_BIT |
+								MFC_V8_BIT,
+	},
+	{
+		.name		= "MPEG4 Encoded Stream",
+		.fourcc		= V4L2_PIX_FMT_MPEG4,
+		.codec_mode	= S5P_MFC_CODEC_MPEG4_DEC,
+		.type		= MFC_FMT_DEC,
+		.num_planes	= 1,
+		.versions	= MFC_V5_BIT | MFC_V6_BIT | MFC_V7_BIT |
+								MFC_V8_BIT,
+	},
+	{
+		.name		= "XviD Encoded Stream",
+		.fourcc		= V4L2_PIX_FMT_XVID,
+		.codec_mode	= S5P_MFC_CODEC_MPEG4_DEC,
+		.type		= MFC_FMT_DEC,
+		.num_planes	= 1,
+		.versions	= MFC_V5_BIT | MFC_V6_BIT | MFC_V7_BIT |
+								MFC_V8_BIT,
+	},
+	{
+		.name		= "VC1 Encoded Stream",
+		.fourcc		= V4L2_PIX_FMT_VC1_ANNEX_G,
+		.codec_mode	= S5P_MFC_CODEC_VC1_DEC,
+		.type		= MFC_FMT_DEC,
+		.num_planes	= 1,
+		.versions	= MFC_V5_BIT | MFC_V6_BIT | MFC_V7_BIT |
+								MFC_V8_BIT,
+	},
+	{
+		.name		= "VC1 RCV Encoded Stream",
+		.fourcc		= V4L2_PIX_FMT_VC1_ANNEX_L,
+		.codec_mode	= S5P_MFC_CODEC_VC1RCV_DEC,
+		.type		= MFC_FMT_DEC,
+		.num_planes	= 1,
+		.versions	= MFC_V5_BIT | MFC_V6_BIT | MFC_V7_BIT |
+								MFC_V8_BIT,
+	},
+	{
+		.name		= "VP8 Encoded Stream",
+		.fourcc		= V4L2_PIX_FMT_VP8,
+		.codec_mode	= S5P_MFC_CODEC_VP8_DEC,
+		.type		= MFC_FMT_DEC,
+		.num_planes	= 1,
+		.versions	= MFC_V6_BIT | MFC_V7_BIT | MFC_V8_BIT,
+	},
+};
+
+#define NUM_FORMATS ARRAY_SIZE(formats)
+
+/* Find selected format description */
+static struct s5p_mfc_fmt *find_format(struct v4l2_format *f, unsigned int t)
+{
+	unsigned int i;
+
+	for (i = 0; i < NUM_FORMATS; i++) {
+		if (formats[i].fourcc == f->fmt.pix_mp.pixelformat &&
+		    formats[i].type == t)
+			return &formats[i];
+	}
+	return NULL;
+}
+
+static struct mfc_control controls[] = {
+	{
+		.id = V4L2_CID_MPEG_MFC51_VIDEO_DECODER_H264_DISPLAY_DELAY,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "H264 Display Delay",
+		.minimum = 0,
+		.maximum = 16383,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_MFC51_VIDEO_DECODER_H264_DISPLAY_DELAY_ENABLE,
+		.type = V4L2_CTRL_TYPE_BOOLEAN,
+		.name = "H264 Display Delay Enable",
+		.minimum = 0,
+		.maximum = 1,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_DECODER_MPEG4_DEBLOCK_FILTER,
+		.type = V4L2_CTRL_TYPE_BOOLEAN,
+		.name = "Mpeg4 Loop Filter Enable",
+		.minimum = 0,
+		.maximum = 1,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_DECODER_SLICE_INTERFACE,
+		.type = V4L2_CTRL_TYPE_BOOLEAN,
+		.name = "Slice Interface Enable",
+		.minimum = 0,
+		.maximum = 1,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_MIN_BUFFERS_FOR_CAPTURE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Minimum number of cap bufs",
+		.minimum = 1,
+		.maximum = 32,
+		.step = 1,
+		.default_value = 1,
+		.is_volatile = 1,
+	},
+};
+
+#define NUM_CTRLS ARRAY_SIZE(controls)
+
+/* Check whether a context should be run on hardware */
+static int s5p_mfc_ctx_ready(struct s5p_mfc_ctx *ctx)
+{
+	/* Context is to parse header */
+	if (ctx->src_queue_cnt >= 1 && ctx->state == MFCINST_GOT_INST)
+		return 1;
+	/* Context is to decode a frame */
+	if (ctx->src_queue_cnt >= 1 &&
+	    ctx->state == MFCINST_RUNNING &&
+	    ctx->dst_queue_cnt >= ctx->pb_count)
+		return 1;
+	/* Context is to return last frame */
+	if (ctx->state == MFCINST_FINISHING &&
+	    ctx->dst_queue_cnt >= ctx->pb_count)
+		return 1;
+	/* Context is to set buffers */
+	if (ctx->src_queue_cnt >= 1 &&
+	    ctx->state == MFCINST_HEAD_PARSED &&
+	    ctx->capture_state == QUEUE_BUFS_MMAPED)
+		return 1;
+	/* Resolution change */
+	if ((ctx->state == MFCINST_RES_CHANGE_INIT ||
+		ctx->state == MFCINST_RES_CHANGE_FLUSH) &&
+		ctx->dst_queue_cnt >= ctx->pb_count)
+		return 1;
+	if (ctx->state == MFCINST_RES_CHANGE_END &&
+		ctx->src_queue_cnt >= 1)
+		return 1;
+	mfc_debug(2, "ctx is not ready\n");
+	return 0;
+}
+
+static struct s5p_mfc_codec_ops decoder_codec_ops = {
+	.pre_seq_start		= NULL,
+	.post_seq_start		= NULL,
+	.pre_frame_start	= NULL,
+	.post_frame_start	= NULL,
+};
+
+/* Query capabilities of the device */
+static int vidioc_querycap(struct file *file, void *priv,
+			   struct v4l2_capability *cap)
+{
+	struct s5p_mfc_dev *dev = video_drvdata(file);
+
+	strncpy(cap->driver, dev->plat_dev->name, sizeof(cap->driver) - 1);
+	strncpy(cap->card, dev->plat_dev->name, sizeof(cap->card) - 1);
+	cap->bus_info[0] = 0;
+	/*
+	 * This is only a mem-to-mem video device. The capture and output
+	 * device capability flags are left only for backward compatibility
+	 * and are scheduled for removal.
+	 */
+	cap->device_caps = V4L2_CAP_VIDEO_M2M_MPLANE | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	return 0;
+}
+
+/* Enumerate format */
+static int vidioc_enum_fmt(struct file *file, struct v4l2_fmtdesc *f,
+							bool out)
+{
+	struct s5p_mfc_dev *dev = video_drvdata(file);
+	struct s5p_mfc_fmt *fmt;
+	int i, j = 0;
+
+	for (i = 0; i < ARRAY_SIZE(formats); ++i) {
+		if (out && formats[i].type != MFC_FMT_DEC)
+			continue;
+		else if (!out && formats[i].type != MFC_FMT_RAW)
+			continue;
+		else if ((dev->variant->version_bit & formats[i].versions) == 0)
+			continue;
+
+		if (j == f->index)
+			break;
+		++j;
+	}
+	if (i == ARRAY_SIZE(formats))
+		return -EINVAL;
+	fmt = &formats[i];
+	strlcpy(f->description, fmt->name, sizeof(f->description));
+	f->pixelformat = fmt->fourcc;
+	return 0;
+}
+
+static int vidioc_enum_fmt_vid_cap_mplane(struct file *file, void *pirv,
+							struct v4l2_fmtdesc *f)
+{
+	return vidioc_enum_fmt(file, f, false);
+}
+
+static int vidioc_enum_fmt_vid_out_mplane(struct file *file, void *priv,
+							struct v4l2_fmtdesc *f)
+{
+	return vidioc_enum_fmt(file, f, true);
+}
+
+/* Get format */
+static int vidioc_g_fmt(struct file *file, void *priv, struct v4l2_format *f)
+{
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+	struct v4l2_pix_format_mplane *pix_mp;
+
+	mfc_debug_enter();
+	pix_mp = &f->fmt.pix_mp;
+	if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE &&
+	    (ctx->state == MFCINST_GOT_INST || ctx->state ==
+						MFCINST_RES_CHANGE_END)) {
+		/* If the MFC is parsing the header,
+		 * so wait until it is finished */
+		s5p_mfc_wait_for_done_ctx(ctx, S5P_MFC_R2H_CMD_SEQ_DONE_RET,
+									0);
+	}
+	if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE &&
+	    ctx->state >= MFCINST_HEAD_PARSED &&
+	    ctx->state < MFCINST_ABORT) {
+		/* This is run on CAPTURE (decode output) */
+		/* Width and height are set to the dimensions
+		   of the movie, the buffer is bigger and
+		   further processing stages should crop to this
+		   rectangle. */
+		pix_mp->width = ctx->buf_width;
+		pix_mp->height = ctx->buf_height;
+		pix_mp->field = V4L2_FIELD_NONE;
+		pix_mp->num_planes = 2;
+		/* Set pixelformat to the format in which MFC
+		   outputs the decoded frame */
+		pix_mp->pixelformat = ctx->dst_fmt->fourcc;
+		pix_mp->plane_fmt[0].bytesperline = ctx->buf_width;
+		pix_mp->plane_fmt[0].sizeimage = ctx->luma_size;
+		pix_mp->plane_fmt[1].bytesperline = ctx->buf_width;
+		pix_mp->plane_fmt[1].sizeimage = ctx->chroma_size;
+	} else if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		/* This is run on OUTPUT
+		   The buffer contains compressed image
+		   so width and height have no meaning */
+		pix_mp->width = 0;
+		pix_mp->height = 0;
+		pix_mp->field = V4L2_FIELD_NONE;
+		pix_mp->plane_fmt[0].bytesperline = ctx->dec_src_buf_size;
+		pix_mp->plane_fmt[0].sizeimage = ctx->dec_src_buf_size;
+		pix_mp->pixelformat = ctx->src_fmt->fourcc;
+		pix_mp->num_planes = ctx->src_fmt->num_planes;
+	} else {
+		mfc_err("Format could not be read\n");
+		mfc_debug(2, "%s-- with error\n", __func__);
+		return -EINVAL;
+	}
+	mfc_debug_leave();
+	return 0;
+}
+
+/* Try format */
+static int vidioc_try_fmt(struct file *file, void *priv, struct v4l2_format *f)
+{
+	struct s5p_mfc_dev *dev = video_drvdata(file);
+	struct s5p_mfc_fmt *fmt;
+
+	mfc_debug(2, "Type is %d\n", f->type);
+	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		fmt = find_format(f, MFC_FMT_DEC);
+		if (!fmt) {
+			mfc_err("Unsupported format for source.\n");
+			return -EINVAL;
+		}
+		if (fmt->codec_mode == S5P_FIMV_CODEC_NONE) {
+			mfc_err("Unknown codec\n");
+			return -EINVAL;
+		}
+		if ((dev->variant->version_bit & fmt->versions) == 0) {
+			mfc_err("Unsupported format by this MFC version.\n");
+			return -EINVAL;
+		}
+	} else if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		fmt = find_format(f, MFC_FMT_RAW);
+		if (!fmt) {
+			mfc_err("Unsupported format for destination.\n");
+			return -EINVAL;
+		}
+		if ((dev->variant->version_bit & fmt->versions) == 0) {
+			mfc_err("Unsupported format by this MFC version.\n");
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+/* Set format */
+static int vidioc_s_fmt(struct file *file, void *priv, struct v4l2_format *f)
+{
+	struct s5p_mfc_dev *dev = video_drvdata(file);
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+	int ret = 0;
+	struct v4l2_pix_format_mplane *pix_mp;
+	struct s5p_mfc_buf_size *buf_size = dev->variant->buf_size;
+
+	mfc_debug_enter();
+	ret = vidioc_try_fmt(file, priv, f);
+	pix_mp = &f->fmt.pix_mp;
+	if (ret)
+		return ret;
+	if (ctx->vq_src.streaming || ctx->vq_dst.streaming) {
+		v4l2_err(&dev->v4l2_dev, "%s queue busy\n", __func__);
+		ret = -EBUSY;
+		goto out;
+	}
+	if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		/* dst_fmt is validated by call to vidioc_try_fmt */
+		ctx->dst_fmt = find_format(f, MFC_FMT_RAW);
+		ret = 0;
+		goto out;
+	} else if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		/* src_fmt is validated by call to vidioc_try_fmt */
+		ctx->src_fmt = find_format(f, MFC_FMT_DEC);
+		ctx->codec_mode = ctx->src_fmt->codec_mode;
+		mfc_debug(2, "The codec number is: %d\n", ctx->codec_mode);
+		pix_mp->height = 0;
+		pix_mp->width = 0;
+		if (pix_mp->plane_fmt[0].sizeimage == 0)
+			pix_mp->plane_fmt[0].sizeimage = ctx->dec_src_buf_size =
+								DEF_CPB_SIZE;
+		else if (pix_mp->plane_fmt[0].sizeimage > buf_size->cpb)
+			ctx->dec_src_buf_size = buf_size->cpb;
+		else
+			ctx->dec_src_buf_size = pix_mp->plane_fmt[0].sizeimage;
+		pix_mp->plane_fmt[0].bytesperline = 0;
+		ctx->state = MFCINST_INIT;
+		ret = 0;
+		goto out;
+	} else {
+		mfc_err("Wrong type error for S_FMT : %d", f->type);
+		ret = -EINVAL;
+		goto out;
+	}
+
+out:
+	mfc_debug_leave();
+	return ret;
+}
+
+static int reqbufs_output(struct s5p_mfc_dev *dev, struct s5p_mfc_ctx *ctx,
+				struct v4l2_requestbuffers *reqbufs)
+{
+	int ret = 0;
+
+	s5p_mfc_clock_on();
+
+	if (reqbufs->count == 0) {
+		mfc_debug(2, "Freeing buffers\n");
+		ret = vb2_reqbufs(&ctx->vq_src, reqbufs);
+		if (ret)
+			goto out;
+		s5p_mfc_close_mfc_inst(dev, ctx);
+		ctx->src_bufs_cnt = 0;
+		ctx->output_state = QUEUE_FREE;
+	} else if (ctx->output_state == QUEUE_FREE) {
+		/* Can only request buffers when we have a valid format set. */
+		WARN_ON(ctx->src_bufs_cnt != 0);
+		if (ctx->state != MFCINST_INIT) {
+			mfc_err("Reqbufs called in an invalid state\n");
+			ret = -EINVAL;
+			goto out;
+		}
+
+		mfc_debug(2, "Allocating %d buffers for OUTPUT queue\n",
+				reqbufs->count);
+		ret = vb2_reqbufs(&ctx->vq_src, reqbufs);
+		if (ret)
+			goto out;
+
+		ret = s5p_mfc_open_mfc_inst(dev, ctx);
+		if (ret) {
+			reqbufs->count = 0;
+			vb2_reqbufs(&ctx->vq_src, reqbufs);
+			goto out;
+		}
+
+		ctx->output_state = QUEUE_BUFS_REQUESTED;
+	} else {
+		mfc_err("Buffers have already been requested\n");
+		ret = -EINVAL;
+	}
+out:
+	s5p_mfc_clock_off();
+	if (ret)
+		mfc_err("Failed allocating buffers for OUTPUT queue\n");
+	return ret;
+}
+
+static int reqbufs_capture(struct s5p_mfc_dev *dev, struct s5p_mfc_ctx *ctx,
+				struct v4l2_requestbuffers *reqbufs)
+{
+	int ret = 0;
+
+	s5p_mfc_clock_on();
+
+	if (reqbufs->count == 0) {
+		mfc_debug(2, "Freeing buffers\n");
+		ret = vb2_reqbufs(&ctx->vq_dst, reqbufs);
+		if (ret)
+			goto out;
+		s5p_mfc_hw_call_void(dev->mfc_ops, release_codec_buffers, ctx);
+		ctx->dst_bufs_cnt = 0;
+	} else if (ctx->capture_state == QUEUE_FREE) {
+		WARN_ON(ctx->dst_bufs_cnt != 0);
+		mfc_debug(2, "Allocating %d buffers for CAPTURE queue\n",
+				reqbufs->count);
+		ret = vb2_reqbufs(&ctx->vq_dst, reqbufs);
+		if (ret)
+			goto out;
+
+		ctx->capture_state = QUEUE_BUFS_REQUESTED;
+		ctx->total_dpb_count = reqbufs->count;
+
+		ret = s5p_mfc_hw_call(dev->mfc_ops, alloc_codec_buffers, ctx);
+		if (ret) {
+			mfc_err("Failed to allocate decoding buffers\n");
+			reqbufs->count = 0;
+			vb2_reqbufs(&ctx->vq_dst, reqbufs);
+			ret = -ENOMEM;
+			ctx->capture_state = QUEUE_FREE;
+			goto out;
+		}
+
+		WARN_ON(ctx->dst_bufs_cnt != ctx->total_dpb_count);
+		ctx->capture_state = QUEUE_BUFS_MMAPED;
+
+		if (s5p_mfc_ctx_ready(ctx))
+			set_work_bit_irqsave(ctx);
+		s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
+		s5p_mfc_wait_for_done_ctx(ctx, S5P_MFC_R2H_CMD_INIT_BUFFERS_RET,
+					  0);
+	} else {
+		mfc_err("Buffers have already been requested\n");
+		ret = -EINVAL;
+	}
+out:
+	s5p_mfc_clock_off();
+	if (ret)
+		mfc_err("Failed allocating buffers for CAPTURE queue\n");
+	return ret;
+}
+
+/* Reqeust buffers */
+static int vidioc_reqbufs(struct file *file, void *priv,
+					  struct v4l2_requestbuffers *reqbufs)
+{
+	struct s5p_mfc_dev *dev = video_drvdata(file);
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+
+	if (reqbufs->memory != V4L2_MEMORY_MMAP) {
+		mfc_err("Only V4L2_MEMORY_MAP is supported\n");
+		return -EINVAL;
+	}
+
+	if (reqbufs->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		return reqbufs_output(dev, ctx, reqbufs);
+	} else if (reqbufs->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		return reqbufs_capture(dev, ctx, reqbufs);
+	} else {
+		mfc_err("Invalid type requested\n");
+		return -EINVAL;
+	}
+}
+
+/* Query buffer */
+static int vidioc_querybuf(struct file *file, void *priv,
+						   struct v4l2_buffer *buf)
+{
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+	int ret;
+	int i;
+
+	if (buf->memory != V4L2_MEMORY_MMAP) {
+		mfc_err("Only mmaped buffers can be used\n");
+		return -EINVAL;
+	}
+	mfc_debug(2, "State: %d, buf->type: %d\n", ctx->state, buf->type);
+	if (ctx->state == MFCINST_GOT_INST &&
+			buf->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		ret = vb2_querybuf(&ctx->vq_src, buf);
+	} else if (ctx->state == MFCINST_RUNNING &&
+			buf->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		ret = vb2_querybuf(&ctx->vq_dst, buf);
+		for (i = 0; i < buf->length; i++)
+			buf->m.planes[i].m.mem_offset += DST_QUEUE_OFF_BASE;
+	} else {
+		mfc_err("vidioc_querybuf called in an inappropriate state\n");
+		ret = -EINVAL;
+	}
+	mfc_debug_leave();
+	return ret;
+}
+
+/* Queue a buffer */
+static int vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
+{
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+
+	if (ctx->state == MFCINST_ERROR) {
+		mfc_err("Call on QBUF after unrecoverable error\n");
+		return -EIO;
+	}
+	if (buf->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		return vb2_qbuf(&ctx->vq_src, buf);
+	else if (buf->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		return vb2_qbuf(&ctx->vq_dst, buf);
+	return -EINVAL;
+}
+
+/* Dequeue a buffer */
+static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
+{
+	const struct v4l2_event ev = {
+		.type = V4L2_EVENT_EOS
+	};
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+	int ret;
+
+	if (ctx->state == MFCINST_ERROR) {
+		mfc_err("Call on DQBUF after unrecoverable error\n");
+		return -EIO;
+	}
+
+	switch (buf->type) {
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
+		return vb2_dqbuf(&ctx->vq_src, buf, file->f_flags & O_NONBLOCK);
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
+		ret = vb2_dqbuf(&ctx->vq_dst, buf, file->f_flags & O_NONBLOCK);
+		if (ret)
+			return ret;
+
+		if (ctx->state == MFCINST_FINISHED &&
+		    (ctx->dst_bufs[buf->index].flags & MFC_BUF_FLAG_EOS))
+			v4l2_event_queue_fh(&ctx->fh, &ev);
+		return 0;
+	default:
+		return -EINVAL;
+	}
+}
+
+/* Export DMA buffer */
+static int vidioc_expbuf(struct file *file, void *priv,
+	struct v4l2_exportbuffer *eb)
+{
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+
+	if (eb->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		return vb2_expbuf(&ctx->vq_src, eb);
+	if (eb->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		return vb2_expbuf(&ctx->vq_dst, eb);
+	return -EINVAL;
+}
+
+/* Stream on */
+static int vidioc_streamon(struct file *file, void *priv,
+			   enum v4l2_buf_type type)
+{
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+	int ret = -EINVAL;
+
+	mfc_debug_enter();
+	if (type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		ret = vb2_streamon(&ctx->vq_src, type);
+	else if (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		ret = vb2_streamon(&ctx->vq_dst, type);
+	mfc_debug_leave();
+	return ret;
+}
+
+/* Stream off, which equals to a pause */
+static int vidioc_streamoff(struct file *file, void *priv,
+			    enum v4l2_buf_type type)
+{
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+
+	if (type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		return vb2_streamoff(&ctx->vq_src, type);
+	else if (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		return vb2_streamoff(&ctx->vq_dst, type);
+	return -EINVAL;
+}
+
+/* Set controls - v4l2 control framework */
+static int s5p_mfc_dec_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct s5p_mfc_ctx *ctx = ctrl_to_ctx(ctrl);
+
+	switch (ctrl->id) {
+	case V4L2_CID_MPEG_MFC51_VIDEO_DECODER_H264_DISPLAY_DELAY:
+		ctx->display_delay = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_MFC51_VIDEO_DECODER_H264_DISPLAY_DELAY_ENABLE:
+		ctx->display_delay_enable = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_DECODER_MPEG4_DEBLOCK_FILTER:
+		ctx->loop_filter_mpeg4 = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_DECODER_SLICE_INTERFACE:
+		ctx->slice_interface = ctrl->val;
+		break;
+	default:
+		mfc_err("Invalid control 0x%08x\n", ctrl->id);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int s5p_mfc_dec_g_v_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct s5p_mfc_ctx *ctx = ctrl_to_ctx(ctrl);
+	struct s5p_mfc_dev *dev = ctx->dev;
+
+	switch (ctrl->id) {
+	case V4L2_CID_MIN_BUFFERS_FOR_CAPTURE:
+		if (ctx->state >= MFCINST_HEAD_PARSED &&
+		    ctx->state < MFCINST_ABORT) {
+			ctrl->val = ctx->pb_count;
+			break;
+		} else if (ctx->state != MFCINST_INIT &&
+				ctx->state != MFCINST_RES_CHANGE_END) {
+			v4l2_err(&dev->v4l2_dev, "Decoding not initialised\n");
+			return -EINVAL;
+		}
+		/* Should wait for the header to be parsed */
+		s5p_mfc_wait_for_done_ctx(ctx,
+				S5P_MFC_R2H_CMD_SEQ_DONE_RET, 0);
+		if (ctx->state >= MFCINST_HEAD_PARSED &&
+		    ctx->state < MFCINST_ABORT) {
+			ctrl->val = ctx->pb_count;
+		} else {
+			v4l2_err(&dev->v4l2_dev, "Decoding not initialised\n");
+			return -EINVAL;
+		}
+		break;
+	}
+	return 0;
+}
+
+
+static const struct v4l2_ctrl_ops s5p_mfc_dec_ctrl_ops = {
+	.s_ctrl = s5p_mfc_dec_s_ctrl,
+	.g_volatile_ctrl = s5p_mfc_dec_g_v_ctrl,
+};
+
+/* Get cropping information */
+static int vidioc_g_crop(struct file *file, void *priv,
+		struct v4l2_crop *cr)
+{
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+	struct s5p_mfc_dev *dev = ctx->dev;
+	u32 left, right, top, bottom;
+
+	if (ctx->state != MFCINST_HEAD_PARSED &&
+	ctx->state != MFCINST_RUNNING && ctx->state != MFCINST_FINISHING
+					&& ctx->state != MFCINST_FINISHED) {
+			mfc_err("Cannont set crop\n");
+			return -EINVAL;
+		}
+	if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_H264) {
+		left = s5p_mfc_hw_call(dev->mfc_ops, get_crop_info_h, ctx);
+		right = left >> S5P_FIMV_SHARED_CROP_RIGHT_SHIFT;
+		left = left & S5P_FIMV_SHARED_CROP_LEFT_MASK;
+		top = s5p_mfc_hw_call(dev->mfc_ops, get_crop_info_v, ctx);
+		bottom = top >> S5P_FIMV_SHARED_CROP_BOTTOM_SHIFT;
+		top = top & S5P_FIMV_SHARED_CROP_TOP_MASK;
+		cr->c.left = left;
+		cr->c.top = top;
+		cr->c.width = ctx->img_width - left - right;
+		cr->c.height = ctx->img_height - top - bottom;
+		mfc_debug(2, "Cropping info [h264]: l=%d t=%d "
+			"w=%d h=%d (r=%d b=%d fw=%d fh=%d\n", left, top,
+			cr->c.width, cr->c.height, right, bottom,
+			ctx->buf_width, ctx->buf_height);
+	} else {
+		cr->c.left = 0;
+		cr->c.top = 0;
+		cr->c.width = ctx->img_width;
+		cr->c.height = ctx->img_height;
+		mfc_debug(2, "Cropping info: w=%d h=%d fw=%d "
+			"fh=%d\n", cr->c.width,	cr->c.height, ctx->buf_width,
+							ctx->buf_height);
+	}
+	return 0;
+}
+
+static int vidioc_decoder_cmd(struct file *file, void *priv,
+			      struct v4l2_decoder_cmd *cmd)
+{
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_buf *buf;
+	unsigned long flags;
+
+	switch (cmd->cmd) {
+	case V4L2_DEC_CMD_STOP:
+		if (cmd->flags != 0)
+			return -EINVAL;
+
+		if (!ctx->vq_src.streaming)
+			return -EINVAL;
+
+		spin_lock_irqsave(&dev->irqlock, flags);
+		if (list_empty(&ctx->src_queue)) {
+			mfc_err("EOS: empty src queue, entering finishing state");
+			ctx->state = MFCINST_FINISHING;
+			if (s5p_mfc_ctx_ready(ctx))
+				set_work_bit_irqsave(ctx);
+			spin_unlock_irqrestore(&dev->irqlock, flags);
+			s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
+		} else {
+			mfc_err("EOS: marking last buffer of stream");
+			buf = list_entry(ctx->src_queue.prev,
+						struct s5p_mfc_buf, list);
+			if (buf->flags & MFC_BUF_FLAG_USED)
+				ctx->state = MFCINST_FINISHING;
+			else
+				buf->flags |= MFC_BUF_FLAG_EOS;
+			spin_unlock_irqrestore(&dev->irqlock, flags);
+		}
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int vidioc_subscribe_event(struct v4l2_fh *fh,
+				const struct  v4l2_event_subscription *sub)
+{
+	switch (sub->type) {
+	case V4L2_EVENT_EOS:
+		return v4l2_event_subscribe(fh, sub, 2, NULL);
+	case V4L2_EVENT_SOURCE_CHANGE:
+		return v4l2_src_change_event_subscribe(fh, sub);
+	default:
+		return -EINVAL;
+	}
+}
+
+
+/* v4l2_ioctl_ops */
+static const struct v4l2_ioctl_ops s5p_mfc_dec_ioctl_ops = {
+	.vidioc_querycap = vidioc_querycap,
+	.vidioc_enum_fmt_vid_cap_mplane = vidioc_enum_fmt_vid_cap_mplane,
+	.vidioc_enum_fmt_vid_out_mplane = vidioc_enum_fmt_vid_out_mplane,
+	.vidioc_g_fmt_vid_cap_mplane = vidioc_g_fmt,
+	.vidioc_g_fmt_vid_out_mplane = vidioc_g_fmt,
+	.vidioc_try_fmt_vid_cap_mplane = vidioc_try_fmt,
+	.vidioc_try_fmt_vid_out_mplane = vidioc_try_fmt,
+	.vidioc_s_fmt_vid_cap_mplane = vidioc_s_fmt,
+	.vidioc_s_fmt_vid_out_mplane = vidioc_s_fmt,
+	.vidioc_reqbufs = vidioc_reqbufs,
+	.vidioc_querybuf = vidioc_querybuf,
+	.vidioc_qbuf = vidioc_qbuf,
+	.vidioc_dqbuf = vidioc_dqbuf,
+	.vidioc_expbuf = vidioc_expbuf,
+	.vidioc_streamon = vidioc_streamon,
+	.vidioc_streamoff = vidioc_streamoff,
+	.vidioc_g_crop = vidioc_g_crop,
+	.vidioc_decoder_cmd = vidioc_decoder_cmd,
+	.vidioc_subscribe_event = vidioc_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+};
+
+static int s5p_mfc_queue_setup(struct vb2_queue *vq,
+			const void *parg, unsigned int *buf_count,
+			unsigned int *plane_count, unsigned int psize[],
+			void *allocators[])
+{
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(vq->drv_priv);
+	struct s5p_mfc_dev *dev = ctx->dev;
+
+	/* Video output for decoding (source)
+	 * this can be set after getting an instance */
+	if (ctx->state == MFCINST_INIT &&
+	    vq->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		/* A single plane is required for input */
+		*plane_count = 1;
+		if (*buf_count < 1)
+			*buf_count = 1;
+		if (*buf_count > MFC_MAX_BUFFERS)
+			*buf_count = MFC_MAX_BUFFERS;
+	/* Video capture for decoding (destination)
+	 * this can be set after the header was parsed */
+	} else if (ctx->state == MFCINST_HEAD_PARSED &&
+		   vq->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		/* Output plane count is 2 - one for Y and one for CbCr */
+		*plane_count = 2;
+		/* Setup buffer count */
+		if (*buf_count < ctx->pb_count)
+			*buf_count = ctx->pb_count;
+		if (*buf_count > ctx->pb_count + MFC_MAX_EXTRA_DPB)
+			*buf_count = ctx->pb_count + MFC_MAX_EXTRA_DPB;
+		if (*buf_count > MFC_MAX_BUFFERS)
+			*buf_count = MFC_MAX_BUFFERS;
+	} else {
+		mfc_err("State seems invalid. State = %d, vq->type = %d\n",
+							ctx->state, vq->type);
+		return -EINVAL;
+	}
+	mfc_debug(2, "Buffer count=%d, plane count=%d\n",
+						*buf_count, *plane_count);
+	if (ctx->state == MFCINST_HEAD_PARSED &&
+	    vq->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		psize[0] = ctx->luma_size;
+		psize[1] = ctx->chroma_size;
+
+		if (IS_MFCV6_PLUS(dev))
+			allocators[0] =
+				ctx->dev->alloc_ctx[MFC_BANK1_ALLOC_CTX];
+		else
+			allocators[0] =
+				ctx->dev->alloc_ctx[MFC_BANK2_ALLOC_CTX];
+		allocators[1] = ctx->dev->alloc_ctx[MFC_BANK1_ALLOC_CTX];
+	} else if (vq->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE &&
+		   ctx->state == MFCINST_INIT) {
+		psize[0] = ctx->dec_src_buf_size;
+		allocators[0] = ctx->dev->alloc_ctx[MFC_BANK1_ALLOC_CTX];
+	} else {
+		mfc_err("This video node is dedicated to decoding. Decoding not initialized\n");
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int s5p_mfc_buf_init(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct vb2_queue *vq = vb->vb2_queue;
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(vq->drv_priv);
+	unsigned int i;
+
+	if (vq->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		if (ctx->capture_state == QUEUE_BUFS_MMAPED)
+			return 0;
+		for (i = 0; i < ctx->dst_fmt->num_planes; i++) {
+			if (IS_ERR_OR_NULL(ERR_PTR(
+					vb2_dma_contig_plane_dma_addr(vb, i)))) {
+				mfc_err("Plane mem not allocated\n");
+				return -EINVAL;
+			}
+		}
+		if (vb2_plane_size(vb, 0) < ctx->luma_size ||
+			vb2_plane_size(vb, 1) < ctx->chroma_size) {
+			mfc_err("Plane buffer (CAPTURE) is too small\n");
+			return -EINVAL;
+		}
+		i = vb->index;
+		ctx->dst_bufs[i].b = vbuf;
+		ctx->dst_bufs[i].cookie.raw.luma =
+					vb2_dma_contig_plane_dma_addr(vb, 0);
+		ctx->dst_bufs[i].cookie.raw.chroma =
+					vb2_dma_contig_plane_dma_addr(vb, 1);
+		ctx->dst_bufs_cnt++;
+	} else if (vq->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		if (IS_ERR_OR_NULL(ERR_PTR(
+					vb2_dma_contig_plane_dma_addr(vb, 0)))) {
+			mfc_err("Plane memory not allocated\n");
+			return -EINVAL;
+		}
+		if (vb2_plane_size(vb, 0) < ctx->dec_src_buf_size) {
+			mfc_err("Plane buffer (OUTPUT) is too small\n");
+			return -EINVAL;
+		}
+
+		i = vb->index;
+		ctx->src_bufs[i].b = vbuf;
+		ctx->src_bufs[i].cookie.stream =
+					vb2_dma_contig_plane_dma_addr(vb, 0);
+		ctx->src_bufs_cnt++;
+	} else {
+		mfc_err("s5p_mfc_buf_init: unknown queue type\n");
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int s5p_mfc_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(q->drv_priv);
+	struct s5p_mfc_dev *dev = ctx->dev;
+
+	v4l2_ctrl_handler_setup(&ctx->ctrl_handler);
+	if (ctx->state == MFCINST_FINISHING ||
+		ctx->state == MFCINST_FINISHED)
+		ctx->state = MFCINST_RUNNING;
+	/* If context is ready then dev = work->data;schedule it to run */
+	if (s5p_mfc_ctx_ready(ctx))
+		set_work_bit_irqsave(ctx);
+	s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
+	return 0;
+}
+
+static void s5p_mfc_stop_streaming(struct vb2_queue *q)
+{
+	unsigned long flags;
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(q->drv_priv);
+	struct s5p_mfc_dev *dev = ctx->dev;
+	int aborted = 0;
+
+	if ((ctx->state == MFCINST_FINISHING ||
+		ctx->state ==  MFCINST_RUNNING) &&
+		dev->curr_ctx == ctx->num && dev->hw_lock) {
+		ctx->state = MFCINST_ABORT;
+		s5p_mfc_wait_for_done_ctx(ctx,
+					S5P_MFC_R2H_CMD_FRAME_DONE_RET, 0);
+		aborted = 1;
+	}
+	if (q->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		spin_lock_irqsave(&dev->irqlock, flags);
+		s5p_mfc_hw_call_void(dev->mfc_ops, cleanup_queue,
+						&ctx->dst_queue, &ctx->vq_dst);
+		INIT_LIST_HEAD(&ctx->dst_queue);
+		ctx->dst_queue_cnt = 0;
+		ctx->dpb_flush_flag = 1;
+		ctx->dec_dst_flag = 0;
+		spin_unlock_irqrestore(&dev->irqlock, flags);
+		if (IS_MFCV6_PLUS(dev) && (ctx->state == MFCINST_RUNNING)) {
+			ctx->state = MFCINST_FLUSH;
+			set_work_bit_irqsave(ctx);
+			s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
+			if (s5p_mfc_wait_for_done_ctx(ctx,
+				S5P_MFC_R2H_CMD_DPB_FLUSH_RET, 0))
+				mfc_err("Err flushing buffers\n");
+		}
+	}
+	if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		spin_lock_irqsave(&dev->irqlock, flags);
+		s5p_mfc_hw_call_void(dev->mfc_ops, cleanup_queue,
+						&ctx->src_queue, &ctx->vq_src);
+		INIT_LIST_HEAD(&ctx->src_queue);
+		ctx->src_queue_cnt = 0;
+		spin_unlock_irqrestore(&dev->irqlock, flags);
+	}
+	if (aborted)
+		ctx->state = MFCINST_RUNNING;
+}
+
+
+static void s5p_mfc_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_queue *vq = vb->vb2_queue;
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(vq->drv_priv);
+	struct s5p_mfc_dev *dev = ctx->dev;
+	unsigned long flags;
+	struct s5p_mfc_buf *mfc_buf;
+
+	if (vq->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		mfc_buf = &ctx->src_bufs[vb->index];
+		mfc_buf->flags &= ~MFC_BUF_FLAG_USED;
+		spin_lock_irqsave(&dev->irqlock, flags);
+		list_add_tail(&mfc_buf->list, &ctx->src_queue);
+		ctx->src_queue_cnt++;
+		spin_unlock_irqrestore(&dev->irqlock, flags);
+	} else if (vq->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		mfc_buf = &ctx->dst_bufs[vb->index];
+		mfc_buf->flags &= ~MFC_BUF_FLAG_USED;
+		/* Mark destination as available for use by MFC */
+		spin_lock_irqsave(&dev->irqlock, flags);
+		set_bit(vb->index, &ctx->dec_dst_flag);
+		list_add_tail(&mfc_buf->list, &ctx->dst_queue);
+		ctx->dst_queue_cnt++;
+		spin_unlock_irqrestore(&dev->irqlock, flags);
+	} else {
+		mfc_err("Unsupported buffer type (%d)\n", vq->type);
+	}
+	if (s5p_mfc_ctx_ready(ctx))
+		set_work_bit_irqsave(ctx);
+	s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
+}
+
+static struct vb2_ops s5p_mfc_dec_qops = {
+	.queue_setup		= s5p_mfc_queue_setup,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
+	.buf_init		= s5p_mfc_buf_init,
+	.start_streaming	= s5p_mfc_start_streaming,
+	.stop_streaming		= s5p_mfc_stop_streaming,
+	.buf_queue		= s5p_mfc_buf_queue,
+};
+
+struct s5p_mfc_codec_ops *get_dec_codec_ops(void)
+{
+	return &decoder_codec_ops;
+}
+
+struct vb2_ops *get_dec_queue_ops(void)
+{
+	return &s5p_mfc_dec_qops;
+}
+
+const struct v4l2_ioctl_ops *get_dec_v4l2_ioctl_ops(void)
+{
+	return &s5p_mfc_dec_ioctl_ops;
+}
+
+#define IS_MFC51_PRIV(x) ((V4L2_CTRL_ID2CLASS(x) == V4L2_CTRL_CLASS_MPEG) \
+						&& V4L2_CTRL_DRIVER_PRIV(x))
+
+int s5p_mfc_dec_ctrls_setup(struct s5p_mfc_ctx *ctx)
+{
+	struct v4l2_ctrl_config cfg;
+	int i;
+
+	v4l2_ctrl_handler_init(&ctx->ctrl_handler, NUM_CTRLS);
+	if (ctx->ctrl_handler.error) {
+		mfc_err("v4l2_ctrl_handler_init failed\n");
+		return ctx->ctrl_handler.error;
+	}
+
+	for (i = 0; i < NUM_CTRLS; i++) {
+		if (IS_MFC51_PRIV(controls[i].id)) {
+			memset(&cfg, 0, sizeof(struct v4l2_ctrl_config));
+			cfg.ops = &s5p_mfc_dec_ctrl_ops;
+			cfg.id = controls[i].id;
+			cfg.min = controls[i].minimum;
+			cfg.max = controls[i].maximum;
+			cfg.def = controls[i].default_value;
+			cfg.name = controls[i].name;
+			cfg.type = controls[i].type;
+
+			cfg.step = controls[i].step;
+			cfg.menu_skip_mask = 0;
+
+			ctx->ctrls[i] = v4l2_ctrl_new_custom(&ctx->ctrl_handler,
+					&cfg, NULL);
+		} else {
+			ctx->ctrls[i] = v4l2_ctrl_new_std(&ctx->ctrl_handler,
+					&s5p_mfc_dec_ctrl_ops,
+					controls[i].id, controls[i].minimum,
+					controls[i].maximum, controls[i].step,
+					controls[i].default_value);
+		}
+		if (ctx->ctrl_handler.error) {
+			mfc_err("Adding control (%d) failed\n", i);
+			return ctx->ctrl_handler.error;
+		}
+		if (controls[i].is_volatile && ctx->ctrls[i])
+			ctx->ctrls[i]->flags |= V4L2_CTRL_FLAG_VOLATILE;
+	}
+	return 0;
+}
+
+void s5p_mfc_dec_ctrls_delete(struct s5p_mfc_ctx *ctx)
+{
+	int i;
+
+	v4l2_ctrl_handler_free(&ctx->ctrl_handler);
+	for (i = 0; i < NUM_CTRLS; i++)
+		ctx->ctrls[i] = NULL;
+}
+
+void s5p_mfc_dec_init(struct s5p_mfc_ctx *ctx)
+{
+	struct v4l2_format f;
+	f.fmt.pix_mp.pixelformat = V4L2_PIX_FMT_H264;
+	ctx->src_fmt = find_format(&f, MFC_FMT_DEC);
+	if (IS_MFCV8(ctx->dev))
+		f.fmt.pix_mp.pixelformat = V4L2_PIX_FMT_NV12M;
+	else if (IS_MFCV6_PLUS(ctx->dev))
+		f.fmt.pix_mp.pixelformat = V4L2_PIX_FMT_NV12MT_16X16;
+	else
+		f.fmt.pix_mp.pixelformat = V4L2_PIX_FMT_NV12MT;
+	ctx->dst_fmt = find_format(&f, MFC_FMT_RAW);
+	mfc_debug(2, "Default src_fmt is %p, dest_fmt is %p\n",
+			ctx->src_fmt, ctx->dst_fmt);
+}
+
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_dec.h b/drivers/media/platform/s5p-mfc/s5p_mfc_dec.h
new file mode 100644
index 0000000..d06a7ca
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_dec.h
@@ -0,0 +1,24 @@
+/*
+ * linux/drivers/media/platform/s5p-mfc/s5p_mfc_dec.h
+ *
+ * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#ifndef S5P_MFC_DEC_H_
+#define S5P_MFC_DEC_H_
+
+struct s5p_mfc_codec_ops *get_dec_codec_ops(void);
+struct vb2_ops *get_dec_queue_ops(void);
+const struct v4l2_ioctl_ops *get_dec_v4l2_ioctl_ops(void);
+struct s5p_mfc_fmt *get_dec_def_fmt(bool src);
+int s5p_mfc_dec_ctrls_setup(struct s5p_mfc_ctx *ctx);
+void s5p_mfc_dec_ctrls_delete(struct s5p_mfc_ctx *ctx);
+void s5p_mfc_dec_init(struct s5p_mfc_ctx *ctx);
+
+#endif /* S5P_MFC_DEC_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c b/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c
new file mode 100644
index 0000000..5c678ec
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c
@@ -0,0 +1,2151 @@
+/*
+ * linux/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c
+ *
+ * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com/
+ *
+ * Jeongtae Park	<jtp.park@samsung.com>
+ * Kamil Debski		<k.debski@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/clk.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-event.h>
+#include <linux/workqueue.h>
+#include <media/v4l2-ctrls.h>
+#include <media/videobuf2-v4l2.h>
+#include "s5p_mfc_common.h"
+#include "s5p_mfc_ctrl.h"
+#include "s5p_mfc_debug.h"
+#include "s5p_mfc_enc.h"
+#include "s5p_mfc_intr.h"
+#include "s5p_mfc_opr.h"
+
+#define DEF_SRC_FMT_ENC	V4L2_PIX_FMT_NV12M
+#define DEF_DST_FMT_ENC	V4L2_PIX_FMT_H264
+
+static struct s5p_mfc_fmt formats[] = {
+	{
+		.name		= "4:2:0 2 Planes 16x16 Tiles",
+		.fourcc		= V4L2_PIX_FMT_NV12MT_16X16,
+		.codec_mode	= S5P_MFC_CODEC_NONE,
+		.type		= MFC_FMT_RAW,
+		.num_planes	= 2,
+		.versions	= MFC_V6_BIT | MFC_V7_BIT,
+	},
+	{
+		.name		= "4:2:0 2 Planes 64x32 Tiles",
+		.fourcc		= V4L2_PIX_FMT_NV12MT,
+		.codec_mode	= S5P_MFC_CODEC_NONE,
+		.type		= MFC_FMT_RAW,
+		.num_planes	= 2,
+		.versions	= MFC_V5_BIT,
+	},
+	{
+		.name		= "4:2:0 2 Planes Y/CbCr",
+		.fourcc		= V4L2_PIX_FMT_NV12M,
+		.codec_mode	= S5P_MFC_CODEC_NONE,
+		.type		= MFC_FMT_RAW,
+		.num_planes	= 2,
+		.versions	= MFC_V5_BIT | MFC_V6_BIT | MFC_V7_BIT |
+								MFC_V8_BIT,
+	},
+	{
+		.name		= "4:2:0 2 Planes Y/CrCb",
+		.fourcc		= V4L2_PIX_FMT_NV21M,
+		.codec_mode	= S5P_MFC_CODEC_NONE,
+		.type		= MFC_FMT_RAW,
+		.num_planes	= 2,
+		.versions	= MFC_V6_BIT | MFC_V7_BIT | MFC_V8_BIT,
+	},
+	{
+		.name		= "H264 Encoded Stream",
+		.fourcc		= V4L2_PIX_FMT_H264,
+		.codec_mode	= S5P_MFC_CODEC_H264_ENC,
+		.type		= MFC_FMT_ENC,
+		.num_planes	= 1,
+		.versions	= MFC_V5_BIT | MFC_V6_BIT | MFC_V7_BIT |
+								MFC_V8_BIT,
+	},
+	{
+		.name		= "MPEG4 Encoded Stream",
+		.fourcc		= V4L2_PIX_FMT_MPEG4,
+		.codec_mode	= S5P_MFC_CODEC_MPEG4_ENC,
+		.type		= MFC_FMT_ENC,
+		.num_planes	= 1,
+		.versions	= MFC_V5_BIT | MFC_V6_BIT | MFC_V7_BIT |
+								MFC_V8_BIT,
+	},
+	{
+		.name		= "H263 Encoded Stream",
+		.fourcc		= V4L2_PIX_FMT_H263,
+		.codec_mode	= S5P_MFC_CODEC_H263_ENC,
+		.type		= MFC_FMT_ENC,
+		.num_planes	= 1,
+		.versions	= MFC_V5_BIT | MFC_V6_BIT | MFC_V7_BIT |
+								MFC_V8_BIT,
+	},
+	{
+		.name		= "VP8 Encoded Stream",
+		.fourcc		= V4L2_PIX_FMT_VP8,
+		.codec_mode	= S5P_MFC_CODEC_VP8_ENC,
+		.type		= MFC_FMT_ENC,
+		.num_planes	= 1,
+		.versions	= MFC_V7_BIT | MFC_V8_BIT,
+	},
+};
+
+#define NUM_FORMATS ARRAY_SIZE(formats)
+static struct s5p_mfc_fmt *find_format(struct v4l2_format *f, unsigned int t)
+{
+	unsigned int i;
+
+	for (i = 0; i < NUM_FORMATS; i++) {
+		if (formats[i].fourcc == f->fmt.pix_mp.pixelformat &&
+		    formats[i].type == t)
+			return &formats[i];
+	}
+	return NULL;
+}
+
+static struct mfc_control controls[] = {
+	{
+		.id = V4L2_CID_MPEG_VIDEO_GOP_SIZE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.minimum = 0,
+		.maximum = (1 << 16) - 1,
+		.step = 1,
+		.default_value = 12,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MODE,
+		.type = V4L2_CTRL_TYPE_MENU,
+		.minimum = V4L2_MPEG_VIDEO_MULTI_SLICE_MODE_SINGLE,
+		.maximum = V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_BYTES,
+		.default_value = V4L2_MPEG_VIDEO_MULTI_SLICE_MODE_SINGLE,
+		.menu_skip_mask = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MAX_MB,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.minimum = 1,
+		.maximum = (1 << 16) - 1,
+		.step = 1,
+		.default_value = 1,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MAX_BYTES,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.minimum = 1900,
+		.maximum = (1 << 30) - 1,
+		.step = 1,
+		.default_value = 1900,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_CYCLIC_INTRA_REFRESH_MB,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.minimum = 0,
+		.maximum = (1 << 16) - 1,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_MFC51_VIDEO_PADDING,
+		.type = V4L2_CTRL_TYPE_BOOLEAN,
+		.name = "Padding Control Enable",
+		.minimum = 0,
+		.maximum = 1,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_MFC51_VIDEO_PADDING_YUV,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Padding Color YUV Value",
+		.minimum = 0,
+		.maximum = (1 << 25) - 1,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_FRAME_RC_ENABLE,
+		.type = V4L2_CTRL_TYPE_BOOLEAN,
+		.minimum = 0,
+		.maximum = 1,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_BITRATE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.minimum = 1,
+		.maximum = (1 << 30) - 1,
+		.step = 1,
+		.default_value = 1,
+	},
+	{
+		.id = V4L2_CID_MPEG_MFC51_VIDEO_RC_REACTION_COEFF,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Rate Control Reaction Coeff.",
+		.minimum = 1,
+		.maximum = (1 << 16) - 1,
+		.step = 1,
+		.default_value = 1,
+	},
+	{
+		.id = V4L2_CID_MPEG_MFC51_VIDEO_FORCE_FRAME_TYPE,
+		.type = V4L2_CTRL_TYPE_MENU,
+		.name = "Force frame type",
+		.minimum = V4L2_MPEG_MFC51_VIDEO_FORCE_FRAME_TYPE_DISABLED,
+		.maximum = V4L2_MPEG_MFC51_VIDEO_FORCE_FRAME_TYPE_NOT_CODED,
+		.default_value = V4L2_MPEG_MFC51_VIDEO_FORCE_FRAME_TYPE_DISABLED,
+		.menu_skip_mask = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_VBV_SIZE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.minimum = 0,
+		.maximum = (1 << 16) - 1,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_MV_H_SEARCH_RANGE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Horizontal MV Search Range",
+		.minimum = 16,
+		.maximum = 128,
+		.step = 16,
+		.default_value = 32,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_MV_V_SEARCH_RANGE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Vertical MV Search Range",
+		.minimum = 16,
+		.maximum = 128,
+		.step = 16,
+		.default_value = 32,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_H264_CPB_SIZE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.minimum = 0,
+		.maximum = (1 << 16) - 1,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_HEADER_MODE,
+		.type = V4L2_CTRL_TYPE_MENU,
+		.minimum = V4L2_MPEG_VIDEO_HEADER_MODE_SEPARATE,
+		.maximum = V4L2_MPEG_VIDEO_HEADER_MODE_JOINED_WITH_1ST_FRAME,
+		.default_value = V4L2_MPEG_VIDEO_HEADER_MODE_SEPARATE,
+		.menu_skip_mask = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE,
+		.type = V4L2_CTRL_TYPE_MENU,
+		.name = "Frame Skip Enable",
+		.minimum = V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_DISABLED,
+		.maximum = V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT,
+		.menu_skip_mask = 0,
+		.default_value = V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_DISABLED,
+	},
+	{
+		.id = V4L2_CID_MPEG_MFC51_VIDEO_RC_FIXED_TARGET_BIT,
+		.type = V4L2_CTRL_TYPE_BOOLEAN,
+		.name = "Fixed Target Bit Enable",
+		.minimum = 0,
+		.maximum = 1,
+		.default_value = 0,
+		.step = 1,
+		.menu_skip_mask = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_B_FRAMES,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.minimum = 0,
+		.maximum = 2,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_H264_PROFILE,
+		.type = V4L2_CTRL_TYPE_MENU,
+		.minimum = V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE,
+		.maximum = V4L2_MPEG_VIDEO_H264_PROFILE_MULTIVIEW_HIGH,
+		.default_value = V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE,
+		.menu_skip_mask = ~(
+				(1 << V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE) |
+				(1 << V4L2_MPEG_VIDEO_H264_PROFILE_MAIN) |
+				(1 << V4L2_MPEG_VIDEO_H264_PROFILE_HIGH)
+				),
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_H264_LEVEL,
+		.type = V4L2_CTRL_TYPE_MENU,
+		.minimum = V4L2_MPEG_VIDEO_H264_LEVEL_1_0,
+		.maximum = V4L2_MPEG_VIDEO_H264_LEVEL_4_0,
+		.default_value = V4L2_MPEG_VIDEO_H264_LEVEL_1_0,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_MPEG4_LEVEL,
+		.type = V4L2_CTRL_TYPE_MENU,
+		.minimum = V4L2_MPEG_VIDEO_MPEG4_LEVEL_0,
+		.maximum = V4L2_MPEG_VIDEO_MPEG4_LEVEL_5,
+		.default_value = V4L2_MPEG_VIDEO_MPEG4_LEVEL_0,
+		.menu_skip_mask = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_MODE,
+		.type = V4L2_CTRL_TYPE_MENU,
+		.minimum = V4L2_MPEG_VIDEO_H264_LOOP_FILTER_MODE_ENABLED,
+		.maximum = V4L2_MPEG_VIDEO_H264_LOOP_FILTER_MODE_DISABLED_AT_SLICE_BOUNDARY,
+		.default_value = V4L2_MPEG_VIDEO_H264_LOOP_FILTER_MODE_ENABLED,
+		.menu_skip_mask = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_ALPHA,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.minimum = -6,
+		.maximum = 6,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_BETA,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.minimum = -6,
+		.maximum = 6,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_H264_ENTROPY_MODE,
+		.type = V4L2_CTRL_TYPE_MENU,
+		.minimum = V4L2_MPEG_VIDEO_H264_ENTROPY_MODE_CAVLC,
+		.maximum = V4L2_MPEG_VIDEO_H264_ENTROPY_MODE_CABAC,
+		.default_value = V4L2_MPEG_VIDEO_H264_ENTROPY_MODE_CAVLC,
+		.menu_skip_mask = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_MFC51_VIDEO_H264_NUM_REF_PIC_FOR_P,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "The Number of Ref. Pic for P",
+		.minimum = 1,
+		.maximum = 2,
+		.step = 1,
+		.default_value = 1,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_H264_8X8_TRANSFORM,
+		.type = V4L2_CTRL_TYPE_BOOLEAN,
+		.minimum = 0,
+		.maximum = 1,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_MB_RC_ENABLE,
+		.type = V4L2_CTRL_TYPE_BOOLEAN,
+		.minimum = 0,
+		.maximum = 1,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_H264_I_FRAME_QP,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.minimum = 0,
+		.maximum = 51,
+		.step = 1,
+		.default_value = 1,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_H264_MIN_QP,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.minimum = 0,
+		.maximum = 51,
+		.step = 1,
+		.default_value = 1,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_H264_MAX_QP,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.minimum = 0,
+		.maximum = 51,
+		.step = 1,
+		.default_value = 51,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_H264_P_FRAME_QP,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.minimum = 0,
+		.maximum = 51,
+		.step = 1,
+		.default_value = 1,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_H264_B_FRAME_QP,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.minimum = 0,
+		.maximum = 51,
+		.step = 1,
+		.default_value = 1,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_H263_I_FRAME_QP,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "H263 I-Frame QP value",
+		.minimum = 1,
+		.maximum = 31,
+		.step = 1,
+		.default_value = 1,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_H263_MIN_QP,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "H263 Minimum QP value",
+		.minimum = 1,
+		.maximum = 31,
+		.step = 1,
+		.default_value = 1,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_H263_MAX_QP,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "H263 Maximum QP value",
+		.minimum = 1,
+		.maximum = 31,
+		.step = 1,
+		.default_value = 31,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_H263_P_FRAME_QP,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "H263 P frame QP value",
+		.minimum = 1,
+		.maximum = 31,
+		.step = 1,
+		.default_value = 1,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_H263_B_FRAME_QP,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "H263 B frame QP value",
+		.minimum = 1,
+		.maximum = 31,
+		.step = 1,
+		.default_value = 1,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_MPEG4_I_FRAME_QP,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "MPEG4 I-Frame QP value",
+		.minimum = 1,
+		.maximum = 31,
+		.step = 1,
+		.default_value = 1,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_MPEG4_MIN_QP,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "MPEG4 Minimum QP value",
+		.minimum = 1,
+		.maximum = 31,
+		.step = 1,
+		.default_value = 1,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_MPEG4_MAX_QP,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "MPEG4 Maximum QP value",
+		.minimum = 0,
+		.maximum = 51,
+		.step = 1,
+		.default_value = 51,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_MPEG4_P_FRAME_QP,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "MPEG4 P frame QP value",
+		.minimum = 1,
+		.maximum = 31,
+		.step = 1,
+		.default_value = 1,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_MPEG4_B_FRAME_QP,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "MPEG4 B frame QP value",
+		.minimum = 1,
+		.maximum = 31,
+		.step = 1,
+		.default_value = 1,
+	},
+	{
+		.id = V4L2_CID_MPEG_MFC51_VIDEO_H264_ADAPTIVE_RC_DARK,
+		.type = V4L2_CTRL_TYPE_BOOLEAN,
+		.name = "H264 Dark Reg Adaptive RC",
+		.minimum = 0,
+		.maximum = 1,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_MFC51_VIDEO_H264_ADAPTIVE_RC_SMOOTH,
+		.type = V4L2_CTRL_TYPE_BOOLEAN,
+		.name = "H264 Smooth Reg Adaptive RC",
+		.minimum = 0,
+		.maximum = 1,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_MFC51_VIDEO_H264_ADAPTIVE_RC_STATIC,
+		.type = V4L2_CTRL_TYPE_BOOLEAN,
+		.name = "H264 Static Reg Adaptive RC",
+		.minimum = 0,
+		.maximum = 1,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_MFC51_VIDEO_H264_ADAPTIVE_RC_ACTIVITY,
+		.type = V4L2_CTRL_TYPE_BOOLEAN,
+		.name = "H264 Activity Reg Adaptive RC",
+		.minimum = 0,
+		.maximum = 1,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_H264_VUI_SAR_ENABLE,
+		.type = V4L2_CTRL_TYPE_BOOLEAN,
+		.minimum = 0,
+		.maximum = 1,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_H264_VUI_SAR_IDC,
+		.type = V4L2_CTRL_TYPE_MENU,
+		.minimum = V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_UNSPECIFIED,
+		.maximum = V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_EXTENDED,
+		.default_value = V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_UNSPECIFIED,
+		.menu_skip_mask = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_H264_VUI_EXT_SAR_WIDTH,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.minimum = 0,
+		.maximum = (1 << 16) - 1,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_H264_VUI_EXT_SAR_HEIGHT,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.minimum = 0,
+		.maximum = (1 << 16) - 1,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_GOP_CLOSURE,
+		.type = V4L2_CTRL_TYPE_BOOLEAN,
+		.minimum = 0,
+		.maximum = 1,
+		.step = 1,
+		.default_value = 1,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_H264_I_PERIOD,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.minimum = 0,
+		.maximum = (1 << 16) - 1,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_MPEG4_PROFILE,
+		.type = V4L2_CTRL_TYPE_MENU,
+		.minimum = V4L2_MPEG_VIDEO_MPEG4_PROFILE_SIMPLE,
+		.maximum = V4L2_MPEG_VIDEO_MPEG4_PROFILE_ADVANCED_SIMPLE,
+		.default_value = V4L2_MPEG_VIDEO_MPEG4_PROFILE_SIMPLE,
+		.menu_skip_mask = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_MPEG4_QPEL,
+		.type = V4L2_CTRL_TYPE_BOOLEAN,
+		.minimum = 0,
+		.maximum = 1,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_VPX_NUM_PARTITIONS,
+		.type = V4L2_CTRL_TYPE_INTEGER_MENU,
+		.maximum = V4L2_CID_MPEG_VIDEO_VPX_8_PARTITIONS,
+		.default_value = V4L2_CID_MPEG_VIDEO_VPX_1_PARTITION,
+		.menu_skip_mask = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_VPX_IMD_DISABLE_4X4,
+		.type = V4L2_CTRL_TYPE_BOOLEAN,
+		.minimum = 0,
+		.maximum = 1,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_VPX_NUM_REF_FRAMES,
+		.type = V4L2_CTRL_TYPE_INTEGER_MENU,
+		.maximum = V4L2_CID_MPEG_VIDEO_VPX_2_REF_FRAME,
+		.default_value = V4L2_CID_MPEG_VIDEO_VPX_1_REF_FRAME,
+		.menu_skip_mask = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_VPX_FILTER_LEVEL,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.minimum = 0,
+		.maximum = 63,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_VPX_FILTER_SHARPNESS,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.minimum = 0,
+		.maximum = 7,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_VPX_GOLDEN_FRAME_REF_PERIOD,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.minimum = 0,
+		.maximum = (1 << 16) - 1,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_VPX_GOLDEN_FRAME_SEL,
+		.type = V4L2_CTRL_TYPE_MENU,
+		.minimum = V4L2_CID_MPEG_VIDEO_VPX_GOLDEN_FRAME_USE_PREV,
+		.maximum = V4L2_CID_MPEG_VIDEO_VPX_GOLDEN_FRAME_USE_REF_PERIOD,
+		.default_value = V4L2_CID_MPEG_VIDEO_VPX_GOLDEN_FRAME_USE_PREV,
+		.menu_skip_mask = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_VPX_MAX_QP,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.minimum = 0,
+		.maximum = 127,
+		.step = 1,
+		.default_value = 127,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_VPX_MIN_QP,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.minimum = 0,
+		.maximum = 11,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_VPX_I_FRAME_QP,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.minimum = 0,
+		.maximum = 127,
+		.step = 1,
+		.default_value = 10,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_VPX_P_FRAME_QP,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.minimum = 0,
+		.maximum = 127,
+		.step = 1,
+		.default_value = 10,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_VPX_PROFILE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.minimum = 0,
+		.maximum = 3,
+		.step = 1,
+		.default_value = 0,
+	},
+	{
+		.id = V4L2_CID_MIN_BUFFERS_FOR_OUTPUT,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Minimum number of output bufs",
+		.minimum = 1,
+		.maximum = 32,
+		.step = 1,
+		.default_value = 1,
+		.is_volatile = 1,
+	},
+};
+
+#define NUM_CTRLS ARRAY_SIZE(controls)
+static const char * const *mfc51_get_menu(u32 id)
+{
+	static const char * const mfc51_video_frame_skip[] = {
+		"Disabled",
+		"Level Limit",
+		"VBV/CPB Limit",
+		NULL,
+	};
+	static const char * const mfc51_video_force_frame[] = {
+		"Disabled",
+		"I Frame",
+		"Not Coded",
+		NULL,
+	};
+	switch (id) {
+	case V4L2_CID_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE:
+		return mfc51_video_frame_skip;
+	case V4L2_CID_MPEG_MFC51_VIDEO_FORCE_FRAME_TYPE:
+		return mfc51_video_force_frame;
+	}
+	return NULL;
+}
+
+static int s5p_mfc_ctx_ready(struct s5p_mfc_ctx *ctx)
+{
+	mfc_debug(2, "src=%d, dst=%d, state=%d\n",
+		  ctx->src_queue_cnt, ctx->dst_queue_cnt, ctx->state);
+	/* context is ready to make header */
+	if (ctx->state == MFCINST_GOT_INST && ctx->dst_queue_cnt >= 1)
+		return 1;
+	/* context is ready to encode a frame */
+	if ((ctx->state == MFCINST_RUNNING ||
+		ctx->state == MFCINST_HEAD_PRODUCED) &&
+		ctx->src_queue_cnt >= 1 && ctx->dst_queue_cnt >= 1)
+		return 1;
+	/* context is ready to encode remaining frames */
+	if (ctx->state == MFCINST_FINISHING &&
+		ctx->dst_queue_cnt >= 1)
+		return 1;
+	mfc_debug(2, "ctx is not ready\n");
+	return 0;
+}
+
+static void cleanup_ref_queue(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_buf *mb_entry;
+
+	/* move buffers in ref queue to src queue */
+	while (!list_empty(&ctx->ref_queue)) {
+		mb_entry = list_entry((&ctx->ref_queue)->next,
+						struct s5p_mfc_buf, list);
+		list_del(&mb_entry->list);
+		ctx->ref_queue_cnt--;
+		list_add_tail(&mb_entry->list, &ctx->src_queue);
+		ctx->src_queue_cnt++;
+	}
+	mfc_debug(2, "enc src count: %d, enc ref count: %d\n",
+		  ctx->src_queue_cnt, ctx->ref_queue_cnt);
+	INIT_LIST_HEAD(&ctx->ref_queue);
+	ctx->ref_queue_cnt = 0;
+}
+
+static int enc_pre_seq_start(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_buf *dst_mb;
+	unsigned long dst_addr;
+	unsigned int dst_size;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev->irqlock, flags);
+	dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
+	dst_addr = vb2_dma_contig_plane_dma_addr(&dst_mb->b->vb2_buf, 0);
+	dst_size = vb2_plane_size(&dst_mb->b->vb2_buf, 0);
+	s5p_mfc_hw_call_void(dev->mfc_ops, set_enc_stream_buffer, ctx, dst_addr,
+			dst_size);
+	spin_unlock_irqrestore(&dev->irqlock, flags);
+	return 0;
+}
+
+static int enc_post_seq_start(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_enc_params *p = &ctx->enc_params;
+	struct s5p_mfc_buf *dst_mb;
+	unsigned long flags;
+	unsigned int enc_pb_count;
+
+	if (p->seq_hdr_mode == V4L2_MPEG_VIDEO_HEADER_MODE_SEPARATE) {
+		spin_lock_irqsave(&dev->irqlock, flags);
+		if (!list_empty(&ctx->dst_queue)) {
+			dst_mb = list_entry(ctx->dst_queue.next,
+					struct s5p_mfc_buf, list);
+			list_del(&dst_mb->list);
+			ctx->dst_queue_cnt--;
+			vb2_set_plane_payload(&dst_mb->b->vb2_buf, 0,
+				s5p_mfc_hw_call(dev->mfc_ops, get_enc_strm_size,
+						dev));
+			vb2_buffer_done(&dst_mb->b->vb2_buf,
+					VB2_BUF_STATE_DONE);
+		}
+		spin_unlock_irqrestore(&dev->irqlock, flags);
+	}
+
+	if (!IS_MFCV6_PLUS(dev)) {
+		ctx->state = MFCINST_RUNNING;
+		if (s5p_mfc_ctx_ready(ctx))
+			set_work_bit_irqsave(ctx);
+		s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
+	} else {
+		enc_pb_count = s5p_mfc_hw_call(dev->mfc_ops,
+				get_enc_dpb_count, dev);
+		if (ctx->pb_count < enc_pb_count)
+			ctx->pb_count = enc_pb_count;
+		ctx->state = MFCINST_HEAD_PRODUCED;
+	}
+
+	return 0;
+}
+
+static int enc_pre_frame_start(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_buf *dst_mb;
+	struct s5p_mfc_buf *src_mb;
+	unsigned long flags;
+	unsigned long src_y_addr, src_c_addr, dst_addr;
+	unsigned int dst_size;
+
+	spin_lock_irqsave(&dev->irqlock, flags);
+	src_mb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
+	src_y_addr = vb2_dma_contig_plane_dma_addr(&src_mb->b->vb2_buf, 0);
+	src_c_addr = vb2_dma_contig_plane_dma_addr(&src_mb->b->vb2_buf, 1);
+	s5p_mfc_hw_call_void(dev->mfc_ops, set_enc_frame_buffer, ctx,
+							src_y_addr, src_c_addr);
+	spin_unlock_irqrestore(&dev->irqlock, flags);
+
+	spin_lock_irqsave(&dev->irqlock, flags);
+	dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
+	dst_addr = vb2_dma_contig_plane_dma_addr(&dst_mb->b->vb2_buf, 0);
+	dst_size = vb2_plane_size(&dst_mb->b->vb2_buf, 0);
+	s5p_mfc_hw_call_void(dev->mfc_ops, set_enc_stream_buffer, ctx, dst_addr,
+			dst_size);
+	spin_unlock_irqrestore(&dev->irqlock, flags);
+
+	return 0;
+}
+
+static int enc_post_frame_start(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_buf *mb_entry;
+	unsigned long enc_y_addr, enc_c_addr;
+	unsigned long mb_y_addr, mb_c_addr;
+	int slice_type;
+	unsigned int strm_size;
+	unsigned long flags;
+
+	slice_type = s5p_mfc_hw_call(dev->mfc_ops, get_enc_slice_type, dev);
+	strm_size = s5p_mfc_hw_call(dev->mfc_ops, get_enc_strm_size, dev);
+	mfc_debug(2, "Encoded slice type: %d\n", slice_type);
+	mfc_debug(2, "Encoded stream size: %d\n", strm_size);
+	mfc_debug(2, "Display order: %d\n",
+		  mfc_read(dev, S5P_FIMV_ENC_SI_PIC_CNT));
+	spin_lock_irqsave(&dev->irqlock, flags);
+	if (slice_type >= 0) {
+		s5p_mfc_hw_call_void(dev->mfc_ops, get_enc_frame_buffer, ctx,
+				&enc_y_addr, &enc_c_addr);
+		list_for_each_entry(mb_entry, &ctx->src_queue, list) {
+			mb_y_addr = vb2_dma_contig_plane_dma_addr(
+					&mb_entry->b->vb2_buf, 0);
+			mb_c_addr = vb2_dma_contig_plane_dma_addr(
+					&mb_entry->b->vb2_buf, 1);
+			if ((enc_y_addr == mb_y_addr) &&
+						(enc_c_addr == mb_c_addr)) {
+				list_del(&mb_entry->list);
+				ctx->src_queue_cnt--;
+				vb2_buffer_done(&mb_entry->b->vb2_buf,
+							VB2_BUF_STATE_DONE);
+				break;
+			}
+		}
+		list_for_each_entry(mb_entry, &ctx->ref_queue, list) {
+			mb_y_addr = vb2_dma_contig_plane_dma_addr(
+					&mb_entry->b->vb2_buf, 0);
+			mb_c_addr = vb2_dma_contig_plane_dma_addr(
+					&mb_entry->b->vb2_buf, 1);
+			if ((enc_y_addr == mb_y_addr) &&
+						(enc_c_addr == mb_c_addr)) {
+				list_del(&mb_entry->list);
+				ctx->ref_queue_cnt--;
+				vb2_buffer_done(&mb_entry->b->vb2_buf,
+							VB2_BUF_STATE_DONE);
+				break;
+			}
+		}
+	}
+	if ((ctx->src_queue_cnt > 0) && (ctx->state == MFCINST_RUNNING)) {
+		mb_entry = list_entry(ctx->src_queue.next, struct s5p_mfc_buf,
+									list);
+		if (mb_entry->flags & MFC_BUF_FLAG_USED) {
+			list_del(&mb_entry->list);
+			ctx->src_queue_cnt--;
+			list_add_tail(&mb_entry->list, &ctx->ref_queue);
+			ctx->ref_queue_cnt++;
+		}
+	}
+	mfc_debug(2, "enc src count: %d, enc ref count: %d\n",
+		  ctx->src_queue_cnt, ctx->ref_queue_cnt);
+	if ((ctx->dst_queue_cnt > 0) && (strm_size > 0)) {
+		mb_entry = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf,
+									list);
+		list_del(&mb_entry->list);
+		ctx->dst_queue_cnt--;
+		switch (slice_type) {
+		case S5P_FIMV_ENC_SI_SLICE_TYPE_I:
+			mb_entry->b->flags |= V4L2_BUF_FLAG_KEYFRAME;
+			break;
+		case S5P_FIMV_ENC_SI_SLICE_TYPE_P:
+			mb_entry->b->flags |= V4L2_BUF_FLAG_PFRAME;
+			break;
+		case S5P_FIMV_ENC_SI_SLICE_TYPE_B:
+			mb_entry->b->flags |= V4L2_BUF_FLAG_BFRAME;
+			break;
+		}
+		vb2_set_plane_payload(&mb_entry->b->vb2_buf, 0, strm_size);
+		vb2_buffer_done(&mb_entry->b->vb2_buf, VB2_BUF_STATE_DONE);
+	}
+	spin_unlock_irqrestore(&dev->irqlock, flags);
+	if ((ctx->src_queue_cnt == 0) || (ctx->dst_queue_cnt == 0))
+		clear_work_bit(ctx);
+
+	return 0;
+}
+
+static struct s5p_mfc_codec_ops encoder_codec_ops = {
+	.pre_seq_start		= enc_pre_seq_start,
+	.post_seq_start		= enc_post_seq_start,
+	.pre_frame_start	= enc_pre_frame_start,
+	.post_frame_start	= enc_post_frame_start,
+};
+
+/* Query capabilities of the device */
+static int vidioc_querycap(struct file *file, void *priv,
+			   struct v4l2_capability *cap)
+{
+	struct s5p_mfc_dev *dev = video_drvdata(file);
+
+	strncpy(cap->driver, dev->plat_dev->name, sizeof(cap->driver) - 1);
+	strncpy(cap->card, dev->plat_dev->name, sizeof(cap->card) - 1);
+	cap->bus_info[0] = 0;
+	/*
+	 * This is only a mem-to-mem video device. The capture and output
+	 * device capability flags are left only for backward compatibility
+	 * and are scheduled for removal.
+	 */
+	cap->device_caps = V4L2_CAP_VIDEO_M2M_MPLANE | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	return 0;
+}
+
+static int vidioc_enum_fmt(struct file *file, struct v4l2_fmtdesc *f,
+							bool out)
+{
+	struct s5p_mfc_dev *dev = video_drvdata(file);
+	struct s5p_mfc_fmt *fmt;
+	int i, j = 0;
+
+	for (i = 0; i < ARRAY_SIZE(formats); ++i) {
+		if (out && formats[i].type != MFC_FMT_RAW)
+			continue;
+		else if (!out && formats[i].type != MFC_FMT_ENC)
+			continue;
+		else if ((dev->variant->version_bit & formats[i].versions) == 0)
+			continue;
+
+		if (j == f->index) {
+			fmt = &formats[i];
+			strlcpy(f->description, fmt->name,
+				sizeof(f->description));
+			f->pixelformat = fmt->fourcc;
+			return 0;
+		}
+		++j;
+	}
+	return -EINVAL;
+}
+
+static int vidioc_enum_fmt_vid_cap_mplane(struct file *file, void *pirv,
+					  struct v4l2_fmtdesc *f)
+{
+	return vidioc_enum_fmt(file, f, false);
+}
+
+static int vidioc_enum_fmt_vid_out_mplane(struct file *file, void *prov,
+					  struct v4l2_fmtdesc *f)
+{
+	return vidioc_enum_fmt(file, f, true);
+}
+
+static int vidioc_g_fmt(struct file *file, void *priv, struct v4l2_format *f)
+{
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+	struct v4l2_pix_format_mplane *pix_fmt_mp = &f->fmt.pix_mp;
+
+	mfc_debug(2, "f->type = %d ctx->state = %d\n", f->type, ctx->state);
+	if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		/* This is run on output (encoder dest) */
+		pix_fmt_mp->width = 0;
+		pix_fmt_mp->height = 0;
+		pix_fmt_mp->field = V4L2_FIELD_NONE;
+		pix_fmt_mp->pixelformat = ctx->dst_fmt->fourcc;
+		pix_fmt_mp->num_planes = ctx->dst_fmt->num_planes;
+
+		pix_fmt_mp->plane_fmt[0].bytesperline = ctx->enc_dst_buf_size;
+		pix_fmt_mp->plane_fmt[0].sizeimage = ctx->enc_dst_buf_size;
+	} else if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		/* This is run on capture (encoder src) */
+		pix_fmt_mp->width = ctx->img_width;
+		pix_fmt_mp->height = ctx->img_height;
+
+		pix_fmt_mp->field = V4L2_FIELD_NONE;
+		pix_fmt_mp->pixelformat = ctx->src_fmt->fourcc;
+		pix_fmt_mp->num_planes = ctx->src_fmt->num_planes;
+
+		pix_fmt_mp->plane_fmt[0].bytesperline = ctx->buf_width;
+		pix_fmt_mp->plane_fmt[0].sizeimage = ctx->luma_size;
+		pix_fmt_mp->plane_fmt[1].bytesperline = ctx->buf_width;
+		pix_fmt_mp->plane_fmt[1].sizeimage = ctx->chroma_size;
+	} else {
+		mfc_err("invalid buf type\n");
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int vidioc_try_fmt(struct file *file, void *priv, struct v4l2_format *f)
+{
+	struct s5p_mfc_dev *dev = video_drvdata(file);
+	struct s5p_mfc_fmt *fmt;
+	struct v4l2_pix_format_mplane *pix_fmt_mp = &f->fmt.pix_mp;
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		fmt = find_format(f, MFC_FMT_ENC);
+		if (!fmt) {
+			mfc_err("failed to try output format\n");
+			return -EINVAL;
+		}
+		if (pix_fmt_mp->plane_fmt[0].sizeimage == 0) {
+			mfc_err("must be set encoding output size\n");
+			return -EINVAL;
+		}
+		if ((dev->variant->version_bit & fmt->versions) == 0) {
+			mfc_err("Unsupported format by this MFC version.\n");
+			return -EINVAL;
+		}
+
+		pix_fmt_mp->plane_fmt[0].bytesperline =
+			pix_fmt_mp->plane_fmt[0].sizeimage;
+	} else if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		fmt = find_format(f, MFC_FMT_RAW);
+		if (!fmt) {
+			mfc_err("failed to try output format\n");
+			return -EINVAL;
+		}
+
+		if (fmt->num_planes != pix_fmt_mp->num_planes) {
+			mfc_err("failed to try output format\n");
+			return -EINVAL;
+		}
+		if ((dev->variant->version_bit & fmt->versions) == 0) {
+			mfc_err("Unsupported format by this MFC version.\n");
+			return -EINVAL;
+		}
+
+		v4l_bound_align_image(&pix_fmt_mp->width, 8, 1920, 1,
+			&pix_fmt_mp->height, 4, 1080, 1, 0);
+	} else {
+		mfc_err("invalid buf type\n");
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int vidioc_s_fmt(struct file *file, void *priv, struct v4l2_format *f)
+{
+	struct s5p_mfc_dev *dev = video_drvdata(file);
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+	struct v4l2_pix_format_mplane *pix_fmt_mp = &f->fmt.pix_mp;
+	int ret = 0;
+
+	ret = vidioc_try_fmt(file, priv, f);
+	if (ret)
+		return ret;
+	if (ctx->vq_src.streaming || ctx->vq_dst.streaming) {
+		v4l2_err(&dev->v4l2_dev, "%s queue busy\n", __func__);
+		ret = -EBUSY;
+		goto out;
+	}
+	if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		/* dst_fmt is validated by call to vidioc_try_fmt */
+		ctx->dst_fmt = find_format(f, MFC_FMT_ENC);
+		ctx->state = MFCINST_INIT;
+		ctx->codec_mode = ctx->dst_fmt->codec_mode;
+		ctx->enc_dst_buf_size =	pix_fmt_mp->plane_fmt[0].sizeimage;
+		pix_fmt_mp->plane_fmt[0].bytesperline = 0;
+		ctx->dst_bufs_cnt = 0;
+		ctx->capture_state = QUEUE_FREE;
+		ret = s5p_mfc_open_mfc_inst(dev, ctx);
+	} else if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		/* src_fmt is validated by call to vidioc_try_fmt */
+		ctx->src_fmt = find_format(f, MFC_FMT_RAW);
+		ctx->img_width = pix_fmt_mp->width;
+		ctx->img_height = pix_fmt_mp->height;
+		mfc_debug(2, "codec number: %d\n", ctx->src_fmt->codec_mode);
+		mfc_debug(2, "fmt - w: %d, h: %d, ctx - w: %d, h: %d\n",
+			pix_fmt_mp->width, pix_fmt_mp->height,
+			ctx->img_width, ctx->img_height);
+
+		s5p_mfc_hw_call_void(dev->mfc_ops, enc_calc_src_size, ctx);
+		pix_fmt_mp->plane_fmt[0].sizeimage = ctx->luma_size;
+		pix_fmt_mp->plane_fmt[0].bytesperline = ctx->buf_width;
+		pix_fmt_mp->plane_fmt[1].sizeimage = ctx->chroma_size;
+		pix_fmt_mp->plane_fmt[1].bytesperline = ctx->buf_width;
+
+		ctx->src_bufs_cnt = 0;
+		ctx->output_state = QUEUE_FREE;
+	} else {
+		mfc_err("invalid buf type\n");
+		ret = -EINVAL;
+	}
+out:
+	mfc_debug_leave();
+	return ret;
+}
+
+static int vidioc_reqbufs(struct file *file, void *priv,
+					  struct v4l2_requestbuffers *reqbufs)
+{
+	struct s5p_mfc_dev *dev = video_drvdata(file);
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+	int ret = 0;
+
+	/* if memory is not mmp or userptr return error */
+	if ((reqbufs->memory != V4L2_MEMORY_MMAP) &&
+		(reqbufs->memory != V4L2_MEMORY_USERPTR))
+		return -EINVAL;
+	if (reqbufs->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		if (reqbufs->count == 0) {
+			ret = vb2_reqbufs(&ctx->vq_dst, reqbufs);
+			ctx->capture_state = QUEUE_FREE;
+			return ret;
+		}
+		if (ctx->capture_state != QUEUE_FREE) {
+			mfc_err("invalid capture state: %d\n",
+							ctx->capture_state);
+			return -EINVAL;
+		}
+		ret = vb2_reqbufs(&ctx->vq_dst, reqbufs);
+		if (ret != 0) {
+			mfc_err("error in vb2_reqbufs() for E(D)\n");
+			return ret;
+		}
+		ctx->capture_state = QUEUE_BUFS_REQUESTED;
+
+		ret = s5p_mfc_hw_call(ctx->dev->mfc_ops,
+				alloc_codec_buffers, ctx);
+		if (ret) {
+			mfc_err("Failed to allocate encoding buffers\n");
+			reqbufs->count = 0;
+			ret = vb2_reqbufs(&ctx->vq_dst, reqbufs);
+			return -ENOMEM;
+		}
+	} else if (reqbufs->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		if (reqbufs->count == 0) {
+			mfc_debug(2, "Freeing buffers\n");
+			ret = vb2_reqbufs(&ctx->vq_src, reqbufs);
+			s5p_mfc_hw_call_void(dev->mfc_ops, release_codec_buffers,
+					ctx);
+			ctx->output_state = QUEUE_FREE;
+			return ret;
+		}
+		if (ctx->output_state != QUEUE_FREE) {
+			mfc_err("invalid output state: %d\n",
+							ctx->output_state);
+			return -EINVAL;
+		}
+
+		if (IS_MFCV6_PLUS(dev)) {
+			/* Check for min encoder buffers */
+			if (ctx->pb_count &&
+				(reqbufs->count < ctx->pb_count)) {
+				reqbufs->count = ctx->pb_count;
+				mfc_debug(2, "Minimum %d output buffers needed\n",
+						ctx->pb_count);
+			} else {
+				ctx->pb_count = reqbufs->count;
+			}
+		}
+
+		ret = vb2_reqbufs(&ctx->vq_src, reqbufs);
+		if (ret != 0) {
+			mfc_err("error in vb2_reqbufs() for E(S)\n");
+			return ret;
+		}
+		ctx->output_state = QUEUE_BUFS_REQUESTED;
+	} else {
+		mfc_err("invalid buf type\n");
+		return -EINVAL;
+	}
+	return ret;
+}
+
+static int vidioc_querybuf(struct file *file, void *priv,
+						   struct v4l2_buffer *buf)
+{
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+	int ret = 0;
+
+	/* if memory is not mmp or userptr return error */
+	if ((buf->memory != V4L2_MEMORY_MMAP) &&
+		(buf->memory != V4L2_MEMORY_USERPTR))
+		return -EINVAL;
+	if (buf->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		if (ctx->state != MFCINST_GOT_INST) {
+			mfc_err("invalid context state: %d\n", ctx->state);
+			return -EINVAL;
+		}
+		ret = vb2_querybuf(&ctx->vq_dst, buf);
+		if (ret != 0) {
+			mfc_err("error in vb2_querybuf() for E(D)\n");
+			return ret;
+		}
+		buf->m.planes[0].m.mem_offset += DST_QUEUE_OFF_BASE;
+	} else if (buf->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		ret = vb2_querybuf(&ctx->vq_src, buf);
+		if (ret != 0) {
+			mfc_err("error in vb2_querybuf() for E(S)\n");
+			return ret;
+		}
+	} else {
+		mfc_err("invalid buf type\n");
+		return -EINVAL;
+	}
+	return ret;
+}
+
+/* Queue a buffer */
+static int vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
+{
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+
+	if (ctx->state == MFCINST_ERROR) {
+		mfc_err("Call on QBUF after unrecoverable error\n");
+		return -EIO;
+	}
+	if (buf->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		if (ctx->state == MFCINST_FINISHING) {
+			mfc_err("Call on QBUF after EOS command\n");
+			return -EIO;
+		}
+		return vb2_qbuf(&ctx->vq_src, buf);
+	} else if (buf->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		return vb2_qbuf(&ctx->vq_dst, buf);
+	}
+	return -EINVAL;
+}
+
+/* Dequeue a buffer */
+static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
+{
+	const struct v4l2_event ev = {
+		.type = V4L2_EVENT_EOS
+	};
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+	int ret;
+
+	if (ctx->state == MFCINST_ERROR) {
+		mfc_err("Call on DQBUF after unrecoverable error\n");
+		return -EIO;
+	}
+	if (buf->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		ret = vb2_dqbuf(&ctx->vq_src, buf, file->f_flags & O_NONBLOCK);
+	} else if (buf->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		ret = vb2_dqbuf(&ctx->vq_dst, buf, file->f_flags & O_NONBLOCK);
+		if (ret == 0 && ctx->state == MFCINST_FINISHED
+					&& list_empty(&ctx->vq_dst.done_list))
+			v4l2_event_queue_fh(&ctx->fh, &ev);
+	} else {
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+/* Export DMA buffer */
+static int vidioc_expbuf(struct file *file, void *priv,
+	struct v4l2_exportbuffer *eb)
+{
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+
+	if (eb->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		return vb2_expbuf(&ctx->vq_src, eb);
+	if (eb->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		return vb2_expbuf(&ctx->vq_dst, eb);
+	return -EINVAL;
+}
+
+/* Stream on */
+static int vidioc_streamon(struct file *file, void *priv,
+			   enum v4l2_buf_type type)
+{
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+
+	if (type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		return vb2_streamon(&ctx->vq_src, type);
+	else if (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		return vb2_streamon(&ctx->vq_dst, type);
+	return -EINVAL;
+}
+
+/* Stream off, which equals to a pause */
+static int vidioc_streamoff(struct file *file, void *priv,
+			    enum v4l2_buf_type type)
+{
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+
+	if (type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		return vb2_streamoff(&ctx->vq_src, type);
+	else if (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		return vb2_streamoff(&ctx->vq_dst, type);
+	return -EINVAL;
+}
+
+static inline int h264_level(enum v4l2_mpeg_video_h264_level lvl)
+{
+	static unsigned int t[V4L2_MPEG_VIDEO_H264_LEVEL_4_0 + 1] = {
+		/* V4L2_MPEG_VIDEO_H264_LEVEL_1_0   */ 10,
+		/* V4L2_MPEG_VIDEO_H264_LEVEL_1B    */ 9,
+		/* V4L2_MPEG_VIDEO_H264_LEVEL_1_1   */ 11,
+		/* V4L2_MPEG_VIDEO_H264_LEVEL_1_2   */ 12,
+		/* V4L2_MPEG_VIDEO_H264_LEVEL_1_3   */ 13,
+		/* V4L2_MPEG_VIDEO_H264_LEVEL_2_0   */ 20,
+		/* V4L2_MPEG_VIDEO_H264_LEVEL_2_1   */ 21,
+		/* V4L2_MPEG_VIDEO_H264_LEVEL_2_2   */ 22,
+		/* V4L2_MPEG_VIDEO_H264_LEVEL_3_0   */ 30,
+		/* V4L2_MPEG_VIDEO_H264_LEVEL_3_1   */ 31,
+		/* V4L2_MPEG_VIDEO_H264_LEVEL_3_2   */ 32,
+		/* V4L2_MPEG_VIDEO_H264_LEVEL_4_0   */ 40,
+	};
+	return t[lvl];
+}
+
+static inline int mpeg4_level(enum v4l2_mpeg_video_mpeg4_level lvl)
+{
+	static unsigned int t[V4L2_MPEG_VIDEO_MPEG4_LEVEL_5 + 1] = {
+		/* V4L2_MPEG_VIDEO_MPEG4_LEVEL_0    */ 0,
+		/* V4L2_MPEG_VIDEO_MPEG4_LEVEL_0B   */ 9,
+		/* V4L2_MPEG_VIDEO_MPEG4_LEVEL_1    */ 1,
+		/* V4L2_MPEG_VIDEO_MPEG4_LEVEL_2    */ 2,
+		/* V4L2_MPEG_VIDEO_MPEG4_LEVEL_3    */ 3,
+		/* V4L2_MPEG_VIDEO_MPEG4_LEVEL_3B   */ 7,
+		/* V4L2_MPEG_VIDEO_MPEG4_LEVEL_4    */ 4,
+		/* V4L2_MPEG_VIDEO_MPEG4_LEVEL_5    */ 5,
+	};
+	return t[lvl];
+}
+
+static inline int vui_sar_idc(enum v4l2_mpeg_video_h264_vui_sar_idc sar)
+{
+	static unsigned int t[V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_EXTENDED + 1] = {
+		/* V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_UNSPECIFIED     */ 0,
+		/* V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_1x1             */ 1,
+		/* V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_12x11           */ 2,
+		/* V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_10x11           */ 3,
+		/* V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_16x11           */ 4,
+		/* V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_40x33           */ 5,
+		/* V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_24x11           */ 6,
+		/* V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_20x11           */ 7,
+		/* V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_32x11           */ 8,
+		/* V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_80x33           */ 9,
+		/* V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_18x11           */ 10,
+		/* V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_15x11           */ 11,
+		/* V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_64x33           */ 12,
+		/* V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_160x99          */ 13,
+		/* V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_4x3             */ 14,
+		/* V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_3x2             */ 15,
+		/* V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_2x1             */ 16,
+		/* V4L2_MPEG_VIDEO_H264_VUI_SAR_IDC_EXTENDED        */ 255,
+	};
+	return t[sar];
+}
+
+static int s5p_mfc_enc_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct s5p_mfc_ctx *ctx = ctrl_to_ctx(ctrl);
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_enc_params *p = &ctx->enc_params;
+	int ret = 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
+		p->gop_size = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MODE:
+		p->slice_mode = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MAX_MB:
+		p->slice_mb = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MAX_BYTES:
+		p->slice_bit = ctrl->val * 8;
+		break;
+	case V4L2_CID_MPEG_VIDEO_CYCLIC_INTRA_REFRESH_MB:
+		p->intra_refresh_mb = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_MFC51_VIDEO_PADDING:
+		p->pad = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_MFC51_VIDEO_PADDING_YUV:
+		p->pad_luma = (ctrl->val >> 16) & 0xff;
+		p->pad_cb = (ctrl->val >> 8) & 0xff;
+		p->pad_cr = (ctrl->val >> 0) & 0xff;
+		break;
+	case V4L2_CID_MPEG_VIDEO_FRAME_RC_ENABLE:
+		p->rc_frame = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_BITRATE:
+		p->rc_bitrate = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_MFC51_VIDEO_RC_REACTION_COEFF:
+		p->rc_reaction_coeff = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_MFC51_VIDEO_FORCE_FRAME_TYPE:
+		ctx->force_frame_type = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_VBV_SIZE:
+		p->vbv_size = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_MV_H_SEARCH_RANGE:
+		p->mv_h_range = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_MV_V_SEARCH_RANGE:
+		p->mv_v_range = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_CPB_SIZE:
+		p->codec.h264.cpb_size = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_HEADER_MODE:
+		p->seq_hdr_mode = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE:
+		p->frame_skip_mode = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_MFC51_VIDEO_RC_FIXED_TARGET_BIT:
+		p->fixed_target_bit = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_B_FRAMES:
+		p->num_b_frame = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_PROFILE:
+		switch (ctrl->val) {
+		case V4L2_MPEG_VIDEO_H264_PROFILE_MAIN:
+			p->codec.h264.profile =
+					S5P_FIMV_ENC_PROFILE_H264_MAIN;
+			break;
+		case V4L2_MPEG_VIDEO_H264_PROFILE_HIGH:
+			p->codec.h264.profile =
+					S5P_FIMV_ENC_PROFILE_H264_HIGH;
+			break;
+		case V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE:
+			p->codec.h264.profile =
+				S5P_FIMV_ENC_PROFILE_H264_BASELINE;
+			break;
+		case V4L2_MPEG_VIDEO_H264_PROFILE_CONSTRAINED_BASELINE:
+			if (IS_MFCV6_PLUS(dev))
+				p->codec.h264.profile =
+				S5P_FIMV_ENC_PROFILE_H264_CONSTRAINED_BASELINE;
+			else
+				ret = -EINVAL;
+			break;
+		default:
+			ret = -EINVAL;
+		}
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_LEVEL:
+		p->codec.h264.level_v4l2 = ctrl->val;
+		p->codec.h264.level = h264_level(ctrl->val);
+		if (p->codec.h264.level < 0) {
+			mfc_err("Level number is wrong\n");
+			ret = p->codec.h264.level;
+		}
+		break;
+	case V4L2_CID_MPEG_VIDEO_MPEG4_LEVEL:
+		p->codec.mpeg4.level_v4l2 = ctrl->val;
+		p->codec.mpeg4.level = mpeg4_level(ctrl->val);
+		if (p->codec.mpeg4.level < 0) {
+			mfc_err("Level number is wrong\n");
+			ret = p->codec.mpeg4.level;
+		}
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_MODE:
+		p->codec.h264.loop_filter_mode = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_ALPHA:
+		p->codec.h264.loop_filter_alpha = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_BETA:
+		p->codec.h264.loop_filter_beta = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_ENTROPY_MODE:
+		p->codec.h264.entropy_mode = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_MFC51_VIDEO_H264_NUM_REF_PIC_FOR_P:
+		p->codec.h264.num_ref_pic_4p = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_8X8_TRANSFORM:
+		p->codec.h264._8x8_transform = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_MB_RC_ENABLE:
+		p->rc_mb = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_I_FRAME_QP:
+		p->codec.h264.rc_frame_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_MIN_QP:
+		p->codec.h264.rc_min_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_MAX_QP:
+		p->codec.h264.rc_max_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_P_FRAME_QP:
+		p->codec.h264.rc_p_frame_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_B_FRAME_QP:
+		p->codec.h264.rc_b_frame_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_MPEG4_I_FRAME_QP:
+	case V4L2_CID_MPEG_VIDEO_H263_I_FRAME_QP:
+		p->codec.mpeg4.rc_frame_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_MPEG4_MIN_QP:
+	case V4L2_CID_MPEG_VIDEO_H263_MIN_QP:
+		p->codec.mpeg4.rc_min_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_MPEG4_MAX_QP:
+	case V4L2_CID_MPEG_VIDEO_H263_MAX_QP:
+		p->codec.mpeg4.rc_max_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_MPEG4_P_FRAME_QP:
+	case V4L2_CID_MPEG_VIDEO_H263_P_FRAME_QP:
+		p->codec.mpeg4.rc_p_frame_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_MPEG4_B_FRAME_QP:
+	case V4L2_CID_MPEG_VIDEO_H263_B_FRAME_QP:
+		p->codec.mpeg4.rc_b_frame_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_MFC51_VIDEO_H264_ADAPTIVE_RC_DARK:
+		p->codec.h264.rc_mb_dark = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_MFC51_VIDEO_H264_ADAPTIVE_RC_SMOOTH:
+		p->codec.h264.rc_mb_smooth = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_MFC51_VIDEO_H264_ADAPTIVE_RC_STATIC:
+		p->codec.h264.rc_mb_static = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_MFC51_VIDEO_H264_ADAPTIVE_RC_ACTIVITY:
+		p->codec.h264.rc_mb_activity = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_VUI_SAR_ENABLE:
+		p->codec.h264.vui_sar = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_VUI_SAR_IDC:
+		p->codec.h264.vui_sar_idc = vui_sar_idc(ctrl->val);
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_VUI_EXT_SAR_WIDTH:
+		p->codec.h264.vui_ext_sar_width = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_VUI_EXT_SAR_HEIGHT:
+		p->codec.h264.vui_ext_sar_height = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_GOP_CLOSURE:
+		p->codec.h264.open_gop = !ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_I_PERIOD:
+		p->codec.h264.open_gop_size = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_MPEG4_PROFILE:
+		switch (ctrl->val) {
+		case V4L2_MPEG_VIDEO_MPEG4_PROFILE_SIMPLE:
+			p->codec.mpeg4.profile =
+				S5P_FIMV_ENC_PROFILE_MPEG4_SIMPLE;
+			break;
+		case V4L2_MPEG_VIDEO_MPEG4_PROFILE_ADVANCED_SIMPLE:
+			p->codec.mpeg4.profile =
+			S5P_FIMV_ENC_PROFILE_MPEG4_ADVANCED_SIMPLE;
+			break;
+		default:
+			ret = -EINVAL;
+		}
+		break;
+	case V4L2_CID_MPEG_VIDEO_MPEG4_QPEL:
+		p->codec.mpeg4.quarter_pixel = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_VPX_NUM_PARTITIONS:
+		p->codec.vp8.num_partitions = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_VPX_IMD_DISABLE_4X4:
+		p->codec.vp8.imd_4x4 = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_VPX_NUM_REF_FRAMES:
+		p->codec.vp8.num_ref = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_VPX_FILTER_LEVEL:
+		p->codec.vp8.filter_level = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_VPX_FILTER_SHARPNESS:
+		p->codec.vp8.filter_sharpness = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_VPX_GOLDEN_FRAME_REF_PERIOD:
+		p->codec.vp8.golden_frame_ref_period = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_VPX_GOLDEN_FRAME_SEL:
+		p->codec.vp8.golden_frame_sel = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_VPX_MIN_QP:
+		p->codec.vp8.rc_min_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_VPX_MAX_QP:
+		p->codec.vp8.rc_max_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_VPX_I_FRAME_QP:
+		p->codec.vp8.rc_frame_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_VPX_P_FRAME_QP:
+		p->codec.vp8.rc_p_frame_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_VPX_PROFILE:
+		p->codec.vp8.profile = ctrl->val;
+		break;
+	default:
+		v4l2_err(&dev->v4l2_dev, "Invalid control, id=%d, val=%d\n",
+							ctrl->id, ctrl->val);
+		ret = -EINVAL;
+	}
+	return ret;
+}
+
+static int s5p_mfc_enc_g_v_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct s5p_mfc_ctx *ctx = ctrl_to_ctx(ctrl);
+	struct s5p_mfc_dev *dev = ctx->dev;
+
+	switch (ctrl->id) {
+	case V4L2_CID_MIN_BUFFERS_FOR_OUTPUT:
+		if (ctx->state >= MFCINST_HEAD_PARSED &&
+		    ctx->state < MFCINST_ABORT) {
+			ctrl->val = ctx->pb_count;
+			break;
+		} else if (ctx->state != MFCINST_INIT) {
+			v4l2_err(&dev->v4l2_dev, "Encoding not initialised\n");
+			return -EINVAL;
+		}
+		/* Should wait for the header to be produced */
+		s5p_mfc_wait_for_done_ctx(ctx,
+				S5P_MFC_R2H_CMD_SEQ_DONE_RET, 0);
+		if (ctx->state >= MFCINST_HEAD_PARSED &&
+		    ctx->state < MFCINST_ABORT) {
+			ctrl->val = ctx->pb_count;
+		} else {
+			v4l2_err(&dev->v4l2_dev, "Encoding not initialised\n");
+			return -EINVAL;
+		}
+		break;
+	}
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops s5p_mfc_enc_ctrl_ops = {
+	.s_ctrl = s5p_mfc_enc_s_ctrl,
+	.g_volatile_ctrl = s5p_mfc_enc_g_v_ctrl,
+};
+
+static int vidioc_s_parm(struct file *file, void *priv,
+			 struct v4l2_streamparm *a)
+{
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+
+	if (a->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		ctx->enc_params.rc_framerate_num =
+					a->parm.output.timeperframe.denominator;
+		ctx->enc_params.rc_framerate_denom =
+					a->parm.output.timeperframe.numerator;
+	} else {
+		mfc_err("Setting FPS is only possible for the output queue\n");
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int vidioc_g_parm(struct file *file, void *priv,
+			 struct v4l2_streamparm *a)
+{
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+
+	if (a->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+		a->parm.output.timeperframe.denominator =
+					ctx->enc_params.rc_framerate_num;
+		a->parm.output.timeperframe.numerator =
+					ctx->enc_params.rc_framerate_denom;
+	} else {
+		mfc_err("Setting FPS is only possible for the output queue\n");
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int vidioc_encoder_cmd(struct file *file, void *priv,
+			      struct v4l2_encoder_cmd *cmd)
+{
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_buf *buf;
+	unsigned long flags;
+
+	switch (cmd->cmd) {
+	case V4L2_ENC_CMD_STOP:
+		if (cmd->flags != 0)
+			return -EINVAL;
+
+		if (!ctx->vq_src.streaming)
+			return -EINVAL;
+
+		spin_lock_irqsave(&dev->irqlock, flags);
+		if (list_empty(&ctx->src_queue)) {
+			mfc_debug(2, "EOS: empty src queue, entering finishing state\n");
+			ctx->state = MFCINST_FINISHING;
+			if (s5p_mfc_ctx_ready(ctx))
+				set_work_bit_irqsave(ctx);
+			spin_unlock_irqrestore(&dev->irqlock, flags);
+			s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
+		} else {
+			mfc_debug(2, "EOS: marking last buffer of stream\n");
+			buf = list_entry(ctx->src_queue.prev,
+						struct s5p_mfc_buf, list);
+			if (buf->flags & MFC_BUF_FLAG_USED)
+				ctx->state = MFCINST_FINISHING;
+			else
+				buf->flags |= MFC_BUF_FLAG_EOS;
+			spin_unlock_irqrestore(&dev->irqlock, flags);
+		}
+		break;
+	default:
+		return -EINVAL;
+
+	}
+	return 0;
+}
+
+static int vidioc_subscribe_event(struct v4l2_fh *fh,
+				  const struct v4l2_event_subscription *sub)
+{
+	switch (sub->type) {
+	case V4L2_EVENT_EOS:
+		return v4l2_event_subscribe(fh, sub, 2, NULL);
+	default:
+		return -EINVAL;
+	}
+}
+
+static const struct v4l2_ioctl_ops s5p_mfc_enc_ioctl_ops = {
+	.vidioc_querycap = vidioc_querycap,
+	.vidioc_enum_fmt_vid_cap_mplane = vidioc_enum_fmt_vid_cap_mplane,
+	.vidioc_enum_fmt_vid_out_mplane = vidioc_enum_fmt_vid_out_mplane,
+	.vidioc_g_fmt_vid_cap_mplane = vidioc_g_fmt,
+	.vidioc_g_fmt_vid_out_mplane = vidioc_g_fmt,
+	.vidioc_try_fmt_vid_cap_mplane = vidioc_try_fmt,
+	.vidioc_try_fmt_vid_out_mplane = vidioc_try_fmt,
+	.vidioc_s_fmt_vid_cap_mplane = vidioc_s_fmt,
+	.vidioc_s_fmt_vid_out_mplane = vidioc_s_fmt,
+	.vidioc_reqbufs = vidioc_reqbufs,
+	.vidioc_querybuf = vidioc_querybuf,
+	.vidioc_qbuf = vidioc_qbuf,
+	.vidioc_dqbuf = vidioc_dqbuf,
+	.vidioc_expbuf = vidioc_expbuf,
+	.vidioc_streamon = vidioc_streamon,
+	.vidioc_streamoff = vidioc_streamoff,
+	.vidioc_s_parm = vidioc_s_parm,
+	.vidioc_g_parm = vidioc_g_parm,
+	.vidioc_encoder_cmd = vidioc_encoder_cmd,
+	.vidioc_subscribe_event = vidioc_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+};
+
+static int check_vb_with_fmt(struct s5p_mfc_fmt *fmt, struct vb2_buffer *vb)
+{
+	int i;
+
+	if (!fmt)
+		return -EINVAL;
+	if (fmt->num_planes != vb->num_planes) {
+		mfc_err("invalid plane number for the format\n");
+		return -EINVAL;
+	}
+	for (i = 0; i < fmt->num_planes; i++) {
+		dma_addr_t dma = vb2_dma_contig_plane_dma_addr(vb, i);
+		if (!dma) {
+			mfc_err("failed to get plane cookie\n");
+			return -EINVAL;
+		}
+		mfc_debug(2, "index: %d, plane[%d] cookie: %pad\n",
+			  vb->index, i, &dma);
+	}
+	return 0;
+}
+
+static int s5p_mfc_queue_setup(struct vb2_queue *vq,
+			const void *parg,
+			unsigned int *buf_count, unsigned int *plane_count,
+			unsigned int psize[], void *allocators[])
+{
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(vq->drv_priv);
+	struct s5p_mfc_dev *dev = ctx->dev;
+
+	if (vq->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		if (ctx->state != MFCINST_GOT_INST) {
+			mfc_err("invalid state: %d\n", ctx->state);
+			return -EINVAL;
+		}
+
+		if (ctx->dst_fmt)
+			*plane_count = ctx->dst_fmt->num_planes;
+		else
+			*plane_count = MFC_ENC_CAP_PLANE_COUNT;
+		if (*buf_count < 1)
+			*buf_count = 1;
+		if (*buf_count > MFC_MAX_BUFFERS)
+			*buf_count = MFC_MAX_BUFFERS;
+		psize[0] = ctx->enc_dst_buf_size;
+		allocators[0] = ctx->dev->alloc_ctx[MFC_BANK1_ALLOC_CTX];
+	} else if (vq->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		if (ctx->src_fmt)
+			*plane_count = ctx->src_fmt->num_planes;
+		else
+			*plane_count = MFC_ENC_OUT_PLANE_COUNT;
+
+		if (*buf_count < 1)
+			*buf_count = 1;
+		if (*buf_count > MFC_MAX_BUFFERS)
+			*buf_count = MFC_MAX_BUFFERS;
+
+		psize[0] = ctx->luma_size;
+		psize[1] = ctx->chroma_size;
+
+		if (IS_MFCV6_PLUS(dev)) {
+			allocators[0] =
+				ctx->dev->alloc_ctx[MFC_BANK1_ALLOC_CTX];
+			allocators[1] =
+				ctx->dev->alloc_ctx[MFC_BANK1_ALLOC_CTX];
+		} else {
+			allocators[0] =
+				ctx->dev->alloc_ctx[MFC_BANK2_ALLOC_CTX];
+			allocators[1] =
+				ctx->dev->alloc_ctx[MFC_BANK2_ALLOC_CTX];
+		}
+	} else {
+		mfc_err("invalid queue type: %d\n", vq->type);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int s5p_mfc_buf_init(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct vb2_queue *vq = vb->vb2_queue;
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(vq->drv_priv);
+	unsigned int i;
+	int ret;
+
+	if (vq->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		ret = check_vb_with_fmt(ctx->dst_fmt, vb);
+		if (ret < 0)
+			return ret;
+		i = vb->index;
+		ctx->dst_bufs[i].b = vbuf;
+		ctx->dst_bufs[i].cookie.stream =
+					vb2_dma_contig_plane_dma_addr(vb, 0);
+		ctx->dst_bufs_cnt++;
+	} else if (vq->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		ret = check_vb_with_fmt(ctx->src_fmt, vb);
+		if (ret < 0)
+			return ret;
+		i = vb->index;
+		ctx->src_bufs[i].b = vbuf;
+		ctx->src_bufs[i].cookie.raw.luma =
+					vb2_dma_contig_plane_dma_addr(vb, 0);
+		ctx->src_bufs[i].cookie.raw.chroma =
+					vb2_dma_contig_plane_dma_addr(vb, 1);
+		ctx->src_bufs_cnt++;
+	} else {
+		mfc_err("invalid queue type: %d\n", vq->type);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int s5p_mfc_buf_prepare(struct vb2_buffer *vb)
+{
+	struct vb2_queue *vq = vb->vb2_queue;
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(vq->drv_priv);
+	int ret;
+
+	if (vq->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		ret = check_vb_with_fmt(ctx->dst_fmt, vb);
+		if (ret < 0)
+			return ret;
+		mfc_debug(2, "plane size: %ld, dst size: %zu\n",
+			vb2_plane_size(vb, 0), ctx->enc_dst_buf_size);
+		if (vb2_plane_size(vb, 0) < ctx->enc_dst_buf_size) {
+			mfc_err("plane size is too small for capture\n");
+			return -EINVAL;
+		}
+	} else if (vq->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		ret = check_vb_with_fmt(ctx->src_fmt, vb);
+		if (ret < 0)
+			return ret;
+		mfc_debug(2, "plane size: %ld, luma size: %d\n",
+			vb2_plane_size(vb, 0), ctx->luma_size);
+		mfc_debug(2, "plane size: %ld, chroma size: %d\n",
+			vb2_plane_size(vb, 1), ctx->chroma_size);
+		if (vb2_plane_size(vb, 0) < ctx->luma_size ||
+		    vb2_plane_size(vb, 1) < ctx->chroma_size) {
+			mfc_err("plane size is too small for output\n");
+			return -EINVAL;
+		}
+	} else {
+		mfc_err("invalid queue type: %d\n", vq->type);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int s5p_mfc_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(q->drv_priv);
+	struct s5p_mfc_dev *dev = ctx->dev;
+
+	if (IS_MFCV6_PLUS(dev) &&
+			(q->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)) {
+
+		if ((ctx->state == MFCINST_GOT_INST) &&
+			(dev->curr_ctx == ctx->num) && dev->hw_lock) {
+			s5p_mfc_wait_for_done_ctx(ctx,
+						S5P_MFC_R2H_CMD_SEQ_DONE_RET,
+						0);
+		}
+
+		if (ctx->src_bufs_cnt < ctx->pb_count) {
+			mfc_err("Need minimum %d OUTPUT buffers\n",
+					ctx->pb_count);
+			return -ENOBUFS;
+		}
+	}
+
+	/* If context is ready then dev = work->data;schedule it to run */
+	if (s5p_mfc_ctx_ready(ctx))
+		set_work_bit_irqsave(ctx);
+	s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
+
+	return 0;
+}
+
+static void s5p_mfc_stop_streaming(struct vb2_queue *q)
+{
+	unsigned long flags;
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(q->drv_priv);
+	struct s5p_mfc_dev *dev = ctx->dev;
+
+	if ((ctx->state == MFCINST_FINISHING ||
+		ctx->state == MFCINST_RUNNING) &&
+		dev->curr_ctx == ctx->num && dev->hw_lock) {
+		ctx->state = MFCINST_ABORT;
+		s5p_mfc_wait_for_done_ctx(ctx, S5P_MFC_R2H_CMD_FRAME_DONE_RET,
+					  0);
+	}
+	ctx->state = MFCINST_FINISHED;
+	spin_lock_irqsave(&dev->irqlock, flags);
+	if (q->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		s5p_mfc_hw_call_void(dev->mfc_ops, cleanup_queue,
+						&ctx->dst_queue, &ctx->vq_dst);
+		INIT_LIST_HEAD(&ctx->dst_queue);
+		ctx->dst_queue_cnt = 0;
+	}
+	if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		cleanup_ref_queue(ctx);
+		s5p_mfc_hw_call_void(dev->mfc_ops, cleanup_queue, &ctx->src_queue,
+				&ctx->vq_src);
+		INIT_LIST_HEAD(&ctx->src_queue);
+		ctx->src_queue_cnt = 0;
+	}
+	spin_unlock_irqrestore(&dev->irqlock, flags);
+}
+
+static void s5p_mfc_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_queue *vq = vb->vb2_queue;
+	struct s5p_mfc_ctx *ctx = fh_to_ctx(vq->drv_priv);
+	struct s5p_mfc_dev *dev = ctx->dev;
+	unsigned long flags;
+	struct s5p_mfc_buf *mfc_buf;
+
+	if (ctx->state == MFCINST_ERROR) {
+		vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
+		cleanup_ref_queue(ctx);
+		return;
+	}
+	if (vq->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
+		mfc_buf = &ctx->dst_bufs[vb->index];
+		mfc_buf->flags &= ~MFC_BUF_FLAG_USED;
+		/* Mark destination as available for use by MFC */
+		spin_lock_irqsave(&dev->irqlock, flags);
+		list_add_tail(&mfc_buf->list, &ctx->dst_queue);
+		ctx->dst_queue_cnt++;
+		spin_unlock_irqrestore(&dev->irqlock, flags);
+	} else if (vq->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		mfc_buf = &ctx->src_bufs[vb->index];
+		mfc_buf->flags &= ~MFC_BUF_FLAG_USED;
+		spin_lock_irqsave(&dev->irqlock, flags);
+		list_add_tail(&mfc_buf->list, &ctx->src_queue);
+		ctx->src_queue_cnt++;
+		spin_unlock_irqrestore(&dev->irqlock, flags);
+	} else {
+		mfc_err("unsupported buffer type (%d)\n", vq->type);
+	}
+	if (s5p_mfc_ctx_ready(ctx))
+		set_work_bit_irqsave(ctx);
+	s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
+}
+
+static struct vb2_ops s5p_mfc_enc_qops = {
+	.queue_setup		= s5p_mfc_queue_setup,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
+	.buf_init		= s5p_mfc_buf_init,
+	.buf_prepare		= s5p_mfc_buf_prepare,
+	.start_streaming	= s5p_mfc_start_streaming,
+	.stop_streaming		= s5p_mfc_stop_streaming,
+	.buf_queue		= s5p_mfc_buf_queue,
+};
+
+struct s5p_mfc_codec_ops *get_enc_codec_ops(void)
+{
+	return &encoder_codec_ops;
+}
+
+struct vb2_ops *get_enc_queue_ops(void)
+{
+	return &s5p_mfc_enc_qops;
+}
+
+const struct v4l2_ioctl_ops *get_enc_v4l2_ioctl_ops(void)
+{
+	return &s5p_mfc_enc_ioctl_ops;
+}
+
+#define IS_MFC51_PRIV(x) ((V4L2_CTRL_ID2CLASS(x) == V4L2_CTRL_CLASS_MPEG) \
+						&& V4L2_CTRL_DRIVER_PRIV(x))
+
+int s5p_mfc_enc_ctrls_setup(struct s5p_mfc_ctx *ctx)
+{
+	struct v4l2_ctrl_config cfg;
+	int i;
+
+	v4l2_ctrl_handler_init(&ctx->ctrl_handler, NUM_CTRLS);
+	if (ctx->ctrl_handler.error) {
+		mfc_err("v4l2_ctrl_handler_init failed\n");
+		return ctx->ctrl_handler.error;
+	}
+	for (i = 0; i < NUM_CTRLS; i++) {
+		if (IS_MFC51_PRIV(controls[i].id)) {
+			memset(&cfg, 0, sizeof(struct v4l2_ctrl_config));
+			cfg.ops = &s5p_mfc_enc_ctrl_ops;
+			cfg.id = controls[i].id;
+			cfg.min = controls[i].minimum;
+			cfg.max = controls[i].maximum;
+			cfg.def = controls[i].default_value;
+			cfg.name = controls[i].name;
+			cfg.type = controls[i].type;
+			cfg.flags = 0;
+
+			if (cfg.type == V4L2_CTRL_TYPE_MENU) {
+				cfg.step = 0;
+				cfg.menu_skip_mask = cfg.menu_skip_mask;
+				cfg.qmenu = mfc51_get_menu(cfg.id);
+			} else {
+				cfg.step = controls[i].step;
+				cfg.menu_skip_mask = 0;
+			}
+			ctx->ctrls[i] = v4l2_ctrl_new_custom(&ctx->ctrl_handler,
+					&cfg, NULL);
+		} else {
+			if ((controls[i].type == V4L2_CTRL_TYPE_MENU) ||
+				(controls[i].type ==
+					V4L2_CTRL_TYPE_INTEGER_MENU)) {
+				ctx->ctrls[i] = v4l2_ctrl_new_std_menu(
+					&ctx->ctrl_handler,
+					&s5p_mfc_enc_ctrl_ops, controls[i].id,
+					controls[i].maximum, 0,
+					controls[i].default_value);
+			} else {
+				ctx->ctrls[i] = v4l2_ctrl_new_std(
+					&ctx->ctrl_handler,
+					&s5p_mfc_enc_ctrl_ops, controls[i].id,
+					controls[i].minimum,
+					controls[i].maximum, controls[i].step,
+					controls[i].default_value);
+			}
+		}
+		if (ctx->ctrl_handler.error) {
+			mfc_err("Adding control (%d) failed\n", i);
+			return ctx->ctrl_handler.error;
+		}
+		if (controls[i].is_volatile && ctx->ctrls[i])
+			ctx->ctrls[i]->flags |= V4L2_CTRL_FLAG_VOLATILE;
+	}
+	v4l2_ctrl_handler_setup(&ctx->ctrl_handler);
+	return 0;
+}
+
+void s5p_mfc_enc_ctrls_delete(struct s5p_mfc_ctx *ctx)
+{
+	int i;
+
+	v4l2_ctrl_handler_free(&ctx->ctrl_handler);
+	for (i = 0; i < NUM_CTRLS; i++)
+		ctx->ctrls[i] = NULL;
+}
+
+void s5p_mfc_enc_init(struct s5p_mfc_ctx *ctx)
+{
+	struct v4l2_format f;
+	f.fmt.pix_mp.pixelformat = DEF_SRC_FMT_ENC;
+	ctx->src_fmt = find_format(&f, MFC_FMT_RAW);
+	f.fmt.pix_mp.pixelformat = DEF_DST_FMT_ENC;
+	ctx->dst_fmt = find_format(&f, MFC_FMT_ENC);
+}
+
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_enc.h b/drivers/media/platform/s5p-mfc/s5p_mfc_enc.h
new file mode 100644
index 0000000..5118d46
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_enc.h
@@ -0,0 +1,24 @@
+/*
+ * linux/drivers/media/platform/s5p-mfc/s5p_mfc_enc.h
+ *
+ * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#ifndef S5P_MFC_ENC_H_
+#define S5P_MFC_ENC_H_
+
+struct s5p_mfc_codec_ops *get_enc_codec_ops(void);
+struct vb2_ops *get_enc_queue_ops(void);
+const struct v4l2_ioctl_ops *get_enc_v4l2_ioctl_ops(void);
+struct s5p_mfc_fmt *get_enc_def_fmt(bool src);
+int s5p_mfc_enc_ctrls_setup(struct s5p_mfc_ctx *ctx);
+void s5p_mfc_enc_ctrls_delete(struct s5p_mfc_ctx *ctx);
+void s5p_mfc_enc_init(struct s5p_mfc_ctx *ctx);
+
+#endif /* S5P_MFC_ENC_H_  */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_intr.c b/drivers/media/platform/s5p-mfc/s5p_mfc_intr.c
new file mode 100644
index 0000000..5b8f0e0
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_intr.c
@@ -0,0 +1,91 @@
+/*
+ * drivers/media/platform/samsung/mfc5/s5p_mfc_intr.c
+ *
+ * C file for Samsung MFC (Multi Function Codec - FIMV) driver
+ * This file contains functions used to wait for command completion.
+ *
+ * Kamil Debski, Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ * http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/io.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include "s5p_mfc_common.h"
+#include "s5p_mfc_debug.h"
+#include "s5p_mfc_intr.h"
+
+int s5p_mfc_wait_for_done_dev(struct s5p_mfc_dev *dev, int command)
+{
+	int ret;
+
+	ret = wait_event_interruptible_timeout(dev->queue,
+		(dev->int_cond && (dev->int_type == command
+		|| dev->int_type == S5P_MFC_R2H_CMD_ERR_RET)),
+		msecs_to_jiffies(MFC_INT_TIMEOUT));
+	if (ret == 0) {
+		mfc_err("Interrupt (dev->int_type:%d, command:%d) timed out\n",
+							dev->int_type, command);
+		return 1;
+	} else if (ret == -ERESTARTSYS) {
+		mfc_err("Interrupted by a signal\n");
+		return 1;
+	}
+	mfc_debug(1, "Finished waiting (dev->int_type:%d, command: %d)\n",
+							dev->int_type, command);
+	if (dev->int_type == S5P_MFC_R2H_CMD_ERR_RET)
+		return 1;
+	return 0;
+}
+
+void s5p_mfc_clean_dev_int_flags(struct s5p_mfc_dev *dev)
+{
+	dev->int_cond = 0;
+	dev->int_type = 0;
+	dev->int_err = 0;
+}
+
+int s5p_mfc_wait_for_done_ctx(struct s5p_mfc_ctx *ctx,
+				    int command, int interrupt)
+{
+	int ret;
+
+	if (interrupt) {
+		ret = wait_event_interruptible_timeout(ctx->queue,
+				(ctx->int_cond && (ctx->int_type == command
+			|| ctx->int_type == S5P_MFC_R2H_CMD_ERR_RET)),
+					msecs_to_jiffies(MFC_INT_TIMEOUT));
+	} else {
+		ret = wait_event_timeout(ctx->queue,
+				(ctx->int_cond && (ctx->int_type == command
+			|| ctx->int_type == S5P_MFC_R2H_CMD_ERR_RET)),
+					msecs_to_jiffies(MFC_INT_TIMEOUT));
+	}
+	if (ret == 0) {
+		mfc_err("Interrupt (ctx->int_type:%d, command:%d) timed out\n",
+							ctx->int_type, command);
+		return 1;
+	} else if (ret == -ERESTARTSYS) {
+		mfc_err("Interrupted by a signal\n");
+		return 1;
+	}
+	mfc_debug(1, "Finished waiting (ctx->int_type:%d, command: %d)\n",
+							ctx->int_type, command);
+	if (ctx->int_type == S5P_MFC_R2H_CMD_ERR_RET)
+		return 1;
+	return 0;
+}
+
+void s5p_mfc_clean_ctx_int_flags(struct s5p_mfc_ctx *ctx)
+{
+	ctx->int_cond = 0;
+	ctx->int_type = 0;
+	ctx->int_err = 0;
+}
+
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_intr.h b/drivers/media/platform/s5p-mfc/s5p_mfc_intr.h
new file mode 100644
index 0000000..18341a8
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_intr.h
@@ -0,0 +1,26 @@
+/*
+ * drivers/media/platform/samsung/mfc5/s5p_mfc_intr.h
+ *
+ * Header file for Samsung MFC (Multi Function Codec - FIMV) driver
+ * It contains waiting functions declarations.
+ *
+ * Kamil Debski, Copyright (C) 2011 Samsung Electronics
+ * http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef S5P_MFC_INTR_H_
+#define S5P_MFC_INTR_H_
+
+#include "s5p_mfc_common.h"
+
+int s5p_mfc_wait_for_done_ctx(struct s5p_mfc_ctx *ctx,
+			      int command, int interrupt);
+int s5p_mfc_wait_for_done_dev(struct s5p_mfc_dev *dev, int command);
+void s5p_mfc_clean_ctx_int_flags(struct s5p_mfc_ctx *ctx);
+void s5p_mfc_clean_dev_int_flags(struct s5p_mfc_dev *dev);
+
+#endif /* S5P_MFC_INTR_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr.c b/drivers/media/platform/s5p-mfc/s5p_mfc_opr.c
new file mode 100644
index 0000000..1e72502
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr.c
@@ -0,0 +1,74 @@
+/*
+ * drivers/media/platform/s5p-mfc/s5p_mfc_opr.c
+ *
+ * Samsung MFC (Multi Function Codec - FIMV) driver
+ * This file contains hw related functions.
+ *
+ * Kamil Debski, Copyright (c) 2012 Samsung Electronics Co., Ltd.
+ * http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include "s5p_mfc_debug.h"
+#include "s5p_mfc_opr.h"
+#include "s5p_mfc_opr_v5.h"
+#include "s5p_mfc_opr_v6.h"
+
+static struct s5p_mfc_hw_ops *s5p_mfc_ops;
+
+void s5p_mfc_init_hw_ops(struct s5p_mfc_dev *dev)
+{
+	if (IS_MFCV6_PLUS(dev)) {
+		s5p_mfc_ops = s5p_mfc_init_hw_ops_v6();
+		dev->warn_start = S5P_FIMV_ERR_WARNINGS_START_V6;
+	} else {
+		s5p_mfc_ops = s5p_mfc_init_hw_ops_v5();
+		dev->warn_start = S5P_FIMV_ERR_WARNINGS_START;
+	}
+	dev->mfc_ops = s5p_mfc_ops;
+}
+
+void s5p_mfc_init_regs(struct s5p_mfc_dev *dev)
+{
+	if (IS_MFCV6_PLUS(dev))
+		dev->mfc_regs = s5p_mfc_init_regs_v6_plus(dev);
+}
+
+int s5p_mfc_alloc_priv_buf(struct device *dev, dma_addr_t base,
+					struct s5p_mfc_priv_buf *b)
+{
+	mfc_debug(3, "Allocating priv: %zu\n", b->size);
+
+	b->virt = dma_alloc_coherent(dev, b->size, &b->dma, GFP_KERNEL);
+
+	if (!b->virt) {
+		mfc_err("Allocating private buffer failed\n");
+		return -ENOMEM;
+	}
+
+	if (b->dma < base) {
+		mfc_err("Invaling memory configuration!\n");
+		mfc_err("Allocated buffer (%pad) is lower than memory base address (%pad)\n",
+			&b->dma, &base);
+		dma_free_coherent(dev, b->size, b->virt, b->dma);
+		return -ENOMEM;
+	}
+
+	mfc_debug(3, "Allocated addr %p %pad\n", b->virt, &b->dma);
+	return 0;
+}
+
+void s5p_mfc_release_priv_buf(struct device *dev,
+						struct s5p_mfc_priv_buf *b)
+{
+	if (b->virt) {
+		dma_free_coherent(dev, b->size, b->virt, b->dma);
+		b->virt = NULL;
+		b->dma = 0;
+		b->size = 0;
+	}
+}
+
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr.h b/drivers/media/platform/s5p-mfc/s5p_mfc_opr.h
new file mode 100644
index 0000000..77a08b1
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr.h
@@ -0,0 +1,343 @@
+/*
+ * drivers/media/platform/s5p-mfc/s5p_mfc_opr.h
+ *
+ * Header file for Samsung MFC (Multi Function Codec - FIMV) driver
+ * Contains declarations of hw related functions.
+ *
+ * Kamil Debski, Copyright (C) 2012 Samsung Electronics Co., Ltd.
+ * http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef S5P_MFC_OPR_H_
+#define S5P_MFC_OPR_H_
+
+#include "s5p_mfc_common.h"
+
+struct s5p_mfc_regs {
+
+	/* codec common registers */
+	volatile void __iomem *risc_on;
+	volatile void __iomem *risc2host_int;
+	volatile void __iomem *host2risc_int;
+	volatile void __iomem *risc_base_address;
+	volatile void __iomem *mfc_reset;
+	volatile void __iomem *host2risc_command;
+	volatile void __iomem *risc2host_command;
+	volatile void __iomem *mfc_bus_reset_ctrl;
+	volatile void __iomem *firmware_version;
+	volatile void __iomem *instance_id;
+	volatile void __iomem *codec_type;
+	volatile void __iomem *context_mem_addr;
+	volatile void __iomem *context_mem_size;
+	volatile void __iomem *pixel_format;
+	volatile void __iomem *metadata_enable;
+	volatile void __iomem *mfc_version;
+	volatile void __iomem *dbg_info_enable;
+	volatile void __iomem *dbg_buffer_addr;
+	volatile void __iomem *dbg_buffer_size;
+	volatile void __iomem *hed_control;
+	volatile void __iomem *mfc_timeout_value;
+	volatile void __iomem *hed_shared_mem_addr;
+	volatile void __iomem *dis_shared_mem_addr;/* only v7 */
+	volatile void __iomem *ret_instance_id;
+	volatile void __iomem *error_code;
+	volatile void __iomem *dbg_buffer_output_size;
+	volatile void __iomem *metadata_status;
+	volatile void __iomem *metadata_addr_mb_info;
+	volatile void __iomem *metadata_size_mb_info;
+	volatile void __iomem *dbg_info_stage_counter;
+
+	/* decoder registers */
+	volatile void __iomem *d_crc_ctrl;
+	volatile void __iomem *d_dec_options;
+	volatile void __iomem *d_display_delay;
+	volatile void __iomem *d_set_frame_width;
+	volatile void __iomem *d_set_frame_height;
+	volatile void __iomem *d_sei_enable;
+	volatile void __iomem *d_min_num_dpb;
+	volatile void __iomem *d_min_first_plane_dpb_size;
+	volatile void __iomem *d_min_second_plane_dpb_size;
+	volatile void __iomem *d_min_third_plane_dpb_size;/* only v8 */
+	volatile void __iomem *d_min_num_mv;
+	volatile void __iomem *d_mvc_num_views;
+	volatile void __iomem *d_min_num_dis;/* only v7 */
+	volatile void __iomem *d_min_first_dis_size;/* only v7 */
+	volatile void __iomem *d_min_second_dis_size;/* only v7 */
+	volatile void __iomem *d_min_third_dis_size;/* only v7 */
+	volatile void __iomem *d_post_filter_luma_dpb0;/*  v7 and v8 */
+	volatile void __iomem *d_post_filter_luma_dpb1;/* v7 and v8 */
+	volatile void __iomem *d_post_filter_luma_dpb2;/* only v7 */
+	volatile void __iomem *d_post_filter_chroma_dpb0;/* v7 and v8 */
+	volatile void __iomem *d_post_filter_chroma_dpb1;/* v7 and v8 */
+	volatile void __iomem *d_post_filter_chroma_dpb2;/* only v7 */
+	volatile void __iomem *d_num_dpb;
+	volatile void __iomem *d_num_mv;
+	volatile void __iomem *d_init_buffer_options;
+	volatile void __iomem *d_first_plane_dpb_stride_size;/* only v8 */
+	volatile void __iomem *d_second_plane_dpb_stride_size;/* only v8 */
+	volatile void __iomem *d_third_plane_dpb_stride_size;/* only v8 */
+	volatile void __iomem *d_first_plane_dpb_size;
+	volatile void __iomem *d_second_plane_dpb_size;
+	volatile void __iomem *d_third_plane_dpb_size;/* only v8 */
+	volatile void __iomem *d_mv_buffer_size;
+	volatile void __iomem *d_first_plane_dpb;
+	volatile void __iomem *d_second_plane_dpb;
+	volatile void __iomem *d_third_plane_dpb;
+	volatile void __iomem *d_mv_buffer;
+	volatile void __iomem *d_scratch_buffer_addr;
+	volatile void __iomem *d_scratch_buffer_size;
+	volatile void __iomem *d_metadata_buffer_addr;
+	volatile void __iomem *d_metadata_buffer_size;
+	volatile void __iomem *d_nal_start_options;/* v7 and v8 */
+	volatile void __iomem *d_cpb_buffer_addr;
+	volatile void __iomem *d_cpb_buffer_size;
+	volatile void __iomem *d_available_dpb_flag_upper;
+	volatile void __iomem *d_available_dpb_flag_lower;
+	volatile void __iomem *d_cpb_buffer_offset;
+	volatile void __iomem *d_slice_if_enable;
+	volatile void __iomem *d_picture_tag;
+	volatile void __iomem *d_stream_data_size;
+	volatile void __iomem *d_dynamic_dpb_flag_upper;/* v7 and v8 */
+	volatile void __iomem *d_dynamic_dpb_flag_lower;/* v7 and v8 */
+	volatile void __iomem *d_display_frame_width;
+	volatile void __iomem *d_display_frame_height;
+	volatile void __iomem *d_display_status;
+	volatile void __iomem *d_display_first_plane_addr;
+	volatile void __iomem *d_display_second_plane_addr;
+	volatile void __iomem *d_display_third_plane_addr;/* only v8 */
+	volatile void __iomem *d_display_frame_type;
+	volatile void __iomem *d_display_crop_info1;
+	volatile void __iomem *d_display_crop_info2;
+	volatile void __iomem *d_display_picture_profile;
+	volatile void __iomem *d_display_luma_crc;/* v7 and v8 */
+	volatile void __iomem *d_display_chroma0_crc;/* v7 and v8 */
+	volatile void __iomem *d_display_chroma1_crc;/* only v8 */
+	volatile void __iomem *d_display_luma_crc_top;/* only v6 */
+	volatile void __iomem *d_display_chroma_crc_top;/* only v6 */
+	volatile void __iomem *d_display_luma_crc_bot;/* only v6 */
+	volatile void __iomem *d_display_chroma_crc_bot;/* only v6 */
+	volatile void __iomem *d_display_aspect_ratio;
+	volatile void __iomem *d_display_extended_ar;
+	volatile void __iomem *d_decoded_frame_width;
+	volatile void __iomem *d_decoded_frame_height;
+	volatile void __iomem *d_decoded_status;
+	volatile void __iomem *d_decoded_first_plane_addr;
+	volatile void __iomem *d_decoded_second_plane_addr;
+	volatile void __iomem *d_decoded_third_plane_addr;/* only v8 */
+	volatile void __iomem *d_decoded_frame_type;
+	volatile void __iomem *d_decoded_crop_info1;
+	volatile void __iomem *d_decoded_crop_info2;
+	volatile void __iomem *d_decoded_picture_profile;
+	volatile void __iomem *d_decoded_nal_size;
+	volatile void __iomem *d_decoded_luma_crc;
+	volatile void __iomem *d_decoded_chroma0_crc;
+	volatile void __iomem *d_decoded_chroma1_crc;/* only v8 */
+	volatile void __iomem *d_ret_picture_tag_top;
+	volatile void __iomem *d_ret_picture_tag_bot;
+	volatile void __iomem *d_ret_picture_time_top;
+	volatile void __iomem *d_ret_picture_time_bot;
+	volatile void __iomem *d_chroma_format;
+	volatile void __iomem *d_vc1_info;/* v7 and v8 */
+	volatile void __iomem *d_mpeg4_info;
+	volatile void __iomem *d_h264_info;
+	volatile void __iomem *d_metadata_addr_concealed_mb;
+	volatile void __iomem *d_metadata_size_concealed_mb;
+	volatile void __iomem *d_metadata_addr_vc1_param;
+	volatile void __iomem *d_metadata_size_vc1_param;
+	volatile void __iomem *d_metadata_addr_sei_nal;
+	volatile void __iomem *d_metadata_size_sei_nal;
+	volatile void __iomem *d_metadata_addr_vui;
+	volatile void __iomem *d_metadata_size_vui;
+	volatile void __iomem *d_metadata_addr_mvcvui;/* v7 and v8 */
+	volatile void __iomem *d_metadata_size_mvcvui;/* v7 and v8 */
+	volatile void __iomem *d_mvc_view_id;
+	volatile void __iomem *d_frame_pack_sei_avail;
+	volatile void __iomem *d_frame_pack_arrgment_id;
+	volatile void __iomem *d_frame_pack_sei_info;
+	volatile void __iomem *d_frame_pack_grid_pos;
+	volatile void __iomem *d_display_recovery_sei_info;/* v7 and v8 */
+	volatile void __iomem *d_decoded_recovery_sei_info;/* v7 and v8 */
+	volatile void __iomem *d_display_first_addr;/* only v7 */
+	volatile void __iomem *d_display_second_addr;/* only v7 */
+	volatile void __iomem *d_display_third_addr;/* only v7 */
+	volatile void __iomem *d_decoded_first_addr;/* only v7 */
+	volatile void __iomem *d_decoded_second_addr;/* only v7 */
+	volatile void __iomem *d_decoded_third_addr;/* only v7 */
+	volatile void __iomem *d_used_dpb_flag_upper;/* v7 and v8 */
+	volatile void __iomem *d_used_dpb_flag_lower;/* v7 and v8 */
+
+	/* encoder registers */
+	volatile void __iomem *e_frame_width;
+	volatile void __iomem *e_frame_height;
+	volatile void __iomem *e_cropped_frame_width;
+	volatile void __iomem *e_cropped_frame_height;
+	volatile void __iomem *e_frame_crop_offset;
+	volatile void __iomem *e_enc_options;
+	volatile void __iomem *e_picture_profile;
+	volatile void __iomem *e_vbv_buffer_size;
+	volatile void __iomem *e_vbv_init_delay;
+	volatile void __iomem *e_fixed_picture_qp;
+	volatile void __iomem *e_rc_config;
+	volatile void __iomem *e_rc_qp_bound;
+	volatile void __iomem *e_rc_qp_bound_pb;/* v7 and v8 */
+	volatile void __iomem *e_rc_mode;
+	volatile void __iomem *e_mb_rc_config;
+	volatile void __iomem *e_padding_ctrl;
+	volatile void __iomem *e_air_threshold;
+	volatile void __iomem *e_mv_hor_range;
+	volatile void __iomem *e_mv_ver_range;
+	volatile void __iomem *e_num_dpb;
+	volatile void __iomem *e_luma_dpb;
+	volatile void __iomem *e_chroma_dpb;
+	volatile void __iomem *e_me_buffer;
+	volatile void __iomem *e_scratch_buffer_addr;
+	volatile void __iomem *e_scratch_buffer_size;
+	volatile void __iomem *e_tmv_buffer0;
+	volatile void __iomem *e_tmv_buffer1;
+	volatile void __iomem *e_ir_buffer_addr;/* v7 and v8 */
+	volatile void __iomem *e_source_first_plane_addr;
+	volatile void __iomem *e_source_second_plane_addr;
+	volatile void __iomem *e_source_third_plane_addr;/* v7 and v8 */
+	volatile void __iomem *e_source_first_plane_stride;/* v7 and v8 */
+	volatile void __iomem *e_source_second_plane_stride;/* v7 and v8 */
+	volatile void __iomem *e_source_third_plane_stride;/* v7 and v8 */
+	volatile void __iomem *e_stream_buffer_addr;
+	volatile void __iomem *e_stream_buffer_size;
+	volatile void __iomem *e_roi_buffer_addr;
+	volatile void __iomem *e_param_change;
+	volatile void __iomem *e_ir_size;
+	volatile void __iomem *e_gop_config;
+	volatile void __iomem *e_mslice_mode;
+	volatile void __iomem *e_mslice_size_mb;
+	volatile void __iomem *e_mslice_size_bits;
+	volatile void __iomem *e_frame_insertion;
+	volatile void __iomem *e_rc_frame_rate;
+	volatile void __iomem *e_rc_bit_rate;
+	volatile void __iomem *e_rc_roi_ctrl;
+	volatile void __iomem *e_picture_tag;
+	volatile void __iomem *e_bit_count_enable;
+	volatile void __iomem *e_max_bit_count;
+	volatile void __iomem *e_min_bit_count;
+	volatile void __iomem *e_metadata_buffer_addr;
+	volatile void __iomem *e_metadata_buffer_size;
+	volatile void __iomem *e_encoded_source_first_plane_addr;
+	volatile void __iomem *e_encoded_source_second_plane_addr;
+	volatile void __iomem *e_encoded_source_third_plane_addr;/* v7 and v8 */
+	volatile void __iomem *e_stream_size;
+	volatile void __iomem *e_slice_type;
+	volatile void __iomem *e_picture_count;
+	volatile void __iomem *e_ret_picture_tag;
+	volatile void __iomem *e_stream_buffer_write_pointer; /*  only v6 */
+	volatile void __iomem *e_recon_luma_dpb_addr;
+	volatile void __iomem *e_recon_chroma_dpb_addr;
+	volatile void __iomem *e_metadata_addr_enc_slice;
+	volatile void __iomem *e_metadata_size_enc_slice;
+	volatile void __iomem *e_mpeg4_options;
+	volatile void __iomem *e_mpeg4_hec_period;
+	volatile void __iomem *e_aspect_ratio;
+	volatile void __iomem *e_extended_sar;
+	volatile void __iomem *e_h264_options;
+	volatile void __iomem *e_h264_options_2;/* v7 and v8 */
+	volatile void __iomem *e_h264_lf_alpha_offset;
+	volatile void __iomem *e_h264_lf_beta_offset;
+	volatile void __iomem *e_h264_i_period;
+	volatile void __iomem *e_h264_fmo_slice_grp_map_type;
+	volatile void __iomem *e_h264_fmo_num_slice_grp_minus1;
+	volatile void __iomem *e_h264_fmo_slice_grp_change_dir;
+	volatile void __iomem *e_h264_fmo_slice_grp_change_rate_minus1;
+	volatile void __iomem *e_h264_fmo_run_length_minus1_0;
+	volatile void __iomem *e_h264_aso_slice_order_0;
+	volatile void __iomem *e_h264_chroma_qp_offset;
+	volatile void __iomem *e_h264_num_t_layer;
+	volatile void __iomem *e_h264_hierarchical_qp_layer0;
+	volatile void __iomem *e_h264_frame_packing_sei_info;
+	volatile void __iomem *e_h264_nal_control;/* v7 and v8 */
+	volatile void __iomem *e_mvc_frame_qp_view1;
+	volatile void __iomem *e_mvc_rc_bit_rate_view1;
+	volatile void __iomem *e_mvc_rc_qbound_view1;
+	volatile void __iomem *e_mvc_rc_mode_view1;
+	volatile void __iomem *e_mvc_inter_view_prediction_on;
+	volatile void __iomem *e_vp8_options;/* v7 and v8 */
+	volatile void __iomem *e_vp8_filter_options;/* v7 and v8 */
+	volatile void __iomem *e_vp8_golden_frame_option;/* v7 and v8 */
+	volatile void __iomem *e_vp8_num_t_layer;/* v7 and v8 */
+	volatile void __iomem *e_vp8_hierarchical_qp_layer0;/* v7 and v8 */
+	volatile void __iomem *e_vp8_hierarchical_qp_layer1;/* v7 and v8 */
+	volatile void __iomem *e_vp8_hierarchical_qp_layer2;/* v7 and v8 */
+};
+
+struct s5p_mfc_hw_ops {
+	int (*alloc_dec_temp_buffers)(struct s5p_mfc_ctx *ctx);
+	void (*release_dec_desc_buffer)(struct s5p_mfc_ctx *ctx);
+	int (*alloc_codec_buffers)(struct s5p_mfc_ctx *ctx);
+	void (*release_codec_buffers)(struct s5p_mfc_ctx *ctx);
+	int (*alloc_instance_buffer)(struct s5p_mfc_ctx *ctx);
+	void (*release_instance_buffer)(struct s5p_mfc_ctx *ctx);
+	int (*alloc_dev_context_buffer)(struct s5p_mfc_dev *dev);
+	void (*release_dev_context_buffer)(struct s5p_mfc_dev *dev);
+	void (*dec_calc_dpb_size)(struct s5p_mfc_ctx *ctx);
+	void (*enc_calc_src_size)(struct s5p_mfc_ctx *ctx);
+	int (*set_dec_stream_buffer)(struct s5p_mfc_ctx *ctx,
+			int buf_addr, unsigned int start_num_byte,
+			unsigned int buf_size);
+	int (*set_dec_frame_buffer)(struct s5p_mfc_ctx *ctx);
+	int (*set_enc_stream_buffer)(struct s5p_mfc_ctx *ctx,
+			unsigned long addr, unsigned int size);
+	void (*set_enc_frame_buffer)(struct s5p_mfc_ctx *ctx,
+			unsigned long y_addr, unsigned long c_addr);
+	void (*get_enc_frame_buffer)(struct s5p_mfc_ctx *ctx,
+			unsigned long *y_addr, unsigned long *c_addr);
+	int (*set_enc_ref_buffer)(struct s5p_mfc_ctx *ctx);
+	int (*init_decode)(struct s5p_mfc_ctx *ctx);
+	int (*init_encode)(struct s5p_mfc_ctx *ctx);
+	int (*encode_one_frame)(struct s5p_mfc_ctx *ctx);
+	void (*try_run)(struct s5p_mfc_dev *dev);
+	void (*cleanup_queue)(struct list_head *lh,
+			struct vb2_queue *vq);
+	void (*clear_int_flags)(struct s5p_mfc_dev *dev);
+	void (*write_info)(struct s5p_mfc_ctx *ctx, unsigned int data,
+			unsigned int ofs);
+	unsigned int (*read_info)(struct s5p_mfc_ctx *ctx,
+			unsigned long ofs);
+	int (*get_dspl_y_adr)(struct s5p_mfc_dev *dev);
+	int (*get_dec_y_adr)(struct s5p_mfc_dev *dev);
+	int (*get_dspl_status)(struct s5p_mfc_dev *dev);
+	int (*get_dec_status)(struct s5p_mfc_dev *dev);
+	int (*get_dec_frame_type)(struct s5p_mfc_dev *dev);
+	int (*get_disp_frame_type)(struct s5p_mfc_ctx *ctx);
+	int (*get_consumed_stream)(struct s5p_mfc_dev *dev);
+	int (*get_int_reason)(struct s5p_mfc_dev *dev);
+	int (*get_int_err)(struct s5p_mfc_dev *dev);
+	int (*err_dec)(unsigned int err);
+	int (*err_dspl)(unsigned int err);
+	int (*get_img_width)(struct s5p_mfc_dev *dev);
+	int (*get_img_height)(struct s5p_mfc_dev *dev);
+	int (*get_dpb_count)(struct s5p_mfc_dev *dev);
+	int (*get_mv_count)(struct s5p_mfc_dev *dev);
+	int (*get_inst_no)(struct s5p_mfc_dev *dev);
+	int (*get_enc_strm_size)(struct s5p_mfc_dev *dev);
+	int (*get_enc_slice_type)(struct s5p_mfc_dev *dev);
+	int (*get_enc_dpb_count)(struct s5p_mfc_dev *dev);
+	int (*get_enc_pic_count)(struct s5p_mfc_dev *dev);
+	int (*get_sei_avail_status)(struct s5p_mfc_ctx *ctx);
+	int (*get_mvc_num_views)(struct s5p_mfc_dev *dev);
+	int (*get_mvc_view_id)(struct s5p_mfc_dev *dev);
+	unsigned int (*get_pic_type_top)(struct s5p_mfc_ctx *ctx);
+	unsigned int (*get_pic_type_bot)(struct s5p_mfc_ctx *ctx);
+	unsigned int (*get_crop_info_h)(struct s5p_mfc_ctx *ctx);
+	unsigned int (*get_crop_info_v)(struct s5p_mfc_ctx *ctx);
+};
+
+void s5p_mfc_init_hw_ops(struct s5p_mfc_dev *dev);
+void s5p_mfc_init_regs(struct s5p_mfc_dev *dev);
+int s5p_mfc_alloc_priv_buf(struct device *dev, dma_addr_t base,
+					struct s5p_mfc_priv_buf *b);
+void s5p_mfc_release_priv_buf(struct device *dev,
+					struct s5p_mfc_priv_buf *b);
+
+
+#endif /* S5P_MFC_OPR_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c
new file mode 100644
index 0000000..873c933
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c
@@ -0,0 +1,1737 @@
+/*
+ * drivers/media/platform/samsung/mfc5/s5p_mfc_opr_v5.c
+ *
+ * Samsung MFC (Multi Function Codec - FIMV) driver
+ * This file contains hw related functions.
+ *
+ * Kamil Debski, Copyright (c) 2011 Samsung Electronics
+ * http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include "s5p_mfc_common.h"
+#include "s5p_mfc_cmd.h"
+#include "s5p_mfc_ctrl.h"
+#include "s5p_mfc_debug.h"
+#include "s5p_mfc_intr.h"
+#include "s5p_mfc_pm.h"
+#include "s5p_mfc_opr.h"
+#include "s5p_mfc_opr_v5.h"
+#include <asm/cacheflush.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/err.h>
+#include <linux/firmware.h>
+#include <linux/io.h>
+#include <linux/jiffies.h>
+#include <linux/mm.h>
+#include <linux/sched.h>
+
+#define OFFSETA(x)		(((x) - dev->bank1) >> MFC_OFFSET_SHIFT)
+#define OFFSETB(x)		(((x) - dev->bank2) >> MFC_OFFSET_SHIFT)
+
+/* Allocate temporary buffers for decoding */
+static int s5p_mfc_alloc_dec_temp_buffers_v5(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_buf_size_v5 *buf_size = dev->variant->buf_size->priv;
+	int ret;
+
+	ctx->dsc.size = buf_size->dsc;
+	ret =  s5p_mfc_alloc_priv_buf(dev->mem_dev_l, dev->bank1, &ctx->dsc);
+	if (ret) {
+		mfc_err("Failed to allocate temporary buffer\n");
+		return ret;
+	}
+
+	BUG_ON(ctx->dsc.dma & ((1 << MFC_BANK1_ALIGN_ORDER) - 1));
+	memset(ctx->dsc.virt, 0, ctx->dsc.size);
+	wmb();
+	return 0;
+}
+
+
+/* Release temporary buffers for decoding */
+static void s5p_mfc_release_dec_desc_buffer_v5(struct s5p_mfc_ctx *ctx)
+{
+	s5p_mfc_release_priv_buf(ctx->dev->mem_dev_l, &ctx->dsc);
+}
+
+/* Allocate codec buffers */
+static int s5p_mfc_alloc_codec_buffers_v5(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	unsigned int enc_ref_y_size = 0;
+	unsigned int enc_ref_c_size = 0;
+	unsigned int guard_width, guard_height;
+	int ret;
+
+	if (ctx->type == MFCINST_DECODER) {
+		mfc_debug(2, "Luma size:%d Chroma size:%d MV size:%d\n",
+			  ctx->luma_size, ctx->chroma_size, ctx->mv_size);
+		mfc_debug(2, "Totals bufs: %d\n", ctx->total_dpb_count);
+	} else if (ctx->type == MFCINST_ENCODER) {
+		enc_ref_y_size = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN)
+			* ALIGN(ctx->img_height, S5P_FIMV_NV12MT_VALIGN);
+		enc_ref_y_size = ALIGN(enc_ref_y_size, S5P_FIMV_NV12MT_SALIGN);
+
+		if (ctx->codec_mode == S5P_MFC_CODEC_H264_ENC) {
+			enc_ref_c_size = ALIGN(ctx->img_width,
+						S5P_FIMV_NV12MT_HALIGN)
+						* ALIGN(ctx->img_height >> 1,
+						S5P_FIMV_NV12MT_VALIGN);
+			enc_ref_c_size = ALIGN(enc_ref_c_size,
+							S5P_FIMV_NV12MT_SALIGN);
+		} else {
+			guard_width = ALIGN(ctx->img_width + 16,
+							S5P_FIMV_NV12MT_HALIGN);
+			guard_height = ALIGN((ctx->img_height >> 1) + 4,
+							S5P_FIMV_NV12MT_VALIGN);
+			enc_ref_c_size = ALIGN(guard_width * guard_height,
+					       S5P_FIMV_NV12MT_SALIGN);
+		}
+		mfc_debug(2, "recon luma size: %d chroma size: %d\n",
+			  enc_ref_y_size, enc_ref_c_size);
+	} else {
+		return -EINVAL;
+	}
+	/* Codecs have different memory requirements */
+	switch (ctx->codec_mode) {
+	case S5P_MFC_CODEC_H264_DEC:
+		ctx->bank1.size =
+		    ALIGN(S5P_FIMV_DEC_NB_IP_SIZE +
+					S5P_FIMV_DEC_VERT_NB_MV_SIZE,
+					S5P_FIMV_DEC_BUF_ALIGN);
+		ctx->bank2.size = ctx->total_dpb_count * ctx->mv_size;
+		break;
+	case S5P_MFC_CODEC_MPEG4_DEC:
+		ctx->bank1.size =
+		    ALIGN(S5P_FIMV_DEC_NB_DCAC_SIZE +
+				     S5P_FIMV_DEC_UPNB_MV_SIZE +
+				     S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE +
+				     S5P_FIMV_DEC_STX_PARSER_SIZE +
+				     S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE,
+				     S5P_FIMV_DEC_BUF_ALIGN);
+		ctx->bank2.size = 0;
+		break;
+	case S5P_MFC_CODEC_VC1RCV_DEC:
+	case S5P_MFC_CODEC_VC1_DEC:
+		ctx->bank1.size =
+		    ALIGN(S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE +
+			     S5P_FIMV_DEC_UPNB_MV_SIZE +
+			     S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE +
+			     S5P_FIMV_DEC_NB_DCAC_SIZE +
+			     3 * S5P_FIMV_DEC_VC1_BITPLANE_SIZE,
+			     S5P_FIMV_DEC_BUF_ALIGN);
+		ctx->bank2.size = 0;
+		break;
+	case S5P_MFC_CODEC_MPEG2_DEC:
+		ctx->bank1.size = 0;
+		ctx->bank2.size = 0;
+		break;
+	case S5P_MFC_CODEC_H263_DEC:
+		ctx->bank1.size =
+		    ALIGN(S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE +
+			     S5P_FIMV_DEC_UPNB_MV_SIZE +
+			     S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE +
+			     S5P_FIMV_DEC_NB_DCAC_SIZE,
+			     S5P_FIMV_DEC_BUF_ALIGN);
+		ctx->bank2.size = 0;
+		break;
+	case S5P_MFC_CODEC_H264_ENC:
+		ctx->bank1.size = (enc_ref_y_size * 2) +
+				   S5P_FIMV_ENC_UPMV_SIZE +
+				   S5P_FIMV_ENC_COLFLG_SIZE +
+				   S5P_FIMV_ENC_INTRAMD_SIZE +
+				   S5P_FIMV_ENC_NBORINFO_SIZE;
+		ctx->bank2.size = (enc_ref_y_size * 2) +
+				   (enc_ref_c_size * 4) +
+				   S5P_FIMV_ENC_INTRAPRED_SIZE;
+		break;
+	case S5P_MFC_CODEC_MPEG4_ENC:
+		ctx->bank1.size = (enc_ref_y_size * 2) +
+				   S5P_FIMV_ENC_UPMV_SIZE +
+				   S5P_FIMV_ENC_COLFLG_SIZE +
+				   S5P_FIMV_ENC_ACDCCOEF_SIZE;
+		ctx->bank2.size = (enc_ref_y_size * 2) +
+				   (enc_ref_c_size * 4);
+		break;
+	case S5P_MFC_CODEC_H263_ENC:
+		ctx->bank1.size = (enc_ref_y_size * 2) +
+				   S5P_FIMV_ENC_UPMV_SIZE +
+				   S5P_FIMV_ENC_ACDCCOEF_SIZE;
+		ctx->bank2.size = (enc_ref_y_size * 2) +
+				   (enc_ref_c_size * 4);
+		break;
+	default:
+		break;
+	}
+	/* Allocate only if memory from bank 1 is necessary */
+	if (ctx->bank1.size > 0) {
+
+		ret = s5p_mfc_alloc_priv_buf(dev->mem_dev_l, dev->bank1,
+					     &ctx->bank1);
+		if (ret) {
+			mfc_err("Failed to allocate Bank1 temporary buffer\n");
+			return ret;
+		}
+		BUG_ON(ctx->bank1.dma & ((1 << MFC_BANK1_ALIGN_ORDER) - 1));
+	}
+	/* Allocate only if memory from bank 2 is necessary */
+	if (ctx->bank2.size > 0) {
+		ret = s5p_mfc_alloc_priv_buf(dev->mem_dev_r, dev->bank2,
+					     &ctx->bank2);
+		if (ret) {
+			mfc_err("Failed to allocate Bank2 temporary buffer\n");
+			s5p_mfc_release_priv_buf(ctx->dev->mem_dev_l, &ctx->bank1);
+			return ret;
+		}
+		BUG_ON(ctx->bank2.dma & ((1 << MFC_BANK2_ALIGN_ORDER) - 1));
+	}
+	return 0;
+}
+
+/* Release buffers allocated for codec */
+static void s5p_mfc_release_codec_buffers_v5(struct s5p_mfc_ctx *ctx)
+{
+	s5p_mfc_release_priv_buf(ctx->dev->mem_dev_l, &ctx->bank1);
+	s5p_mfc_release_priv_buf(ctx->dev->mem_dev_r, &ctx->bank2);
+}
+
+/* Allocate memory for instance data buffer */
+static int s5p_mfc_alloc_instance_buffer_v5(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_buf_size_v5 *buf_size = dev->variant->buf_size->priv;
+	int ret;
+
+	if (ctx->codec_mode == S5P_MFC_CODEC_H264_DEC ||
+		ctx->codec_mode == S5P_MFC_CODEC_H264_ENC)
+		ctx->ctx.size = buf_size->h264_ctx;
+	else
+		ctx->ctx.size = buf_size->non_h264_ctx;
+
+	ret = s5p_mfc_alloc_priv_buf(dev->mem_dev_l, dev->bank1, &ctx->ctx);
+	if (ret) {
+		mfc_err("Failed to allocate instance buffer\n");
+		return ret;
+	}
+	ctx->ctx.ofs = OFFSETA(ctx->ctx.dma);
+
+	/* Zero content of the allocated memory */
+	memset(ctx->ctx.virt, 0, ctx->ctx.size);
+	wmb();
+
+	/* Initialize shared memory */
+	ctx->shm.size = buf_size->shm;
+	ret = s5p_mfc_alloc_priv_buf(dev->mem_dev_l, dev->bank1, &ctx->shm);
+	if (ret) {
+		mfc_err("Failed to allocate shared memory buffer\n");
+		s5p_mfc_release_priv_buf(dev->mem_dev_l, &ctx->ctx);
+		return ret;
+	}
+
+	/* shared memory offset only keeps the offset from base (port a) */
+	ctx->shm.ofs = ctx->shm.dma - dev->bank1;
+	BUG_ON(ctx->shm.ofs & ((1 << MFC_BANK1_ALIGN_ORDER) - 1));
+
+	memset(ctx->shm.virt, 0, buf_size->shm);
+	wmb();
+	return 0;
+}
+
+/* Release instance buffer */
+static void s5p_mfc_release_instance_buffer_v5(struct s5p_mfc_ctx *ctx)
+{
+	s5p_mfc_release_priv_buf(ctx->dev->mem_dev_l, &ctx->ctx);
+	s5p_mfc_release_priv_buf(ctx->dev->mem_dev_l, &ctx->shm);
+}
+
+static int s5p_mfc_alloc_dev_context_buffer_v5(struct s5p_mfc_dev *dev)
+{
+	/* NOP */
+
+	return 0;
+}
+
+static void s5p_mfc_release_dev_context_buffer_v5(struct s5p_mfc_dev *dev)
+{
+	/* NOP */
+}
+
+static void s5p_mfc_write_info_v5(struct s5p_mfc_ctx *ctx, unsigned int data,
+			unsigned int ofs)
+{
+	*(u32 *)(ctx->shm.virt + ofs) = data;
+	wmb();
+}
+
+static unsigned int s5p_mfc_read_info_v5(struct s5p_mfc_ctx *ctx,
+				unsigned long ofs)
+{
+	rmb();
+	return *(u32 *)(ctx->shm.virt + ofs);
+}
+
+static void s5p_mfc_dec_calc_dpb_size_v5(struct s5p_mfc_ctx *ctx)
+{
+	unsigned int guard_width, guard_height;
+
+	ctx->buf_width = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN);
+	ctx->buf_height = ALIGN(ctx->img_height, S5P_FIMV_NV12MT_VALIGN);
+	mfc_debug(2,
+		"SEQ Done: Movie dimensions %dx%d, buffer dimensions: %dx%d\n",
+		ctx->img_width,	ctx->img_height, ctx->buf_width,
+		ctx->buf_height);
+
+	if (ctx->codec_mode == S5P_MFC_CODEC_H264_DEC) {
+		ctx->luma_size = ALIGN(ctx->buf_width * ctx->buf_height,
+				S5P_FIMV_DEC_BUF_ALIGN);
+		ctx->chroma_size = ALIGN(ctx->buf_width *
+				ALIGN((ctx->img_height >> 1),
+					S5P_FIMV_NV12MT_VALIGN),
+				S5P_FIMV_DEC_BUF_ALIGN);
+		ctx->mv_size = ALIGN(ctx->buf_width *
+				ALIGN((ctx->buf_height >> 2),
+					S5P_FIMV_NV12MT_VALIGN),
+				S5P_FIMV_DEC_BUF_ALIGN);
+	} else {
+		guard_width =
+			ALIGN(ctx->img_width + 24, S5P_FIMV_NV12MT_HALIGN);
+		guard_height =
+			ALIGN(ctx->img_height + 16, S5P_FIMV_NV12MT_VALIGN);
+		ctx->luma_size = ALIGN(guard_width * guard_height,
+				S5P_FIMV_DEC_BUF_ALIGN);
+
+		guard_width =
+			ALIGN(ctx->img_width + 16, S5P_FIMV_NV12MT_HALIGN);
+		guard_height =
+			ALIGN((ctx->img_height >> 1) + 4,
+					S5P_FIMV_NV12MT_VALIGN);
+		ctx->chroma_size = ALIGN(guard_width * guard_height,
+				S5P_FIMV_DEC_BUF_ALIGN);
+
+		ctx->mv_size = 0;
+	}
+}
+
+static void s5p_mfc_enc_calc_src_size_v5(struct s5p_mfc_ctx *ctx)
+{
+	if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12M) {
+		ctx->buf_width = ALIGN(ctx->img_width, S5P_FIMV_NV12M_HALIGN);
+
+		ctx->luma_size = ALIGN(ctx->img_width, S5P_FIMV_NV12M_HALIGN)
+			* ALIGN(ctx->img_height, S5P_FIMV_NV12M_LVALIGN);
+		ctx->chroma_size = ALIGN(ctx->img_width, S5P_FIMV_NV12M_HALIGN)
+			* ALIGN((ctx->img_height >> 1), S5P_FIMV_NV12M_CVALIGN);
+
+		ctx->luma_size = ALIGN(ctx->luma_size, S5P_FIMV_NV12M_SALIGN);
+		ctx->chroma_size =
+			ALIGN(ctx->chroma_size, S5P_FIMV_NV12M_SALIGN);
+	} else if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12MT) {
+		ctx->buf_width = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN);
+
+		ctx->luma_size = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN)
+			* ALIGN(ctx->img_height, S5P_FIMV_NV12MT_VALIGN);
+		ctx->chroma_size =
+			ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN)
+			* ALIGN((ctx->img_height >> 1), S5P_FIMV_NV12MT_VALIGN);
+
+		ctx->luma_size = ALIGN(ctx->luma_size, S5P_FIMV_NV12MT_SALIGN);
+		ctx->chroma_size =
+			ALIGN(ctx->chroma_size, S5P_FIMV_NV12MT_SALIGN);
+	}
+}
+
+/* Set registers for decoding temporary buffers */
+static void s5p_mfc_set_dec_desc_buffer(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_buf_size_v5 *buf_size = dev->variant->buf_size->priv;
+
+	mfc_write(dev, OFFSETA(ctx->dsc.dma), S5P_FIMV_SI_CH0_DESC_ADR);
+	mfc_write(dev, buf_size->dsc, S5P_FIMV_SI_CH0_DESC_SIZE);
+}
+
+/* Set registers for shared buffer */
+static void s5p_mfc_set_shared_buffer(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	mfc_write(dev, ctx->shm.ofs, S5P_FIMV_SI_CH0_HOST_WR_ADR);
+}
+
+/* Set registers for decoding stream buffer */
+static int s5p_mfc_set_dec_stream_buffer_v5(struct s5p_mfc_ctx *ctx,
+		int buf_addr, unsigned int start_num_byte,
+		unsigned int buf_size)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+
+	mfc_write(dev, OFFSETA(buf_addr), S5P_FIMV_SI_CH0_SB_ST_ADR);
+	mfc_write(dev, ctx->dec_src_buf_size, S5P_FIMV_SI_CH0_CPB_SIZE);
+	mfc_write(dev, buf_size, S5P_FIMV_SI_CH0_SB_FRM_SIZE);
+	s5p_mfc_write_info_v5(ctx, start_num_byte, START_BYTE_NUM);
+	return 0;
+}
+
+/* Set decoding frame buffer */
+static int s5p_mfc_set_dec_frame_buffer_v5(struct s5p_mfc_ctx *ctx)
+{
+	unsigned int frame_size_lu, i;
+	unsigned int frame_size_ch, frame_size_mv;
+	struct s5p_mfc_dev *dev = ctx->dev;
+	unsigned int dpb;
+	size_t buf_addr1, buf_addr2;
+	int buf_size1, buf_size2;
+
+	buf_addr1 = ctx->bank1.dma;
+	buf_size1 = ctx->bank1.size;
+	buf_addr2 = ctx->bank2.dma;
+	buf_size2 = ctx->bank2.size;
+	dpb = mfc_read(dev, S5P_FIMV_SI_CH0_DPB_CONF_CTRL) &
+						~S5P_FIMV_DPB_COUNT_MASK;
+	mfc_write(dev, ctx->total_dpb_count | dpb,
+						S5P_FIMV_SI_CH0_DPB_CONF_CTRL);
+	s5p_mfc_set_shared_buffer(ctx);
+	switch (ctx->codec_mode) {
+	case S5P_MFC_CODEC_H264_DEC:
+		mfc_write(dev, OFFSETA(buf_addr1),
+						S5P_FIMV_H264_VERT_NB_MV_ADR);
+		buf_addr1 += S5P_FIMV_DEC_VERT_NB_MV_SIZE;
+		buf_size1 -= S5P_FIMV_DEC_VERT_NB_MV_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H264_NB_IP_ADR);
+		buf_addr1 += S5P_FIMV_DEC_NB_IP_SIZE;
+		buf_size1 -= S5P_FIMV_DEC_NB_IP_SIZE;
+		break;
+	case S5P_MFC_CODEC_MPEG4_DEC:
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_NB_DCAC_ADR);
+		buf_addr1 += S5P_FIMV_DEC_NB_DCAC_SIZE;
+		buf_size1 -= S5P_FIMV_DEC_NB_DCAC_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_UP_NB_MV_ADR);
+		buf_addr1 += S5P_FIMV_DEC_UPNB_MV_SIZE;
+		buf_size1 -= S5P_FIMV_DEC_UPNB_MV_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_SA_MV_ADR);
+		buf_addr1 += S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
+		buf_size1 -= S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_SP_ADR);
+		buf_addr1 += S5P_FIMV_DEC_STX_PARSER_SIZE;
+		buf_size1 -= S5P_FIMV_DEC_STX_PARSER_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_OT_LINE_ADR);
+		buf_addr1 += S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
+		buf_size1 -= S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
+		break;
+	case S5P_MFC_CODEC_H263_DEC:
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_OT_LINE_ADR);
+		buf_addr1 += S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
+		buf_size1 -= S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_UP_NB_MV_ADR);
+		buf_addr1 += S5P_FIMV_DEC_UPNB_MV_SIZE;
+		buf_size1 -= S5P_FIMV_DEC_UPNB_MV_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_SA_MV_ADR);
+		buf_addr1 += S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
+		buf_size1 -= S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_NB_DCAC_ADR);
+		buf_addr1 += S5P_FIMV_DEC_NB_DCAC_SIZE;
+		buf_size1 -= S5P_FIMV_DEC_NB_DCAC_SIZE;
+		break;
+	case S5P_MFC_CODEC_VC1_DEC:
+	case S5P_MFC_CODEC_VC1RCV_DEC:
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_NB_DCAC_ADR);
+		buf_addr1 += S5P_FIMV_DEC_NB_DCAC_SIZE;
+		buf_size1 -= S5P_FIMV_DEC_NB_DCAC_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_OT_LINE_ADR);
+		buf_addr1 += S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
+		buf_size1 -= S5P_FIMV_DEC_OVERLAP_TRANSFORM_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_UP_NB_MV_ADR);
+		buf_addr1 += S5P_FIMV_DEC_UPNB_MV_SIZE;
+		buf_size1 -= S5P_FIMV_DEC_UPNB_MV_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_SA_MV_ADR);
+		buf_addr1 += S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
+		buf_size1 -= S5P_FIMV_DEC_SUB_ANCHOR_MV_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_BITPLANE3_ADR);
+		buf_addr1 += S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
+		buf_size1 -= S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_BITPLANE2_ADR);
+		buf_addr1 += S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
+		buf_size1 -= S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_VC1_BITPLANE1_ADR);
+		buf_addr1 += S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
+		buf_size1 -= S5P_FIMV_DEC_VC1_BITPLANE_SIZE;
+		break;
+	case S5P_MFC_CODEC_MPEG2_DEC:
+		break;
+	default:
+		mfc_err("Unknown codec for decoding (%x)\n",
+			ctx->codec_mode);
+		return -EINVAL;
+	}
+	frame_size_lu = ctx->luma_size;
+	frame_size_ch = ctx->chroma_size;
+	frame_size_mv = ctx->mv_size;
+	mfc_debug(2, "Frm size: %d ch: %d mv: %d\n", frame_size_lu, frame_size_ch,
+								frame_size_mv);
+	for (i = 0; i < ctx->total_dpb_count; i++) {
+		/* Bank2 */
+		mfc_debug(2, "Luma %d: %zx\n", i,
+					ctx->dst_bufs[i].cookie.raw.luma);
+		mfc_write(dev, OFFSETB(ctx->dst_bufs[i].cookie.raw.luma),
+						S5P_FIMV_DEC_LUMA_ADR + i * 4);
+		mfc_debug(2, "\tChroma %d: %zx\n", i,
+					ctx->dst_bufs[i].cookie.raw.chroma);
+		mfc_write(dev, OFFSETA(ctx->dst_bufs[i].cookie.raw.chroma),
+					       S5P_FIMV_DEC_CHROMA_ADR + i * 4);
+		if (ctx->codec_mode == S5P_MFC_CODEC_H264_DEC) {
+			mfc_debug(2, "\tBuf2: %zx, size: %d\n",
+							buf_addr2, buf_size2);
+			mfc_write(dev, OFFSETB(buf_addr2),
+						S5P_FIMV_H264_MV_ADR + i * 4);
+			buf_addr2 += frame_size_mv;
+			buf_size2 -= frame_size_mv;
+		}
+	}
+	mfc_debug(2, "Buf1: %zu, buf_size1: %d\n", buf_addr1, buf_size1);
+	mfc_debug(2, "Buf 1/2 size after: %d/%d (frames %d)\n",
+			buf_size1,  buf_size2, ctx->total_dpb_count);
+	if (buf_size1 < 0 || buf_size2 < 0) {
+		mfc_debug(2, "Not enough memory has been allocated\n");
+		return -ENOMEM;
+	}
+	s5p_mfc_write_info_v5(ctx, frame_size_lu, ALLOC_LUMA_DPB_SIZE);
+	s5p_mfc_write_info_v5(ctx, frame_size_ch, ALLOC_CHROMA_DPB_SIZE);
+	if (ctx->codec_mode == S5P_MFC_CODEC_H264_DEC)
+		s5p_mfc_write_info_v5(ctx, frame_size_mv, ALLOC_MV_SIZE);
+	mfc_write(dev, ((S5P_FIMV_CH_INIT_BUFS & S5P_FIMV_CH_MASK)
+					<< S5P_FIMV_CH_SHIFT) | (ctx->inst_no),
+						S5P_FIMV_SI_CH0_INST_ID);
+	return 0;
+}
+
+/* Set registers for encoding stream buffer */
+static int s5p_mfc_set_enc_stream_buffer_v5(struct s5p_mfc_ctx *ctx,
+		unsigned long addr, unsigned int size)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+
+	mfc_write(dev, OFFSETA(addr), S5P_FIMV_ENC_SI_CH0_SB_ADR);
+	mfc_write(dev, size, S5P_FIMV_ENC_SI_CH0_SB_SIZE);
+	return 0;
+}
+
+static void s5p_mfc_set_enc_frame_buffer_v5(struct s5p_mfc_ctx *ctx,
+		unsigned long y_addr, unsigned long c_addr)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+
+	mfc_write(dev, OFFSETB(y_addr), S5P_FIMV_ENC_SI_CH0_CUR_Y_ADR);
+	mfc_write(dev, OFFSETB(c_addr), S5P_FIMV_ENC_SI_CH0_CUR_C_ADR);
+}
+
+static void s5p_mfc_get_enc_frame_buffer_v5(struct s5p_mfc_ctx *ctx,
+		unsigned long *y_addr, unsigned long *c_addr)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+
+	*y_addr = dev->bank2 + (mfc_read(dev, S5P_FIMV_ENCODED_Y_ADDR)
+							<< MFC_OFFSET_SHIFT);
+	*c_addr = dev->bank2 + (mfc_read(dev, S5P_FIMV_ENCODED_C_ADDR)
+							<< MFC_OFFSET_SHIFT);
+}
+
+/* Set encoding ref & codec buffer */
+static int s5p_mfc_set_enc_ref_buffer_v5(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	size_t buf_addr1, buf_addr2;
+	size_t buf_size1, buf_size2;
+	unsigned int enc_ref_y_size, enc_ref_c_size;
+	unsigned int guard_width, guard_height;
+	int i;
+
+	buf_addr1 = ctx->bank1.dma;
+	buf_size1 = ctx->bank1.size;
+	buf_addr2 = ctx->bank2.dma;
+	buf_size2 = ctx->bank2.size;
+	enc_ref_y_size = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN)
+		* ALIGN(ctx->img_height, S5P_FIMV_NV12MT_VALIGN);
+	enc_ref_y_size = ALIGN(enc_ref_y_size, S5P_FIMV_NV12MT_SALIGN);
+	if (ctx->codec_mode == S5P_MFC_CODEC_H264_ENC) {
+		enc_ref_c_size = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN)
+			* ALIGN((ctx->img_height >> 1), S5P_FIMV_NV12MT_VALIGN);
+		enc_ref_c_size = ALIGN(enc_ref_c_size, S5P_FIMV_NV12MT_SALIGN);
+	} else {
+		guard_width = ALIGN(ctx->img_width + 16,
+						S5P_FIMV_NV12MT_HALIGN);
+		guard_height = ALIGN((ctx->img_height >> 1) + 4,
+						S5P_FIMV_NV12MT_VALIGN);
+		enc_ref_c_size = ALIGN(guard_width * guard_height,
+				       S5P_FIMV_NV12MT_SALIGN);
+	}
+	mfc_debug(2, "buf_size1: %zu, buf_size2: %zu\n", buf_size1, buf_size2);
+	switch (ctx->codec_mode) {
+	case S5P_MFC_CODEC_H264_ENC:
+		for (i = 0; i < 2; i++) {
+			mfc_write(dev, OFFSETA(buf_addr1),
+				S5P_FIMV_ENC_REF0_LUMA_ADR + (4 * i));
+			buf_addr1 += enc_ref_y_size;
+			buf_size1 -= enc_ref_y_size;
+
+			mfc_write(dev, OFFSETB(buf_addr2),
+				S5P_FIMV_ENC_REF2_LUMA_ADR + (4 * i));
+			buf_addr2 += enc_ref_y_size;
+			buf_size2 -= enc_ref_y_size;
+		}
+		for (i = 0; i < 4; i++) {
+			mfc_write(dev, OFFSETB(buf_addr2),
+				S5P_FIMV_ENC_REF0_CHROMA_ADR + (4 * i));
+			buf_addr2 += enc_ref_c_size;
+			buf_size2 -= enc_ref_c_size;
+		}
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H264_UP_MV_ADR);
+		buf_addr1 += S5P_FIMV_ENC_UPMV_SIZE;
+		buf_size1 -= S5P_FIMV_ENC_UPMV_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1),
+					S5P_FIMV_H264_COZERO_FLAG_ADR);
+		buf_addr1 += S5P_FIMV_ENC_COLFLG_SIZE;
+		buf_size1 -= S5P_FIMV_ENC_COLFLG_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1),
+					S5P_FIMV_H264_UP_INTRA_MD_ADR);
+		buf_addr1 += S5P_FIMV_ENC_INTRAMD_SIZE;
+		buf_size1 -= S5P_FIMV_ENC_INTRAMD_SIZE;
+		mfc_write(dev, OFFSETB(buf_addr2),
+					S5P_FIMV_H264_UP_INTRA_PRED_ADR);
+		buf_addr2 += S5P_FIMV_ENC_INTRAPRED_SIZE;
+		buf_size2 -= S5P_FIMV_ENC_INTRAPRED_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1),
+					S5P_FIMV_H264_NBOR_INFO_ADR);
+		buf_addr1 += S5P_FIMV_ENC_NBORINFO_SIZE;
+		buf_size1 -= S5P_FIMV_ENC_NBORINFO_SIZE;
+		mfc_debug(2, "buf_size1: %zu, buf_size2: %zu\n",
+			buf_size1, buf_size2);
+		break;
+	case S5P_MFC_CODEC_MPEG4_ENC:
+		for (i = 0; i < 2; i++) {
+			mfc_write(dev, OFFSETA(buf_addr1),
+				S5P_FIMV_ENC_REF0_LUMA_ADR + (4 * i));
+			buf_addr1 += enc_ref_y_size;
+			buf_size1 -= enc_ref_y_size;
+			mfc_write(dev, OFFSETB(buf_addr2),
+				S5P_FIMV_ENC_REF2_LUMA_ADR + (4 * i));
+			buf_addr2 += enc_ref_y_size;
+			buf_size2 -= enc_ref_y_size;
+		}
+		for (i = 0; i < 4; i++) {
+			mfc_write(dev, OFFSETB(buf_addr2),
+				S5P_FIMV_ENC_REF0_CHROMA_ADR + (4 * i));
+			buf_addr2 += enc_ref_c_size;
+			buf_size2 -= enc_ref_c_size;
+		}
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_MPEG4_UP_MV_ADR);
+		buf_addr1 += S5P_FIMV_ENC_UPMV_SIZE;
+		buf_size1 -= S5P_FIMV_ENC_UPMV_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1),
+						S5P_FIMV_MPEG4_COZERO_FLAG_ADR);
+		buf_addr1 += S5P_FIMV_ENC_COLFLG_SIZE;
+		buf_size1 -= S5P_FIMV_ENC_COLFLG_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1),
+						S5P_FIMV_MPEG4_ACDC_COEF_ADR);
+		buf_addr1 += S5P_FIMV_ENC_ACDCCOEF_SIZE;
+		buf_size1 -= S5P_FIMV_ENC_ACDCCOEF_SIZE;
+		mfc_debug(2, "buf_size1: %zu, buf_size2: %zu\n",
+			buf_size1, buf_size2);
+		break;
+	case S5P_MFC_CODEC_H263_ENC:
+		for (i = 0; i < 2; i++) {
+			mfc_write(dev, OFFSETA(buf_addr1),
+				S5P_FIMV_ENC_REF0_LUMA_ADR + (4 * i));
+			buf_addr1 += enc_ref_y_size;
+			buf_size1 -= enc_ref_y_size;
+			mfc_write(dev, OFFSETB(buf_addr2),
+				S5P_FIMV_ENC_REF2_LUMA_ADR + (4 * i));
+			buf_addr2 += enc_ref_y_size;
+			buf_size2 -= enc_ref_y_size;
+		}
+		for (i = 0; i < 4; i++) {
+			mfc_write(dev, OFFSETB(buf_addr2),
+				S5P_FIMV_ENC_REF0_CHROMA_ADR + (4 * i));
+			buf_addr2 += enc_ref_c_size;
+			buf_size2 -= enc_ref_c_size;
+		}
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_UP_MV_ADR);
+		buf_addr1 += S5P_FIMV_ENC_UPMV_SIZE;
+		buf_size1 -= S5P_FIMV_ENC_UPMV_SIZE;
+		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_ACDC_COEF_ADR);
+		buf_addr1 += S5P_FIMV_ENC_ACDCCOEF_SIZE;
+		buf_size1 -= S5P_FIMV_ENC_ACDCCOEF_SIZE;
+		mfc_debug(2, "buf_size1: %zu, buf_size2: %zu\n",
+			buf_size1, buf_size2);
+		break;
+	default:
+		mfc_err("Unknown codec set for encoding: %d\n",
+			ctx->codec_mode);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int s5p_mfc_set_enc_params(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_enc_params *p = &ctx->enc_params;
+	unsigned int reg;
+	unsigned int shm;
+
+	/* width */
+	mfc_write(dev, ctx->img_width, S5P_FIMV_ENC_HSIZE_PX);
+	/* height */
+	mfc_write(dev, ctx->img_height, S5P_FIMV_ENC_VSIZE_PX);
+	/* pictype : enable, IDR period */
+	reg = mfc_read(dev, S5P_FIMV_ENC_PIC_TYPE_CTRL);
+	reg |= (1 << 18);
+	reg &= ~(0xFFFF);
+	reg |= p->gop_size;
+	mfc_write(dev, reg, S5P_FIMV_ENC_PIC_TYPE_CTRL);
+	mfc_write(dev, 0, S5P_FIMV_ENC_B_RECON_WRITE_ON);
+	/* multi-slice control */
+	/* multi-slice MB number or bit size */
+	mfc_write(dev, p->slice_mode, S5P_FIMV_ENC_MSLICE_CTRL);
+	if (p->slice_mode == V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_MB) {
+		mfc_write(dev, p->slice_mb, S5P_FIMV_ENC_MSLICE_MB);
+	} else if (p->slice_mode == V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_BYTES) {
+		mfc_write(dev, p->slice_bit, S5P_FIMV_ENC_MSLICE_BIT);
+	} else {
+		mfc_write(dev, 0, S5P_FIMV_ENC_MSLICE_MB);
+		mfc_write(dev, 0, S5P_FIMV_ENC_MSLICE_BIT);
+	}
+	/* cyclic intra refresh */
+	mfc_write(dev, p->intra_refresh_mb, S5P_FIMV_ENC_CIR_CTRL);
+	/* memory structure cur. frame */
+	if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12M)
+		mfc_write(dev, 0, S5P_FIMV_ENC_MAP_FOR_CUR);
+	else if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12MT)
+		mfc_write(dev, 3, S5P_FIMV_ENC_MAP_FOR_CUR);
+	/* padding control & value */
+	reg = mfc_read(dev, S5P_FIMV_ENC_PADDING_CTRL);
+	if (p->pad) {
+		/** enable */
+		reg |= (1 << 31);
+		/** cr value */
+		reg &= ~(0xFF << 16);
+		reg |= (p->pad_cr << 16);
+		/** cb value */
+		reg &= ~(0xFF << 8);
+		reg |= (p->pad_cb << 8);
+		/** y value */
+		reg &= ~(0xFF);
+		reg |= (p->pad_luma);
+	} else {
+		/** disable & all value clear */
+		reg = 0;
+	}
+	mfc_write(dev, reg, S5P_FIMV_ENC_PADDING_CTRL);
+	/* rate control config. */
+	reg = mfc_read(dev, S5P_FIMV_ENC_RC_CONFIG);
+	/** frame-level rate control */
+	reg &= ~(0x1 << 9);
+	reg |= (p->rc_frame << 9);
+	mfc_write(dev, reg, S5P_FIMV_ENC_RC_CONFIG);
+	/* bit rate */
+	if (p->rc_frame)
+		mfc_write(dev, p->rc_bitrate,
+			S5P_FIMV_ENC_RC_BIT_RATE);
+	else
+		mfc_write(dev, 0, S5P_FIMV_ENC_RC_BIT_RATE);
+	/* reaction coefficient */
+	if (p->rc_frame)
+		mfc_write(dev, p->rc_reaction_coeff, S5P_FIMV_ENC_RC_RPARA);
+	shm = s5p_mfc_read_info_v5(ctx, EXT_ENC_CONTROL);
+	/* seq header ctrl */
+	shm &= ~(0x1 << 3);
+	shm |= (p->seq_hdr_mode << 3);
+	/* frame skip mode */
+	shm &= ~(0x3 << 1);
+	shm |= (p->frame_skip_mode << 1);
+	s5p_mfc_write_info_v5(ctx, shm, EXT_ENC_CONTROL);
+	/* fixed target bit */
+	s5p_mfc_write_info_v5(ctx, p->fixed_target_bit, RC_CONTROL_CONFIG);
+	return 0;
+}
+
+static int s5p_mfc_set_enc_params_h264(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_enc_params *p = &ctx->enc_params;
+	struct s5p_mfc_h264_enc_params *p_264 = &p->codec.h264;
+	unsigned int reg;
+	unsigned int shm;
+
+	s5p_mfc_set_enc_params(ctx);
+	/* pictype : number of B */
+	reg = mfc_read(dev, S5P_FIMV_ENC_PIC_TYPE_CTRL);
+	/* num_b_frame - 0 ~ 2 */
+	reg &= ~(0x3 << 16);
+	reg |= (p->num_b_frame << 16);
+	mfc_write(dev, reg, S5P_FIMV_ENC_PIC_TYPE_CTRL);
+	/* profile & level */
+	reg = mfc_read(dev, S5P_FIMV_ENC_PROFILE);
+	/* level */
+	reg &= ~(0xFF << 8);
+	reg |= (p_264->level << 8);
+	/* profile - 0 ~ 2 */
+	reg &= ~(0x3F);
+	reg |= p_264->profile;
+	mfc_write(dev, reg, S5P_FIMV_ENC_PROFILE);
+	/* interlace  */
+	mfc_write(dev, p_264->interlace, S5P_FIMV_ENC_PIC_STRUCT);
+	/* height */
+	if (p_264->interlace)
+		mfc_write(dev, ctx->img_height >> 1, S5P_FIMV_ENC_VSIZE_PX);
+	/* loopfilter ctrl */
+	mfc_write(dev, p_264->loop_filter_mode, S5P_FIMV_ENC_LF_CTRL);
+	/* loopfilter alpha offset */
+	if (p_264->loop_filter_alpha < 0) {
+		reg = 0x10;
+		reg |= (0xFF - p_264->loop_filter_alpha) + 1;
+	} else {
+		reg = 0x00;
+		reg |= (p_264->loop_filter_alpha & 0xF);
+	}
+	mfc_write(dev, reg, S5P_FIMV_ENC_ALPHA_OFF);
+	/* loopfilter beta offset */
+	if (p_264->loop_filter_beta < 0) {
+		reg = 0x10;
+		reg |= (0xFF - p_264->loop_filter_beta) + 1;
+	} else {
+		reg = 0x00;
+		reg |= (p_264->loop_filter_beta & 0xF);
+	}
+	mfc_write(dev, reg, S5P_FIMV_ENC_BETA_OFF);
+	/* entropy coding mode */
+	if (p_264->entropy_mode == V4L2_MPEG_VIDEO_H264_ENTROPY_MODE_CABAC)
+		mfc_write(dev, 1, S5P_FIMV_ENC_H264_ENTROPY_MODE);
+	else
+		mfc_write(dev, 0, S5P_FIMV_ENC_H264_ENTROPY_MODE);
+	/* number of ref. picture */
+	reg = mfc_read(dev, S5P_FIMV_ENC_H264_NUM_OF_REF);
+	/* num of ref. pictures of P */
+	reg &= ~(0x3 << 5);
+	reg |= (p_264->num_ref_pic_4p << 5);
+	/* max number of ref. pictures */
+	reg &= ~(0x1F);
+	reg |= p_264->max_ref_pic;
+	mfc_write(dev, reg, S5P_FIMV_ENC_H264_NUM_OF_REF);
+	/* 8x8 transform enable */
+	mfc_write(dev, p_264->_8x8_transform, S5P_FIMV_ENC_H264_TRANS_FLAG);
+	/* rate control config. */
+	reg = mfc_read(dev, S5P_FIMV_ENC_RC_CONFIG);
+	/* macroblock level rate control */
+	reg &= ~(0x1 << 8);
+	reg |= (p->rc_mb << 8);
+	/* frame QP */
+	reg &= ~(0x3F);
+	reg |= p_264->rc_frame_qp;
+	mfc_write(dev, reg, S5P_FIMV_ENC_RC_CONFIG);
+	/* frame rate */
+	if (p->rc_frame && p->rc_framerate_denom)
+		mfc_write(dev, p->rc_framerate_num * 1000
+			/ p->rc_framerate_denom, S5P_FIMV_ENC_RC_FRAME_RATE);
+	else
+		mfc_write(dev, 0, S5P_FIMV_ENC_RC_FRAME_RATE);
+	/* max & min value of QP */
+	reg = mfc_read(dev, S5P_FIMV_ENC_RC_QBOUND);
+	/* max QP */
+	reg &= ~(0x3F << 8);
+	reg |= (p_264->rc_max_qp << 8);
+	/* min QP */
+	reg &= ~(0x3F);
+	reg |= p_264->rc_min_qp;
+	mfc_write(dev, reg, S5P_FIMV_ENC_RC_QBOUND);
+	/* macroblock adaptive scaling features */
+	if (p->rc_mb) {
+		reg = mfc_read(dev, S5P_FIMV_ENC_RC_MB_CTRL);
+		/* dark region */
+		reg &= ~(0x1 << 3);
+		reg |= (p_264->rc_mb_dark << 3);
+		/* smooth region */
+		reg &= ~(0x1 << 2);
+		reg |= (p_264->rc_mb_smooth << 2);
+		/* static region */
+		reg &= ~(0x1 << 1);
+		reg |= (p_264->rc_mb_static << 1);
+		/* high activity region */
+		reg &= ~(0x1);
+		reg |= p_264->rc_mb_activity;
+		mfc_write(dev, reg, S5P_FIMV_ENC_RC_MB_CTRL);
+	}
+	if (!p->rc_frame && !p->rc_mb) {
+		shm = s5p_mfc_read_info_v5(ctx, P_B_FRAME_QP);
+		shm &= ~(0xFFF);
+		shm |= ((p_264->rc_b_frame_qp & 0x3F) << 6);
+		shm |= (p_264->rc_p_frame_qp & 0x3F);
+		s5p_mfc_write_info_v5(ctx, shm, P_B_FRAME_QP);
+	}
+	/* extended encoder ctrl */
+	shm = s5p_mfc_read_info_v5(ctx, EXT_ENC_CONTROL);
+	/* AR VUI control */
+	shm &= ~(0x1 << 15);
+	shm |= (p_264->vui_sar << 1);
+	s5p_mfc_write_info_v5(ctx, shm, EXT_ENC_CONTROL);
+	if (p_264->vui_sar) {
+		/* aspect ration IDC */
+		shm = s5p_mfc_read_info_v5(ctx, SAMPLE_ASPECT_RATIO_IDC);
+		shm &= ~(0xFF);
+		shm |= p_264->vui_sar_idc;
+		s5p_mfc_write_info_v5(ctx, shm, SAMPLE_ASPECT_RATIO_IDC);
+		if (p_264->vui_sar_idc == 0xFF) {
+			/* sample  AR info */
+			shm = s5p_mfc_read_info_v5(ctx, EXTENDED_SAR);
+			shm &= ~(0xFFFFFFFF);
+			shm |= p_264->vui_ext_sar_width << 16;
+			shm |= p_264->vui_ext_sar_height;
+			s5p_mfc_write_info_v5(ctx, shm, EXTENDED_SAR);
+		}
+	}
+	/* intra picture period for H.264 */
+	shm = s5p_mfc_read_info_v5(ctx, H264_I_PERIOD);
+	/* control */
+	shm &= ~(0x1 << 16);
+	shm |= (p_264->open_gop << 16);
+	/* value */
+	if (p_264->open_gop) {
+		shm &= ~(0xFFFF);
+		shm |= p_264->open_gop_size;
+	}
+	s5p_mfc_write_info_v5(ctx, shm, H264_I_PERIOD);
+	/* extended encoder ctrl */
+	shm = s5p_mfc_read_info_v5(ctx, EXT_ENC_CONTROL);
+	/* vbv buffer size */
+	if (p->frame_skip_mode ==
+			V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
+		shm &= ~(0xFFFF << 16);
+		shm |= (p_264->cpb_size << 16);
+	}
+	s5p_mfc_write_info_v5(ctx, shm, EXT_ENC_CONTROL);
+	return 0;
+}
+
+static int s5p_mfc_set_enc_params_mpeg4(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_enc_params *p = &ctx->enc_params;
+	struct s5p_mfc_mpeg4_enc_params *p_mpeg4 = &p->codec.mpeg4;
+	unsigned int reg;
+	unsigned int shm;
+	unsigned int framerate;
+
+	s5p_mfc_set_enc_params(ctx);
+	/* pictype : number of B */
+	reg = mfc_read(dev, S5P_FIMV_ENC_PIC_TYPE_CTRL);
+	/* num_b_frame - 0 ~ 2 */
+	reg &= ~(0x3 << 16);
+	reg |= (p->num_b_frame << 16);
+	mfc_write(dev, reg, S5P_FIMV_ENC_PIC_TYPE_CTRL);
+	/* profile & level */
+	reg = mfc_read(dev, S5P_FIMV_ENC_PROFILE);
+	/* level */
+	reg &= ~(0xFF << 8);
+	reg |= (p_mpeg4->level << 8);
+	/* profile - 0 ~ 2 */
+	reg &= ~(0x3F);
+	reg |= p_mpeg4->profile;
+	mfc_write(dev, reg, S5P_FIMV_ENC_PROFILE);
+	/* quarter_pixel */
+	mfc_write(dev, p_mpeg4->quarter_pixel, S5P_FIMV_ENC_MPEG4_QUART_PXL);
+	/* qp */
+	if (!p->rc_frame) {
+		shm = s5p_mfc_read_info_v5(ctx, P_B_FRAME_QP);
+		shm &= ~(0xFFF);
+		shm |= ((p_mpeg4->rc_b_frame_qp & 0x3F) << 6);
+		shm |= (p_mpeg4->rc_p_frame_qp & 0x3F);
+		s5p_mfc_write_info_v5(ctx, shm, P_B_FRAME_QP);
+	}
+	/* frame rate */
+	if (p->rc_frame) {
+		if (p->rc_framerate_denom > 0) {
+			framerate = p->rc_framerate_num * 1000 /
+						p->rc_framerate_denom;
+			mfc_write(dev, framerate,
+				S5P_FIMV_ENC_RC_FRAME_RATE);
+			shm = s5p_mfc_read_info_v5(ctx, RC_VOP_TIMING);
+			shm &= ~(0xFFFFFFFF);
+			shm |= (1 << 31);
+			shm |= ((p->rc_framerate_num & 0x7FFF) << 16);
+			shm |= (p->rc_framerate_denom & 0xFFFF);
+			s5p_mfc_write_info_v5(ctx, shm, RC_VOP_TIMING);
+		}
+	} else {
+		mfc_write(dev, 0, S5P_FIMV_ENC_RC_FRAME_RATE);
+	}
+	/* rate control config. */
+	reg = mfc_read(dev, S5P_FIMV_ENC_RC_CONFIG);
+	/* frame QP */
+	reg &= ~(0x3F);
+	reg |= p_mpeg4->rc_frame_qp;
+	mfc_write(dev, reg, S5P_FIMV_ENC_RC_CONFIG);
+	/* max & min value of QP */
+	reg = mfc_read(dev, S5P_FIMV_ENC_RC_QBOUND);
+	/* max QP */
+	reg &= ~(0x3F << 8);
+	reg |= (p_mpeg4->rc_max_qp << 8);
+	/* min QP */
+	reg &= ~(0x3F);
+	reg |= p_mpeg4->rc_min_qp;
+	mfc_write(dev, reg, S5P_FIMV_ENC_RC_QBOUND);
+	/* extended encoder ctrl */
+	shm = s5p_mfc_read_info_v5(ctx, EXT_ENC_CONTROL);
+	/* vbv buffer size */
+	if (p->frame_skip_mode ==
+			V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
+		shm &= ~(0xFFFF << 16);
+		shm |= (p->vbv_size << 16);
+	}
+	s5p_mfc_write_info_v5(ctx, shm, EXT_ENC_CONTROL);
+	return 0;
+}
+
+static int s5p_mfc_set_enc_params_h263(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_enc_params *p = &ctx->enc_params;
+	struct s5p_mfc_mpeg4_enc_params *p_h263 = &p->codec.mpeg4;
+	unsigned int reg;
+	unsigned int shm;
+
+	s5p_mfc_set_enc_params(ctx);
+	/* qp */
+	if (!p->rc_frame) {
+		shm = s5p_mfc_read_info_v5(ctx, P_B_FRAME_QP);
+		shm &= ~(0xFFF);
+		shm |= (p_h263->rc_p_frame_qp & 0x3F);
+		s5p_mfc_write_info_v5(ctx, shm, P_B_FRAME_QP);
+	}
+	/* frame rate */
+	if (p->rc_frame && p->rc_framerate_denom)
+		mfc_write(dev, p->rc_framerate_num * 1000
+			/ p->rc_framerate_denom, S5P_FIMV_ENC_RC_FRAME_RATE);
+	else
+		mfc_write(dev, 0, S5P_FIMV_ENC_RC_FRAME_RATE);
+	/* rate control config. */
+	reg = mfc_read(dev, S5P_FIMV_ENC_RC_CONFIG);
+	/* frame QP */
+	reg &= ~(0x3F);
+	reg |= p_h263->rc_frame_qp;
+	mfc_write(dev, reg, S5P_FIMV_ENC_RC_CONFIG);
+	/* max & min value of QP */
+	reg = mfc_read(dev, S5P_FIMV_ENC_RC_QBOUND);
+	/* max QP */
+	reg &= ~(0x3F << 8);
+	reg |= (p_h263->rc_max_qp << 8);
+	/* min QP */
+	reg &= ~(0x3F);
+	reg |= p_h263->rc_min_qp;
+	mfc_write(dev, reg, S5P_FIMV_ENC_RC_QBOUND);
+	/* extended encoder ctrl */
+	shm = s5p_mfc_read_info_v5(ctx, EXT_ENC_CONTROL);
+	/* vbv buffer size */
+	if (p->frame_skip_mode ==
+			V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
+		shm &= ~(0xFFFF << 16);
+		shm |= (p->vbv_size << 16);
+	}
+	s5p_mfc_write_info_v5(ctx, shm, EXT_ENC_CONTROL);
+	return 0;
+}
+
+/* Initialize decoding */
+static int s5p_mfc_init_decode_v5(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+
+	s5p_mfc_set_shared_buffer(ctx);
+	/* Setup loop filter, for decoding this is only valid for MPEG4 */
+	if (ctx->codec_mode == S5P_MFC_CODEC_MPEG4_DEC)
+		mfc_write(dev, ctx->loop_filter_mpeg4, S5P_FIMV_ENC_LF_CTRL);
+	else
+		mfc_write(dev, 0, S5P_FIMV_ENC_LF_CTRL);
+	mfc_write(dev, ((ctx->slice_interface & S5P_FIMV_SLICE_INT_MASK) <<
+		S5P_FIMV_SLICE_INT_SHIFT) | (ctx->display_delay_enable <<
+		S5P_FIMV_DDELAY_ENA_SHIFT) | ((ctx->display_delay &
+		S5P_FIMV_DDELAY_VAL_MASK) << S5P_FIMV_DDELAY_VAL_SHIFT),
+		S5P_FIMV_SI_CH0_DPB_CONF_CTRL);
+	mfc_write(dev,
+	((S5P_FIMV_CH_SEQ_HEADER & S5P_FIMV_CH_MASK) << S5P_FIMV_CH_SHIFT)
+				| (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
+	return 0;
+}
+
+static void s5p_mfc_set_flush(struct s5p_mfc_ctx *ctx, int flush)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	unsigned int dpb;
+
+	if (flush)
+		dpb = mfc_read(dev, S5P_FIMV_SI_CH0_DPB_CONF_CTRL) | (
+			S5P_FIMV_DPB_FLUSH_MASK << S5P_FIMV_DPB_FLUSH_SHIFT);
+	else
+		dpb = mfc_read(dev, S5P_FIMV_SI_CH0_DPB_CONF_CTRL) &
+			~(S5P_FIMV_DPB_FLUSH_MASK << S5P_FIMV_DPB_FLUSH_SHIFT);
+	mfc_write(dev, dpb, S5P_FIMV_SI_CH0_DPB_CONF_CTRL);
+}
+
+/* Decode a single frame */
+static int s5p_mfc_decode_one_frame_v5(struct s5p_mfc_ctx *ctx,
+					enum s5p_mfc_decode_arg last_frame)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+
+	mfc_write(dev, ctx->dec_dst_flag, S5P_FIMV_SI_CH0_RELEASE_BUF);
+	s5p_mfc_set_shared_buffer(ctx);
+	s5p_mfc_set_flush(ctx, ctx->dpb_flush_flag);
+	/* Issue different commands to instance basing on whether it
+	 * is the last frame or not. */
+	switch (last_frame) {
+	case MFC_DEC_FRAME:
+		mfc_write(dev, ((S5P_FIMV_CH_FRAME_START & S5P_FIMV_CH_MASK) <<
+		S5P_FIMV_CH_SHIFT) | (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
+		break;
+	case MFC_DEC_LAST_FRAME:
+		mfc_write(dev, ((S5P_FIMV_CH_LAST_FRAME & S5P_FIMV_CH_MASK) <<
+		S5P_FIMV_CH_SHIFT) | (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
+		break;
+	case MFC_DEC_RES_CHANGE:
+		mfc_write(dev, ((S5P_FIMV_CH_FRAME_START_REALLOC &
+		S5P_FIMV_CH_MASK) << S5P_FIMV_CH_SHIFT) | (ctx->inst_no),
+		S5P_FIMV_SI_CH0_INST_ID);
+		break;
+	}
+	mfc_debug(2, "Decoding a usual frame\n");
+	return 0;
+}
+
+static int s5p_mfc_init_encode_v5(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+
+	if (ctx->codec_mode == S5P_MFC_CODEC_H264_ENC)
+		s5p_mfc_set_enc_params_h264(ctx);
+	else if (ctx->codec_mode == S5P_MFC_CODEC_MPEG4_ENC)
+		s5p_mfc_set_enc_params_mpeg4(ctx);
+	else if (ctx->codec_mode == S5P_MFC_CODEC_H263_ENC)
+		s5p_mfc_set_enc_params_h263(ctx);
+	else {
+		mfc_err("Unknown codec for encoding (%x)\n",
+			ctx->codec_mode);
+		return -EINVAL;
+	}
+	s5p_mfc_set_shared_buffer(ctx);
+	mfc_write(dev, ((S5P_FIMV_CH_SEQ_HEADER << 16) & 0x70000) |
+		(ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
+	return 0;
+}
+
+/* Encode a single frame */
+static int s5p_mfc_encode_one_frame_v5(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	int cmd;
+	/* memory structure cur. frame */
+	if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12M)
+		mfc_write(dev, 0, S5P_FIMV_ENC_MAP_FOR_CUR);
+	else if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12MT)
+		mfc_write(dev, 3, S5P_FIMV_ENC_MAP_FOR_CUR);
+	s5p_mfc_set_shared_buffer(ctx);
+
+	if (ctx->state == MFCINST_FINISHING)
+		cmd = S5P_FIMV_CH_LAST_FRAME;
+	else
+		cmd = S5P_FIMV_CH_FRAME_START;
+	mfc_write(dev, ((cmd & S5P_FIMV_CH_MASK) << S5P_FIMV_CH_SHIFT)
+				| (ctx->inst_no), S5P_FIMV_SI_CH0_INST_ID);
+
+	return 0;
+}
+
+static int s5p_mfc_get_new_ctx(struct s5p_mfc_dev *dev)
+{
+	unsigned long flags;
+	int new_ctx;
+	int cnt;
+
+	spin_lock_irqsave(&dev->condlock, flags);
+	new_ctx = (dev->curr_ctx + 1) % MFC_NUM_CONTEXTS;
+	cnt = 0;
+	while (!test_bit(new_ctx, &dev->ctx_work_bits)) {
+		new_ctx = (new_ctx + 1) % MFC_NUM_CONTEXTS;
+		if (++cnt > MFC_NUM_CONTEXTS) {
+			/* No contexts to run */
+			spin_unlock_irqrestore(&dev->condlock, flags);
+			return -EAGAIN;
+		}
+	}
+	spin_unlock_irqrestore(&dev->condlock, flags);
+	return new_ctx;
+}
+
+static void s5p_mfc_run_res_change(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+
+	s5p_mfc_set_dec_stream_buffer_v5(ctx, 0, 0, 0);
+	dev->curr_ctx = ctx->num;
+	s5p_mfc_decode_one_frame_v5(ctx, MFC_DEC_RES_CHANGE);
+}
+
+static int s5p_mfc_run_dec_frame(struct s5p_mfc_ctx *ctx, int last_frame)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_buf *temp_vb;
+	unsigned long flags;
+
+	if (ctx->state == MFCINST_FINISHING) {
+		last_frame = MFC_DEC_LAST_FRAME;
+		s5p_mfc_set_dec_stream_buffer_v5(ctx, 0, 0, 0);
+		dev->curr_ctx = ctx->num;
+		s5p_mfc_decode_one_frame_v5(ctx, last_frame);
+		return 0;
+	}
+
+	spin_lock_irqsave(&dev->irqlock, flags);
+	/* Frames are being decoded */
+	if (list_empty(&ctx->src_queue)) {
+		mfc_debug(2, "No src buffers\n");
+		spin_unlock_irqrestore(&dev->irqlock, flags);
+		return -EAGAIN;
+	}
+	/* Get the next source buffer */
+	temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
+	temp_vb->flags |= MFC_BUF_FLAG_USED;
+	s5p_mfc_set_dec_stream_buffer_v5(ctx,
+		vb2_dma_contig_plane_dma_addr(&temp_vb->b->vb2_buf, 0),
+		ctx->consumed_stream, temp_vb->b->vb2_buf.planes[0].bytesused);
+	spin_unlock_irqrestore(&dev->irqlock, flags);
+	dev->curr_ctx = ctx->num;
+	if (temp_vb->b->vb2_buf.planes[0].bytesused == 0) {
+		last_frame = MFC_DEC_LAST_FRAME;
+		mfc_debug(2, "Setting ctx->state to FINISHING\n");
+		ctx->state = MFCINST_FINISHING;
+	}
+	s5p_mfc_decode_one_frame_v5(ctx, last_frame);
+	return 0;
+}
+
+static int s5p_mfc_run_enc_frame(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	unsigned long flags;
+	struct s5p_mfc_buf *dst_mb;
+	struct s5p_mfc_buf *src_mb;
+	unsigned long src_y_addr, src_c_addr, dst_addr;
+	unsigned int dst_size;
+
+	spin_lock_irqsave(&dev->irqlock, flags);
+	if (list_empty(&ctx->src_queue) && ctx->state != MFCINST_FINISHING) {
+		mfc_debug(2, "no src buffers\n");
+		spin_unlock_irqrestore(&dev->irqlock, flags);
+		return -EAGAIN;
+	}
+	if (list_empty(&ctx->dst_queue)) {
+		mfc_debug(2, "no dst buffers\n");
+		spin_unlock_irqrestore(&dev->irqlock, flags);
+		return -EAGAIN;
+	}
+	if (list_empty(&ctx->src_queue)) {
+		/* send null frame */
+		s5p_mfc_set_enc_frame_buffer_v5(ctx, dev->bank2, dev->bank2);
+		src_mb = NULL;
+	} else {
+		src_mb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf,
+									list);
+		src_mb->flags |= MFC_BUF_FLAG_USED;
+		if (src_mb->b->vb2_buf.planes[0].bytesused == 0) {
+			/* send null frame */
+			s5p_mfc_set_enc_frame_buffer_v5(ctx, dev->bank2,
+								dev->bank2);
+			ctx->state = MFCINST_FINISHING;
+		} else {
+			src_y_addr = vb2_dma_contig_plane_dma_addr(
+					&src_mb->b->vb2_buf, 0);
+			src_c_addr = vb2_dma_contig_plane_dma_addr(
+					&src_mb->b->vb2_buf, 1);
+			s5p_mfc_set_enc_frame_buffer_v5(ctx, src_y_addr,
+								src_c_addr);
+			if (src_mb->flags & MFC_BUF_FLAG_EOS)
+				ctx->state = MFCINST_FINISHING;
+		}
+	}
+	dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
+	dst_mb->flags |= MFC_BUF_FLAG_USED;
+	dst_addr = vb2_dma_contig_plane_dma_addr(&dst_mb->b->vb2_buf, 0);
+	dst_size = vb2_plane_size(&dst_mb->b->vb2_buf, 0);
+	s5p_mfc_set_enc_stream_buffer_v5(ctx, dst_addr, dst_size);
+	spin_unlock_irqrestore(&dev->irqlock, flags);
+	dev->curr_ctx = ctx->num;
+	mfc_debug(2, "encoding buffer with index=%d state=%d\n",
+		  src_mb ? src_mb->b->vb2_buf.index : -1, ctx->state);
+	s5p_mfc_encode_one_frame_v5(ctx);
+	return 0;
+}
+
+static void s5p_mfc_run_init_dec(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	unsigned long flags;
+	struct s5p_mfc_buf *temp_vb;
+
+	/* Initializing decoding - parsing header */
+	spin_lock_irqsave(&dev->irqlock, flags);
+	mfc_debug(2, "Preparing to init decoding\n");
+	temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
+	s5p_mfc_set_dec_desc_buffer(ctx);
+	mfc_debug(2, "Header size: %d\n",
+			temp_vb->b->vb2_buf.planes[0].bytesused);
+	s5p_mfc_set_dec_stream_buffer_v5(ctx,
+			vb2_dma_contig_plane_dma_addr(&temp_vb->b->vb2_buf, 0),
+			0, temp_vb->b->vb2_buf.planes[0].bytesused);
+	spin_unlock_irqrestore(&dev->irqlock, flags);
+	dev->curr_ctx = ctx->num;
+	s5p_mfc_init_decode_v5(ctx);
+}
+
+static void s5p_mfc_run_init_enc(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	unsigned long flags;
+	struct s5p_mfc_buf *dst_mb;
+	unsigned long dst_addr;
+	unsigned int dst_size;
+
+	s5p_mfc_set_enc_ref_buffer_v5(ctx);
+	spin_lock_irqsave(&dev->irqlock, flags);
+	dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
+	dst_addr = vb2_dma_contig_plane_dma_addr(&dst_mb->b->vb2_buf, 0);
+	dst_size = vb2_plane_size(&dst_mb->b->vb2_buf, 0);
+	s5p_mfc_set_enc_stream_buffer_v5(ctx, dst_addr, dst_size);
+	spin_unlock_irqrestore(&dev->irqlock, flags);
+	dev->curr_ctx = ctx->num;
+	s5p_mfc_init_encode_v5(ctx);
+}
+
+static int s5p_mfc_run_init_dec_buffers(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	unsigned long flags;
+	struct s5p_mfc_buf *temp_vb;
+	int ret;
+
+	/*
+	 * Header was parsed now starting processing
+	 * First set the output frame buffers
+	 */
+	if (ctx->capture_state != QUEUE_BUFS_MMAPED) {
+		mfc_err("It seems that not all destionation buffers were "
+			"mmaped\nMFC requires that all destination are mmaped "
+			"before starting processing\n");
+		return -EAGAIN;
+	}
+	spin_lock_irqsave(&dev->irqlock, flags);
+	if (list_empty(&ctx->src_queue)) {
+		mfc_err("Header has been deallocated in the middle of"
+			" initialization\n");
+		spin_unlock_irqrestore(&dev->irqlock, flags);
+		return -EIO;
+	}
+	temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
+	mfc_debug(2, "Header size: %d\n",
+			temp_vb->b->vb2_buf.planes[0].bytesused);
+	s5p_mfc_set_dec_stream_buffer_v5(ctx,
+			vb2_dma_contig_plane_dma_addr(&temp_vb->b->vb2_buf, 0),
+			0, temp_vb->b->vb2_buf.planes[0].bytesused);
+	spin_unlock_irqrestore(&dev->irqlock, flags);
+	dev->curr_ctx = ctx->num;
+	ret = s5p_mfc_set_dec_frame_buffer_v5(ctx);
+	if (ret) {
+		mfc_err("Failed to alloc frame mem\n");
+		ctx->state = MFCINST_ERROR;
+	}
+	return ret;
+}
+
+/* Try running an operation on hardware */
+static void s5p_mfc_try_run_v5(struct s5p_mfc_dev *dev)
+{
+	struct s5p_mfc_ctx *ctx;
+	int new_ctx;
+	unsigned int ret = 0;
+
+	if (test_bit(0, &dev->enter_suspend)) {
+		mfc_debug(1, "Entering suspend so do not schedule any jobs\n");
+		return;
+	}
+	/* Check whether hardware is not running */
+	if (test_and_set_bit(0, &dev->hw_lock) != 0) {
+		/* This is perfectly ok, the scheduled ctx should wait */
+		mfc_debug(1, "Couldn't lock HW\n");
+		return;
+	}
+	/* Choose the context to run */
+	new_ctx = s5p_mfc_get_new_ctx(dev);
+	if (new_ctx < 0) {
+		/* No contexts to run */
+		if (test_and_clear_bit(0, &dev->hw_lock) == 0) {
+			mfc_err("Failed to unlock hardware\n");
+			return;
+		}
+		mfc_debug(1, "No ctx is scheduled to be run\n");
+		return;
+	}
+	ctx = dev->ctx[new_ctx];
+	/* Got context to run in ctx */
+	/*
+	 * Last frame has already been sent to MFC.
+	 * Now obtaining frames from MFC buffer
+	 */
+	s5p_mfc_clock_on();
+	s5p_mfc_clean_ctx_int_flags(ctx);
+
+	if (ctx->type == MFCINST_DECODER) {
+		s5p_mfc_set_dec_desc_buffer(ctx);
+		switch (ctx->state) {
+		case MFCINST_FINISHING:
+			s5p_mfc_run_dec_frame(ctx, MFC_DEC_LAST_FRAME);
+			break;
+		case MFCINST_RUNNING:
+			ret = s5p_mfc_run_dec_frame(ctx, MFC_DEC_FRAME);
+			break;
+		case MFCINST_INIT:
+			ret = s5p_mfc_hw_call(dev->mfc_cmds, open_inst_cmd,
+					ctx);
+			break;
+		case MFCINST_RETURN_INST:
+			ret = s5p_mfc_hw_call(dev->mfc_cmds, close_inst_cmd,
+					ctx);
+			break;
+		case MFCINST_GOT_INST:
+			s5p_mfc_run_init_dec(ctx);
+			break;
+		case MFCINST_HEAD_PARSED:
+			ret = s5p_mfc_run_init_dec_buffers(ctx);
+			mfc_debug(1, "head parsed\n");
+			break;
+		case MFCINST_RES_CHANGE_INIT:
+			s5p_mfc_run_res_change(ctx);
+			break;
+		case MFCINST_RES_CHANGE_FLUSH:
+			s5p_mfc_run_dec_frame(ctx, MFC_DEC_FRAME);
+			break;
+		case MFCINST_RES_CHANGE_END:
+			mfc_debug(2, "Finished remaining frames after resolution change\n");
+			ctx->capture_state = QUEUE_FREE;
+			mfc_debug(2, "Will re-init the codec\n");
+			s5p_mfc_run_init_dec(ctx);
+			break;
+		default:
+			ret = -EAGAIN;
+		}
+	} else if (ctx->type == MFCINST_ENCODER) {
+		switch (ctx->state) {
+		case MFCINST_FINISHING:
+		case MFCINST_RUNNING:
+			ret = s5p_mfc_run_enc_frame(ctx);
+			break;
+		case MFCINST_INIT:
+			ret = s5p_mfc_hw_call(dev->mfc_cmds, open_inst_cmd,
+					ctx);
+			break;
+		case MFCINST_RETURN_INST:
+			ret = s5p_mfc_hw_call(dev->mfc_cmds, close_inst_cmd,
+					ctx);
+			break;
+		case MFCINST_GOT_INST:
+			s5p_mfc_run_init_enc(ctx);
+			break;
+		default:
+			ret = -EAGAIN;
+		}
+	} else {
+		mfc_err("Invalid context type: %d\n", ctx->type);
+		ret = -EAGAIN;
+	}
+
+	if (ret) {
+		/* Free hardware lock */
+		if (test_and_clear_bit(0, &dev->hw_lock) == 0)
+			mfc_err("Failed to unlock hardware\n");
+
+		/* This is in deed imporant, as no operation has been
+		 * scheduled, reduce the clock count as no one will
+		 * ever do this, because no interrupt related to this try_run
+		 * will ever come from hardware. */
+		s5p_mfc_clock_off();
+	}
+}
+
+
+static void s5p_mfc_cleanup_queue_v5(struct list_head *lh, struct vb2_queue *vq)
+{
+	struct s5p_mfc_buf *b;
+	int i;
+
+	while (!list_empty(lh)) {
+		b = list_entry(lh->next, struct s5p_mfc_buf, list);
+		for (i = 0; i < b->b->vb2_buf.num_planes; i++)
+			vb2_set_plane_payload(&b->b->vb2_buf, i, 0);
+		vb2_buffer_done(&b->b->vb2_buf, VB2_BUF_STATE_ERROR);
+		list_del(&b->list);
+	}
+}
+
+static void s5p_mfc_clear_int_flags_v5(struct s5p_mfc_dev *dev)
+{
+	mfc_write(dev, 0, S5P_FIMV_RISC_HOST_INT);
+	mfc_write(dev, 0, S5P_FIMV_RISC2HOST_CMD);
+	mfc_write(dev, 0xffff, S5P_FIMV_SI_RTN_CHID);
+}
+
+static int s5p_mfc_get_dspl_y_adr_v5(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_SI_DISPLAY_Y_ADR) << MFC_OFFSET_SHIFT;
+}
+
+static int s5p_mfc_get_dec_y_adr_v5(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_SI_DECODE_Y_ADR) << MFC_OFFSET_SHIFT;
+}
+
+static int s5p_mfc_get_dspl_status_v5(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_SI_DISPLAY_STATUS);
+}
+
+static int s5p_mfc_get_dec_status_v5(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_SI_DECODE_STATUS);
+}
+
+static int s5p_mfc_get_dec_frame_type_v5(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_DECODE_FRAME_TYPE) &
+		S5P_FIMV_DECODE_FRAME_MASK;
+}
+
+static int s5p_mfc_get_disp_frame_type_v5(struct s5p_mfc_ctx *ctx)
+{
+	return (s5p_mfc_read_info_v5(ctx, DISP_PIC_FRAME_TYPE) >>
+			S5P_FIMV_SHARED_DISP_FRAME_TYPE_SHIFT) &
+			S5P_FIMV_DECODE_FRAME_MASK;
+}
+
+static int s5p_mfc_get_consumed_stream_v5(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_SI_CONSUMED_BYTES);
+}
+
+static int s5p_mfc_get_int_reason_v5(struct s5p_mfc_dev *dev)
+{
+	int reason;
+	reason = mfc_read(dev, S5P_FIMV_RISC2HOST_CMD) &
+		S5P_FIMV_RISC2HOST_CMD_MASK;
+	switch (reason) {
+	case S5P_FIMV_R2H_CMD_OPEN_INSTANCE_RET:
+		reason = S5P_MFC_R2H_CMD_OPEN_INSTANCE_RET;
+		break;
+	case S5P_FIMV_R2H_CMD_CLOSE_INSTANCE_RET:
+		reason = S5P_MFC_R2H_CMD_CLOSE_INSTANCE_RET;
+		break;
+	case S5P_FIMV_R2H_CMD_SEQ_DONE_RET:
+		reason = S5P_MFC_R2H_CMD_SEQ_DONE_RET;
+		break;
+	case S5P_FIMV_R2H_CMD_FRAME_DONE_RET:
+		reason = S5P_MFC_R2H_CMD_FRAME_DONE_RET;
+		break;
+	case S5P_FIMV_R2H_CMD_SLICE_DONE_RET:
+		reason = S5P_MFC_R2H_CMD_SLICE_DONE_RET;
+		break;
+	case S5P_FIMV_R2H_CMD_SYS_INIT_RET:
+		reason = S5P_MFC_R2H_CMD_SYS_INIT_RET;
+		break;
+	case S5P_FIMV_R2H_CMD_FW_STATUS_RET:
+		reason = S5P_MFC_R2H_CMD_FW_STATUS_RET;
+		break;
+	case S5P_FIMV_R2H_CMD_SLEEP_RET:
+		reason = S5P_MFC_R2H_CMD_SLEEP_RET;
+		break;
+	case S5P_FIMV_R2H_CMD_WAKEUP_RET:
+		reason = S5P_MFC_R2H_CMD_WAKEUP_RET;
+		break;
+	case S5P_FIMV_R2H_CMD_INIT_BUFFERS_RET:
+		reason = S5P_MFC_R2H_CMD_INIT_BUFFERS_RET;
+		break;
+	case S5P_FIMV_R2H_CMD_ENC_COMPLETE_RET:
+		reason = S5P_MFC_R2H_CMD_COMPLETE_SEQ_RET;
+		break;
+	case S5P_FIMV_R2H_CMD_ERR_RET:
+		reason = S5P_MFC_R2H_CMD_ERR_RET;
+		break;
+	default:
+		reason = S5P_MFC_R2H_CMD_EMPTY;
+	}
+	return reason;
+}
+
+static int s5p_mfc_get_int_err_v5(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_RISC2HOST_ARG2);
+}
+
+static int s5p_mfc_err_dec_v5(unsigned int err)
+{
+	return (err & S5P_FIMV_ERR_DEC_MASK) >> S5P_FIMV_ERR_DEC_SHIFT;
+}
+
+static int s5p_mfc_err_dspl_v5(unsigned int err)
+{
+	return (err & S5P_FIMV_ERR_DSPL_MASK) >> S5P_FIMV_ERR_DSPL_SHIFT;
+}
+
+static int s5p_mfc_get_img_width_v5(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_SI_HRESOL);
+}
+
+static int s5p_mfc_get_img_height_v5(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_SI_VRESOL);
+}
+
+static int s5p_mfc_get_dpb_count_v5(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_SI_BUF_NUMBER);
+}
+
+static int s5p_mfc_get_mv_count_v5(struct s5p_mfc_dev *dev)
+{
+	/* NOP */
+	return -1;
+}
+
+static int s5p_mfc_get_inst_no_v5(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_RISC2HOST_ARG1);
+}
+
+static int s5p_mfc_get_enc_strm_size_v5(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_ENC_SI_STRM_SIZE);
+}
+
+static int s5p_mfc_get_enc_slice_type_v5(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_ENC_SI_SLICE_TYPE);
+}
+
+static int s5p_mfc_get_enc_dpb_count_v5(struct s5p_mfc_dev *dev)
+{
+	return -1;
+}
+
+static int s5p_mfc_get_enc_pic_count_v5(struct s5p_mfc_dev *dev)
+{
+	return mfc_read(dev, S5P_FIMV_ENC_SI_PIC_CNT);
+}
+
+static int s5p_mfc_get_sei_avail_status_v5(struct s5p_mfc_ctx *ctx)
+{
+	return s5p_mfc_read_info_v5(ctx, FRAME_PACK_SEI_AVAIL);
+}
+
+static int s5p_mfc_get_mvc_num_views_v5(struct s5p_mfc_dev *dev)
+{
+	return -1;
+}
+
+static int s5p_mfc_get_mvc_view_id_v5(struct s5p_mfc_dev *dev)
+{
+	return -1;
+}
+
+static unsigned int s5p_mfc_get_pic_type_top_v5(struct s5p_mfc_ctx *ctx)
+{
+	return s5p_mfc_read_info_v5(ctx, PIC_TIME_TOP);
+}
+
+static unsigned int s5p_mfc_get_pic_type_bot_v5(struct s5p_mfc_ctx *ctx)
+{
+	return s5p_mfc_read_info_v5(ctx, PIC_TIME_BOT);
+}
+
+static unsigned int s5p_mfc_get_crop_info_h_v5(struct s5p_mfc_ctx *ctx)
+{
+	return s5p_mfc_read_info_v5(ctx, CROP_INFO_H);
+}
+
+static unsigned int s5p_mfc_get_crop_info_v_v5(struct s5p_mfc_ctx *ctx)
+{
+	return s5p_mfc_read_info_v5(ctx, CROP_INFO_V);
+}
+
+/* Initialize opr function pointers for MFC v5 */
+static struct s5p_mfc_hw_ops s5p_mfc_ops_v5 = {
+	.alloc_dec_temp_buffers = s5p_mfc_alloc_dec_temp_buffers_v5,
+	.release_dec_desc_buffer = s5p_mfc_release_dec_desc_buffer_v5,
+	.alloc_codec_buffers = s5p_mfc_alloc_codec_buffers_v5,
+	.release_codec_buffers = s5p_mfc_release_codec_buffers_v5,
+	.alloc_instance_buffer = s5p_mfc_alloc_instance_buffer_v5,
+	.release_instance_buffer = s5p_mfc_release_instance_buffer_v5,
+	.alloc_dev_context_buffer = s5p_mfc_alloc_dev_context_buffer_v5,
+	.release_dev_context_buffer = s5p_mfc_release_dev_context_buffer_v5,
+	.dec_calc_dpb_size = s5p_mfc_dec_calc_dpb_size_v5,
+	.enc_calc_src_size = s5p_mfc_enc_calc_src_size_v5,
+	.set_dec_stream_buffer = s5p_mfc_set_dec_stream_buffer_v5,
+	.set_dec_frame_buffer = s5p_mfc_set_dec_frame_buffer_v5,
+	.set_enc_stream_buffer = s5p_mfc_set_enc_stream_buffer_v5,
+	.set_enc_frame_buffer = s5p_mfc_set_enc_frame_buffer_v5,
+	.get_enc_frame_buffer = s5p_mfc_get_enc_frame_buffer_v5,
+	.set_enc_ref_buffer = s5p_mfc_set_enc_ref_buffer_v5,
+	.init_decode = s5p_mfc_init_decode_v5,
+	.init_encode = s5p_mfc_init_encode_v5,
+	.encode_one_frame = s5p_mfc_encode_one_frame_v5,
+	.try_run = s5p_mfc_try_run_v5,
+	.cleanup_queue = s5p_mfc_cleanup_queue_v5,
+	.clear_int_flags = s5p_mfc_clear_int_flags_v5,
+	.write_info = s5p_mfc_write_info_v5,
+	.read_info = s5p_mfc_read_info_v5,
+	.get_dspl_y_adr = s5p_mfc_get_dspl_y_adr_v5,
+	.get_dec_y_adr = s5p_mfc_get_dec_y_adr_v5,
+	.get_dspl_status = s5p_mfc_get_dspl_status_v5,
+	.get_dec_status = s5p_mfc_get_dec_status_v5,
+	.get_dec_frame_type = s5p_mfc_get_dec_frame_type_v5,
+	.get_disp_frame_type = s5p_mfc_get_disp_frame_type_v5,
+	.get_consumed_stream = s5p_mfc_get_consumed_stream_v5,
+	.get_int_reason = s5p_mfc_get_int_reason_v5,
+	.get_int_err = s5p_mfc_get_int_err_v5,
+	.err_dec = s5p_mfc_err_dec_v5,
+	.err_dspl = s5p_mfc_err_dspl_v5,
+	.get_img_width = s5p_mfc_get_img_width_v5,
+	.get_img_height = s5p_mfc_get_img_height_v5,
+	.get_dpb_count = s5p_mfc_get_dpb_count_v5,
+	.get_mv_count = s5p_mfc_get_mv_count_v5,
+	.get_inst_no = s5p_mfc_get_inst_no_v5,
+	.get_enc_strm_size = s5p_mfc_get_enc_strm_size_v5,
+	.get_enc_slice_type = s5p_mfc_get_enc_slice_type_v5,
+	.get_enc_dpb_count = s5p_mfc_get_enc_dpb_count_v5,
+	.get_enc_pic_count = s5p_mfc_get_enc_pic_count_v5,
+	.get_sei_avail_status = s5p_mfc_get_sei_avail_status_v5,
+	.get_mvc_num_views = s5p_mfc_get_mvc_num_views_v5,
+	.get_mvc_view_id = s5p_mfc_get_mvc_view_id_v5,
+	.get_pic_type_top = s5p_mfc_get_pic_type_top_v5,
+	.get_pic_type_bot = s5p_mfc_get_pic_type_bot_v5,
+	.get_crop_info_h = s5p_mfc_get_crop_info_h_v5,
+	.get_crop_info_v = s5p_mfc_get_crop_info_v_v5,
+};
+
+struct s5p_mfc_hw_ops *s5p_mfc_init_hw_ops_v5(void)
+{
+	return &s5p_mfc_ops_v5;
+}
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.h b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.h
new file mode 100644
index 0000000..ffee39a
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.h
@@ -0,0 +1,85 @@
+/*
+ * drivers/media/platform/samsung/mfc5/s5p_mfc_opr_v5.h
+ *
+ * Header file for Samsung MFC (Multi Function Codec - FIMV) driver
+ * Contains declarations of hw related functions.
+ *
+ * Kamil Debski, Copyright (C) 2011 Samsung Electronics
+ * http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef S5P_MFC_OPR_V5_H_
+#define S5P_MFC_OPR_V5_H_
+
+#include "s5p_mfc_common.h"
+#include "s5p_mfc_opr.h"
+
+enum MFC_SHM_OFS {
+	EXTENEDED_DECODE_STATUS	= 0x00,	/* D */
+	SET_FRAME_TAG		= 0x04, /* D */
+	GET_FRAME_TAG_TOP	= 0x08, /* D */
+	GET_FRAME_TAG_BOT	= 0x0C, /* D */
+	PIC_TIME_TOP		= 0x10, /* D */
+	PIC_TIME_BOT		= 0x14, /* D */
+	START_BYTE_NUM		= 0x18, /* D */
+
+	CROP_INFO_H		= 0x20, /* D */
+	CROP_INFO_V		= 0x24, /* D */
+	EXT_ENC_CONTROL		= 0x28,	/* E */
+	ENC_PARAM_CHANGE	= 0x2C,	/* E */
+	RC_VOP_TIMING		= 0x30,	/* E, MPEG4 */
+	HEC_PERIOD		= 0x34,	/* E, MPEG4 */
+	METADATA_ENABLE		= 0x38, /* C */
+	METADATA_STATUS		= 0x3C, /* C */
+	METADATA_DISPLAY_INDEX	= 0x40,	/* C */
+	EXT_METADATA_START_ADDR	= 0x44, /* C */
+	PUT_EXTRADATA		= 0x48, /* C */
+	EXTRADATA_ADDR		= 0x4C, /* C */
+
+	ALLOC_LUMA_DPB_SIZE	= 0x64,	/* D */
+	ALLOC_CHROMA_DPB_SIZE	= 0x68,	/* D */
+	ALLOC_MV_SIZE		= 0x6C,	/* D */
+	P_B_FRAME_QP		= 0x70,	/* E */
+	SAMPLE_ASPECT_RATIO_IDC	= 0x74, /* E, H.264, depend on
+				ASPECT_RATIO_VUI_ENABLE in EXT_ENC_CONTROL */
+	EXTENDED_SAR		= 0x78, /* E, H.264, depned on
+				ASPECT_RATIO_VUI_ENABLE in EXT_ENC_CONTROL */
+	DISP_PIC_PROFILE	= 0x7C, /* D */
+	FLUSH_CMD_TYPE		= 0x80, /* C */
+	FLUSH_CMD_INBUF1	= 0x84, /* C */
+	FLUSH_CMD_INBUF2	= 0x88, /* C */
+	FLUSH_CMD_OUTBUF	= 0x8C, /* E */
+	NEW_RC_BIT_RATE		= 0x90, /* E, format as RC_BIT_RATE(0xC5A8)
+			depend on RC_BIT_RATE_CHANGE in ENC_PARAM_CHANGE */
+	NEW_RC_FRAME_RATE	= 0x94, /* E, format as RC_FRAME_RATE(0xD0D0)
+			depend on RC_FRAME_RATE_CHANGE in ENC_PARAM_CHANGE */
+	NEW_I_PERIOD		= 0x98, /* E, format as I_FRM_CTRL(0xC504)
+			depend on I_PERIOD_CHANGE in ENC_PARAM_CHANGE */
+	H264_I_PERIOD		= 0x9C, /* E, H.264, open GOP */
+	RC_CONTROL_CONFIG	= 0xA0, /* E */
+	BATCH_INPUT_ADDR	= 0xA4, /* E */
+	BATCH_OUTPUT_ADDR	= 0xA8, /* E */
+	BATCH_OUTPUT_SIZE	= 0xAC, /* E */
+	MIN_LUMA_DPB_SIZE	= 0xB0, /* D */
+	DEVICE_FORMAT_ID	= 0xB4, /* C */
+	H264_POC_TYPE		= 0xB8, /* D */
+	MIN_CHROMA_DPB_SIZE	= 0xBC, /* D */
+	DISP_PIC_FRAME_TYPE	= 0xC0, /* D */
+	FREE_LUMA_DPB		= 0xC4, /* D, VC1 MPEG4 */
+	ASPECT_RATIO_INFO	= 0xC8, /* D, MPEG4 */
+	EXTENDED_PAR		= 0xCC, /* D, MPEG4 */
+	DBG_HISTORY_INPUT0	= 0xD0, /* C */
+	DBG_HISTORY_INPUT1	= 0xD4,	/* C */
+	DBG_HISTORY_OUTPUT	= 0xD8,	/* C */
+	HIERARCHICAL_P_QP	= 0xE0, /* E, H.264 */
+	FRAME_PACK_SEI_ENABLE	= 0x168, /* C */
+	FRAME_PACK_SEI_AVAIL	= 0x16c, /* D */
+	FRAME_PACK_SEI_INFO	= 0x17c, /* E */
+};
+
+struct s5p_mfc_hw_ops *s5p_mfc_init_hw_ops_v5(void);
+#endif /* S5P_MFC_OPR_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
new file mode 100644
index 0000000..b958453
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
@@ -0,0 +1,2331 @@
+/*
+ * drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
+ *
+ * Samsung MFC (Multi Function Codec - FIMV) driver
+ * This file contains hw related functions.
+ *
+ * Copyright (c) 2012 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#undef DEBUG
+
+#include <linux/delay.h>
+#include <linux/mm.h>
+#include <linux/io.h>
+#include <linux/jiffies.h>
+#include <linux/firmware.h>
+#include <linux/err.h>
+#include <linux/sched.h>
+#include <linux/dma-mapping.h>
+
+#include <asm/cacheflush.h>
+
+#include "s5p_mfc_common.h"
+#include "s5p_mfc_cmd.h"
+#include "s5p_mfc_intr.h"
+#include "s5p_mfc_pm.h"
+#include "s5p_mfc_debug.h"
+#include "s5p_mfc_opr.h"
+#include "s5p_mfc_opr_v6.h"
+
+/* #define S5P_MFC_DEBUG_REGWRITE  */
+#ifdef S5P_MFC_DEBUG_REGWRITE
+#undef writel
+#define writel(v, r)							\
+	do {								\
+		pr_err("MFCWRITE(%p): %08x\n", r, (unsigned int)v);	\
+	__raw_writel(v, r);						\
+	} while (0)
+#endif /* S5P_MFC_DEBUG_REGWRITE */
+
+#define IS_MFCV6_V2(dev) (!IS_MFCV7_PLUS(dev) && dev->fw_ver == MFC_FW_V2)
+
+/* Allocate temporary buffers for decoding */
+static int s5p_mfc_alloc_dec_temp_buffers_v6(struct s5p_mfc_ctx *ctx)
+{
+	/* NOP */
+
+	return 0;
+}
+
+/* Release temproary buffers for decoding */
+static void s5p_mfc_release_dec_desc_buffer_v6(struct s5p_mfc_ctx *ctx)
+{
+	/* NOP */
+}
+
+/* Allocate codec buffers */
+static int s5p_mfc_alloc_codec_buffers_v6(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	unsigned int mb_width, mb_height;
+	int ret;
+
+	mb_width = MB_WIDTH(ctx->img_width);
+	mb_height = MB_HEIGHT(ctx->img_height);
+
+	if (ctx->type == MFCINST_DECODER) {
+		mfc_debug(2, "Luma size:%d Chroma size:%d MV size:%d\n",
+			  ctx->luma_size, ctx->chroma_size, ctx->mv_size);
+		mfc_debug(2, "Totals bufs: %d\n", ctx->total_dpb_count);
+	} else if (ctx->type == MFCINST_ENCODER) {
+		if (IS_MFCV8(dev))
+			ctx->tmv_buffer_size = S5P_FIMV_NUM_TMV_BUFFERS_V6 *
+			ALIGN(S5P_FIMV_TMV_BUFFER_SIZE_V8(mb_width, mb_height),
+			S5P_FIMV_TMV_BUFFER_ALIGN_V6);
+		else
+			ctx->tmv_buffer_size = S5P_FIMV_NUM_TMV_BUFFERS_V6 *
+			ALIGN(S5P_FIMV_TMV_BUFFER_SIZE_V6(mb_width, mb_height),
+			S5P_FIMV_TMV_BUFFER_ALIGN_V6);
+
+		ctx->luma_dpb_size = ALIGN((mb_width * mb_height) *
+				S5P_FIMV_LUMA_MB_TO_PIXEL_V6,
+				S5P_FIMV_LUMA_DPB_BUFFER_ALIGN_V6);
+		ctx->chroma_dpb_size = ALIGN((mb_width * mb_height) *
+				S5P_FIMV_CHROMA_MB_TO_PIXEL_V6,
+				S5P_FIMV_CHROMA_DPB_BUFFER_ALIGN_V6);
+		if (IS_MFCV8(dev))
+			ctx->me_buffer_size = ALIGN(S5P_FIMV_ME_BUFFER_SIZE_V8(
+						ctx->img_width, ctx->img_height,
+						mb_width, mb_height),
+						S5P_FIMV_ME_BUFFER_ALIGN_V6);
+		else
+			ctx->me_buffer_size = ALIGN(S5P_FIMV_ME_BUFFER_SIZE_V6(
+						ctx->img_width, ctx->img_height,
+						mb_width, mb_height),
+						S5P_FIMV_ME_BUFFER_ALIGN_V6);
+
+		mfc_debug(2, "recon luma size: %zu chroma size: %zu\n",
+			  ctx->luma_dpb_size, ctx->chroma_dpb_size);
+	} else {
+		return -EINVAL;
+	}
+
+	/* Codecs have different memory requirements */
+	switch (ctx->codec_mode) {
+	case S5P_MFC_CODEC_H264_DEC:
+	case S5P_MFC_CODEC_H264_MVC_DEC:
+		if (IS_MFCV8(dev))
+			ctx->scratch_buf_size =
+				S5P_FIMV_SCRATCH_BUF_SIZE_H264_DEC_V8(
+					mb_width,
+					mb_height);
+		else
+			ctx->scratch_buf_size =
+				S5P_FIMV_SCRATCH_BUF_SIZE_H264_DEC_V6(
+					mb_width,
+					mb_height);
+		ctx->scratch_buf_size = ALIGN(ctx->scratch_buf_size,
+				S5P_FIMV_SCRATCH_BUFFER_ALIGN_V6);
+		ctx->bank1.size =
+			ctx->scratch_buf_size +
+			(ctx->mv_count * ctx->mv_size);
+		break;
+	case S5P_MFC_CODEC_MPEG4_DEC:
+		if (IS_MFCV7_PLUS(dev)) {
+			ctx->scratch_buf_size =
+				S5P_FIMV_SCRATCH_BUF_SIZE_MPEG4_DEC_V7(
+						mb_width,
+						mb_height);
+		} else {
+			ctx->scratch_buf_size =
+				S5P_FIMV_SCRATCH_BUF_SIZE_MPEG4_DEC_V6(
+						mb_width,
+						mb_height);
+		}
+
+		ctx->scratch_buf_size = ALIGN(ctx->scratch_buf_size,
+				S5P_FIMV_SCRATCH_BUFFER_ALIGN_V6);
+		ctx->bank1.size = ctx->scratch_buf_size;
+		break;
+	case S5P_MFC_CODEC_VC1RCV_DEC:
+	case S5P_MFC_CODEC_VC1_DEC:
+		ctx->scratch_buf_size =
+			S5P_FIMV_SCRATCH_BUF_SIZE_VC1_DEC_V6(
+					mb_width,
+					mb_height);
+		ctx->scratch_buf_size = ALIGN(ctx->scratch_buf_size,
+				S5P_FIMV_SCRATCH_BUFFER_ALIGN_V6);
+		ctx->bank1.size = ctx->scratch_buf_size;
+		break;
+	case S5P_MFC_CODEC_MPEG2_DEC:
+		ctx->bank1.size = 0;
+		ctx->bank2.size = 0;
+		break;
+	case S5P_MFC_CODEC_H263_DEC:
+		ctx->scratch_buf_size =
+			S5P_FIMV_SCRATCH_BUF_SIZE_H263_DEC_V6(
+					mb_width,
+					mb_height);
+		ctx->scratch_buf_size = ALIGN(ctx->scratch_buf_size,
+				S5P_FIMV_SCRATCH_BUFFER_ALIGN_V6);
+		ctx->bank1.size = ctx->scratch_buf_size;
+		break;
+	case S5P_MFC_CODEC_VP8_DEC:
+		if (IS_MFCV8(dev))
+			ctx->scratch_buf_size =
+				S5P_FIMV_SCRATCH_BUF_SIZE_VP8_DEC_V8(
+						mb_width,
+						mb_height);
+		else
+			ctx->scratch_buf_size =
+				S5P_FIMV_SCRATCH_BUF_SIZE_VP8_DEC_V6(
+						mb_width,
+						mb_height);
+		ctx->scratch_buf_size = ALIGN(ctx->scratch_buf_size,
+				S5P_FIMV_SCRATCH_BUFFER_ALIGN_V6);
+		ctx->bank1.size = ctx->scratch_buf_size;
+		break;
+	case S5P_MFC_CODEC_H264_ENC:
+		if (IS_MFCV8(dev))
+			ctx->scratch_buf_size =
+				S5P_FIMV_SCRATCH_BUF_SIZE_H264_ENC_V8(
+					mb_width,
+					mb_height);
+		else
+			ctx->scratch_buf_size =
+				S5P_FIMV_SCRATCH_BUF_SIZE_H264_ENC_V6(
+						mb_width,
+						mb_height);
+		ctx->scratch_buf_size = ALIGN(ctx->scratch_buf_size,
+				S5P_FIMV_SCRATCH_BUFFER_ALIGN_V6);
+		ctx->bank1.size =
+			ctx->scratch_buf_size + ctx->tmv_buffer_size +
+			(ctx->pb_count * (ctx->luma_dpb_size +
+			ctx->chroma_dpb_size + ctx->me_buffer_size));
+		ctx->bank2.size = 0;
+		break;
+	case S5P_MFC_CODEC_MPEG4_ENC:
+	case S5P_MFC_CODEC_H263_ENC:
+		ctx->scratch_buf_size =
+			S5P_FIMV_SCRATCH_BUF_SIZE_MPEG4_ENC_V6(
+					mb_width,
+					mb_height);
+		ctx->scratch_buf_size = ALIGN(ctx->scratch_buf_size,
+				S5P_FIMV_SCRATCH_BUFFER_ALIGN_V6);
+		ctx->bank1.size =
+			ctx->scratch_buf_size + ctx->tmv_buffer_size +
+			(ctx->pb_count * (ctx->luma_dpb_size +
+			ctx->chroma_dpb_size + ctx->me_buffer_size));
+		ctx->bank2.size = 0;
+		break;
+	case S5P_MFC_CODEC_VP8_ENC:
+		if (IS_MFCV8(dev))
+			ctx->scratch_buf_size =
+				S5P_FIMV_SCRATCH_BUF_SIZE_VP8_ENC_V8(
+					mb_width,
+					mb_height);
+		else
+			ctx->scratch_buf_size =
+				S5P_FIMV_SCRATCH_BUF_SIZE_VP8_ENC_V7(
+						mb_width,
+						mb_height);
+		ctx->scratch_buf_size = ALIGN(ctx->scratch_buf_size,
+				S5P_FIMV_SCRATCH_BUFFER_ALIGN_V6);
+		ctx->bank1.size =
+			ctx->scratch_buf_size + ctx->tmv_buffer_size +
+			(ctx->pb_count * (ctx->luma_dpb_size +
+			ctx->chroma_dpb_size + ctx->me_buffer_size));
+		ctx->bank2.size = 0;
+		break;
+	default:
+		break;
+	}
+
+	/* Allocate only if memory from bank 1 is necessary */
+	if (ctx->bank1.size > 0) {
+		ret = s5p_mfc_alloc_priv_buf(dev->mem_dev_l, dev->bank1,
+					     &ctx->bank1);
+		if (ret) {
+			mfc_err("Failed to allocate Bank1 memory\n");
+			return ret;
+		}
+		BUG_ON(ctx->bank1.dma & ((1 << MFC_BANK1_ALIGN_ORDER) - 1));
+	}
+	return 0;
+}
+
+/* Release buffers allocated for codec */
+static void s5p_mfc_release_codec_buffers_v6(struct s5p_mfc_ctx *ctx)
+{
+	s5p_mfc_release_priv_buf(ctx->dev->mem_dev_l, &ctx->bank1);
+}
+
+/* Allocate memory for instance data buffer */
+static int s5p_mfc_alloc_instance_buffer_v6(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_buf_size_v6 *buf_size = dev->variant->buf_size->priv;
+	int ret;
+
+	mfc_debug_enter();
+
+	switch (ctx->codec_mode) {
+	case S5P_MFC_CODEC_H264_DEC:
+	case S5P_MFC_CODEC_H264_MVC_DEC:
+		ctx->ctx.size = buf_size->h264_dec_ctx;
+		break;
+	case S5P_MFC_CODEC_MPEG4_DEC:
+	case S5P_MFC_CODEC_H263_DEC:
+	case S5P_MFC_CODEC_VC1RCV_DEC:
+	case S5P_MFC_CODEC_VC1_DEC:
+	case S5P_MFC_CODEC_MPEG2_DEC:
+	case S5P_MFC_CODEC_VP8_DEC:
+		ctx->ctx.size = buf_size->other_dec_ctx;
+		break;
+	case S5P_MFC_CODEC_H264_ENC:
+		ctx->ctx.size = buf_size->h264_enc_ctx;
+		break;
+	case S5P_MFC_CODEC_MPEG4_ENC:
+	case S5P_MFC_CODEC_H263_ENC:
+	case S5P_MFC_CODEC_VP8_ENC:
+		ctx->ctx.size = buf_size->other_enc_ctx;
+		break;
+	default:
+		ctx->ctx.size = 0;
+		mfc_err("Codec type(%d) should be checked!\n", ctx->codec_mode);
+		break;
+	}
+
+	ret = s5p_mfc_alloc_priv_buf(dev->mem_dev_l, dev->bank1, &ctx->ctx);
+	if (ret) {
+		mfc_err("Failed to allocate instance buffer\n");
+		return ret;
+	}
+
+	memset(ctx->ctx.virt, 0, ctx->ctx.size);
+	wmb();
+
+	mfc_debug_leave();
+
+	return 0;
+}
+
+/* Release instance buffer */
+static void s5p_mfc_release_instance_buffer_v6(struct s5p_mfc_ctx *ctx)
+{
+	s5p_mfc_release_priv_buf(ctx->dev->mem_dev_l, &ctx->ctx);
+}
+
+/* Allocate context buffers for SYS_INIT */
+static int s5p_mfc_alloc_dev_context_buffer_v6(struct s5p_mfc_dev *dev)
+{
+	struct s5p_mfc_buf_size_v6 *buf_size = dev->variant->buf_size->priv;
+	int ret;
+
+	mfc_debug_enter();
+
+	dev->ctx_buf.size = buf_size->dev_ctx;
+	ret = s5p_mfc_alloc_priv_buf(dev->mem_dev_l, dev->bank1,
+				     &dev->ctx_buf);
+	if (ret) {
+		mfc_err("Failed to allocate device context buffer\n");
+		return ret;
+	}
+
+	memset(dev->ctx_buf.virt, 0, buf_size->dev_ctx);
+	wmb();
+
+	mfc_debug_leave();
+
+	return 0;
+}
+
+/* Release context buffers for SYS_INIT */
+static void s5p_mfc_release_dev_context_buffer_v6(struct s5p_mfc_dev *dev)
+{
+	s5p_mfc_release_priv_buf(dev->mem_dev_l, &dev->ctx_buf);
+}
+
+static int calc_plane(int width, int height)
+{
+	int mbX, mbY;
+
+	mbX = DIV_ROUND_UP(width, S5P_FIMV_NUM_PIXELS_IN_MB_ROW_V6);
+	mbY = DIV_ROUND_UP(height, S5P_FIMV_NUM_PIXELS_IN_MB_COL_V6);
+
+	if (width * height < S5P_FIMV_MAX_FRAME_SIZE_V6)
+		mbY = (mbY + 1) / 2 * 2;
+
+	return (mbX * S5P_FIMV_NUM_PIXELS_IN_MB_COL_V6) *
+		(mbY * S5P_FIMV_NUM_PIXELS_IN_MB_ROW_V6);
+}
+
+static void s5p_mfc_dec_calc_dpb_size_v6(struct s5p_mfc_ctx *ctx)
+{
+	ctx->buf_width = ALIGN(ctx->img_width, S5P_FIMV_NV12MT_HALIGN_V6);
+	ctx->buf_height = ALIGN(ctx->img_height, S5P_FIMV_NV12MT_VALIGN_V6);
+	mfc_debug(2, "SEQ Done: Movie dimensions %dx%d,\n"
+			"buffer dimensions: %dx%d\n", ctx->img_width,
+			ctx->img_height, ctx->buf_width, ctx->buf_height);
+
+	ctx->luma_size = calc_plane(ctx->img_width, ctx->img_height);
+	ctx->chroma_size = calc_plane(ctx->img_width, (ctx->img_height >> 1));
+	if (IS_MFCV8(ctx->dev)) {
+		/* MFCv8 needs additional 64 bytes for luma,chroma dpb*/
+		ctx->luma_size += S5P_FIMV_D_ALIGN_PLANE_SIZE_V8;
+		ctx->chroma_size += S5P_FIMV_D_ALIGN_PLANE_SIZE_V8;
+	}
+
+	if (ctx->codec_mode == S5P_MFC_CODEC_H264_DEC ||
+			ctx->codec_mode == S5P_MFC_CODEC_H264_MVC_DEC) {
+		ctx->mv_size = S5P_MFC_DEC_MV_SIZE_V6(ctx->img_width,
+				ctx->img_height);
+		ctx->mv_size = ALIGN(ctx->mv_size, 16);
+	} else {
+		ctx->mv_size = 0;
+	}
+}
+
+static void s5p_mfc_enc_calc_src_size_v6(struct s5p_mfc_ctx *ctx)
+{
+	unsigned int mb_width, mb_height;
+
+	mb_width = MB_WIDTH(ctx->img_width);
+	mb_height = MB_HEIGHT(ctx->img_height);
+
+	ctx->buf_width = ALIGN(ctx->img_width, S5P_FIMV_NV12M_HALIGN_V6);
+	ctx->luma_size = ALIGN((mb_width * mb_height) * 256, 256);
+	ctx->chroma_size = ALIGN((mb_width * mb_height) * 128, 256);
+
+	/* MFCv7 needs pad bytes for Luma and Chroma */
+	if (IS_MFCV7_PLUS(ctx->dev)) {
+		ctx->luma_size += MFC_LUMA_PAD_BYTES_V7;
+		ctx->chroma_size += MFC_CHROMA_PAD_BYTES_V7;
+	}
+}
+
+/* Set registers for decoding stream buffer */
+static int s5p_mfc_set_dec_stream_buffer_v6(struct s5p_mfc_ctx *ctx,
+			int buf_addr, unsigned int start_num_byte,
+			unsigned int strm_size)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
+	struct s5p_mfc_buf_size *buf_size = dev->variant->buf_size;
+
+	mfc_debug_enter();
+	mfc_debug(2, "inst_no: %d, buf_addr: 0x%08x,\n"
+		"buf_size: 0x%08x (%d)\n",
+		ctx->inst_no, buf_addr, strm_size, strm_size);
+	writel(strm_size, mfc_regs->d_stream_data_size);
+	writel(buf_addr, mfc_regs->d_cpb_buffer_addr);
+	writel(buf_size->cpb, mfc_regs->d_cpb_buffer_size);
+	writel(start_num_byte, mfc_regs->d_cpb_buffer_offset);
+
+	mfc_debug_leave();
+	return 0;
+}
+
+/* Set decoding frame buffer */
+static int s5p_mfc_set_dec_frame_buffer_v6(struct s5p_mfc_ctx *ctx)
+{
+	unsigned int frame_size, i;
+	unsigned int frame_size_ch, frame_size_mv;
+	struct s5p_mfc_dev *dev = ctx->dev;
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
+	size_t buf_addr1;
+	int buf_size1;
+	int align_gap;
+
+	buf_addr1 = ctx->bank1.dma;
+	buf_size1 = ctx->bank1.size;
+
+	mfc_debug(2, "Buf1: %p (%d)\n", (void *)buf_addr1, buf_size1);
+	mfc_debug(2, "Total DPB COUNT: %d\n", ctx->total_dpb_count);
+	mfc_debug(2, "Setting display delay to %d\n", ctx->display_delay);
+
+	writel(ctx->total_dpb_count, mfc_regs->d_num_dpb);
+	writel(ctx->luma_size, mfc_regs->d_first_plane_dpb_size);
+	writel(ctx->chroma_size, mfc_regs->d_second_plane_dpb_size);
+
+	writel(buf_addr1, mfc_regs->d_scratch_buffer_addr);
+	writel(ctx->scratch_buf_size, mfc_regs->d_scratch_buffer_size);
+
+	if (IS_MFCV8(dev)) {
+		writel(ctx->img_width,
+			mfc_regs->d_first_plane_dpb_stride_size);
+		writel(ctx->img_width,
+			mfc_regs->d_second_plane_dpb_stride_size);
+	}
+
+	buf_addr1 += ctx->scratch_buf_size;
+	buf_size1 -= ctx->scratch_buf_size;
+
+	if (ctx->codec_mode == S5P_FIMV_CODEC_H264_DEC ||
+			ctx->codec_mode == S5P_FIMV_CODEC_H264_MVC_DEC){
+		writel(ctx->mv_size, mfc_regs->d_mv_buffer_size);
+		writel(ctx->mv_count, mfc_regs->d_num_mv);
+	}
+
+	frame_size = ctx->luma_size;
+	frame_size_ch = ctx->chroma_size;
+	frame_size_mv = ctx->mv_size;
+	mfc_debug(2, "Frame size: %d ch: %d mv: %d\n",
+			frame_size, frame_size_ch, frame_size_mv);
+
+	for (i = 0; i < ctx->total_dpb_count; i++) {
+		/* Bank2 */
+		mfc_debug(2, "Luma %d: %zx\n", i,
+					ctx->dst_bufs[i].cookie.raw.luma);
+		writel(ctx->dst_bufs[i].cookie.raw.luma,
+				mfc_regs->d_first_plane_dpb + i * 4);
+		mfc_debug(2, "\tChroma %d: %zx\n", i,
+					ctx->dst_bufs[i].cookie.raw.chroma);
+		writel(ctx->dst_bufs[i].cookie.raw.chroma,
+				mfc_regs->d_second_plane_dpb + i * 4);
+	}
+	if (ctx->codec_mode == S5P_MFC_CODEC_H264_DEC ||
+			ctx->codec_mode == S5P_MFC_CODEC_H264_MVC_DEC) {
+		for (i = 0; i < ctx->mv_count; i++) {
+			/* To test alignment */
+			align_gap = buf_addr1;
+			buf_addr1 = ALIGN(buf_addr1, 16);
+			align_gap = buf_addr1 - align_gap;
+			buf_size1 -= align_gap;
+
+			mfc_debug(2, "\tBuf1: %zx, size: %d\n",
+					buf_addr1, buf_size1);
+			writel(buf_addr1, mfc_regs->d_mv_buffer + i * 4);
+			buf_addr1 += frame_size_mv;
+			buf_size1 -= frame_size_mv;
+		}
+	}
+
+	mfc_debug(2, "Buf1: %zu, buf_size1: %d (frames %d)\n",
+			buf_addr1, buf_size1, ctx->total_dpb_count);
+	if (buf_size1 < 0) {
+		mfc_debug(2, "Not enough memory has been allocated.\n");
+		return -ENOMEM;
+	}
+
+	writel(ctx->inst_no, mfc_regs->instance_id);
+	s5p_mfc_hw_call_void(dev->mfc_cmds, cmd_host2risc, dev,
+			S5P_FIMV_CH_INIT_BUFS_V6, NULL);
+
+	mfc_debug(2, "After setting buffers.\n");
+	return 0;
+}
+
+/* Set registers for encoding stream buffer */
+static int s5p_mfc_set_enc_stream_buffer_v6(struct s5p_mfc_ctx *ctx,
+		unsigned long addr, unsigned int size)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
+
+	writel(addr, mfc_regs->e_stream_buffer_addr); /* 16B align */
+	writel(size, mfc_regs->e_stream_buffer_size);
+
+	mfc_debug(2, "stream buf addr: 0x%08lx, size: 0x%x\n",
+		  addr, size);
+
+	return 0;
+}
+
+static void s5p_mfc_set_enc_frame_buffer_v6(struct s5p_mfc_ctx *ctx,
+		unsigned long y_addr, unsigned long c_addr)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
+
+	writel(y_addr, mfc_regs->e_source_first_plane_addr);
+	writel(c_addr, mfc_regs->e_source_second_plane_addr);
+
+	mfc_debug(2, "enc src y buf addr: 0x%08lx\n", y_addr);
+	mfc_debug(2, "enc src c buf addr: 0x%08lx\n", c_addr);
+}
+
+static void s5p_mfc_get_enc_frame_buffer_v6(struct s5p_mfc_ctx *ctx,
+		unsigned long *y_addr, unsigned long *c_addr)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
+	unsigned long enc_recon_y_addr, enc_recon_c_addr;
+
+	*y_addr = readl(mfc_regs->e_encoded_source_first_plane_addr);
+	*c_addr = readl(mfc_regs->e_encoded_source_second_plane_addr);
+
+	enc_recon_y_addr = readl(mfc_regs->e_recon_luma_dpb_addr);
+	enc_recon_c_addr = readl(mfc_regs->e_recon_chroma_dpb_addr);
+
+	mfc_debug(2, "recon y addr: 0x%08lx y_addr: 0x%08lx\n", enc_recon_y_addr, *y_addr);
+	mfc_debug(2, "recon c addr: 0x%08lx\n", enc_recon_c_addr);
+}
+
+/* Set encoding ref & codec buffer */
+static int s5p_mfc_set_enc_ref_buffer_v6(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
+	size_t buf_addr1;
+	int i, buf_size1;
+
+	mfc_debug_enter();
+
+	buf_addr1 = ctx->bank1.dma;
+	buf_size1 = ctx->bank1.size;
+
+	mfc_debug(2, "Buf1: %p (%d)\n", (void *)buf_addr1, buf_size1);
+
+	for (i = 0; i < ctx->pb_count; i++) {
+		writel(buf_addr1, mfc_regs->e_luma_dpb + (4 * i));
+		buf_addr1 += ctx->luma_dpb_size;
+		writel(buf_addr1, mfc_regs->e_chroma_dpb + (4 * i));
+		buf_addr1 += ctx->chroma_dpb_size;
+		writel(buf_addr1, mfc_regs->e_me_buffer + (4 * i));
+		buf_addr1 += ctx->me_buffer_size;
+		buf_size1 -= (ctx->luma_dpb_size + ctx->chroma_dpb_size +
+			ctx->me_buffer_size);
+	}
+
+	writel(buf_addr1, mfc_regs->e_scratch_buffer_addr);
+	writel(ctx->scratch_buf_size, mfc_regs->e_scratch_buffer_size);
+	buf_addr1 += ctx->scratch_buf_size;
+	buf_size1 -= ctx->scratch_buf_size;
+
+	writel(buf_addr1, mfc_regs->e_tmv_buffer0);
+	buf_addr1 += ctx->tmv_buffer_size >> 1;
+	writel(buf_addr1, mfc_regs->e_tmv_buffer1);
+	buf_addr1 += ctx->tmv_buffer_size >> 1;
+	buf_size1 -= ctx->tmv_buffer_size;
+
+	mfc_debug(2, "Buf1: %zu, buf_size1: %d (ref frames %d)\n",
+			buf_addr1, buf_size1, ctx->pb_count);
+	if (buf_size1 < 0) {
+		mfc_debug(2, "Not enough memory has been allocated.\n");
+		return -ENOMEM;
+	}
+
+	writel(ctx->inst_no, mfc_regs->instance_id);
+	s5p_mfc_hw_call_void(dev->mfc_cmds, cmd_host2risc, dev,
+			S5P_FIMV_CH_INIT_BUFS_V6, NULL);
+
+	mfc_debug_leave();
+
+	return 0;
+}
+
+static int s5p_mfc_set_slice_mode(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
+
+	/* multi-slice control */
+	/* multi-slice MB number or bit size */
+	writel(ctx->slice_mode, mfc_regs->e_mslice_mode);
+	if (ctx->slice_mode == V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_MB) {
+		writel(ctx->slice_size.mb, mfc_regs->e_mslice_size_mb);
+	} else if (ctx->slice_mode ==
+			V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_BYTES) {
+		writel(ctx->slice_size.bits, mfc_regs->e_mslice_size_bits);
+	} else {
+		writel(0x0, mfc_regs->e_mslice_size_mb);
+		writel(0x0, mfc_regs->e_mslice_size_bits);
+	}
+
+	return 0;
+}
+
+static int s5p_mfc_set_enc_params(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
+	struct s5p_mfc_enc_params *p = &ctx->enc_params;
+	unsigned int reg = 0;
+
+	mfc_debug_enter();
+
+	/* width */
+	writel(ctx->img_width, mfc_regs->e_frame_width); /* 16 align */
+	/* height */
+	writel(ctx->img_height, mfc_regs->e_frame_height); /* 16 align */
+
+	/* cropped width */
+	writel(ctx->img_width, mfc_regs->e_cropped_frame_width);
+	/* cropped height */
+	writel(ctx->img_height, mfc_regs->e_cropped_frame_height);
+	/* cropped offset */
+	writel(0x0, mfc_regs->e_frame_crop_offset);
+
+	/* pictype : IDR period */
+	reg = 0;
+	reg |= p->gop_size & 0xFFFF;
+	writel(reg, mfc_regs->e_gop_config);
+
+	/* multi-slice control */
+	/* multi-slice MB number or bit size */
+	ctx->slice_mode = p->slice_mode;
+	reg = 0;
+	if (p->slice_mode == V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_MB) {
+		reg |= (0x1 << 3);
+		writel(reg, mfc_regs->e_enc_options);
+		ctx->slice_size.mb = p->slice_mb;
+	} else if (p->slice_mode == V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_BYTES) {
+		reg |= (0x1 << 3);
+		writel(reg, mfc_regs->e_enc_options);
+		ctx->slice_size.bits = p->slice_bit;
+	} else {
+		reg &= ~(0x1 << 3);
+		writel(reg, mfc_regs->e_enc_options);
+	}
+
+	s5p_mfc_set_slice_mode(ctx);
+
+	/* cyclic intra refresh */
+	writel(p->intra_refresh_mb, mfc_regs->e_ir_size);
+	reg = readl(mfc_regs->e_enc_options);
+	if (p->intra_refresh_mb == 0)
+		reg &= ~(0x1 << 4);
+	else
+		reg |= (0x1 << 4);
+	writel(reg, mfc_regs->e_enc_options);
+
+	/* 'NON_REFERENCE_STORE_ENABLE' for debugging */
+	reg = readl(mfc_regs->e_enc_options);
+	reg &= ~(0x1 << 9);
+	writel(reg, mfc_regs->e_enc_options);
+
+	/* memory structure cur. frame */
+	if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12M) {
+		/* 0: Linear, 1: 2D tiled*/
+		reg = readl(mfc_regs->e_enc_options);
+		reg &= ~(0x1 << 7);
+		writel(reg, mfc_regs->e_enc_options);
+		/* 0: NV12(CbCr), 1: NV21(CrCb) */
+		writel(0x0, mfc_regs->pixel_format);
+	} else if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV21M) {
+		/* 0: Linear, 1: 2D tiled*/
+		reg = readl(mfc_regs->e_enc_options);
+		reg &= ~(0x1 << 7);
+		writel(reg, mfc_regs->e_enc_options);
+		/* 0: NV12(CbCr), 1: NV21(CrCb) */
+		writel(0x1, mfc_regs->pixel_format);
+	} else if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12MT_16X16) {
+		/* 0: Linear, 1: 2D tiled*/
+		reg = readl(mfc_regs->e_enc_options);
+		reg |= (0x1 << 7);
+		writel(reg, mfc_regs->e_enc_options);
+		/* 0: NV12(CbCr), 1: NV21(CrCb) */
+		writel(0x0, mfc_regs->pixel_format);
+	}
+
+	/* memory structure recon. frame */
+	/* 0: Linear, 1: 2D tiled */
+	reg = readl(mfc_regs->e_enc_options);
+	reg |= (0x1 << 8);
+	writel(reg, mfc_regs->e_enc_options);
+
+	/* padding control & value */
+	writel(0x0, mfc_regs->e_padding_ctrl);
+	if (p->pad) {
+		reg = 0;
+		/** enable */
+		reg |= (1 << 31);
+		/** cr value */
+		reg |= ((p->pad_cr & 0xFF) << 16);
+		/** cb value */
+		reg |= ((p->pad_cb & 0xFF) << 8);
+		/** y value */
+		reg |= p->pad_luma & 0xFF;
+		writel(reg, mfc_regs->e_padding_ctrl);
+	}
+
+	/* rate control config. */
+	reg = 0;
+	/* frame-level rate control */
+	reg |= ((p->rc_frame & 0x1) << 9);
+	writel(reg, mfc_regs->e_rc_config);
+
+	/* bit rate */
+	if (p->rc_frame)
+		writel(p->rc_bitrate,
+			mfc_regs->e_rc_bit_rate);
+	else
+		writel(1, mfc_regs->e_rc_bit_rate);
+
+	/* reaction coefficient */
+	if (p->rc_frame) {
+		if (p->rc_reaction_coeff < TIGHT_CBR_MAX) /* tight CBR */
+			writel(1, mfc_regs->e_rc_mode);
+		else					  /* loose CBR */
+			writel(2, mfc_regs->e_rc_mode);
+	}
+
+	/* seq header ctrl */
+	reg = readl(mfc_regs->e_enc_options);
+	reg &= ~(0x1 << 2);
+	reg |= ((p->seq_hdr_mode & 0x1) << 2);
+
+	/* frame skip mode */
+	reg &= ~(0x3);
+	reg |= (p->frame_skip_mode & 0x3);
+	writel(reg, mfc_regs->e_enc_options);
+
+	/* 'DROP_CONTROL_ENABLE', disable */
+	reg = readl(mfc_regs->e_rc_config);
+	reg &= ~(0x1 << 10);
+	writel(reg, mfc_regs->e_rc_config);
+
+	/* setting for MV range [16, 256] */
+	reg = (p->mv_h_range & S5P_FIMV_E_MV_RANGE_V6_MASK);
+	writel(reg, mfc_regs->e_mv_hor_range);
+
+	reg = (p->mv_v_range & S5P_FIMV_E_MV_RANGE_V6_MASK);
+	writel(reg, mfc_regs->e_mv_ver_range);
+
+	writel(0x0, mfc_regs->e_frame_insertion);
+	writel(0x0, mfc_regs->e_roi_buffer_addr);
+	writel(0x0, mfc_regs->e_param_change);
+	writel(0x0, mfc_regs->e_rc_roi_ctrl);
+	writel(0x0, mfc_regs->e_picture_tag);
+
+	writel(0x0, mfc_regs->e_bit_count_enable);
+	writel(0x0, mfc_regs->e_max_bit_count);
+	writel(0x0, mfc_regs->e_min_bit_count);
+
+	writel(0x0, mfc_regs->e_metadata_buffer_addr);
+	writel(0x0, mfc_regs->e_metadata_buffer_size);
+
+	mfc_debug_leave();
+
+	return 0;
+}
+
+static int s5p_mfc_set_enc_params_h264(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
+	struct s5p_mfc_enc_params *p = &ctx->enc_params;
+	struct s5p_mfc_h264_enc_params *p_h264 = &p->codec.h264;
+	unsigned int reg = 0;
+	int i;
+
+	mfc_debug_enter();
+
+	s5p_mfc_set_enc_params(ctx);
+
+	/* pictype : number of B */
+	reg = readl(mfc_regs->e_gop_config);
+	reg &= ~(0x3 << 16);
+	reg |= ((p->num_b_frame & 0x3) << 16);
+	writel(reg, mfc_regs->e_gop_config);
+
+	/* profile & level */
+	reg = 0;
+	/** level */
+	reg |= ((p_h264->level & 0xFF) << 8);
+	/** profile - 0 ~ 3 */
+	reg |= p_h264->profile & 0x3F;
+	writel(reg, mfc_regs->e_picture_profile);
+
+	/* rate control config. */
+	reg = readl(mfc_regs->e_rc_config);
+	/** macroblock level rate control */
+	reg &= ~(0x1 << 8);
+	reg |= ((p->rc_mb & 0x1) << 8);
+	writel(reg, mfc_regs->e_rc_config);
+
+	/** frame QP */
+	reg &= ~(0x3F);
+	reg |= p_h264->rc_frame_qp & 0x3F;
+	writel(reg, mfc_regs->e_rc_config);
+
+	/* max & min value of QP */
+	reg = 0;
+	/** max QP */
+	reg |= ((p_h264->rc_max_qp & 0x3F) << 8);
+	/** min QP */
+	reg |= p_h264->rc_min_qp & 0x3F;
+	writel(reg, mfc_regs->e_rc_qp_bound);
+
+	/* other QPs */
+	writel(0x0, mfc_regs->e_fixed_picture_qp);
+	if (!p->rc_frame && !p->rc_mb) {
+		reg = 0;
+		reg |= ((p_h264->rc_b_frame_qp & 0x3F) << 16);
+		reg |= ((p_h264->rc_p_frame_qp & 0x3F) << 8);
+		reg |= p_h264->rc_frame_qp & 0x3F;
+		writel(reg, mfc_regs->e_fixed_picture_qp);
+	}
+
+	/* frame rate */
+	if (p->rc_frame && p->rc_framerate_num && p->rc_framerate_denom) {
+		reg = 0;
+		reg |= ((p->rc_framerate_num & 0xFFFF) << 16);
+		reg |= p->rc_framerate_denom & 0xFFFF;
+		writel(reg, mfc_regs->e_rc_frame_rate);
+	}
+
+	/* vbv buffer size */
+	if (p->frame_skip_mode ==
+			V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
+		writel(p_h264->cpb_size & 0xFFFF,
+				mfc_regs->e_vbv_buffer_size);
+
+		if (p->rc_frame)
+			writel(p->vbv_delay, mfc_regs->e_vbv_init_delay);
+	}
+
+	/* interlace */
+	reg = 0;
+	reg |= ((p_h264->interlace & 0x1) << 3);
+	writel(reg, mfc_regs->e_h264_options);
+
+	/* height */
+	if (p_h264->interlace) {
+		writel(ctx->img_height >> 1,
+				mfc_regs->e_frame_height); /* 32 align */
+		/* cropped height */
+		writel(ctx->img_height >> 1,
+				mfc_regs->e_cropped_frame_height);
+	}
+
+	/* loop filter ctrl */
+	reg = readl(mfc_regs->e_h264_options);
+	reg &= ~(0x3 << 1);
+	reg |= ((p_h264->loop_filter_mode & 0x3) << 1);
+	writel(reg, mfc_regs->e_h264_options);
+
+	/* loopfilter alpha offset */
+	if (p_h264->loop_filter_alpha < 0) {
+		reg = 0x10;
+		reg |= (0xFF - p_h264->loop_filter_alpha) + 1;
+	} else {
+		reg = 0x00;
+		reg |= (p_h264->loop_filter_alpha & 0xF);
+	}
+	writel(reg, mfc_regs->e_h264_lf_alpha_offset);
+
+	/* loopfilter beta offset */
+	if (p_h264->loop_filter_beta < 0) {
+		reg = 0x10;
+		reg |= (0xFF - p_h264->loop_filter_beta) + 1;
+	} else {
+		reg = 0x00;
+		reg |= (p_h264->loop_filter_beta & 0xF);
+	}
+	writel(reg, mfc_regs->e_h264_lf_beta_offset);
+
+	/* entropy coding mode */
+	reg = readl(mfc_regs->e_h264_options);
+	reg &= ~(0x1);
+	reg |= p_h264->entropy_mode & 0x1;
+	writel(reg, mfc_regs->e_h264_options);
+
+	/* number of ref. picture */
+	reg = readl(mfc_regs->e_h264_options);
+	reg &= ~(0x1 << 7);
+	reg |= (((p_h264->num_ref_pic_4p - 1) & 0x1) << 7);
+	writel(reg, mfc_regs->e_h264_options);
+
+	/* 8x8 transform enable */
+	reg = readl(mfc_regs->e_h264_options);
+	reg &= ~(0x3 << 12);
+	reg |= ((p_h264->_8x8_transform & 0x3) << 12);
+	writel(reg, mfc_regs->e_h264_options);
+
+	/* macroblock adaptive scaling features */
+	writel(0x0, mfc_regs->e_mb_rc_config);
+	if (p->rc_mb) {
+		reg = 0;
+		/** dark region */
+		reg |= ((p_h264->rc_mb_dark & 0x1) << 3);
+		/** smooth region */
+		reg |= ((p_h264->rc_mb_smooth & 0x1) << 2);
+		/** static region */
+		reg |= ((p_h264->rc_mb_static & 0x1) << 1);
+		/** high activity region */
+		reg |= p_h264->rc_mb_activity & 0x1;
+		writel(reg, mfc_regs->e_mb_rc_config);
+	}
+
+	/* aspect ratio VUI */
+	readl(mfc_regs->e_h264_options);
+	reg &= ~(0x1 << 5);
+	reg |= ((p_h264->vui_sar & 0x1) << 5);
+	writel(reg, mfc_regs->e_h264_options);
+
+	writel(0x0, mfc_regs->e_aspect_ratio);
+	writel(0x0, mfc_regs->e_extended_sar);
+	if (p_h264->vui_sar) {
+		/* aspect ration IDC */
+		reg = 0;
+		reg |= p_h264->vui_sar_idc & 0xFF;
+		writel(reg, mfc_regs->e_aspect_ratio);
+		if (p_h264->vui_sar_idc == 0xFF) {
+			/* extended SAR */
+			reg = 0;
+			reg |= (p_h264->vui_ext_sar_width & 0xFFFF) << 16;
+			reg |= p_h264->vui_ext_sar_height & 0xFFFF;
+			writel(reg, mfc_regs->e_extended_sar);
+		}
+	}
+
+	/* intra picture period for H.264 open GOP */
+	/* control */
+	readl(mfc_regs->e_h264_options);
+	reg &= ~(0x1 << 4);
+	reg |= ((p_h264->open_gop & 0x1) << 4);
+	writel(reg, mfc_regs->e_h264_options);
+
+	/* value */
+	writel(0x0, mfc_regs->e_h264_i_period);
+	if (p_h264->open_gop) {
+		reg = 0;
+		reg |= p_h264->open_gop_size & 0xFFFF;
+		writel(reg, mfc_regs->e_h264_i_period);
+	}
+
+	/* 'WEIGHTED_BI_PREDICTION' for B is disable */
+	readl(mfc_regs->e_h264_options);
+	reg &= ~(0x3 << 9);
+	writel(reg, mfc_regs->e_h264_options);
+
+	/* 'CONSTRAINED_INTRA_PRED_ENABLE' is disable */
+	readl(mfc_regs->e_h264_options);
+	reg &= ~(0x1 << 14);
+	writel(reg, mfc_regs->e_h264_options);
+
+	/* ASO */
+	readl(mfc_regs->e_h264_options);
+	reg &= ~(0x1 << 6);
+	reg |= ((p_h264->aso & 0x1) << 6);
+	writel(reg, mfc_regs->e_h264_options);
+
+	/* hier qp enable */
+	readl(mfc_regs->e_h264_options);
+	reg &= ~(0x1 << 8);
+	reg |= ((p_h264->open_gop & 0x1) << 8);
+	writel(reg, mfc_regs->e_h264_options);
+	reg = 0;
+	if (p_h264->hier_qp && p_h264->hier_qp_layer) {
+		reg |= (p_h264->hier_qp_type & 0x1) << 0x3;
+		reg |= p_h264->hier_qp_layer & 0x7;
+		writel(reg, mfc_regs->e_h264_num_t_layer);
+		/* QP value for each layer */
+		for (i = 0; i < p_h264->hier_qp_layer &&
+				i < ARRAY_SIZE(p_h264->hier_qp_layer_qp); i++) {
+			writel(p_h264->hier_qp_layer_qp[i],
+				mfc_regs->e_h264_hierarchical_qp_layer0
+				+ i * 4);
+		}
+	}
+	/* number of coding layer should be zero when hierarchical is disable */
+	writel(reg, mfc_regs->e_h264_num_t_layer);
+
+	/* frame packing SEI generation */
+	readl(mfc_regs->e_h264_options);
+	reg &= ~(0x1 << 25);
+	reg |= ((p_h264->sei_frame_packing & 0x1) << 25);
+	writel(reg, mfc_regs->e_h264_options);
+	if (p_h264->sei_frame_packing) {
+		reg = 0;
+		/** current frame0 flag */
+		reg |= ((p_h264->sei_fp_curr_frame_0 & 0x1) << 2);
+		/** arrangement type */
+		reg |= p_h264->sei_fp_arrangement_type & 0x3;
+		writel(reg, mfc_regs->e_h264_frame_packing_sei_info);
+	}
+
+	if (p_h264->fmo) {
+		switch (p_h264->fmo_map_type) {
+		case V4L2_MPEG_VIDEO_H264_FMO_MAP_TYPE_INTERLEAVED_SLICES:
+			if (p_h264->fmo_slice_grp > 4)
+				p_h264->fmo_slice_grp = 4;
+			for (i = 0; i < (p_h264->fmo_slice_grp & 0xF); i++)
+				writel(p_h264->fmo_run_len[i] - 1,
+					mfc_regs->e_h264_fmo_run_length_minus1_0
+					+ i * 4);
+			break;
+		case V4L2_MPEG_VIDEO_H264_FMO_MAP_TYPE_SCATTERED_SLICES:
+			if (p_h264->fmo_slice_grp > 4)
+				p_h264->fmo_slice_grp = 4;
+			break;
+		case V4L2_MPEG_VIDEO_H264_FMO_MAP_TYPE_RASTER_SCAN:
+		case V4L2_MPEG_VIDEO_H264_FMO_MAP_TYPE_WIPE_SCAN:
+			if (p_h264->fmo_slice_grp > 2)
+				p_h264->fmo_slice_grp = 2;
+			writel(p_h264->fmo_chg_dir & 0x1,
+				mfc_regs->e_h264_fmo_slice_grp_change_dir);
+			/* the valid range is 0 ~ number of macroblocks -1 */
+			writel(p_h264->fmo_chg_rate,
+			mfc_regs->e_h264_fmo_slice_grp_change_rate_minus1);
+			break;
+		default:
+			mfc_err("Unsupported map type for FMO: %d\n",
+					p_h264->fmo_map_type);
+			p_h264->fmo_map_type = 0;
+			p_h264->fmo_slice_grp = 1;
+			break;
+		}
+
+		writel(p_h264->fmo_map_type,
+				mfc_regs->e_h264_fmo_slice_grp_map_type);
+		writel(p_h264->fmo_slice_grp - 1,
+				mfc_regs->e_h264_fmo_num_slice_grp_minus1);
+	} else {
+		writel(0, mfc_regs->e_h264_fmo_num_slice_grp_minus1);
+	}
+
+	mfc_debug_leave();
+
+	return 0;
+}
+
+static int s5p_mfc_set_enc_params_mpeg4(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
+	struct s5p_mfc_enc_params *p = &ctx->enc_params;
+	struct s5p_mfc_mpeg4_enc_params *p_mpeg4 = &p->codec.mpeg4;
+	unsigned int reg = 0;
+
+	mfc_debug_enter();
+
+	s5p_mfc_set_enc_params(ctx);
+
+	/* pictype : number of B */
+	reg = readl(mfc_regs->e_gop_config);
+	reg &= ~(0x3 << 16);
+	reg |= ((p->num_b_frame & 0x3) << 16);
+	writel(reg, mfc_regs->e_gop_config);
+
+	/* profile & level */
+	reg = 0;
+	/** level */
+	reg |= ((p_mpeg4->level & 0xFF) << 8);
+	/** profile - 0 ~ 1 */
+	reg |= p_mpeg4->profile & 0x3F;
+	writel(reg, mfc_regs->e_picture_profile);
+
+	/* rate control config. */
+	reg = readl(mfc_regs->e_rc_config);
+	/** macroblock level rate control */
+	reg &= ~(0x1 << 8);
+	reg |= ((p->rc_mb & 0x1) << 8);
+	writel(reg, mfc_regs->e_rc_config);
+
+	/** frame QP */
+	reg &= ~(0x3F);
+	reg |= p_mpeg4->rc_frame_qp & 0x3F;
+	writel(reg, mfc_regs->e_rc_config);
+
+	/* max & min value of QP */
+	reg = 0;
+	/** max QP */
+	reg |= ((p_mpeg4->rc_max_qp & 0x3F) << 8);
+	/** min QP */
+	reg |= p_mpeg4->rc_min_qp & 0x3F;
+	writel(reg, mfc_regs->e_rc_qp_bound);
+
+	/* other QPs */
+	writel(0x0, mfc_regs->e_fixed_picture_qp);
+	if (!p->rc_frame && !p->rc_mb) {
+		reg = 0;
+		reg |= ((p_mpeg4->rc_b_frame_qp & 0x3F) << 16);
+		reg |= ((p_mpeg4->rc_p_frame_qp & 0x3F) << 8);
+		reg |= p_mpeg4->rc_frame_qp & 0x3F;
+		writel(reg, mfc_regs->e_fixed_picture_qp);
+	}
+
+	/* frame rate */
+	if (p->rc_frame && p->rc_framerate_num && p->rc_framerate_denom) {
+		reg = 0;
+		reg |= ((p->rc_framerate_num & 0xFFFF) << 16);
+		reg |= p->rc_framerate_denom & 0xFFFF;
+		writel(reg, mfc_regs->e_rc_frame_rate);
+	}
+
+	/* vbv buffer size */
+	if (p->frame_skip_mode ==
+			V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
+		writel(p->vbv_size & 0xFFFF, mfc_regs->e_vbv_buffer_size);
+
+		if (p->rc_frame)
+			writel(p->vbv_delay, mfc_regs->e_vbv_init_delay);
+	}
+
+	/* Disable HEC */
+	writel(0x0, mfc_regs->e_mpeg4_options);
+	writel(0x0, mfc_regs->e_mpeg4_hec_period);
+
+	mfc_debug_leave();
+
+	return 0;
+}
+
+static int s5p_mfc_set_enc_params_h263(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
+	struct s5p_mfc_enc_params *p = &ctx->enc_params;
+	struct s5p_mfc_mpeg4_enc_params *p_h263 = &p->codec.mpeg4;
+	unsigned int reg = 0;
+
+	mfc_debug_enter();
+
+	s5p_mfc_set_enc_params(ctx);
+
+	/* profile & level */
+	reg = 0;
+	/** profile */
+	reg |= (0x1 << 4);
+	writel(reg, mfc_regs->e_picture_profile);
+
+	/* rate control config. */
+	reg = readl(mfc_regs->e_rc_config);
+	/** macroblock level rate control */
+	reg &= ~(0x1 << 8);
+	reg |= ((p->rc_mb & 0x1) << 8);
+	writel(reg, mfc_regs->e_rc_config);
+
+	/** frame QP */
+	reg &= ~(0x3F);
+	reg |= p_h263->rc_frame_qp & 0x3F;
+	writel(reg, mfc_regs->e_rc_config);
+
+	/* max & min value of QP */
+	reg = 0;
+	/** max QP */
+	reg |= ((p_h263->rc_max_qp & 0x3F) << 8);
+	/** min QP */
+	reg |= p_h263->rc_min_qp & 0x3F;
+	writel(reg, mfc_regs->e_rc_qp_bound);
+
+	/* other QPs */
+	writel(0x0, mfc_regs->e_fixed_picture_qp);
+	if (!p->rc_frame && !p->rc_mb) {
+		reg = 0;
+		reg |= ((p_h263->rc_b_frame_qp & 0x3F) << 16);
+		reg |= ((p_h263->rc_p_frame_qp & 0x3F) << 8);
+		reg |= p_h263->rc_frame_qp & 0x3F;
+		writel(reg, mfc_regs->e_fixed_picture_qp);
+	}
+
+	/* frame rate */
+	if (p->rc_frame && p->rc_framerate_num && p->rc_framerate_denom) {
+		reg = 0;
+		reg |= ((p->rc_framerate_num & 0xFFFF) << 16);
+		reg |= p->rc_framerate_denom & 0xFFFF;
+		writel(reg, mfc_regs->e_rc_frame_rate);
+	}
+
+	/* vbv buffer size */
+	if (p->frame_skip_mode ==
+			V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
+		writel(p->vbv_size & 0xFFFF, mfc_regs->e_vbv_buffer_size);
+
+		if (p->rc_frame)
+			writel(p->vbv_delay, mfc_regs->e_vbv_init_delay);
+	}
+
+	mfc_debug_leave();
+
+	return 0;
+}
+
+static int s5p_mfc_set_enc_params_vp8(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
+	struct s5p_mfc_enc_params *p = &ctx->enc_params;
+	struct s5p_mfc_vp8_enc_params *p_vp8 = &p->codec.vp8;
+	unsigned int reg = 0;
+	unsigned int val = 0;
+
+	mfc_debug_enter();
+
+	s5p_mfc_set_enc_params(ctx);
+
+	/* pictype : number of B */
+	reg = readl(mfc_regs->e_gop_config);
+	reg &= ~(0x3 << 16);
+	reg |= ((p->num_b_frame & 0x3) << 16);
+	writel(reg, mfc_regs->e_gop_config);
+
+	/* profile - 0 ~ 3 */
+	reg = p_vp8->profile & 0x3;
+	writel(reg, mfc_regs->e_picture_profile);
+
+	/* rate control config. */
+	reg = readl(mfc_regs->e_rc_config);
+	/** macroblock level rate control */
+	reg &= ~(0x1 << 8);
+	reg |= ((p->rc_mb & 0x1) << 8);
+	writel(reg, mfc_regs->e_rc_config);
+
+	/* frame rate */
+	if (p->rc_frame && p->rc_framerate_num && p->rc_framerate_denom) {
+		reg = 0;
+		reg |= ((p->rc_framerate_num & 0xFFFF) << 16);
+		reg |= p->rc_framerate_denom & 0xFFFF;
+		writel(reg, mfc_regs->e_rc_frame_rate);
+	}
+
+	/* frame QP */
+	reg &= ~(0x7F);
+	reg |= p_vp8->rc_frame_qp & 0x7F;
+	writel(reg, mfc_regs->e_rc_config);
+
+	/* other QPs */
+	writel(0x0, mfc_regs->e_fixed_picture_qp);
+	if (!p->rc_frame && !p->rc_mb) {
+		reg = 0;
+		reg |= ((p_vp8->rc_p_frame_qp & 0x7F) << 8);
+		reg |= p_vp8->rc_frame_qp & 0x7F;
+		writel(reg, mfc_regs->e_fixed_picture_qp);
+	}
+
+	/* max QP */
+	reg = ((p_vp8->rc_max_qp & 0x7F) << 8);
+	/* min QP */
+	reg |= p_vp8->rc_min_qp & 0x7F;
+	writel(reg, mfc_regs->e_rc_qp_bound);
+
+	/* vbv buffer size */
+	if (p->frame_skip_mode ==
+			V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
+		writel(p->vbv_size & 0xFFFF, mfc_regs->e_vbv_buffer_size);
+
+		if (p->rc_frame)
+			writel(p->vbv_delay, mfc_regs->e_vbv_init_delay);
+	}
+
+	/* VP8 specific params */
+	reg = 0;
+	reg |= (p_vp8->imd_4x4 & 0x1) << 10;
+	switch (p_vp8->num_partitions) {
+	case V4L2_CID_MPEG_VIDEO_VPX_1_PARTITION:
+		val = 0;
+		break;
+	case V4L2_CID_MPEG_VIDEO_VPX_2_PARTITIONS:
+		val = 2;
+		break;
+	case V4L2_CID_MPEG_VIDEO_VPX_4_PARTITIONS:
+		val = 4;
+		break;
+	case V4L2_CID_MPEG_VIDEO_VPX_8_PARTITIONS:
+		val = 8;
+		break;
+	}
+	reg |= (val & 0xF) << 3;
+	reg |= (p_vp8->num_ref & 0x2);
+	writel(reg, mfc_regs->e_vp8_options);
+
+	mfc_debug_leave();
+
+	return 0;
+}
+
+/* Initialize decoding */
+static int s5p_mfc_init_decode_v6(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
+	unsigned int reg = 0;
+	int fmo_aso_ctrl = 0;
+
+	mfc_debug_enter();
+	mfc_debug(2, "InstNo: %d/%d\n", ctx->inst_no,
+			S5P_FIMV_CH_SEQ_HEADER_V6);
+	mfc_debug(2, "BUFs: %08x %08x %08x\n",
+		  readl(mfc_regs->d_cpb_buffer_addr),
+		  readl(mfc_regs->d_cpb_buffer_addr),
+		  readl(mfc_regs->d_cpb_buffer_addr));
+
+	/* FMO_ASO_CTRL - 0: Enable, 1: Disable */
+	reg |= (fmo_aso_ctrl << S5P_FIMV_D_OPT_FMO_ASO_CTRL_MASK_V6);
+
+	if (ctx->display_delay_enable) {
+		reg |= (0x1 << S5P_FIMV_D_OPT_DDELAY_EN_SHIFT_V6);
+		writel(ctx->display_delay, mfc_regs->d_display_delay);
+	}
+
+	if (IS_MFCV7_PLUS(dev) || IS_MFCV6_V2(dev)) {
+		writel(reg, mfc_regs->d_dec_options);
+		reg = 0;
+	}
+
+	/* Setup loop filter, for decoding this is only valid for MPEG4 */
+	if (ctx->codec_mode == S5P_MFC_CODEC_MPEG4_DEC) {
+		mfc_debug(2, "Set loop filter to: %d\n",
+				ctx->loop_filter_mpeg4);
+		reg |= (ctx->loop_filter_mpeg4 <<
+				S5P_FIMV_D_OPT_LF_CTRL_SHIFT_V6);
+	}
+	if (ctx->dst_fmt->fourcc == V4L2_PIX_FMT_NV12MT_16X16)
+		reg |= (0x1 << S5P_FIMV_D_OPT_TILE_MODE_SHIFT_V6);
+
+	if (IS_MFCV7_PLUS(dev) || IS_MFCV6_V2(dev))
+		writel(reg, mfc_regs->d_init_buffer_options);
+	else
+		writel(reg, mfc_regs->d_dec_options);
+
+	/* 0: NV12(CbCr), 1: NV21(CrCb) */
+	if (ctx->dst_fmt->fourcc == V4L2_PIX_FMT_NV21M)
+		writel(0x1, mfc_regs->pixel_format);
+	else
+		writel(0x0, mfc_regs->pixel_format);
+
+
+	/* sei parse */
+	writel(ctx->sei_fp_parse & 0x1, mfc_regs->d_sei_enable);
+
+	writel(ctx->inst_no, mfc_regs->instance_id);
+	s5p_mfc_hw_call_void(dev->mfc_cmds, cmd_host2risc, dev,
+			S5P_FIMV_CH_SEQ_HEADER_V6, NULL);
+
+	mfc_debug_leave();
+	return 0;
+}
+
+static inline void s5p_mfc_set_flush(struct s5p_mfc_ctx *ctx, int flush)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
+
+	if (flush) {
+		dev->curr_ctx = ctx->num;
+		writel(ctx->inst_no, mfc_regs->instance_id);
+		s5p_mfc_hw_call_void(dev->mfc_cmds, cmd_host2risc, dev,
+				S5P_FIMV_H2R_CMD_FLUSH_V6, NULL);
+	}
+}
+
+/* Decode a single frame */
+static int s5p_mfc_decode_one_frame_v6(struct s5p_mfc_ctx *ctx,
+			enum s5p_mfc_decode_arg last_frame)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
+
+	writel(ctx->dec_dst_flag, mfc_regs->d_available_dpb_flag_lower);
+	writel(ctx->slice_interface & 0x1, mfc_regs->d_slice_if_enable);
+
+	writel(ctx->inst_no, mfc_regs->instance_id);
+	/* Issue different commands to instance basing on whether it
+	 * is the last frame or not. */
+	switch (last_frame) {
+	case 0:
+		s5p_mfc_hw_call_void(dev->mfc_cmds, cmd_host2risc, dev,
+				S5P_FIMV_CH_FRAME_START_V6, NULL);
+		break;
+	case 1:
+		s5p_mfc_hw_call_void(dev->mfc_cmds, cmd_host2risc, dev,
+				S5P_FIMV_CH_LAST_FRAME_V6, NULL);
+		break;
+	default:
+		mfc_err("Unsupported last frame arg.\n");
+		return -EINVAL;
+	}
+
+	mfc_debug(2, "Decoding a usual frame.\n");
+	return 0;
+}
+
+static int s5p_mfc_init_encode_v6(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
+
+	if (ctx->codec_mode == S5P_MFC_CODEC_H264_ENC)
+		s5p_mfc_set_enc_params_h264(ctx);
+	else if (ctx->codec_mode == S5P_MFC_CODEC_MPEG4_ENC)
+		s5p_mfc_set_enc_params_mpeg4(ctx);
+	else if (ctx->codec_mode == S5P_MFC_CODEC_H263_ENC)
+		s5p_mfc_set_enc_params_h263(ctx);
+	else if (ctx->codec_mode == S5P_MFC_CODEC_VP8_ENC)
+		s5p_mfc_set_enc_params_vp8(ctx);
+	else {
+		mfc_err("Unknown codec for encoding (%x).\n",
+			ctx->codec_mode);
+		return -EINVAL;
+	}
+
+	/* Set stride lengths for v7 & above */
+	if (IS_MFCV7_PLUS(dev)) {
+		writel(ctx->img_width, mfc_regs->e_source_first_plane_stride);
+		writel(ctx->img_width, mfc_regs->e_source_second_plane_stride);
+	}
+
+	writel(ctx->inst_no, mfc_regs->instance_id);
+	s5p_mfc_hw_call_void(dev->mfc_cmds, cmd_host2risc, dev,
+			S5P_FIMV_CH_SEQ_HEADER_V6, NULL);
+
+	return 0;
+}
+
+static int s5p_mfc_h264_set_aso_slice_order_v6(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
+	struct s5p_mfc_enc_params *p = &ctx->enc_params;
+	struct s5p_mfc_h264_enc_params *p_h264 = &p->codec.h264;
+	int i;
+
+	if (p_h264->aso) {
+		for (i = 0; i < ARRAY_SIZE(p_h264->aso_slice_order); i++) {
+			writel(p_h264->aso_slice_order[i],
+				mfc_regs->e_h264_aso_slice_order_0 + i * 4);
+		}
+	}
+	return 0;
+}
+
+/* Encode a single frame */
+static int s5p_mfc_encode_one_frame_v6(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
+	int cmd;
+
+	mfc_debug(2, "++\n");
+
+	/* memory structure cur. frame */
+
+	if (ctx->codec_mode == S5P_MFC_CODEC_H264_ENC)
+		s5p_mfc_h264_set_aso_slice_order_v6(ctx);
+
+	s5p_mfc_set_slice_mode(ctx);
+
+	if (ctx->state != MFCINST_FINISHING)
+		cmd = S5P_FIMV_CH_FRAME_START_V6;
+	else
+		cmd = S5P_FIMV_CH_LAST_FRAME_V6;
+
+	writel(ctx->inst_no, mfc_regs->instance_id);
+	s5p_mfc_hw_call_void(dev->mfc_cmds, cmd_host2risc, dev, cmd, NULL);
+
+	mfc_debug(2, "--\n");
+
+	return 0;
+}
+
+static inline int s5p_mfc_get_new_ctx(struct s5p_mfc_dev *dev)
+{
+	unsigned long flags;
+	int new_ctx;
+	int cnt;
+
+	spin_lock_irqsave(&dev->condlock, flags);
+	mfc_debug(2, "Previous context: %d (bits %08lx)\n", dev->curr_ctx,
+							dev->ctx_work_bits);
+	new_ctx = (dev->curr_ctx + 1) % MFC_NUM_CONTEXTS;
+	cnt = 0;
+	while (!test_bit(new_ctx, &dev->ctx_work_bits)) {
+		new_ctx = (new_ctx + 1) % MFC_NUM_CONTEXTS;
+		cnt++;
+		if (cnt > MFC_NUM_CONTEXTS) {
+			/* No contexts to run */
+			spin_unlock_irqrestore(&dev->condlock, flags);
+			return -EAGAIN;
+		}
+	}
+	spin_unlock_irqrestore(&dev->condlock, flags);
+	return new_ctx;
+}
+
+static inline void s5p_mfc_run_dec_last_frames(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+
+	s5p_mfc_set_dec_stream_buffer_v6(ctx, 0, 0, 0);
+	dev->curr_ctx = ctx->num;
+	s5p_mfc_decode_one_frame_v6(ctx, MFC_DEC_LAST_FRAME);
+}
+
+static inline int s5p_mfc_run_dec_frame(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	struct s5p_mfc_buf *temp_vb;
+	unsigned long flags;
+	int last_frame = 0;
+
+	if (ctx->state == MFCINST_FINISHING) {
+		last_frame = MFC_DEC_LAST_FRAME;
+		s5p_mfc_set_dec_stream_buffer_v6(ctx, 0, 0, 0);
+		dev->curr_ctx = ctx->num;
+		s5p_mfc_clean_ctx_int_flags(ctx);
+		s5p_mfc_decode_one_frame_v6(ctx, last_frame);
+		return 0;
+	}
+
+	spin_lock_irqsave(&dev->irqlock, flags);
+	/* Frames are being decoded */
+	if (list_empty(&ctx->src_queue)) {
+		mfc_debug(2, "No src buffers.\n");
+		spin_unlock_irqrestore(&dev->irqlock, flags);
+		return -EAGAIN;
+	}
+	/* Get the next source buffer */
+	temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
+	temp_vb->flags |= MFC_BUF_FLAG_USED;
+	s5p_mfc_set_dec_stream_buffer_v6(ctx,
+		vb2_dma_contig_plane_dma_addr(&temp_vb->b->vb2_buf, 0),
+			ctx->consumed_stream,
+			temp_vb->b->vb2_buf.planes[0].bytesused);
+	spin_unlock_irqrestore(&dev->irqlock, flags);
+
+	dev->curr_ctx = ctx->num;
+	if (temp_vb->b->vb2_buf.planes[0].bytesused == 0) {
+		last_frame = 1;
+		mfc_debug(2, "Setting ctx->state to FINISHING\n");
+		ctx->state = MFCINST_FINISHING;
+	}
+	s5p_mfc_decode_one_frame_v6(ctx, last_frame);
+
+	return 0;
+}
+
+static inline int s5p_mfc_run_enc_frame(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	unsigned long flags;
+	struct s5p_mfc_buf *dst_mb;
+	struct s5p_mfc_buf *src_mb;
+	unsigned long src_y_addr, src_c_addr, dst_addr;
+	/*
+	unsigned int src_y_size, src_c_size;
+	*/
+	unsigned int dst_size;
+
+	spin_lock_irqsave(&dev->irqlock, flags);
+
+	if (list_empty(&ctx->src_queue) && ctx->state != MFCINST_FINISHING) {
+		mfc_debug(2, "no src buffers.\n");
+		spin_unlock_irqrestore(&dev->irqlock, flags);
+		return -EAGAIN;
+	}
+
+	if (list_empty(&ctx->dst_queue)) {
+		mfc_debug(2, "no dst buffers.\n");
+		spin_unlock_irqrestore(&dev->irqlock, flags);
+		return -EAGAIN;
+	}
+
+	if (list_empty(&ctx->src_queue)) {
+		/* send null frame */
+		s5p_mfc_set_enc_frame_buffer_v6(ctx, 0, 0);
+		src_mb = NULL;
+	} else {
+		src_mb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
+		src_mb->flags |= MFC_BUF_FLAG_USED;
+		if (src_mb->b->vb2_buf.planes[0].bytesused == 0) {
+			s5p_mfc_set_enc_frame_buffer_v6(ctx, 0, 0);
+			ctx->state = MFCINST_FINISHING;
+		} else {
+			src_y_addr = vb2_dma_contig_plane_dma_addr(&src_mb->b->vb2_buf, 0);
+			src_c_addr = vb2_dma_contig_plane_dma_addr(&src_mb->b->vb2_buf, 1);
+
+			mfc_debug(2, "enc src y addr: 0x%08lx\n", src_y_addr);
+			mfc_debug(2, "enc src c addr: 0x%08lx\n", src_c_addr);
+
+			s5p_mfc_set_enc_frame_buffer_v6(ctx, src_y_addr, src_c_addr);
+			if (src_mb->flags & MFC_BUF_FLAG_EOS)
+				ctx->state = MFCINST_FINISHING;
+		}
+	}
+
+	dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
+	dst_mb->flags |= MFC_BUF_FLAG_USED;
+	dst_addr = vb2_dma_contig_plane_dma_addr(&dst_mb->b->vb2_buf, 0);
+	dst_size = vb2_plane_size(&dst_mb->b->vb2_buf, 0);
+
+	s5p_mfc_set_enc_stream_buffer_v6(ctx, dst_addr, dst_size);
+
+	spin_unlock_irqrestore(&dev->irqlock, flags);
+
+	dev->curr_ctx = ctx->num;
+	s5p_mfc_encode_one_frame_v6(ctx);
+
+	return 0;
+}
+
+static inline void s5p_mfc_run_init_dec(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	unsigned long flags;
+	struct s5p_mfc_buf *temp_vb;
+
+	/* Initializing decoding - parsing header */
+	spin_lock_irqsave(&dev->irqlock, flags);
+	mfc_debug(2, "Preparing to init decoding.\n");
+	temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
+	mfc_debug(2, "Header size: %d\n", temp_vb->b->vb2_buf.planes[0].bytesused);
+	s5p_mfc_set_dec_stream_buffer_v6(ctx,
+		vb2_dma_contig_plane_dma_addr(&temp_vb->b->vb2_buf, 0), 0,
+			temp_vb->b->vb2_buf.planes[0].bytesused);
+	spin_unlock_irqrestore(&dev->irqlock, flags);
+	dev->curr_ctx = ctx->num;
+	s5p_mfc_init_decode_v6(ctx);
+}
+
+static inline void s5p_mfc_run_init_enc(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	unsigned long flags;
+	struct s5p_mfc_buf *dst_mb;
+	unsigned long dst_addr;
+	unsigned int dst_size;
+
+	spin_lock_irqsave(&dev->irqlock, flags);
+
+	dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
+	dst_addr = vb2_dma_contig_plane_dma_addr(&dst_mb->b->vb2_buf, 0);
+	dst_size = vb2_plane_size(&dst_mb->b->vb2_buf, 0);
+	s5p_mfc_set_enc_stream_buffer_v6(ctx, dst_addr, dst_size);
+	spin_unlock_irqrestore(&dev->irqlock, flags);
+	dev->curr_ctx = ctx->num;
+	s5p_mfc_init_encode_v6(ctx);
+}
+
+static inline int s5p_mfc_run_init_dec_buffers(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	int ret;
+	/* Header was parsed now start processing
+	 * First set the output frame buffers
+	 * s5p_mfc_alloc_dec_buffers(ctx); */
+
+	if (ctx->capture_state != QUEUE_BUFS_MMAPED) {
+		mfc_err("It seems that not all destionation buffers were\n"
+			"mmaped.MFC requires that all destination are mmaped\n"
+			"before starting processing.\n");
+		return -EAGAIN;
+	}
+
+	dev->curr_ctx = ctx->num;
+	ret = s5p_mfc_set_dec_frame_buffer_v6(ctx);
+	if (ret) {
+		mfc_err("Failed to alloc frame mem.\n");
+		ctx->state = MFCINST_ERROR;
+	}
+	return ret;
+}
+
+static inline int s5p_mfc_run_init_enc_buffers(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	int ret;
+
+	dev->curr_ctx = ctx->num;
+	ret = s5p_mfc_set_enc_ref_buffer_v6(ctx);
+	if (ret) {
+		mfc_err("Failed to alloc frame mem.\n");
+		ctx->state = MFCINST_ERROR;
+	}
+	return ret;
+}
+
+/* Try running an operation on hardware */
+static void s5p_mfc_try_run_v6(struct s5p_mfc_dev *dev)
+{
+	struct s5p_mfc_ctx *ctx;
+	int new_ctx;
+	unsigned int ret = 0;
+
+	mfc_debug(1, "Try run dev: %p\n", dev);
+
+	/* Check whether hardware is not running */
+	if (test_and_set_bit(0, &dev->hw_lock) != 0) {
+		/* This is perfectly ok, the scheduled ctx should wait */
+		mfc_debug(1, "Couldn't lock HW.\n");
+		return;
+	}
+
+	/* Choose the context to run */
+	new_ctx = s5p_mfc_get_new_ctx(dev);
+	if (new_ctx < 0) {
+		/* No contexts to run */
+		if (test_and_clear_bit(0, &dev->hw_lock) == 0) {
+			mfc_err("Failed to unlock hardware.\n");
+			return;
+		}
+
+		mfc_debug(1, "No ctx is scheduled to be run.\n");
+		return;
+	}
+
+	mfc_debug(1, "New context: %d\n", new_ctx);
+	ctx = dev->ctx[new_ctx];
+	mfc_debug(1, "Setting new context to %p\n", ctx);
+	/* Got context to run in ctx */
+	mfc_debug(1, "ctx->dst_queue_cnt=%d ctx->dpb_count=%d ctx->src_queue_cnt=%d\n",
+		ctx->dst_queue_cnt, ctx->pb_count, ctx->src_queue_cnt);
+	mfc_debug(1, "ctx->state=%d\n", ctx->state);
+	/* Last frame has already been sent to MFC
+	 * Now obtaining frames from MFC buffer */
+
+	s5p_mfc_clock_on();
+	s5p_mfc_clean_ctx_int_flags(ctx);
+
+	if (ctx->type == MFCINST_DECODER) {
+		switch (ctx->state) {
+		case MFCINST_FINISHING:
+			s5p_mfc_run_dec_last_frames(ctx);
+			break;
+		case MFCINST_RUNNING:
+			ret = s5p_mfc_run_dec_frame(ctx);
+			break;
+		case MFCINST_INIT:
+			ret = s5p_mfc_hw_call(dev->mfc_cmds, open_inst_cmd,
+					ctx);
+			break;
+		case MFCINST_RETURN_INST:
+			ret = s5p_mfc_hw_call(dev->mfc_cmds, close_inst_cmd,
+					ctx);
+			break;
+		case MFCINST_GOT_INST:
+			s5p_mfc_run_init_dec(ctx);
+			break;
+		case MFCINST_HEAD_PARSED:
+			ret = s5p_mfc_run_init_dec_buffers(ctx);
+			break;
+		case MFCINST_FLUSH:
+			s5p_mfc_set_flush(ctx, ctx->dpb_flush_flag);
+			break;
+		case MFCINST_RES_CHANGE_INIT:
+			s5p_mfc_run_dec_last_frames(ctx);
+			break;
+		case MFCINST_RES_CHANGE_FLUSH:
+			s5p_mfc_run_dec_last_frames(ctx);
+			break;
+		case MFCINST_RES_CHANGE_END:
+			mfc_debug(2, "Finished remaining frames after resolution change.\n");
+			ctx->capture_state = QUEUE_FREE;
+			mfc_debug(2, "Will re-init the codec`.\n");
+			s5p_mfc_run_init_dec(ctx);
+			break;
+		default:
+			ret = -EAGAIN;
+		}
+	} else if (ctx->type == MFCINST_ENCODER) {
+		switch (ctx->state) {
+		case MFCINST_FINISHING:
+		case MFCINST_RUNNING:
+			ret = s5p_mfc_run_enc_frame(ctx);
+			break;
+		case MFCINST_INIT:
+			ret = s5p_mfc_hw_call(dev->mfc_cmds, open_inst_cmd,
+					ctx);
+			break;
+		case MFCINST_RETURN_INST:
+			ret = s5p_mfc_hw_call(dev->mfc_cmds, close_inst_cmd,
+					ctx);
+			break;
+		case MFCINST_GOT_INST:
+			s5p_mfc_run_init_enc(ctx);
+			break;
+		case MFCINST_HEAD_PRODUCED:
+			ret = s5p_mfc_run_init_enc_buffers(ctx);
+			break;
+		default:
+			ret = -EAGAIN;
+		}
+	} else {
+		mfc_err("invalid context type: %d\n", ctx->type);
+		ret = -EAGAIN;
+	}
+
+	if (ret) {
+		/* Free hardware lock */
+		if (test_and_clear_bit(0, &dev->hw_lock) == 0)
+			mfc_err("Failed to unlock hardware.\n");
+
+		/* This is in deed imporant, as no operation has been
+		 * scheduled, reduce the clock count as no one will
+		 * ever do this, because no interrupt related to this try_run
+		 * will ever come from hardware. */
+		s5p_mfc_clock_off();
+	}
+}
+
+
+static void s5p_mfc_cleanup_queue_v6(struct list_head *lh, struct vb2_queue *vq)
+{
+	struct s5p_mfc_buf *b;
+	int i;
+
+	while (!list_empty(lh)) {
+		b = list_entry(lh->next, struct s5p_mfc_buf, list);
+		for (i = 0; i < b->b->vb2_buf.num_planes; i++)
+			vb2_set_plane_payload(&b->b->vb2_buf, i, 0);
+		vb2_buffer_done(&b->b->vb2_buf, VB2_BUF_STATE_ERROR);
+		list_del(&b->list);
+	}
+}
+
+static void s5p_mfc_clear_int_flags_v6(struct s5p_mfc_dev *dev)
+{
+	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
+	writel(0, mfc_regs->risc2host_command);
+	writel(0, mfc_regs->risc2host_int);
+}
+
+static void s5p_mfc_write_info_v6(struct s5p_mfc_ctx *ctx, unsigned int data,
+		unsigned int ofs)
+{
+	s5p_mfc_clock_on();
+	writel(data, (void __iomem *)((unsigned long)ofs));
+	s5p_mfc_clock_off();
+}
+
+static unsigned int
+s5p_mfc_read_info_v6(struct s5p_mfc_ctx *ctx, unsigned long ofs)
+{
+	int ret;
+
+	s5p_mfc_clock_on();
+	ret = readl((void __iomem *)ofs);
+	s5p_mfc_clock_off();
+
+	return ret;
+}
+
+static int s5p_mfc_get_dspl_y_adr_v6(struct s5p_mfc_dev *dev)
+{
+	return readl(dev->mfc_regs->d_display_first_plane_addr);
+}
+
+static int s5p_mfc_get_dec_y_adr_v6(struct s5p_mfc_dev *dev)
+{
+	return readl(dev->mfc_regs->d_decoded_first_plane_addr);
+}
+
+static int s5p_mfc_get_dspl_status_v6(struct s5p_mfc_dev *dev)
+{
+	return readl(dev->mfc_regs->d_display_status);
+}
+
+static int s5p_mfc_get_dec_status_v6(struct s5p_mfc_dev *dev)
+{
+	return readl(dev->mfc_regs->d_decoded_status);
+}
+
+static int s5p_mfc_get_dec_frame_type_v6(struct s5p_mfc_dev *dev)
+{
+	return readl(dev->mfc_regs->d_decoded_frame_type) &
+		S5P_FIMV_DECODE_FRAME_MASK_V6;
+}
+
+static int s5p_mfc_get_disp_frame_type_v6(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	return readl(dev->mfc_regs->d_display_frame_type) &
+		S5P_FIMV_DECODE_FRAME_MASK_V6;
+}
+
+static int s5p_mfc_get_consumed_stream_v6(struct s5p_mfc_dev *dev)
+{
+	return readl(dev->mfc_regs->d_decoded_nal_size);
+}
+
+static int s5p_mfc_get_int_reason_v6(struct s5p_mfc_dev *dev)
+{
+	return readl(dev->mfc_regs->risc2host_command) &
+		S5P_FIMV_RISC2HOST_CMD_MASK;
+}
+
+static int s5p_mfc_get_int_err_v6(struct s5p_mfc_dev *dev)
+{
+	return readl(dev->mfc_regs->error_code);
+}
+
+static int s5p_mfc_err_dec_v6(unsigned int err)
+{
+	return (err & S5P_FIMV_ERR_DEC_MASK_V6) >> S5P_FIMV_ERR_DEC_SHIFT_V6;
+}
+
+static int s5p_mfc_err_dspl_v6(unsigned int err)
+{
+	return (err & S5P_FIMV_ERR_DSPL_MASK_V6) >> S5P_FIMV_ERR_DSPL_SHIFT_V6;
+}
+
+static int s5p_mfc_get_img_width_v6(struct s5p_mfc_dev *dev)
+{
+	return readl(dev->mfc_regs->d_display_frame_width);
+}
+
+static int s5p_mfc_get_img_height_v6(struct s5p_mfc_dev *dev)
+{
+	return readl(dev->mfc_regs->d_display_frame_height);
+}
+
+static int s5p_mfc_get_dpb_count_v6(struct s5p_mfc_dev *dev)
+{
+	return readl(dev->mfc_regs->d_min_num_dpb);
+}
+
+static int s5p_mfc_get_mv_count_v6(struct s5p_mfc_dev *dev)
+{
+	return readl(dev->mfc_regs->d_min_num_mv);
+}
+
+static int s5p_mfc_get_inst_no_v6(struct s5p_mfc_dev *dev)
+{
+	return readl(dev->mfc_regs->ret_instance_id);
+}
+
+static int s5p_mfc_get_enc_dpb_count_v6(struct s5p_mfc_dev *dev)
+{
+	return readl(dev->mfc_regs->e_num_dpb);
+}
+
+static int s5p_mfc_get_enc_strm_size_v6(struct s5p_mfc_dev *dev)
+{
+	return readl(dev->mfc_regs->e_stream_size);
+}
+
+static int s5p_mfc_get_enc_slice_type_v6(struct s5p_mfc_dev *dev)
+{
+	return readl(dev->mfc_regs->e_slice_type);
+}
+
+static int s5p_mfc_get_enc_pic_count_v6(struct s5p_mfc_dev *dev)
+{
+	return readl(dev->mfc_regs->e_picture_count);
+}
+
+static int s5p_mfc_get_sei_avail_status_v6(struct s5p_mfc_ctx *ctx)
+{
+	struct s5p_mfc_dev *dev = ctx->dev;
+	return readl(dev->mfc_regs->d_frame_pack_sei_avail);
+}
+
+static int s5p_mfc_get_mvc_num_views_v6(struct s5p_mfc_dev *dev)
+{
+	return readl(dev->mfc_regs->d_mvc_num_views);
+}
+
+static int s5p_mfc_get_mvc_view_id_v6(struct s5p_mfc_dev *dev)
+{
+	return readl(dev->mfc_regs->d_mvc_view_id);
+}
+
+static unsigned int s5p_mfc_get_pic_type_top_v6(struct s5p_mfc_ctx *ctx)
+{
+	return s5p_mfc_read_info_v6(ctx,
+		(__force unsigned long) ctx->dev->mfc_regs->d_ret_picture_tag_top);
+}
+
+static unsigned int s5p_mfc_get_pic_type_bot_v6(struct s5p_mfc_ctx *ctx)
+{
+	return s5p_mfc_read_info_v6(ctx,
+		(__force unsigned long) ctx->dev->mfc_regs->d_ret_picture_tag_bot);
+}
+
+static unsigned int s5p_mfc_get_crop_info_h_v6(struct s5p_mfc_ctx *ctx)
+{
+	return s5p_mfc_read_info_v6(ctx,
+		(__force unsigned long) ctx->dev->mfc_regs->d_display_crop_info1);
+}
+
+static unsigned int s5p_mfc_get_crop_info_v_v6(struct s5p_mfc_ctx *ctx)
+{
+	return s5p_mfc_read_info_v6(ctx,
+		(__force unsigned long) ctx->dev->mfc_regs->d_display_crop_info2);
+}
+
+static struct s5p_mfc_regs mfc_regs;
+
+/* Initialize registers for MFC v6 onwards */
+const struct s5p_mfc_regs *s5p_mfc_init_regs_v6_plus(struct s5p_mfc_dev *dev)
+{
+	memset(&mfc_regs, 0, sizeof(mfc_regs));
+
+#define S5P_MFC_REG_ADDR(dev, reg) ((dev)->regs_base + (reg))
+#define R(m, r) mfc_regs.m = S5P_MFC_REG_ADDR(dev, r)
+	/* codec common registers */
+	R(risc_on, S5P_FIMV_RISC_ON_V6);
+	R(risc2host_int, S5P_FIMV_RISC2HOST_INT_V6);
+	R(host2risc_int, S5P_FIMV_HOST2RISC_INT_V6);
+	R(risc_base_address, S5P_FIMV_RISC_BASE_ADDRESS_V6);
+	R(mfc_reset, S5P_FIMV_MFC_RESET_V6);
+	R(host2risc_command, S5P_FIMV_HOST2RISC_CMD_V6);
+	R(risc2host_command, S5P_FIMV_RISC2HOST_CMD_V6);
+	R(firmware_version, S5P_FIMV_FW_VERSION_V6);
+	R(instance_id, S5P_FIMV_INSTANCE_ID_V6);
+	R(codec_type, S5P_FIMV_CODEC_TYPE_V6);
+	R(context_mem_addr, S5P_FIMV_CONTEXT_MEM_ADDR_V6);
+	R(context_mem_size, S5P_FIMV_CONTEXT_MEM_SIZE_V6);
+	R(pixel_format, S5P_FIMV_PIXEL_FORMAT_V6);
+	R(ret_instance_id, S5P_FIMV_RET_INSTANCE_ID_V6);
+	R(error_code, S5P_FIMV_ERROR_CODE_V6);
+
+	/* decoder registers */
+	R(d_crc_ctrl, S5P_FIMV_D_CRC_CTRL_V6);
+	R(d_dec_options, S5P_FIMV_D_DEC_OPTIONS_V6);
+	R(d_display_delay, S5P_FIMV_D_DISPLAY_DELAY_V6);
+	R(d_sei_enable, S5P_FIMV_D_SEI_ENABLE_V6);
+	R(d_min_num_dpb, S5P_FIMV_D_MIN_NUM_DPB_V6);
+	R(d_min_num_mv, S5P_FIMV_D_MIN_NUM_MV_V6);
+	R(d_mvc_num_views, S5P_FIMV_D_MVC_NUM_VIEWS_V6);
+	R(d_num_dpb, S5P_FIMV_D_NUM_DPB_V6);
+	R(d_num_mv, S5P_FIMV_D_NUM_MV_V6);
+	R(d_init_buffer_options, S5P_FIMV_D_INIT_BUFFER_OPTIONS_V6);
+	R(d_first_plane_dpb_size, S5P_FIMV_D_LUMA_DPB_SIZE_V6);
+	R(d_second_plane_dpb_size, S5P_FIMV_D_CHROMA_DPB_SIZE_V6);
+	R(d_mv_buffer_size, S5P_FIMV_D_MV_BUFFER_SIZE_V6);
+	R(d_first_plane_dpb, S5P_FIMV_D_LUMA_DPB_V6);
+	R(d_second_plane_dpb, S5P_FIMV_D_CHROMA_DPB_V6);
+	R(d_mv_buffer, S5P_FIMV_D_MV_BUFFER_V6);
+	R(d_scratch_buffer_addr, S5P_FIMV_D_SCRATCH_BUFFER_ADDR_V6);
+	R(d_scratch_buffer_size, S5P_FIMV_D_SCRATCH_BUFFER_SIZE_V6);
+	R(d_cpb_buffer_addr, S5P_FIMV_D_CPB_BUFFER_ADDR_V6);
+	R(d_cpb_buffer_size, S5P_FIMV_D_CPB_BUFFER_SIZE_V6);
+	R(d_available_dpb_flag_lower, S5P_FIMV_D_AVAILABLE_DPB_FLAG_LOWER_V6);
+	R(d_cpb_buffer_offset, S5P_FIMV_D_CPB_BUFFER_OFFSET_V6);
+	R(d_slice_if_enable, S5P_FIMV_D_SLICE_IF_ENABLE_V6);
+	R(d_stream_data_size, S5P_FIMV_D_STREAM_DATA_SIZE_V6);
+	R(d_display_frame_width, S5P_FIMV_D_DISPLAY_FRAME_WIDTH_V6);
+	R(d_display_frame_height, S5P_FIMV_D_DISPLAY_FRAME_HEIGHT_V6);
+	R(d_display_status, S5P_FIMV_D_DISPLAY_STATUS_V6);
+	R(d_display_first_plane_addr, S5P_FIMV_D_DISPLAY_LUMA_ADDR_V6);
+	R(d_display_second_plane_addr, S5P_FIMV_D_DISPLAY_CHROMA_ADDR_V6);
+	R(d_display_frame_type, S5P_FIMV_D_DISPLAY_FRAME_TYPE_V6);
+	R(d_display_crop_info1, S5P_FIMV_D_DISPLAY_CROP_INFO1_V6);
+	R(d_display_crop_info2, S5P_FIMV_D_DISPLAY_CROP_INFO2_V6);
+	R(d_display_aspect_ratio, S5P_FIMV_D_DISPLAY_ASPECT_RATIO_V6);
+	R(d_display_extended_ar, S5P_FIMV_D_DISPLAY_EXTENDED_AR_V6);
+	R(d_decoded_status, S5P_FIMV_D_DECODED_STATUS_V6);
+	R(d_decoded_first_plane_addr, S5P_FIMV_D_DECODED_LUMA_ADDR_V6);
+	R(d_decoded_second_plane_addr, S5P_FIMV_D_DECODED_CHROMA_ADDR_V6);
+	R(d_decoded_frame_type, S5P_FIMV_D_DECODED_FRAME_TYPE_V6);
+	R(d_decoded_nal_size, S5P_FIMV_D_DECODED_NAL_SIZE_V6);
+	R(d_ret_picture_tag_top, S5P_FIMV_D_RET_PICTURE_TAG_TOP_V6);
+	R(d_ret_picture_tag_bot, S5P_FIMV_D_RET_PICTURE_TAG_BOT_V6);
+	R(d_h264_info, S5P_FIMV_D_H264_INFO_V6);
+	R(d_mvc_view_id, S5P_FIMV_D_MVC_VIEW_ID_V6);
+	R(d_frame_pack_sei_avail, S5P_FIMV_D_FRAME_PACK_SEI_AVAIL_V6);
+
+	/* encoder registers */
+	R(e_frame_width, S5P_FIMV_E_FRAME_WIDTH_V6);
+	R(e_frame_height, S5P_FIMV_E_FRAME_HEIGHT_V6);
+	R(e_cropped_frame_width, S5P_FIMV_E_CROPPED_FRAME_WIDTH_V6);
+	R(e_cropped_frame_height, S5P_FIMV_E_CROPPED_FRAME_HEIGHT_V6);
+	R(e_frame_crop_offset, S5P_FIMV_E_FRAME_CROP_OFFSET_V6);
+	R(e_enc_options, S5P_FIMV_E_ENC_OPTIONS_V6);
+	R(e_picture_profile, S5P_FIMV_E_PICTURE_PROFILE_V6);
+	R(e_vbv_buffer_size, S5P_FIMV_E_VBV_BUFFER_SIZE_V6);
+	R(e_vbv_init_delay, S5P_FIMV_E_VBV_INIT_DELAY_V6);
+	R(e_fixed_picture_qp, S5P_FIMV_E_FIXED_PICTURE_QP_V6);
+	R(e_rc_config, S5P_FIMV_E_RC_CONFIG_V6);
+	R(e_rc_qp_bound, S5P_FIMV_E_RC_QP_BOUND_V6);
+	R(e_rc_mode, S5P_FIMV_E_RC_RPARAM_V6);
+	R(e_mb_rc_config, S5P_FIMV_E_MB_RC_CONFIG_V6);
+	R(e_padding_ctrl, S5P_FIMV_E_PADDING_CTRL_V6);
+	R(e_mv_hor_range, S5P_FIMV_E_MV_HOR_RANGE_V6);
+	R(e_mv_ver_range, S5P_FIMV_E_MV_VER_RANGE_V6);
+	R(e_num_dpb, S5P_FIMV_E_NUM_DPB_V6);
+	R(e_luma_dpb, S5P_FIMV_E_LUMA_DPB_V6);
+	R(e_chroma_dpb, S5P_FIMV_E_CHROMA_DPB_V6);
+	R(e_me_buffer, S5P_FIMV_E_ME_BUFFER_V6);
+	R(e_scratch_buffer_addr, S5P_FIMV_E_SCRATCH_BUFFER_ADDR_V6);
+	R(e_scratch_buffer_size, S5P_FIMV_E_SCRATCH_BUFFER_SIZE_V6);
+	R(e_tmv_buffer0, S5P_FIMV_E_TMV_BUFFER0_V6);
+	R(e_tmv_buffer1, S5P_FIMV_E_TMV_BUFFER1_V6);
+	R(e_source_first_plane_addr, S5P_FIMV_E_SOURCE_LUMA_ADDR_V6);
+	R(e_source_second_plane_addr, S5P_FIMV_E_SOURCE_CHROMA_ADDR_V6);
+	R(e_stream_buffer_addr, S5P_FIMV_E_STREAM_BUFFER_ADDR_V6);
+	R(e_stream_buffer_size, S5P_FIMV_E_STREAM_BUFFER_SIZE_V6);
+	R(e_roi_buffer_addr, S5P_FIMV_E_ROI_BUFFER_ADDR_V6);
+	R(e_param_change, S5P_FIMV_E_PARAM_CHANGE_V6);
+	R(e_ir_size, S5P_FIMV_E_IR_SIZE_V6);
+	R(e_gop_config, S5P_FIMV_E_GOP_CONFIG_V6);
+	R(e_mslice_mode, S5P_FIMV_E_MSLICE_MODE_V6);
+	R(e_mslice_size_mb, S5P_FIMV_E_MSLICE_SIZE_MB_V6);
+	R(e_mslice_size_bits, S5P_FIMV_E_MSLICE_SIZE_BITS_V6);
+	R(e_frame_insertion, S5P_FIMV_E_FRAME_INSERTION_V6);
+	R(e_rc_frame_rate, S5P_FIMV_E_RC_FRAME_RATE_V6);
+	R(e_rc_bit_rate, S5P_FIMV_E_RC_BIT_RATE_V6);
+	R(e_rc_roi_ctrl, S5P_FIMV_E_RC_ROI_CTRL_V6);
+	R(e_picture_tag, S5P_FIMV_E_PICTURE_TAG_V6);
+	R(e_bit_count_enable, S5P_FIMV_E_BIT_COUNT_ENABLE_V6);
+	R(e_max_bit_count, S5P_FIMV_E_MAX_BIT_COUNT_V6);
+	R(e_min_bit_count, S5P_FIMV_E_MIN_BIT_COUNT_V6);
+	R(e_metadata_buffer_addr, S5P_FIMV_E_METADATA_BUFFER_ADDR_V6);
+	R(e_metadata_buffer_size, S5P_FIMV_E_METADATA_BUFFER_SIZE_V6);
+	R(e_encoded_source_first_plane_addr,
+			S5P_FIMV_E_ENCODED_SOURCE_LUMA_ADDR_V6);
+	R(e_encoded_source_second_plane_addr,
+			S5P_FIMV_E_ENCODED_SOURCE_CHROMA_ADDR_V6);
+	R(e_stream_size, S5P_FIMV_E_STREAM_SIZE_V6);
+	R(e_slice_type, S5P_FIMV_E_SLICE_TYPE_V6);
+	R(e_picture_count, S5P_FIMV_E_PICTURE_COUNT_V6);
+	R(e_ret_picture_tag, S5P_FIMV_E_RET_PICTURE_TAG_V6);
+	R(e_recon_luma_dpb_addr, S5P_FIMV_E_RECON_LUMA_DPB_ADDR_V6);
+	R(e_recon_chroma_dpb_addr, S5P_FIMV_E_RECON_CHROMA_DPB_ADDR_V6);
+	R(e_mpeg4_options, S5P_FIMV_E_MPEG4_OPTIONS_V6);
+	R(e_mpeg4_hec_period, S5P_FIMV_E_MPEG4_HEC_PERIOD_V6);
+	R(e_aspect_ratio, S5P_FIMV_E_ASPECT_RATIO_V6);
+	R(e_extended_sar, S5P_FIMV_E_EXTENDED_SAR_V6);
+	R(e_h264_options, S5P_FIMV_E_H264_OPTIONS_V6);
+	R(e_h264_lf_alpha_offset, S5P_FIMV_E_H264_LF_ALPHA_OFFSET_V6);
+	R(e_h264_lf_beta_offset, S5P_FIMV_E_H264_LF_BETA_OFFSET_V6);
+	R(e_h264_i_period, S5P_FIMV_E_H264_I_PERIOD_V6);
+	R(e_h264_fmo_slice_grp_map_type,
+			S5P_FIMV_E_H264_FMO_SLICE_GRP_MAP_TYPE_V6);
+	R(e_h264_fmo_num_slice_grp_minus1,
+			S5P_FIMV_E_H264_FMO_NUM_SLICE_GRP_MINUS1_V6);
+	R(e_h264_fmo_slice_grp_change_dir,
+			S5P_FIMV_E_H264_FMO_SLICE_GRP_CHANGE_DIR_V6);
+	R(e_h264_fmo_slice_grp_change_rate_minus1,
+			S5P_FIMV_E_H264_FMO_SLICE_GRP_CHANGE_RATE_MINUS1_V6);
+	R(e_h264_fmo_run_length_minus1_0,
+			S5P_FIMV_E_H264_FMO_RUN_LENGTH_MINUS1_0_V6);
+	R(e_h264_aso_slice_order_0, S5P_FIMV_E_H264_ASO_SLICE_ORDER_0_V6);
+	R(e_h264_num_t_layer, S5P_FIMV_E_H264_NUM_T_LAYER_V6);
+	R(e_h264_hierarchical_qp_layer0,
+			S5P_FIMV_E_H264_HIERARCHICAL_QP_LAYER0_V6);
+	R(e_h264_frame_packing_sei_info,
+			S5P_FIMV_E_H264_FRAME_PACKING_SEI_INFO_V6);
+
+	if (!IS_MFCV7_PLUS(dev))
+		goto done;
+
+	/* Initialize registers used in MFC v7+ */
+	R(e_source_first_plane_addr, S5P_FIMV_E_SOURCE_FIRST_ADDR_V7);
+	R(e_source_second_plane_addr, S5P_FIMV_E_SOURCE_SECOND_ADDR_V7);
+	R(e_source_third_plane_addr, S5P_FIMV_E_SOURCE_THIRD_ADDR_V7);
+	R(e_source_first_plane_stride, S5P_FIMV_E_SOURCE_FIRST_STRIDE_V7);
+	R(e_source_second_plane_stride, S5P_FIMV_E_SOURCE_SECOND_STRIDE_V7);
+	R(e_source_third_plane_stride, S5P_FIMV_E_SOURCE_THIRD_STRIDE_V7);
+	R(e_encoded_source_first_plane_addr,
+			S5P_FIMV_E_ENCODED_SOURCE_FIRST_ADDR_V7);
+	R(e_encoded_source_second_plane_addr,
+			S5P_FIMV_E_ENCODED_SOURCE_SECOND_ADDR_V7);
+	R(e_vp8_options, S5P_FIMV_E_VP8_OPTIONS_V7);
+
+	if (!IS_MFCV8(dev))
+		goto done;
+
+	/* Initialize registers used in MFC v8 only.
+	 * Also, over-write the registers which have
+	 * a different offset for MFC v8. */
+	R(d_stream_data_size, S5P_FIMV_D_STREAM_DATA_SIZE_V8);
+	R(d_cpb_buffer_addr, S5P_FIMV_D_CPB_BUFFER_ADDR_V8);
+	R(d_cpb_buffer_size, S5P_FIMV_D_CPB_BUFFER_SIZE_V8);
+	R(d_cpb_buffer_offset, S5P_FIMV_D_CPB_BUFFER_OFFSET_V8);
+	R(d_first_plane_dpb_size, S5P_FIMV_D_FIRST_PLANE_DPB_SIZE_V8);
+	R(d_second_plane_dpb_size, S5P_FIMV_D_SECOND_PLANE_DPB_SIZE_V8);
+	R(d_scratch_buffer_addr, S5P_FIMV_D_SCRATCH_BUFFER_ADDR_V8);
+	R(d_scratch_buffer_size, S5P_FIMV_D_SCRATCH_BUFFER_SIZE_V8);
+	R(d_first_plane_dpb_stride_size,
+			S5P_FIMV_D_FIRST_PLANE_DPB_STRIDE_SIZE_V8);
+	R(d_second_plane_dpb_stride_size,
+			S5P_FIMV_D_SECOND_PLANE_DPB_STRIDE_SIZE_V8);
+	R(d_mv_buffer_size, S5P_FIMV_D_MV_BUFFER_SIZE_V8);
+	R(d_num_mv, S5P_FIMV_D_NUM_MV_V8);
+	R(d_first_plane_dpb, S5P_FIMV_D_FIRST_PLANE_DPB_V8);
+	R(d_second_plane_dpb, S5P_FIMV_D_SECOND_PLANE_DPB_V8);
+	R(d_mv_buffer, S5P_FIMV_D_MV_BUFFER_V8);
+	R(d_init_buffer_options, S5P_FIMV_D_INIT_BUFFER_OPTIONS_V8);
+	R(d_available_dpb_flag_lower, S5P_FIMV_D_AVAILABLE_DPB_FLAG_LOWER_V8);
+	R(d_slice_if_enable, S5P_FIMV_D_SLICE_IF_ENABLE_V8);
+	R(d_display_first_plane_addr, S5P_FIMV_D_DISPLAY_FIRST_PLANE_ADDR_V8);
+	R(d_display_second_plane_addr, S5P_FIMV_D_DISPLAY_SECOND_PLANE_ADDR_V8);
+	R(d_decoded_first_plane_addr, S5P_FIMV_D_DECODED_FIRST_PLANE_ADDR_V8);
+	R(d_decoded_second_plane_addr, S5P_FIMV_D_DECODED_SECOND_PLANE_ADDR_V8);
+	R(d_display_status, S5P_FIMV_D_DISPLAY_STATUS_V8);
+	R(d_decoded_status, S5P_FIMV_D_DECODED_STATUS_V8);
+	R(d_decoded_frame_type, S5P_FIMV_D_DECODED_FRAME_TYPE_V8);
+	R(d_display_frame_type, S5P_FIMV_D_DISPLAY_FRAME_TYPE_V8);
+	R(d_decoded_nal_size, S5P_FIMV_D_DECODED_NAL_SIZE_V8);
+	R(d_display_frame_width, S5P_FIMV_D_DISPLAY_FRAME_WIDTH_V8);
+	R(d_display_frame_height, S5P_FIMV_D_DISPLAY_FRAME_HEIGHT_V8);
+	R(d_frame_pack_sei_avail, S5P_FIMV_D_FRAME_PACK_SEI_AVAIL_V8);
+	R(d_mvc_num_views, S5P_FIMV_D_MVC_NUM_VIEWS_V8);
+	R(d_mvc_view_id, S5P_FIMV_D_MVC_VIEW_ID_V8);
+	R(d_ret_picture_tag_top, S5P_FIMV_D_RET_PICTURE_TAG_TOP_V8);
+	R(d_ret_picture_tag_bot, S5P_FIMV_D_RET_PICTURE_TAG_BOT_V8);
+	R(d_display_crop_info1, S5P_FIMV_D_DISPLAY_CROP_INFO1_V8);
+	R(d_display_crop_info2, S5P_FIMV_D_DISPLAY_CROP_INFO2_V8);
+
+	/* encoder registers */
+	R(e_padding_ctrl, S5P_FIMV_E_PADDING_CTRL_V8);
+	R(e_rc_config, S5P_FIMV_E_RC_CONFIG_V8);
+	R(e_rc_mode, S5P_FIMV_E_RC_RPARAM_V8);
+	R(e_mv_hor_range, S5P_FIMV_E_MV_HOR_RANGE_V8);
+	R(e_mv_ver_range, S5P_FIMV_E_MV_VER_RANGE_V8);
+	R(e_rc_qp_bound, S5P_FIMV_E_RC_QP_BOUND_V8);
+	R(e_fixed_picture_qp, S5P_FIMV_E_FIXED_PICTURE_QP_V8);
+	R(e_vbv_buffer_size, S5P_FIMV_E_VBV_BUFFER_SIZE_V8);
+	R(e_vbv_init_delay, S5P_FIMV_E_VBV_INIT_DELAY_V8);
+	R(e_mb_rc_config, S5P_FIMV_E_MB_RC_CONFIG_V8);
+	R(e_aspect_ratio, S5P_FIMV_E_ASPECT_RATIO_V8);
+	R(e_extended_sar, S5P_FIMV_E_EXTENDED_SAR_V8);
+	R(e_h264_options, S5P_FIMV_E_H264_OPTIONS_V8);
+
+done:
+	return &mfc_regs;
+#undef S5P_MFC_REG_ADDR
+#undef R
+}
+
+/* Initialize opr function pointers for MFC v6 */
+static struct s5p_mfc_hw_ops s5p_mfc_ops_v6 = {
+	.alloc_dec_temp_buffers = s5p_mfc_alloc_dec_temp_buffers_v6,
+	.release_dec_desc_buffer = s5p_mfc_release_dec_desc_buffer_v6,
+	.alloc_codec_buffers = s5p_mfc_alloc_codec_buffers_v6,
+	.release_codec_buffers = s5p_mfc_release_codec_buffers_v6,
+	.alloc_instance_buffer = s5p_mfc_alloc_instance_buffer_v6,
+	.release_instance_buffer = s5p_mfc_release_instance_buffer_v6,
+	.alloc_dev_context_buffer =
+		s5p_mfc_alloc_dev_context_buffer_v6,
+	.release_dev_context_buffer =
+		s5p_mfc_release_dev_context_buffer_v6,
+	.dec_calc_dpb_size = s5p_mfc_dec_calc_dpb_size_v6,
+	.enc_calc_src_size = s5p_mfc_enc_calc_src_size_v6,
+	.set_dec_stream_buffer = s5p_mfc_set_dec_stream_buffer_v6,
+	.set_dec_frame_buffer = s5p_mfc_set_dec_frame_buffer_v6,
+	.set_enc_stream_buffer = s5p_mfc_set_enc_stream_buffer_v6,
+	.set_enc_frame_buffer = s5p_mfc_set_enc_frame_buffer_v6,
+	.get_enc_frame_buffer = s5p_mfc_get_enc_frame_buffer_v6,
+	.set_enc_ref_buffer = s5p_mfc_set_enc_ref_buffer_v6,
+	.init_decode = s5p_mfc_init_decode_v6,
+	.init_encode = s5p_mfc_init_encode_v6,
+	.encode_one_frame = s5p_mfc_encode_one_frame_v6,
+	.try_run = s5p_mfc_try_run_v6,
+	.cleanup_queue = s5p_mfc_cleanup_queue_v6,
+	.clear_int_flags = s5p_mfc_clear_int_flags_v6,
+	.write_info = s5p_mfc_write_info_v6,
+	.read_info = s5p_mfc_read_info_v6,
+	.get_dspl_y_adr = s5p_mfc_get_dspl_y_adr_v6,
+	.get_dec_y_adr = s5p_mfc_get_dec_y_adr_v6,
+	.get_dspl_status = s5p_mfc_get_dspl_status_v6,
+	.get_dec_status = s5p_mfc_get_dec_status_v6,
+	.get_dec_frame_type = s5p_mfc_get_dec_frame_type_v6,
+	.get_disp_frame_type = s5p_mfc_get_disp_frame_type_v6,
+	.get_consumed_stream = s5p_mfc_get_consumed_stream_v6,
+	.get_int_reason = s5p_mfc_get_int_reason_v6,
+	.get_int_err = s5p_mfc_get_int_err_v6,
+	.err_dec = s5p_mfc_err_dec_v6,
+	.err_dspl = s5p_mfc_err_dspl_v6,
+	.get_img_width = s5p_mfc_get_img_width_v6,
+	.get_img_height = s5p_mfc_get_img_height_v6,
+	.get_dpb_count = s5p_mfc_get_dpb_count_v6,
+	.get_mv_count = s5p_mfc_get_mv_count_v6,
+	.get_inst_no = s5p_mfc_get_inst_no_v6,
+	.get_enc_strm_size = s5p_mfc_get_enc_strm_size_v6,
+	.get_enc_slice_type = s5p_mfc_get_enc_slice_type_v6,
+	.get_enc_dpb_count = s5p_mfc_get_enc_dpb_count_v6,
+	.get_enc_pic_count = s5p_mfc_get_enc_pic_count_v6,
+	.get_sei_avail_status = s5p_mfc_get_sei_avail_status_v6,
+	.get_mvc_num_views = s5p_mfc_get_mvc_num_views_v6,
+	.get_mvc_view_id = s5p_mfc_get_mvc_view_id_v6,
+	.get_pic_type_top = s5p_mfc_get_pic_type_top_v6,
+	.get_pic_type_bot = s5p_mfc_get_pic_type_bot_v6,
+	.get_crop_info_h = s5p_mfc_get_crop_info_h_v6,
+	.get_crop_info_v = s5p_mfc_get_crop_info_v_v6,
+};
+
+struct s5p_mfc_hw_ops *s5p_mfc_init_hw_ops_v6(void)
+{
+	return &s5p_mfc_ops_v6;
+}
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.h b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.h
new file mode 100644
index 0000000..8055848
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.h
@@ -0,0 +1,45 @@
+/*
+ * drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.h
+ *
+ * Header file for Samsung MFC (Multi Function Codec - FIMV) driver
+ * Contains declarations of hw related functions.
+ *
+ * Copyright (c) 2012 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef S5P_MFC_OPR_V6_H_
+#define S5P_MFC_OPR_V6_H_
+
+#include "s5p_mfc_common.h"
+#include "s5p_mfc_opr.h"
+
+#define MFC_CTRL_MODE_CUSTOM	MFC_CTRL_MODE_SFR
+
+#define MB_WIDTH(x_size)		DIV_ROUND_UP(x_size, 16)
+#define MB_HEIGHT(y_size)		DIV_ROUND_UP(y_size, 16)
+#define S5P_MFC_DEC_MV_SIZE_V6(x, y)	(MB_WIDTH(x) * \
+					(((MB_HEIGHT(y)+1)/2)*2) * 64 + 128)
+
+/* Definition */
+#define ENC_MULTI_SLICE_MB_MAX		((1 << 30) - 1)
+#define ENC_MULTI_SLICE_BIT_MIN		2800
+#define ENC_INTRA_REFRESH_MB_MAX	((1 << 18) - 1)
+#define ENC_VBV_BUF_SIZE_MAX		((1 << 30) - 1)
+#define ENC_H264_LOOP_FILTER_AB_MIN	-12
+#define ENC_H264_LOOP_FILTER_AB_MAX	12
+#define ENC_H264_RC_FRAME_RATE_MAX	((1 << 16) - 1)
+#define ENC_H263_RC_FRAME_RATE_MAX	((1 << 16) - 1)
+#define ENC_H264_PROFILE_MAX		3
+#define ENC_H264_LEVEL_MAX		42
+#define ENC_MPEG4_VOP_TIME_RES_MAX	((1 << 16) - 1)
+#define FRAME_DELTA_H264_H263		1
+#define TIGHT_CBR_MAX			10
+
+struct s5p_mfc_hw_ops *s5p_mfc_init_hw_ops_v6(void);
+const struct s5p_mfc_regs *s5p_mfc_init_regs_v6_plus(struct s5p_mfc_dev *dev);
+#endif /* S5P_MFC_OPR_V6_H_ */
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_pm.c b/drivers/media/platform/s5p-mfc/s5p_mfc_pm.c
new file mode 100644
index 0000000..5f97a33
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_pm.c
@@ -0,0 +1,139 @@
+/*
+ * linux/drivers/media/platform/s5p-mfc/s5p_mfc_pm.c
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include "s5p_mfc_common.h"
+#include "s5p_mfc_debug.h"
+#include "s5p_mfc_pm.h"
+
+#define MFC_GATE_CLK_NAME	"mfc"
+#define MFC_SCLK_NAME		"sclk_mfc"
+#define MFC_SCLK_RATE		(200 * 1000000)
+
+#define CLK_DEBUG
+
+static struct s5p_mfc_pm *pm;
+static struct s5p_mfc_dev *p_dev;
+
+#ifdef CLK_DEBUG
+static atomic_t clk_ref;
+#endif
+
+int s5p_mfc_init_pm(struct s5p_mfc_dev *dev)
+{
+	int ret = 0;
+
+	pm = &dev->pm;
+	p_dev = dev;
+	pm->clock_gate = clk_get(&dev->plat_dev->dev, MFC_GATE_CLK_NAME);
+	if (IS_ERR(pm->clock_gate)) {
+		mfc_err("Failed to get clock-gating control\n");
+		ret = PTR_ERR(pm->clock_gate);
+		goto err_g_ip_clk;
+	}
+
+	ret = clk_prepare(pm->clock_gate);
+	if (ret) {
+		mfc_err("Failed to prepare clock-gating control\n");
+		goto err_p_ip_clk;
+	}
+
+	if (dev->variant->version != MFC_VERSION_V6) {
+		pm->clock = clk_get(&dev->plat_dev->dev, MFC_SCLK_NAME);
+		if (IS_ERR(pm->clock)) {
+			mfc_info("Failed to get MFC special clock control\n");
+		} else {
+			clk_set_rate(pm->clock, MFC_SCLK_RATE);
+			ret = clk_prepare_enable(pm->clock);
+			if (ret) {
+				mfc_err("Failed to enable MFC special clock\n");
+				goto err_s_clk;
+			}
+		}
+	}
+
+	atomic_set(&pm->power, 0);
+#ifdef CONFIG_PM
+	pm->device = &dev->plat_dev->dev;
+	pm_runtime_enable(pm->device);
+#endif
+#ifdef CLK_DEBUG
+	atomic_set(&clk_ref, 0);
+#endif
+	return 0;
+
+err_s_clk:
+	clk_put(pm->clock);
+err_p_ip_clk:
+	clk_put(pm->clock_gate);
+err_g_ip_clk:
+	return ret;
+}
+
+void s5p_mfc_final_pm(struct s5p_mfc_dev *dev)
+{
+	if (dev->variant->version != MFC_VERSION_V6 &&
+	    pm->clock) {
+		clk_disable_unprepare(pm->clock);
+		clk_put(pm->clock);
+	}
+	clk_unprepare(pm->clock_gate);
+	clk_put(pm->clock_gate);
+#ifdef CONFIG_PM
+	pm_runtime_disable(pm->device);
+#endif
+}
+
+int s5p_mfc_clock_on(void)
+{
+	int ret;
+#ifdef CLK_DEBUG
+	atomic_inc(&clk_ref);
+	mfc_debug(3, "+ %d\n", atomic_read(&clk_ref));
+#endif
+	ret = clk_enable(pm->clock_gate);
+	return ret;
+}
+
+void s5p_mfc_clock_off(void)
+{
+#ifdef CLK_DEBUG
+	atomic_dec(&clk_ref);
+	mfc_debug(3, "- %d\n", atomic_read(&clk_ref));
+#endif
+	clk_disable(pm->clock_gate);
+}
+
+int s5p_mfc_power_on(void)
+{
+#ifdef CONFIG_PM
+	return pm_runtime_get_sync(pm->device);
+#else
+	atomic_set(&pm->power, 1);
+	return 0;
+#endif
+}
+
+int s5p_mfc_power_off(void)
+{
+#ifdef CONFIG_PM
+	return pm_runtime_put_sync(pm->device);
+#else
+	atomic_set(&pm->power, 0);
+	return 0;
+#endif
+}
+
+
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_pm.h b/drivers/media/platform/s5p-mfc/s5p_mfc_pm.h
new file mode 100644
index 0000000..875c534
--- /dev/null
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_pm.h
@@ -0,0 +1,24 @@
+/*
+ * linux/drivers/media/platform/s5p-mfc/s5p_mfc_pm.h
+ *
+ * Copyright (C) 2011 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com/
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#ifndef S5P_MFC_PM_H_
+#define S5P_MFC_PM_H_
+
+int s5p_mfc_init_pm(struct s5p_mfc_dev *dev);
+void s5p_mfc_final_pm(struct s5p_mfc_dev *dev);
+
+int s5p_mfc_clock_on(void);
+void s5p_mfc_clock_off(void);
+int s5p_mfc_power_on(void);
+int s5p_mfc_power_off(void);
+
+#endif /* S5P_MFC_PM_H_ */
diff --git a/drivers/media/platform/s5p-tv/Kconfig b/drivers/media/platform/s5p-tv/Kconfig
new file mode 100644
index 0000000..697aaed
--- /dev/null
+++ b/drivers/media/platform/s5p-tv/Kconfig
@@ -0,0 +1,88 @@
+# drivers/media/platform/s5p-tv/Kconfig
+#
+# Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
+#	http://www.samsung.com/
+# Tomasz Stanislawski <t.stanislaws@samsung.com>
+#
+# Licensed under GPL
+
+config VIDEO_SAMSUNG_S5P_TV
+	bool "Samsung TV driver for S5P platform"
+	depends on PM
+	depends on ARCH_S5PV210 || ARCH_EXYNOS || COMPILE_TEST
+	default n
+	---help---
+	  Say Y here to enable selecting the TV output devices for
+	  Samsung S5P platform.
+
+if VIDEO_SAMSUNG_S5P_TV
+
+config VIDEO_SAMSUNG_S5P_HDMI
+	tristate "Samsung HDMI Driver"
+	depends on VIDEO_V4L2
+	depends on I2C
+	depends on VIDEO_SAMSUNG_S5P_TV
+	select VIDEO_SAMSUNG_S5P_HDMIPHY
+	help
+	  Say Y here if you want support for the HDMI output
+	  interface in S5P Samsung SoC. The driver can be compiled
+	  as module. It is an auxiliary driver, that exposes a V4L2
+	  subdev for use by other drivers. This driver requires
+	  hdmiphy driver to work correctly.
+
+config VIDEO_SAMSUNG_S5P_HDMI_DEBUG
+	bool "Enable debug for HDMI Driver"
+	depends on VIDEO_SAMSUNG_S5P_HDMI
+	default n
+	help
+	  Enables debugging for HDMI driver.
+
+config VIDEO_SAMSUNG_S5P_HDMIPHY
+	tristate "Samsung HDMIPHY Driver"
+	depends on VIDEO_DEV && VIDEO_V4L2 && I2C
+	depends on VIDEO_SAMSUNG_S5P_TV
+	help
+	  Say Y here if you want support for the physical HDMI
+	  interface in S5P Samsung SoC. The driver can be compiled
+	  as module. It is an I2C driver, that exposes a V4L2
+	  subdev for use by other drivers.
+
+config VIDEO_SAMSUNG_S5P_SII9234
+	tristate "Samsung SII9234 Driver"
+	depends on VIDEO_DEV && VIDEO_V4L2 && I2C
+	depends on VIDEO_SAMSUNG_S5P_TV
+	help
+	  Say Y here if you want support for the MHL interface
+	  in S5P Samsung SoC. The driver can be compiled
+	  as module. It is an I2C driver, that exposes a V4L2
+	  subdev for use by other drivers.
+
+config VIDEO_SAMSUNG_S5P_SDO
+	tristate "Samsung Analog TV Driver"
+	depends on VIDEO_DEV && VIDEO_V4L2
+	depends on VIDEO_SAMSUNG_S5P_TV
+	help
+	  Say Y here if you want support for the analog TV output
+	  interface in S5P Samsung SoC. The driver can be compiled
+	  as module. It is an auxiliary driver, that exposes a V4L2
+	  subdev for use by other drivers. This driver requires
+	  hdmiphy driver to work correctly.
+
+config VIDEO_SAMSUNG_S5P_MIXER
+	tristate "Samsung Mixer and Video Processor Driver"
+	depends on VIDEO_DEV && VIDEO_V4L2
+	depends on VIDEO_SAMSUNG_S5P_TV
+	depends on HAS_DMA
+	select VIDEOBUF2_DMA_CONTIG
+	help
+	  Say Y here if you want support for the Mixer in Samsung S5P SoCs.
+	  This device produce image data to one of output interfaces.
+
+config VIDEO_SAMSUNG_S5P_MIXER_DEBUG
+	bool "Enable debug for Mixer Driver"
+	depends on VIDEO_SAMSUNG_S5P_MIXER
+	default n
+	help
+	  Enables debugging for Mixer driver.
+
+endif # VIDEO_SAMSUNG_S5P_TV
diff --git a/drivers/media/platform/s5p-tv/Makefile b/drivers/media/platform/s5p-tv/Makefile
new file mode 100644
index 0000000..7cd4790
--- /dev/null
+++ b/drivers/media/platform/s5p-tv/Makefile
@@ -0,0 +1,19 @@
+# drivers/media/platform/samsung/tvout/Makefile
+#
+# Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
+#	http://www.samsung.com/
+# Tomasz Stanislawski <t.stanislaws@samsung.com>
+#
+# Licensed under GPL
+
+obj-$(CONFIG_VIDEO_SAMSUNG_S5P_HDMIPHY) += s5p-hdmiphy.o
+s5p-hdmiphy-y += hdmiphy_drv.o
+obj-$(CONFIG_VIDEO_SAMSUNG_S5P_SII9234) += s5p-sii9234.o
+s5p-sii9234-y += sii9234_drv.o
+obj-$(CONFIG_VIDEO_SAMSUNG_S5P_HDMI) += s5p-hdmi.o
+s5p-hdmi-y += hdmi_drv.o
+obj-$(CONFIG_VIDEO_SAMSUNG_S5P_SDO) += s5p-sdo.o
+s5p-sdo-y += sdo_drv.o
+obj-$(CONFIG_VIDEO_SAMSUNG_S5P_MIXER) += s5p-mixer.o
+s5p-mixer-y += mixer_drv.o mixer_video.o mixer_reg.o mixer_grp_layer.o mixer_vp_layer.o
+
diff --git a/drivers/media/platform/s5p-tv/hdmi_drv.c b/drivers/media/platform/s5p-tv/hdmi_drv.c
new file mode 100644
index 0000000..7994075
--- /dev/null
+++ b/drivers/media/platform/s5p-tv/hdmi_drv.c
@@ -0,0 +1,1059 @@
+/*
+ * Samsung HDMI interface driver
+ *
+ * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
+ *
+ * Tomasz Stanislawski, <t.stanislaws@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published
+ * by the Free Software Foundiation. either version 2 of the License,
+ * or (at your option) any later version
+ */
+
+#define pr_fmt(fmt) "s5p-tv (hdmi_drv): " fmt
+
+#ifdef CONFIG_VIDEO_SAMSUNG_S5P_HDMI_DEBUG
+#define DEBUG
+#endif
+
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/i2c.h>
+#include <linux/platform_device.h>
+#include <media/v4l2-subdev.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/delay.h>
+#include <linux/bug.h>
+#include <linux/pm_runtime.h>
+#include <linux/clk.h>
+#include <linux/regulator/consumer.h>
+#include <linux/v4l2-dv-timings.h>
+
+#include <media/s5p_hdmi.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-dv-timings.h>
+
+#include "regs-hdmi.h"
+
+MODULE_AUTHOR("Tomasz Stanislawski, <t.stanislaws@samsung.com>");
+MODULE_DESCRIPTION("Samsung HDMI");
+MODULE_LICENSE("GPL");
+
+struct hdmi_pulse {
+	u32 beg;
+	u32 end;
+};
+
+struct hdmi_timings {
+	struct hdmi_pulse hact;
+	u32 hsyn_pol; /* 0 - high, 1 - low */
+	struct hdmi_pulse hsyn;
+	u32 interlaced;
+	struct hdmi_pulse vact[2];
+	u32 vsyn_pol; /* 0 - high, 1 - low */
+	u32 vsyn_off;
+	struct hdmi_pulse vsyn[2];
+};
+
+struct hdmi_resources {
+	struct clk *hdmi;
+	struct clk *sclk_hdmi;
+	struct clk *sclk_pixel;
+	struct clk *sclk_hdmiphy;
+	struct clk *hdmiphy;
+	struct regulator_bulk_data *regul_bulk;
+	int regul_count;
+};
+
+struct hdmi_device {
+	/** base address of HDMI registers */
+	void __iomem *regs;
+	/** HDMI interrupt */
+	unsigned int irq;
+	/** pointer to device parent */
+	struct device *dev;
+	/** subdev generated by HDMI device */
+	struct v4l2_subdev sd;
+	/** V4L2 device structure */
+	struct v4l2_device v4l2_dev;
+	/** subdev of HDMIPHY interface */
+	struct v4l2_subdev *phy_sd;
+	/** subdev of MHL interface */
+	struct v4l2_subdev *mhl_sd;
+	/** configuration of current graphic mode */
+	const struct hdmi_timings *cur_conf;
+	/** flag indicating that timings are dirty */
+	int cur_conf_dirty;
+	/** current timings */
+	struct v4l2_dv_timings cur_timings;
+	/** other resources */
+	struct hdmi_resources res;
+};
+
+static const struct platform_device_id hdmi_driver_types[] = {
+	{
+		.name		= "s5pv210-hdmi",
+	}, {
+		.name		= "exynos4-hdmi",
+	}, {
+		/* end node */
+	}
+};
+
+static const struct v4l2_subdev_ops hdmi_sd_ops;
+
+static struct hdmi_device *sd_to_hdmi_dev(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct hdmi_device, sd);
+}
+
+static inline
+void hdmi_write(struct hdmi_device *hdev, u32 reg_id, u32 value)
+{
+	writel(value, hdev->regs + reg_id);
+}
+
+static inline
+void hdmi_write_mask(struct hdmi_device *hdev, u32 reg_id, u32 value, u32 mask)
+{
+	u32 old = readl(hdev->regs + reg_id);
+	value = (value & mask) | (old & ~mask);
+	writel(value, hdev->regs + reg_id);
+}
+
+static inline
+void hdmi_writeb(struct hdmi_device *hdev, u32 reg_id, u8 value)
+{
+	writeb(value, hdev->regs + reg_id);
+}
+
+static inline
+void hdmi_writebn(struct hdmi_device *hdev, u32 reg_id, int n, u32 value)
+{
+	switch (n) {
+	default:
+		writeb(value >> 24, hdev->regs + reg_id + 12);
+	case 3:
+		writeb(value >> 16, hdev->regs + reg_id + 8);
+	case 2:
+		writeb(value >>  8, hdev->regs + reg_id + 4);
+	case 1:
+		writeb(value >>  0, hdev->regs + reg_id + 0);
+	}
+}
+
+static inline u32 hdmi_read(struct hdmi_device *hdev, u32 reg_id)
+{
+	return readl(hdev->regs + reg_id);
+}
+
+static irqreturn_t hdmi_irq_handler(int irq, void *dev_data)
+{
+	struct hdmi_device *hdev = dev_data;
+	u32 intc_flag;
+
+	(void)irq;
+	intc_flag = hdmi_read(hdev, HDMI_INTC_FLAG);
+	/* clearing flags for HPD plug/unplug */
+	if (intc_flag & HDMI_INTC_FLAG_HPD_UNPLUG) {
+		pr_info("unplugged\n");
+		hdmi_write_mask(hdev, HDMI_INTC_FLAG, ~0,
+			HDMI_INTC_FLAG_HPD_UNPLUG);
+	}
+	if (intc_flag & HDMI_INTC_FLAG_HPD_PLUG) {
+		pr_info("plugged\n");
+		hdmi_write_mask(hdev, HDMI_INTC_FLAG, ~0,
+			HDMI_INTC_FLAG_HPD_PLUG);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static void hdmi_reg_init(struct hdmi_device *hdev)
+{
+	/* enable HPD interrupts */
+	hdmi_write_mask(hdev, HDMI_INTC_CON, ~0, HDMI_INTC_EN_GLOBAL |
+		HDMI_INTC_EN_HPD_PLUG | HDMI_INTC_EN_HPD_UNPLUG);
+	/* choose DVI mode */
+	hdmi_write_mask(hdev, HDMI_MODE_SEL,
+		HDMI_MODE_DVI_EN, HDMI_MODE_MASK);
+	hdmi_write_mask(hdev, HDMI_CON_2, ~0,
+		HDMI_DVI_PERAMBLE_EN | HDMI_DVI_BAND_EN);
+	/* disable bluescreen */
+	hdmi_write_mask(hdev, HDMI_CON_0, 0, HDMI_BLUE_SCR_EN);
+	/* choose bluescreen (fecal) color */
+	hdmi_writeb(hdev, HDMI_BLUE_SCREEN_0, 0x12);
+	hdmi_writeb(hdev, HDMI_BLUE_SCREEN_1, 0x34);
+	hdmi_writeb(hdev, HDMI_BLUE_SCREEN_2, 0x56);
+}
+
+static void hdmi_timing_apply(struct hdmi_device *hdev,
+	const struct hdmi_timings *t)
+{
+	/* setting core registers */
+	hdmi_writebn(hdev, HDMI_H_BLANK_0, 2, t->hact.beg);
+	hdmi_writebn(hdev, HDMI_H_SYNC_GEN_0, 3,
+		(t->hsyn_pol << 20) | (t->hsyn.end << 10) | t->hsyn.beg);
+	hdmi_writeb(hdev, HDMI_VSYNC_POL, t->vsyn_pol);
+	hdmi_writebn(hdev, HDMI_V_BLANK_0, 3,
+		(t->vact[0].beg << 11) | t->vact[0].end);
+	hdmi_writebn(hdev, HDMI_V_SYNC_GEN_1_0, 3,
+		(t->vsyn[0].beg << 12) | t->vsyn[0].end);
+	if (t->interlaced) {
+		u32 vsyn_trans = t->hsyn.beg + t->vsyn_off;
+
+		hdmi_writeb(hdev, HDMI_INT_PRO_MODE, 1);
+		hdmi_writebn(hdev, HDMI_H_V_LINE_0, 3,
+			(t->hact.end << 12) | t->vact[1].end);
+		hdmi_writebn(hdev, HDMI_V_BLANK_F_0, 3,
+			(t->vact[1].end << 11) | t->vact[1].beg);
+		hdmi_writebn(hdev, HDMI_V_SYNC_GEN_2_0, 3,
+			(t->vsyn[1].beg << 12) | t->vsyn[1].end);
+		hdmi_writebn(hdev, HDMI_V_SYNC_GEN_3_0, 3,
+			(vsyn_trans << 12) | vsyn_trans);
+	} else {
+		hdmi_writeb(hdev, HDMI_INT_PRO_MODE, 0);
+		hdmi_writebn(hdev, HDMI_H_V_LINE_0, 3,
+			(t->hact.end << 12) | t->vact[0].end);
+	}
+
+	/* Timing generator registers */
+	hdmi_writebn(hdev, HDMI_TG_H_FSZ_L, 2, t->hact.end);
+	hdmi_writebn(hdev, HDMI_TG_HACT_ST_L, 2, t->hact.beg);
+	hdmi_writebn(hdev, HDMI_TG_HACT_SZ_L, 2, t->hact.end - t->hact.beg);
+	hdmi_writebn(hdev, HDMI_TG_VSYNC_L, 2, t->vsyn[0].beg);
+	hdmi_writebn(hdev, HDMI_TG_VACT_ST_L, 2, t->vact[0].beg);
+	hdmi_writebn(hdev, HDMI_TG_VACT_SZ_L, 2,
+		t->vact[0].end - t->vact[0].beg);
+	hdmi_writebn(hdev, HDMI_TG_VSYNC_TOP_HDMI_L, 2, t->vsyn[0].beg);
+	hdmi_writebn(hdev, HDMI_TG_FIELD_TOP_HDMI_L, 2, t->vsyn[0].beg);
+	if (t->interlaced) {
+		hdmi_write_mask(hdev, HDMI_TG_CMD, ~0, HDMI_TG_FIELD_EN);
+		hdmi_writebn(hdev, HDMI_TG_V_FSZ_L, 2, t->vact[1].end);
+		hdmi_writebn(hdev, HDMI_TG_VSYNC2_L, 2, t->vsyn[1].beg);
+		hdmi_writebn(hdev, HDMI_TG_FIELD_CHG_L, 2, t->vact[0].end);
+		hdmi_writebn(hdev, HDMI_TG_VACT_ST2_L, 2, t->vact[1].beg);
+		hdmi_writebn(hdev, HDMI_TG_VSYNC_BOT_HDMI_L, 2, t->vsyn[1].beg);
+		hdmi_writebn(hdev, HDMI_TG_FIELD_BOT_HDMI_L, 2, t->vsyn[1].beg);
+	} else {
+		hdmi_write_mask(hdev, HDMI_TG_CMD, 0, HDMI_TG_FIELD_EN);
+		hdmi_writebn(hdev, HDMI_TG_V_FSZ_L, 2, t->vact[0].end);
+	}
+}
+
+static int hdmi_conf_apply(struct hdmi_device *hdmi_dev)
+{
+	struct device *dev = hdmi_dev->dev;
+	const struct hdmi_timings *conf = hdmi_dev->cur_conf;
+	int ret;
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	/* skip if conf is already synchronized with HW */
+	if (!hdmi_dev->cur_conf_dirty)
+		return 0;
+
+	/* reset hdmiphy */
+	hdmi_write_mask(hdmi_dev, HDMI_PHY_RSTOUT, ~0, HDMI_PHY_SW_RSTOUT);
+	mdelay(10);
+	hdmi_write_mask(hdmi_dev, HDMI_PHY_RSTOUT,  0, HDMI_PHY_SW_RSTOUT);
+	mdelay(10);
+
+	/* configure timings */
+	ret = v4l2_subdev_call(hdmi_dev->phy_sd, video, s_dv_timings,
+				&hdmi_dev->cur_timings);
+	if (ret) {
+		dev_err(dev, "failed to set timings\n");
+		return ret;
+	}
+
+	/* resetting HDMI core */
+	hdmi_write_mask(hdmi_dev, HDMI_CORE_RSTOUT,  0, HDMI_CORE_SW_RSTOUT);
+	mdelay(10);
+	hdmi_write_mask(hdmi_dev, HDMI_CORE_RSTOUT, ~0, HDMI_CORE_SW_RSTOUT);
+	mdelay(10);
+
+	hdmi_reg_init(hdmi_dev);
+
+	/* setting core registers */
+	hdmi_timing_apply(hdmi_dev, conf);
+
+	hdmi_dev->cur_conf_dirty = 0;
+
+	return 0;
+}
+
+static void hdmi_dumpregs(struct hdmi_device *hdev, char *prefix)
+{
+#define DUMPREG(reg_id) \
+	dev_dbg(hdev->dev, "%s:" #reg_id " = %08x\n", prefix, \
+		readl(hdev->regs + reg_id))
+
+	dev_dbg(hdev->dev, "%s: ---- CONTROL REGISTERS ----\n", prefix);
+	DUMPREG(HDMI_INTC_FLAG);
+	DUMPREG(HDMI_INTC_CON);
+	DUMPREG(HDMI_HPD_STATUS);
+	DUMPREG(HDMI_PHY_RSTOUT);
+	DUMPREG(HDMI_PHY_VPLL);
+	DUMPREG(HDMI_PHY_CMU);
+	DUMPREG(HDMI_CORE_RSTOUT);
+
+	dev_dbg(hdev->dev, "%s: ---- CORE REGISTERS ----\n", prefix);
+	DUMPREG(HDMI_CON_0);
+	DUMPREG(HDMI_CON_1);
+	DUMPREG(HDMI_CON_2);
+	DUMPREG(HDMI_SYS_STATUS);
+	DUMPREG(HDMI_PHY_STATUS);
+	DUMPREG(HDMI_STATUS_EN);
+	DUMPREG(HDMI_HPD);
+	DUMPREG(HDMI_MODE_SEL);
+	DUMPREG(HDMI_HPD_GEN);
+	DUMPREG(HDMI_DC_CONTROL);
+	DUMPREG(HDMI_VIDEO_PATTERN_GEN);
+
+	dev_dbg(hdev->dev, "%s: ---- CORE SYNC REGISTERS ----\n", prefix);
+	DUMPREG(HDMI_H_BLANK_0);
+	DUMPREG(HDMI_H_BLANK_1);
+	DUMPREG(HDMI_V_BLANK_0);
+	DUMPREG(HDMI_V_BLANK_1);
+	DUMPREG(HDMI_V_BLANK_2);
+	DUMPREG(HDMI_H_V_LINE_0);
+	DUMPREG(HDMI_H_V_LINE_1);
+	DUMPREG(HDMI_H_V_LINE_2);
+	DUMPREG(HDMI_VSYNC_POL);
+	DUMPREG(HDMI_INT_PRO_MODE);
+	DUMPREG(HDMI_V_BLANK_F_0);
+	DUMPREG(HDMI_V_BLANK_F_1);
+	DUMPREG(HDMI_V_BLANK_F_2);
+	DUMPREG(HDMI_H_SYNC_GEN_0);
+	DUMPREG(HDMI_H_SYNC_GEN_1);
+	DUMPREG(HDMI_H_SYNC_GEN_2);
+	DUMPREG(HDMI_V_SYNC_GEN_1_0);
+	DUMPREG(HDMI_V_SYNC_GEN_1_1);
+	DUMPREG(HDMI_V_SYNC_GEN_1_2);
+	DUMPREG(HDMI_V_SYNC_GEN_2_0);
+	DUMPREG(HDMI_V_SYNC_GEN_2_1);
+	DUMPREG(HDMI_V_SYNC_GEN_2_2);
+	DUMPREG(HDMI_V_SYNC_GEN_3_0);
+	DUMPREG(HDMI_V_SYNC_GEN_3_1);
+	DUMPREG(HDMI_V_SYNC_GEN_3_2);
+
+	dev_dbg(hdev->dev, "%s: ---- TG REGISTERS ----\n", prefix);
+	DUMPREG(HDMI_TG_CMD);
+	DUMPREG(HDMI_TG_H_FSZ_L);
+	DUMPREG(HDMI_TG_H_FSZ_H);
+	DUMPREG(HDMI_TG_HACT_ST_L);
+	DUMPREG(HDMI_TG_HACT_ST_H);
+	DUMPREG(HDMI_TG_HACT_SZ_L);
+	DUMPREG(HDMI_TG_HACT_SZ_H);
+	DUMPREG(HDMI_TG_V_FSZ_L);
+	DUMPREG(HDMI_TG_V_FSZ_H);
+	DUMPREG(HDMI_TG_VSYNC_L);
+	DUMPREG(HDMI_TG_VSYNC_H);
+	DUMPREG(HDMI_TG_VSYNC2_L);
+	DUMPREG(HDMI_TG_VSYNC2_H);
+	DUMPREG(HDMI_TG_VACT_ST_L);
+	DUMPREG(HDMI_TG_VACT_ST_H);
+	DUMPREG(HDMI_TG_VACT_SZ_L);
+	DUMPREG(HDMI_TG_VACT_SZ_H);
+	DUMPREG(HDMI_TG_FIELD_CHG_L);
+	DUMPREG(HDMI_TG_FIELD_CHG_H);
+	DUMPREG(HDMI_TG_VACT_ST2_L);
+	DUMPREG(HDMI_TG_VACT_ST2_H);
+	DUMPREG(HDMI_TG_VSYNC_TOP_HDMI_L);
+	DUMPREG(HDMI_TG_VSYNC_TOP_HDMI_H);
+	DUMPREG(HDMI_TG_VSYNC_BOT_HDMI_L);
+	DUMPREG(HDMI_TG_VSYNC_BOT_HDMI_H);
+	DUMPREG(HDMI_TG_FIELD_TOP_HDMI_L);
+	DUMPREG(HDMI_TG_FIELD_TOP_HDMI_H);
+	DUMPREG(HDMI_TG_FIELD_BOT_HDMI_L);
+	DUMPREG(HDMI_TG_FIELD_BOT_HDMI_H);
+#undef DUMPREG
+}
+
+static const struct hdmi_timings hdmi_timings_480p = {
+	.hact = { .beg = 138, .end = 858 },
+	.hsyn_pol = 1,
+	.hsyn = { .beg = 16, .end = 16 + 62 },
+	.interlaced = 0,
+	.vact[0] = { .beg = 42 + 3, .end = 522 + 3 },
+	.vsyn_pol = 1,
+	.vsyn[0] = { .beg = 6 + 3, .end = 12 + 3},
+};
+
+static const struct hdmi_timings hdmi_timings_576p50 = {
+	.hact = { .beg = 144, .end = 864 },
+	.hsyn_pol = 1,
+	.hsyn = { .beg = 12, .end = 12 + 64 },
+	.interlaced = 0,
+	.vact[0] = { .beg = 44 + 5, .end = 620 + 5 },
+	.vsyn_pol = 1,
+	.vsyn[0] = { .beg = 0 + 5, .end = 5 + 5},
+};
+
+static const struct hdmi_timings hdmi_timings_720p60 = {
+	.hact = { .beg = 370, .end = 1650 },
+	.hsyn_pol = 0,
+	.hsyn = { .beg = 110, .end = 110 + 40 },
+	.interlaced = 0,
+	.vact[0] = { .beg = 25 + 5, .end = 745 + 5 },
+	.vsyn_pol = 0,
+	.vsyn[0] = { .beg = 0 + 5, .end = 5 + 5},
+};
+
+static const struct hdmi_timings hdmi_timings_720p50 = {
+	.hact = { .beg = 700, .end = 1980 },
+	.hsyn_pol = 0,
+	.hsyn = { .beg = 440, .end = 440 + 40 },
+	.interlaced = 0,
+	.vact[0] = { .beg = 25 + 5, .end = 745 + 5 },
+	.vsyn_pol = 0,
+	.vsyn[0] = { .beg = 0 + 5, .end = 5 + 5},
+};
+
+static const struct hdmi_timings hdmi_timings_1080p24 = {
+	.hact = { .beg = 830, .end = 2750 },
+	.hsyn_pol = 0,
+	.hsyn = { .beg = 638, .end = 638 + 44 },
+	.interlaced = 0,
+	.vact[0] = { .beg = 41 + 4, .end = 1121 + 4 },
+	.vsyn_pol = 0,
+	.vsyn[0] = { .beg = 0 + 4, .end = 5 + 4},
+};
+
+static const struct hdmi_timings hdmi_timings_1080p60 = {
+	.hact = { .beg = 280, .end = 2200 },
+	.hsyn_pol = 0,
+	.hsyn = { .beg = 88, .end = 88 + 44 },
+	.interlaced = 0,
+	.vact[0] = { .beg = 41 + 4, .end = 1121 + 4 },
+	.vsyn_pol = 0,
+	.vsyn[0] = { .beg = 0 + 4, .end = 5 + 4},
+};
+
+static const struct hdmi_timings hdmi_timings_1080i60 = {
+	.hact = { .beg = 280, .end = 2200 },
+	.hsyn_pol = 0,
+	.hsyn = { .beg = 88, .end = 88 + 44 },
+	.interlaced = 1,
+	.vact[0] = { .beg = 20 + 2, .end = 560 + 2 },
+	.vact[1] = { .beg = 583 + 2, .end = 1123 + 2 },
+	.vsyn_pol = 0,
+	.vsyn_off = 1100,
+	.vsyn[0] = { .beg = 0 + 2, .end = 5 + 2},
+	.vsyn[1] = { .beg = 562 + 2, .end = 567 + 2},
+};
+
+static const struct hdmi_timings hdmi_timings_1080i50 = {
+	.hact = { .beg = 720, .end = 2640 },
+	.hsyn_pol = 0,
+	.hsyn = { .beg = 528, .end = 528 + 44 },
+	.interlaced = 1,
+	.vact[0] = { .beg = 20 + 2, .end = 560 + 2 },
+	.vact[1] = { .beg = 583 + 2, .end = 1123 + 2 },
+	.vsyn_pol = 0,
+	.vsyn_off = 1320,
+	.vsyn[0] = { .beg = 0 + 2, .end = 5 + 2},
+	.vsyn[1] = { .beg = 562 + 2, .end = 567 + 2},
+};
+
+static const struct hdmi_timings hdmi_timings_1080p50 = {
+	.hact = { .beg = 720, .end = 2640 },
+	.hsyn_pol = 0,
+	.hsyn = { .beg = 528, .end = 528 + 44 },
+	.interlaced = 0,
+	.vact[0] = { .beg = 41 + 4, .end = 1121 + 4 },
+	.vsyn_pol = 0,
+	.vsyn[0] = { .beg = 0 + 4, .end = 5 + 4},
+};
+
+/* default hdmi_timings index of the timings configured on probe */
+#define HDMI_DEFAULT_TIMINGS_IDX (0)
+
+static const struct {
+	bool reduced_fps;
+	const struct v4l2_dv_timings dv_timings;
+	const struct hdmi_timings *hdmi_timings;
+} hdmi_timings[] = {
+	{ false, V4L2_DV_BT_CEA_720X480P59_94, &hdmi_timings_480p    },
+	{ false, V4L2_DV_BT_CEA_720X576P50,    &hdmi_timings_576p50  },
+	{ false, V4L2_DV_BT_CEA_1280X720P50,   &hdmi_timings_720p50  },
+	{ true,  V4L2_DV_BT_CEA_1280X720P60,   &hdmi_timings_720p60  },
+	{ false, V4L2_DV_BT_CEA_1920X1080P24,  &hdmi_timings_1080p24 },
+	{ false, V4L2_DV_BT_CEA_1920X1080P30,  &hdmi_timings_1080p60 },
+	{ false, V4L2_DV_BT_CEA_1920X1080P50,  &hdmi_timings_1080p50 },
+	{ false, V4L2_DV_BT_CEA_1920X1080I50,  &hdmi_timings_1080i50 },
+	{ false, V4L2_DV_BT_CEA_1920X1080I60,  &hdmi_timings_1080i60 },
+	{ false, V4L2_DV_BT_CEA_1920X1080P60,  &hdmi_timings_1080p60 },
+};
+
+static int hdmi_streamon(struct hdmi_device *hdev)
+{
+	struct device *dev = hdev->dev;
+	struct hdmi_resources *res = &hdev->res;
+	int ret, tries;
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	ret = hdmi_conf_apply(hdev);
+	if (ret)
+		return ret;
+
+	ret = v4l2_subdev_call(hdev->phy_sd, video, s_stream, 1);
+	if (ret)
+		return ret;
+
+	/* waiting for HDMIPHY's PLL to get to steady state */
+	for (tries = 100; tries; --tries) {
+		u32 val = hdmi_read(hdev, HDMI_PHY_STATUS);
+		if (val & HDMI_PHY_STATUS_READY)
+			break;
+		mdelay(1);
+	}
+	/* steady state not achieved */
+	if (tries == 0) {
+		dev_err(dev, "hdmiphy's pll could not reach steady state.\n");
+		v4l2_subdev_call(hdev->phy_sd, video, s_stream, 0);
+		hdmi_dumpregs(hdev, "hdmiphy - s_stream");
+		return -EIO;
+	}
+
+	/* starting MHL */
+	ret = v4l2_subdev_call(hdev->mhl_sd, video, s_stream, 1);
+	if (hdev->mhl_sd && ret) {
+		v4l2_subdev_call(hdev->phy_sd, video, s_stream, 0);
+		hdmi_dumpregs(hdev, "mhl - s_stream");
+		return -EIO;
+	}
+
+	/* hdmiphy clock is used for HDMI in streaming mode */
+	clk_disable(res->sclk_hdmi);
+	clk_set_parent(res->sclk_hdmi, res->sclk_hdmiphy);
+	clk_enable(res->sclk_hdmi);
+
+	/* enable HDMI and timing generator */
+	hdmi_write_mask(hdev, HDMI_CON_0, ~0, HDMI_EN);
+	hdmi_write_mask(hdev, HDMI_TG_CMD, ~0, HDMI_TG_EN);
+	hdmi_dumpregs(hdev, "streamon");
+	return 0;
+}
+
+static int hdmi_streamoff(struct hdmi_device *hdev)
+{
+	struct device *dev = hdev->dev;
+	struct hdmi_resources *res = &hdev->res;
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	hdmi_write_mask(hdev, HDMI_CON_0, 0, HDMI_EN);
+	hdmi_write_mask(hdev, HDMI_TG_CMD, 0, HDMI_TG_EN);
+
+	/* pixel(vpll) clock is used for HDMI in config mode */
+	clk_disable(res->sclk_hdmi);
+	clk_set_parent(res->sclk_hdmi, res->sclk_pixel);
+	clk_enable(res->sclk_hdmi);
+
+	v4l2_subdev_call(hdev->mhl_sd, video, s_stream, 0);
+	v4l2_subdev_call(hdev->phy_sd, video, s_stream, 0);
+
+	hdmi_dumpregs(hdev, "streamoff");
+	return 0;
+}
+
+static int hdmi_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct hdmi_device *hdev = sd_to_hdmi_dev(sd);
+	struct device *dev = hdev->dev;
+
+	dev_dbg(dev, "%s(%d)\n", __func__, enable);
+	if (enable)
+		return hdmi_streamon(hdev);
+	return hdmi_streamoff(hdev);
+}
+
+static int hdmi_resource_poweron(struct hdmi_resources *res)
+{
+	int ret;
+
+	/* turn HDMI power on */
+	ret = regulator_bulk_enable(res->regul_count, res->regul_bulk);
+	if (ret < 0)
+		return ret;
+	/* power-on hdmi physical interface */
+	clk_enable(res->hdmiphy);
+	/* use VPP as parent clock; HDMIPHY is not working yet */
+	clk_set_parent(res->sclk_hdmi, res->sclk_pixel);
+	/* turn clocks on */
+	clk_enable(res->sclk_hdmi);
+
+	return 0;
+}
+
+static void hdmi_resource_poweroff(struct hdmi_resources *res)
+{
+	/* turn clocks off */
+	clk_disable(res->sclk_hdmi);
+	/* power-off hdmiphy */
+	clk_disable(res->hdmiphy);
+	/* turn HDMI power off */
+	regulator_bulk_disable(res->regul_count, res->regul_bulk);
+}
+
+static int hdmi_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct hdmi_device *hdev = sd_to_hdmi_dev(sd);
+	int ret;
+
+	if (on)
+		ret = pm_runtime_get_sync(hdev->dev);
+	else
+		ret = pm_runtime_put_sync(hdev->dev);
+	/* only values < 0 indicate errors */
+	return ret < 0 ? ret : 0;
+}
+
+static int hdmi_s_dv_timings(struct v4l2_subdev *sd,
+	struct v4l2_dv_timings *timings)
+{
+	struct hdmi_device *hdev = sd_to_hdmi_dev(sd);
+	struct device *dev = hdev->dev;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(hdmi_timings); i++)
+		if (v4l2_match_dv_timings(&hdmi_timings[i].dv_timings,
+					timings, 0))
+			break;
+	if (i == ARRAY_SIZE(hdmi_timings)) {
+		dev_err(dev, "timings not supported\n");
+		return -EINVAL;
+	}
+	hdev->cur_conf = hdmi_timings[i].hdmi_timings;
+	hdev->cur_conf_dirty = 1;
+	hdev->cur_timings = *timings;
+	if (!hdmi_timings[i].reduced_fps)
+		hdev->cur_timings.bt.flags &= ~V4L2_DV_FL_CAN_REDUCE_FPS;
+	return 0;
+}
+
+static int hdmi_g_dv_timings(struct v4l2_subdev *sd,
+	struct v4l2_dv_timings *timings)
+{
+	*timings = sd_to_hdmi_dev(sd)->cur_timings;
+	return 0;
+}
+
+static int hdmi_get_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+	struct hdmi_device *hdev = sd_to_hdmi_dev(sd);
+	const struct hdmi_timings *t = hdev->cur_conf;
+
+	dev_dbg(hdev->dev, "%s\n", __func__);
+	if (!hdev->cur_conf)
+		return -EINVAL;
+	if (format->pad)
+		return -EINVAL;
+
+	memset(fmt, 0, sizeof(*fmt));
+	fmt->width = t->hact.end - t->hact.beg;
+	fmt->height = t->vact[0].end - t->vact[0].beg;
+	fmt->code = MEDIA_BUS_FMT_FIXED; /* means RGB888 */
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+	if (t->interlaced) {
+		fmt->field = V4L2_FIELD_INTERLACED;
+		fmt->height *= 2;
+	} else {
+		fmt->field = V4L2_FIELD_NONE;
+	}
+	return 0;
+}
+
+static int hdmi_enum_dv_timings(struct v4l2_subdev *sd,
+	struct v4l2_enum_dv_timings *timings)
+{
+	if (timings->pad != 0)
+		return -EINVAL;
+	if (timings->index >= ARRAY_SIZE(hdmi_timings))
+		return -EINVAL;
+	timings->timings = hdmi_timings[timings->index].dv_timings;
+	if (!hdmi_timings[timings->index].reduced_fps)
+		timings->timings.bt.flags &= ~V4L2_DV_FL_CAN_REDUCE_FPS;
+	return 0;
+}
+
+static int hdmi_dv_timings_cap(struct v4l2_subdev *sd,
+	struct v4l2_dv_timings_cap *cap)
+{
+	struct hdmi_device *hdev = sd_to_hdmi_dev(sd);
+
+	if (cap->pad != 0)
+		return -EINVAL;
+
+	/* Let the phy fill in the pixelclock range */
+	v4l2_subdev_call(hdev->phy_sd, pad, dv_timings_cap, cap);
+	cap->type = V4L2_DV_BT_656_1120;
+	cap->bt.min_width = 720;
+	cap->bt.max_width = 1920;
+	cap->bt.min_height = 480;
+	cap->bt.max_height = 1080;
+	cap->bt.standards = V4L2_DV_BT_STD_CEA861;
+	cap->bt.capabilities = V4L2_DV_BT_CAP_INTERLACED |
+			       V4L2_DV_BT_CAP_PROGRESSIVE;
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops hdmi_sd_core_ops = {
+	.s_power = hdmi_s_power,
+};
+
+static const struct v4l2_subdev_video_ops hdmi_sd_video_ops = {
+	.s_dv_timings = hdmi_s_dv_timings,
+	.g_dv_timings = hdmi_g_dv_timings,
+	.s_stream = hdmi_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops hdmi_sd_pad_ops = {
+	.enum_dv_timings = hdmi_enum_dv_timings,
+	.dv_timings_cap = hdmi_dv_timings_cap,
+	.get_fmt = hdmi_get_fmt,
+};
+
+static const struct v4l2_subdev_ops hdmi_sd_ops = {
+	.core = &hdmi_sd_core_ops,
+	.video = &hdmi_sd_video_ops,
+	.pad = &hdmi_sd_pad_ops,
+};
+
+static int hdmi_runtime_suspend(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct hdmi_device *hdev = sd_to_hdmi_dev(sd);
+
+	dev_dbg(dev, "%s\n", __func__);
+	v4l2_subdev_call(hdev->mhl_sd, core, s_power, 0);
+	hdmi_resource_poweroff(&hdev->res);
+	/* flag that device context is lost */
+	hdev->cur_conf_dirty = 1;
+	return 0;
+}
+
+static int hdmi_runtime_resume(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct hdmi_device *hdev = sd_to_hdmi_dev(sd);
+	int ret;
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	ret = hdmi_resource_poweron(&hdev->res);
+	if (ret < 0)
+		return ret;
+
+	/* starting MHL */
+	ret = v4l2_subdev_call(hdev->mhl_sd, core, s_power, 1);
+	if (hdev->mhl_sd && ret)
+		goto fail;
+
+	dev_dbg(dev, "poweron succeed\n");
+
+	return 0;
+
+fail:
+	hdmi_resource_poweroff(&hdev->res);
+	dev_err(dev, "poweron failed\n");
+
+	return ret;
+}
+
+static const struct dev_pm_ops hdmi_pm_ops = {
+	.runtime_suspend = hdmi_runtime_suspend,
+	.runtime_resume	 = hdmi_runtime_resume,
+};
+
+static void hdmi_resource_clear_clocks(struct hdmi_resources *res)
+{
+	res->hdmi	 = ERR_PTR(-EINVAL);
+	res->sclk_hdmi	 = ERR_PTR(-EINVAL);
+	res->sclk_pixel	 = ERR_PTR(-EINVAL);
+	res->sclk_hdmiphy = ERR_PTR(-EINVAL);
+	res->hdmiphy	 = ERR_PTR(-EINVAL);
+}
+
+static void hdmi_resources_cleanup(struct hdmi_device *hdev)
+{
+	struct hdmi_resources *res = &hdev->res;
+
+	dev_dbg(hdev->dev, "HDMI resource cleanup\n");
+	/* put clocks, power */
+	if (res->regul_count)
+		regulator_bulk_free(res->regul_count, res->regul_bulk);
+	/* kfree is NULL-safe */
+	kfree(res->regul_bulk);
+	if (!IS_ERR(res->hdmiphy))
+		clk_put(res->hdmiphy);
+	if (!IS_ERR(res->sclk_hdmiphy))
+		clk_put(res->sclk_hdmiphy);
+	if (!IS_ERR(res->sclk_pixel))
+		clk_put(res->sclk_pixel);
+	if (!IS_ERR(res->sclk_hdmi))
+		clk_put(res->sclk_hdmi);
+	if (!IS_ERR(res->hdmi))
+		clk_put(res->hdmi);
+	memset(res, 0, sizeof(*res));
+	hdmi_resource_clear_clocks(res);
+}
+
+static int hdmi_resources_init(struct hdmi_device *hdev)
+{
+	struct device *dev = hdev->dev;
+	struct hdmi_resources *res = &hdev->res;
+	static char *supply[] = {
+		"hdmi-en",
+		"vdd",
+		"vdd_osc",
+		"vdd_pll",
+	};
+	int i, ret;
+
+	dev_dbg(dev, "HDMI resource init\n");
+
+	memset(res, 0, sizeof(*res));
+	hdmi_resource_clear_clocks(res);
+
+	/* get clocks, power */
+	res->hdmi = clk_get(dev, "hdmi");
+	if (IS_ERR(res->hdmi)) {
+		dev_err(dev, "failed to get clock 'hdmi'\n");
+		goto fail;
+	}
+	res->sclk_hdmi = clk_get(dev, "sclk_hdmi");
+	if (IS_ERR(res->sclk_hdmi)) {
+		dev_err(dev, "failed to get clock 'sclk_hdmi'\n");
+		goto fail;
+	}
+	res->sclk_pixel = clk_get(dev, "sclk_pixel");
+	if (IS_ERR(res->sclk_pixel)) {
+		dev_err(dev, "failed to get clock 'sclk_pixel'\n");
+		goto fail;
+	}
+	res->sclk_hdmiphy = clk_get(dev, "sclk_hdmiphy");
+	if (IS_ERR(res->sclk_hdmiphy)) {
+		dev_err(dev, "failed to get clock 'sclk_hdmiphy'\n");
+		goto fail;
+	}
+	res->hdmiphy = clk_get(dev, "hdmiphy");
+	if (IS_ERR(res->hdmiphy)) {
+		dev_err(dev, "failed to get clock 'hdmiphy'\n");
+		goto fail;
+	}
+	res->regul_bulk = kcalloc(ARRAY_SIZE(supply),
+				  sizeof(res->regul_bulk[0]), GFP_KERNEL);
+	if (!res->regul_bulk) {
+		dev_err(dev, "failed to get memory for regulators\n");
+		goto fail;
+	}
+	for (i = 0; i < ARRAY_SIZE(supply); ++i) {
+		res->regul_bulk[i].supply = supply[i];
+		res->regul_bulk[i].consumer = NULL;
+	}
+
+	ret = regulator_bulk_get(dev, ARRAY_SIZE(supply), res->regul_bulk);
+	if (ret) {
+		dev_err(dev, "failed to get regulators\n");
+		goto fail;
+	}
+	res->regul_count = ARRAY_SIZE(supply);
+
+	return 0;
+fail:
+	dev_err(dev, "HDMI resource init - failed\n");
+	hdmi_resources_cleanup(hdev);
+	return -ENODEV;
+}
+
+static int hdmi_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct resource *res;
+	struct i2c_adapter *adapter;
+	struct v4l2_subdev *sd;
+	struct hdmi_device *hdmi_dev = NULL;
+	struct s5p_hdmi_platform_data *pdata = dev->platform_data;
+	int ret;
+
+	dev_dbg(dev, "probe start\n");
+
+	if (!pdata) {
+		dev_err(dev, "platform data is missing\n");
+		ret = -ENODEV;
+		goto fail;
+	}
+
+	hdmi_dev = devm_kzalloc(&pdev->dev, sizeof(*hdmi_dev), GFP_KERNEL);
+	if (!hdmi_dev) {
+		dev_err(dev, "out of memory\n");
+		ret = -ENOMEM;
+		goto fail;
+	}
+
+	hdmi_dev->dev = dev;
+
+	ret = hdmi_resources_init(hdmi_dev);
+	if (ret)
+		goto fail;
+
+	/* mapping HDMI registers */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res == NULL) {
+		dev_err(dev, "get memory resource failed.\n");
+		ret = -ENXIO;
+		goto fail_init;
+	}
+
+	hdmi_dev->regs = devm_ioremap(&pdev->dev, res->start,
+				      resource_size(res));
+	if (hdmi_dev->regs == NULL) {
+		dev_err(dev, "register mapping failed.\n");
+		ret = -ENXIO;
+		goto fail_init;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (res == NULL) {
+		dev_err(dev, "get interrupt resource failed.\n");
+		ret = -ENXIO;
+		goto fail_init;
+	}
+
+	ret = devm_request_irq(&pdev->dev, res->start, hdmi_irq_handler, 0,
+			       "hdmi", hdmi_dev);
+	if (ret) {
+		dev_err(dev, "request interrupt failed.\n");
+		goto fail_init;
+	}
+	hdmi_dev->irq = res->start;
+
+	/* setting v4l2 name to prevent WARN_ON in v4l2_device_register */
+	strlcpy(hdmi_dev->v4l2_dev.name, dev_name(dev),
+		sizeof(hdmi_dev->v4l2_dev.name));
+	/* passing NULL owner prevents driver from erasing drvdata */
+	ret = v4l2_device_register(NULL, &hdmi_dev->v4l2_dev);
+	if (ret) {
+		dev_err(dev, "could not register v4l2 device.\n");
+		goto fail_init;
+	}
+
+	/* testing if hdmiphy info is present */
+	if (!pdata->hdmiphy_info) {
+		dev_err(dev, "hdmiphy info is missing in platform data\n");
+		ret = -ENXIO;
+		goto fail_vdev;
+	}
+
+	adapter = i2c_get_adapter(pdata->hdmiphy_bus);
+	if (adapter == NULL) {
+		dev_err(dev, "hdmiphy adapter request failed\n");
+		ret = -ENXIO;
+		goto fail_vdev;
+	}
+
+	hdmi_dev->phy_sd = v4l2_i2c_new_subdev_board(&hdmi_dev->v4l2_dev,
+		adapter, pdata->hdmiphy_info, NULL);
+	/* on failure or not adapter is no longer useful */
+	i2c_put_adapter(adapter);
+	if (hdmi_dev->phy_sd == NULL) {
+		dev_err(dev, "missing subdev for hdmiphy\n");
+		ret = -ENODEV;
+		goto fail_vdev;
+	}
+
+	/* initialization of MHL interface if present */
+	if (pdata->mhl_info) {
+		adapter = i2c_get_adapter(pdata->mhl_bus);
+		if (adapter == NULL) {
+			dev_err(dev, "MHL adapter request failed\n");
+			ret = -ENXIO;
+			goto fail_vdev;
+		}
+
+		hdmi_dev->mhl_sd = v4l2_i2c_new_subdev_board(
+			&hdmi_dev->v4l2_dev, adapter,
+			pdata->mhl_info, NULL);
+		/* on failure or not adapter is no longer useful */
+		i2c_put_adapter(adapter);
+		if (hdmi_dev->mhl_sd == NULL) {
+			dev_err(dev, "missing subdev for MHL\n");
+			ret = -ENODEV;
+			goto fail_vdev;
+		}
+	}
+
+	clk_enable(hdmi_dev->res.hdmi);
+
+	pm_runtime_enable(dev);
+
+	sd = &hdmi_dev->sd;
+	v4l2_subdev_init(sd, &hdmi_sd_ops);
+	sd->owner = THIS_MODULE;
+
+	strlcpy(sd->name, "s5p-hdmi", sizeof(sd->name));
+	hdmi_dev->cur_timings =
+		hdmi_timings[HDMI_DEFAULT_TIMINGS_IDX].dv_timings;
+	/* FIXME: missing fail timings is not supported */
+	hdmi_dev->cur_conf =
+		hdmi_timings[HDMI_DEFAULT_TIMINGS_IDX].hdmi_timings;
+	hdmi_dev->cur_conf_dirty = 1;
+
+	/* storing subdev for call that have only access to struct device */
+	dev_set_drvdata(dev, sd);
+
+	dev_info(dev, "probe successful\n");
+
+	return 0;
+
+fail_vdev:
+	v4l2_device_unregister(&hdmi_dev->v4l2_dev);
+
+fail_init:
+	hdmi_resources_cleanup(hdmi_dev);
+
+fail:
+	dev_err(dev, "probe failed\n");
+	return ret;
+}
+
+static int hdmi_remove(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct hdmi_device *hdmi_dev = sd_to_hdmi_dev(sd);
+
+	pm_runtime_disable(dev);
+	clk_disable(hdmi_dev->res.hdmi);
+	v4l2_device_unregister(&hdmi_dev->v4l2_dev);
+	disable_irq(hdmi_dev->irq);
+	hdmi_resources_cleanup(hdmi_dev);
+	dev_info(dev, "remove successful\n");
+
+	return 0;
+}
+
+static struct platform_driver hdmi_driver __refdata = {
+	.probe = hdmi_probe,
+	.remove = hdmi_remove,
+	.id_table = hdmi_driver_types,
+	.driver = {
+		.name = "s5p-hdmi",
+		.pm = &hdmi_pm_ops,
+	}
+};
+
+module_platform_driver(hdmi_driver);
diff --git a/drivers/media/platform/s5p-tv/hdmiphy_drv.c b/drivers/media/platform/s5p-tv/hdmiphy_drv.c
new file mode 100644
index 0000000..aae6523
--- /dev/null
+++ b/drivers/media/platform/s5p-tv/hdmiphy_drv.c
@@ -0,0 +1,324 @@
+/*
+ * Samsung HDMI Physical interface driver
+ *
+ * Copyright (C) 2010-2011 Samsung Electronics Co.Ltd
+ * Author: Tomasz Stanislawski <t.stanislaws@samsung.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ */
+
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/err.h>
+
+#include <media/v4l2-subdev.h>
+
+MODULE_AUTHOR("Tomasz Stanislawski <t.stanislaws@samsung.com>");
+MODULE_DESCRIPTION("Samsung HDMI Physical interface driver");
+MODULE_LICENSE("GPL");
+
+struct hdmiphy_conf {
+	unsigned long pixclk;
+	const u8 *data;
+};
+
+struct hdmiphy_ctx {
+	struct v4l2_subdev sd;
+	const struct hdmiphy_conf *conf_tab;
+};
+
+static const struct hdmiphy_conf hdmiphy_conf_s5pv210[] = {
+	{ .pixclk = 27000000, .data = (u8 [32]) {
+		0x01, 0x05, 0x00, 0xD8, 0x10, 0x1C, 0x30, 0x40,
+		0x6B, 0x10, 0x02, 0x52, 0xDF, 0xF2, 0x54, 0x87,
+		0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
+		0x22, 0x40, 0xE3, 0x26, 0x00, 0x00, 0x00, 0x00, }
+	},
+	{ .pixclk = 27027000, .data = (u8 [32]) {
+		0x01, 0x05, 0x00, 0xD4, 0x10, 0x9C, 0x09, 0x64,
+		0x6B, 0x10, 0x02, 0x52, 0xDF, 0xF2, 0x54, 0x87,
+		0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
+		0x22, 0x40, 0xE2, 0x26, 0x00, 0x00, 0x00, 0x00, }
+	},
+	{ .pixclk = 74176000, .data = (u8 [32]) {
+		0x01, 0x05, 0x00, 0xD8, 0x10, 0x9C, 0xEF, 0x5B,
+		0x6D, 0x10, 0x01, 0x52, 0xEF, 0xF3, 0x54, 0xB9,
+		0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
+		0x22, 0x40, 0xA5, 0x26, 0x01, 0x00, 0x00, 0x00, }
+	},
+	{ .pixclk = 74250000, .data = (u8 [32]) {
+		0x01, 0x05, 0x00, 0xD8, 0x10, 0x9C, 0xF8, 0x40,
+		0x6A, 0x10, 0x01, 0x52, 0xFF, 0xF1, 0x54, 0xBA,
+		0x84, 0x00, 0x10, 0x38, 0x00, 0x08, 0x10, 0xE0,
+		0x22, 0x40, 0xA4, 0x26, 0x01, 0x00, 0x00, 0x00, }
+	},
+	{ /* end marker */ }
+};
+
+static const struct hdmiphy_conf hdmiphy_conf_exynos4210[] = {
+	{ .pixclk = 27000000, .data = (u8 [32]) {
+		0x01, 0x05, 0x00, 0xD8, 0x10, 0x1C, 0x30, 0x40,
+		0x6B, 0x10, 0x02, 0x51, 0xDF, 0xF2, 0x54, 0x87,
+		0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
+		0x22, 0x40, 0xE3, 0x26, 0x00, 0x00, 0x00, 0x00, }
+	},
+	{ .pixclk = 27027000, .data = (u8 [32]) {
+		0x01, 0x05, 0x00, 0xD4, 0x10, 0x9C, 0x09, 0x64,
+		0x6B, 0x10, 0x02, 0x51, 0xDF, 0xF2, 0x54, 0x87,
+		0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
+		0x22, 0x40, 0xE2, 0x26, 0x00, 0x00, 0x00, 0x00, }
+	},
+	{ .pixclk = 74176000, .data = (u8 [32]) {
+		0x01, 0x05, 0x00, 0xD8, 0x10, 0x9C, 0xEF, 0x5B,
+		0x6D, 0x10, 0x01, 0x51, 0xEF, 0xF3, 0x54, 0xB9,
+		0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
+		0x22, 0x40, 0xA5, 0x26, 0x01, 0x00, 0x00, 0x00, }
+	},
+	{ .pixclk = 74250000, .data = (u8 [32]) {
+		0x01, 0x05, 0x00, 0xD8, 0x10, 0x9C, 0xF8, 0x40,
+		0x6A, 0x10, 0x01, 0x51, 0xFF, 0xF1, 0x54, 0xBA,
+		0x84, 0x00, 0x10, 0x38, 0x00, 0x08, 0x10, 0xE0,
+		0x22, 0x40, 0xA4, 0x26, 0x01, 0x00, 0x00, 0x00, }
+	},
+	{ .pixclk = 148352000, .data = (u8 [32]) {
+		0x01, 0x05, 0x00, 0xD8, 0x10, 0x9C, 0xEF, 0x5B,
+		0x6D, 0x18, 0x00, 0x51, 0xEF, 0xF3, 0x54, 0xB9,
+		0x84, 0x00, 0x30, 0x38, 0x00, 0x08, 0x10, 0xE0,
+		0x11, 0x40, 0xA5, 0x26, 0x02, 0x00, 0x00, 0x00, }
+	},
+	{ .pixclk = 148500000, .data = (u8 [32]) {
+		0x01, 0x05, 0x00, 0xD8, 0x10, 0x9C, 0xF8, 0x40,
+		0x6A, 0x18, 0x00, 0x51, 0xFF, 0xF1, 0x54, 0xBA,
+		0x84, 0x00, 0x10, 0x38, 0x00, 0x08, 0x10, 0xE0,
+		0x11, 0x40, 0xA4, 0x26, 0x02, 0x00, 0x00, 0x00, }
+	},
+	{ /* end marker */ }
+};
+
+static const struct hdmiphy_conf hdmiphy_conf_exynos4212[] = {
+	{ .pixclk = 27000000, .data = (u8 [32]) {
+		0x01, 0x11, 0x2D, 0x75, 0x00, 0x01, 0x00, 0x08,
+		0x82, 0x00, 0x0E, 0xD9, 0x45, 0xA0, 0x34, 0xC0,
+		0x0B, 0x80, 0x12, 0x87, 0x08, 0x24, 0x24, 0x71,
+		0x54, 0xE3, 0x24, 0x00, 0x00, 0x00, 0x01, 0x00, }
+	},
+	{ .pixclk = 27027000, .data = (u8 [32]) {
+		0x01, 0x91, 0x2D, 0x72, 0x00, 0x64, 0x12, 0x08,
+		0x43, 0x20, 0x0E, 0xD9, 0x45, 0xA0, 0x34, 0xC0,
+		0x0B, 0x80, 0x12, 0x87, 0x08, 0x24, 0x24, 0x71,
+		0x54, 0xE2, 0x24, 0x00, 0x00, 0x00, 0x01, 0x00, }
+	},
+	{ .pixclk = 74176000, .data = (u8 [32]) {
+		0x01, 0x91, 0x3E, 0x35, 0x00, 0x5B, 0xDE, 0x08,
+		0x82, 0x20, 0x73, 0xD9, 0x45, 0xA0, 0x34, 0xC0,
+		0x0B, 0x80, 0x12, 0x87, 0x08, 0x24, 0x24, 0x52,
+		0x54, 0xA5, 0x24, 0x01, 0x00, 0x00, 0x01, 0x00, }
+	},
+	{ .pixclk = 74250000, .data = (u8 [32]) {
+		0x01, 0x91, 0x3E, 0x35, 0x00, 0x40, 0xF0, 0x08,
+		0x82, 0x20, 0x73, 0xD9, 0x45, 0xA0, 0x34, 0xC0,
+		0x0B, 0x80, 0x12, 0x87, 0x08, 0x24, 0x24, 0x52,
+		0x54, 0xA4, 0x24, 0x01, 0x00, 0x00, 0x01, 0x00, }
+	},
+	{ .pixclk = 148500000, .data = (u8 [32]) {
+		0x01, 0x91, 0x3E, 0x15, 0x00, 0x40, 0xF0, 0x08,
+		0x82, 0x20, 0x73, 0xD9, 0x45, 0xA0, 0x34, 0xC0,
+		0x0B, 0x80, 0x12, 0x87, 0x08, 0x24, 0x24, 0xA4,
+		0x54, 0x4A, 0x25, 0x03, 0x00, 0x00, 0x01, 0x00, }
+	},
+	{ /* end marker */ }
+};
+
+static const struct hdmiphy_conf hdmiphy_conf_exynos4412[] = {
+	{ .pixclk = 27000000, .data = (u8 [32]) {
+		0x01, 0x11, 0x2D, 0x75, 0x40, 0x01, 0x00, 0x08,
+		0x82, 0x00, 0x0E, 0xD9, 0x45, 0xA0, 0xAC, 0x80,
+		0x08, 0x80, 0x11, 0x84, 0x02, 0x22, 0x44, 0x86,
+		0x54, 0xE4, 0x24, 0x00, 0x00, 0x00, 0x01, 0x00, }
+	},
+	{ .pixclk = 27027000, .data = (u8 [32]) {
+		0x01, 0x91, 0x2D, 0x72, 0x40, 0x64, 0x12, 0x08,
+		0x43, 0x20, 0x0E, 0xD9, 0x45, 0xA0, 0xAC, 0x80,
+		0x08, 0x80, 0x11, 0x84, 0x02, 0x22, 0x44, 0x86,
+		0x54, 0xE3, 0x24, 0x00, 0x00, 0x00, 0x01, 0x00, }
+	},
+	{ .pixclk = 74176000, .data = (u8 [32]) {
+		0x01, 0x91, 0x1F, 0x10, 0x40, 0x5B, 0xEF, 0x08,
+		0x81, 0x20, 0xB9, 0xD8, 0x45, 0xA0, 0xAC, 0x80,
+		0x08, 0x80, 0x11, 0x84, 0x02, 0x22, 0x44, 0x86,
+		0x54, 0xA6, 0x24, 0x01, 0x00, 0x00, 0x01, 0x00, }
+	},
+	{ .pixclk = 74250000, .data = (u8 [32]) {
+		0x01, 0x91, 0x1F, 0x10, 0x40, 0x40, 0xF8, 0x08,
+		0x81, 0x20, 0xBA, 0xD8, 0x45, 0xA0, 0xAC, 0x80,
+		0x08, 0x80, 0x11, 0x84, 0x02, 0x22, 0x44, 0x86,
+		0x54, 0xA5, 0x24, 0x01, 0x00, 0x00, 0x01, 0x00, }
+	},
+	{ .pixclk = 148500000, .data = (u8 [32]) {
+		0x01, 0x91, 0x1F, 0x00, 0x40, 0x40, 0xF8, 0x08,
+		0x81, 0x20, 0xBA, 0xD8, 0x45, 0xA0, 0xAC, 0x80,
+		0x08, 0x80, 0x11, 0x84, 0x02, 0x22, 0x44, 0x86,
+		0x54, 0x4B, 0x25, 0x03, 0x00, 0x00, 0x01, 0x00, }
+	},
+	{ /* end marker */ }
+};
+
+static inline struct hdmiphy_ctx *sd_to_ctx(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct hdmiphy_ctx, sd);
+}
+
+static const u8 *hdmiphy_find_conf(unsigned long pixclk,
+		const struct hdmiphy_conf *conf)
+{
+	for (; conf->pixclk; ++conf)
+		if (conf->pixclk == pixclk)
+			return conf->data;
+	return NULL;
+}
+
+static int hdmiphy_s_power(struct v4l2_subdev *sd, int on)
+{
+	/* to be implemented */
+	return 0;
+}
+
+static int hdmiphy_s_dv_timings(struct v4l2_subdev *sd,
+	struct v4l2_dv_timings *timings)
+{
+	const u8 *data;
+	u8 buffer[32];
+	int ret;
+	struct hdmiphy_ctx *ctx = sd_to_ctx(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct device *dev = &client->dev;
+	unsigned long pixclk = timings->bt.pixelclock;
+
+	dev_info(dev, "s_dv_timings\n");
+	if ((timings->bt.flags & V4L2_DV_FL_REDUCED_FPS) && pixclk == 74250000)
+		pixclk = 74176000;
+	data = hdmiphy_find_conf(pixclk, ctx->conf_tab);
+	if (!data) {
+		dev_err(dev, "format not supported\n");
+		return -EINVAL;
+	}
+
+	/* storing configuration to the device */
+	memcpy(buffer, data, 32);
+	ret = i2c_master_send(client, buffer, 32);
+	if (ret != 32) {
+		dev_err(dev, "failed to configure HDMIPHY via I2C\n");
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int hdmiphy_dv_timings_cap(struct v4l2_subdev *sd,
+	struct v4l2_dv_timings_cap *cap)
+{
+	if (cap->pad != 0)
+		return -EINVAL;
+
+	cap->type = V4L2_DV_BT_656_1120;
+	/* The phy only determines the pixelclock, leave the other values
+	 * at 0 to signify that we have no information for them. */
+	cap->bt.min_pixelclock = 27000000;
+	cap->bt.max_pixelclock = 148500000;
+	return 0;
+}
+
+static int hdmiphy_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct device *dev = &client->dev;
+	u8 buffer[2];
+	int ret;
+
+	dev_info(dev, "s_stream(%d)\n", enable);
+	/* going to/from configuration from/to operation mode */
+	buffer[0] = 0x1f;
+	buffer[1] = enable ? 0x80 : 0x00;
+
+	ret = i2c_master_send(client, buffer, 2);
+	if (ret != 2) {
+		dev_err(dev, "stream (%d) failed\n", enable);
+		return -EIO;
+	}
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops hdmiphy_core_ops = {
+	.s_power =  hdmiphy_s_power,
+};
+
+static const struct v4l2_subdev_video_ops hdmiphy_video_ops = {
+	.s_dv_timings = hdmiphy_s_dv_timings,
+	.s_stream =  hdmiphy_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops hdmiphy_pad_ops = {
+	.dv_timings_cap = hdmiphy_dv_timings_cap,
+};
+
+static const struct v4l2_subdev_ops hdmiphy_ops = {
+	.core = &hdmiphy_core_ops,
+	.video = &hdmiphy_video_ops,
+	.pad = &hdmiphy_pad_ops,
+};
+
+static int hdmiphy_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	struct hdmiphy_ctx *ctx;
+
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx)
+		return -ENOMEM;
+
+	ctx->conf_tab = (struct hdmiphy_conf *)id->driver_data;
+	v4l2_i2c_subdev_init(&ctx->sd, client, &hdmiphy_ops);
+
+	dev_info(&client->dev, "probe successful\n");
+	return 0;
+}
+
+static int hdmiphy_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct hdmiphy_ctx *ctx = sd_to_ctx(sd);
+
+	kfree(ctx);
+	dev_info(&client->dev, "remove successful\n");
+
+	return 0;
+}
+
+static const struct i2c_device_id hdmiphy_id[] = {
+	{ "hdmiphy", (unsigned long)hdmiphy_conf_exynos4210 },
+	{ "hdmiphy-s5pv210", (unsigned long)hdmiphy_conf_s5pv210 },
+	{ "hdmiphy-exynos4210", (unsigned long)hdmiphy_conf_exynos4210 },
+	{ "hdmiphy-exynos4212", (unsigned long)hdmiphy_conf_exynos4212 },
+	{ "hdmiphy-exynos4412", (unsigned long)hdmiphy_conf_exynos4412 },
+	{ },
+};
+MODULE_DEVICE_TABLE(i2c, hdmiphy_id);
+
+static struct i2c_driver hdmiphy_driver = {
+	.driver = {
+		.name	= "s5p-hdmiphy",
+	},
+	.probe		= hdmiphy_probe,
+	.remove		= hdmiphy_remove,
+	.id_table = hdmiphy_id,
+};
+
+module_i2c_driver(hdmiphy_driver);
diff --git a/drivers/media/platform/s5p-tv/mixer.h b/drivers/media/platform/s5p-tv/mixer.h
new file mode 100644
index 0000000..42cd270
--- /dev/null
+++ b/drivers/media/platform/s5p-tv/mixer.h
@@ -0,0 +1,366 @@
+/*
+ * Samsung TV Mixer driver
+ *
+ * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
+ *
+ * Tomasz Stanislawski, <t.stanislaws@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published
+ * by the Free Software Foundiation. either version 2 of the License,
+ * or (at your option) any later version
+ */
+
+#ifndef SAMSUNG_MIXER_H
+#define SAMSUNG_MIXER_H
+
+#ifdef CONFIG_VIDEO_SAMSUNG_S5P_MIXER_DEBUG
+	#define DEBUG
+#endif
+
+#include <linux/fb.h>
+#include <linux/irqreturn.h>
+#include <linux/kernel.h>
+#include <linux/spinlock.h>
+#include <linux/wait.h>
+#include <media/v4l2-device.h>
+#include <media/videobuf2-v4l2.h>
+
+#include "regs-mixer.h"
+
+/** maximum number of output interfaces */
+#define MXR_MAX_OUTPUTS 2
+/** maximum number of input interfaces (layers) */
+#define MXR_MAX_LAYERS 3
+#define MXR_DRIVER_NAME "s5p-mixer"
+/** maximal number of planes for every layer */
+#define MXR_MAX_PLANES	2
+
+#define MXR_ENABLE 1
+#define MXR_DISABLE 0
+
+/** description of a macroblock for packed formats */
+struct mxr_block {
+	/** vertical number of pixels in macroblock */
+	unsigned int width;
+	/** horizontal number of pixels in macroblock */
+	unsigned int height;
+	/** size of block in bytes */
+	unsigned int size;
+};
+
+/** description of supported format */
+struct mxr_format {
+	/** format name/mnemonic */
+	const char *name;
+	/** fourcc identifier */
+	u32 fourcc;
+	/** colorspace identifier */
+	enum v4l2_colorspace colorspace;
+	/** number of planes in image data */
+	int num_planes;
+	/** description of block for each plane */
+	struct mxr_block plane[MXR_MAX_PLANES];
+	/** number of subframes in image data */
+	int num_subframes;
+	/** specifies to which subframe belong given plane */
+	int plane2subframe[MXR_MAX_PLANES];
+	/** internal code, driver dependent */
+	unsigned long cookie;
+};
+
+/** description of crop configuration for image */
+struct mxr_crop {
+	/** width of layer in pixels */
+	unsigned int full_width;
+	/** height of layer in pixels */
+	unsigned int full_height;
+	/** horizontal offset of first pixel to be displayed */
+	unsigned int x_offset;
+	/** vertical offset of first pixel to be displayed */
+	unsigned int y_offset;
+	/** width of displayed data in pixels */
+	unsigned int width;
+	/** height of displayed data in pixels */
+	unsigned int height;
+	/** indicate which fields are present in buffer */
+	unsigned int field;
+};
+
+/** stages of geometry operations */
+enum mxr_geometry_stage {
+	MXR_GEOMETRY_SINK,
+	MXR_GEOMETRY_COMPOSE,
+	MXR_GEOMETRY_CROP,
+	MXR_GEOMETRY_SOURCE,
+};
+
+/* flag indicating that offset should be 0 */
+#define MXR_NO_OFFSET	0x80000000
+
+/** description of transformation from source to destination image */
+struct mxr_geometry {
+	/** cropping for source image */
+	struct mxr_crop src;
+	/** cropping for destination image */
+	struct mxr_crop dst;
+	/** layer-dependant description of horizontal scaling */
+	unsigned int x_ratio;
+	/** layer-dependant description of vertical scaling */
+	unsigned int y_ratio;
+};
+
+/** instance of a buffer */
+struct mxr_buffer {
+	/** common v4l buffer stuff -- must be first */
+	struct vb2_v4l2_buffer vb;
+	/** node for layer's lists */
+	struct list_head	list;
+};
+
+
+/** internal states of layer */
+enum mxr_layer_state {
+	/** layers is not shown */
+	MXR_LAYER_IDLE = 0,
+	/** layer is shown */
+	MXR_LAYER_STREAMING,
+	/** state before STREAMOFF is finished */
+	MXR_LAYER_STREAMING_FINISH,
+};
+
+/** forward declarations */
+struct mxr_device;
+struct mxr_layer;
+
+/** callback for layers operation */
+struct mxr_layer_ops {
+	/* TODO: try to port it to subdev API */
+	/** handler for resource release function */
+	void (*release)(struct mxr_layer *);
+	/** setting buffer to HW */
+	void (*buffer_set)(struct mxr_layer *, struct mxr_buffer *);
+	/** setting format and geometry in HW */
+	void (*format_set)(struct mxr_layer *);
+	/** streaming stop/start */
+	void (*stream_set)(struct mxr_layer *, int);
+	/** adjusting geometry */
+	void (*fix_geometry)(struct mxr_layer *,
+		enum mxr_geometry_stage, unsigned long);
+};
+
+/** layer instance, a single window and content displayed on output */
+struct mxr_layer {
+	/** parent mixer device */
+	struct mxr_device *mdev;
+	/** layer index (unique identifier) */
+	int idx;
+	/** callbacks for layer methods */
+	struct mxr_layer_ops ops;
+	/** format array */
+	const struct mxr_format **fmt_array;
+	/** size of format array */
+	unsigned long fmt_array_size;
+
+	/** lock for protection of list and state fields */
+	spinlock_t enq_slock;
+	/** list for enqueued buffers */
+	struct list_head enq_list;
+	/** buffer currently owned by hardware in temporary registers */
+	struct mxr_buffer *update_buf;
+	/** buffer currently owned by hardware in shadow registers */
+	struct mxr_buffer *shadow_buf;
+	/** state of layer IDLE/STREAMING */
+	enum mxr_layer_state state;
+
+	/** mutex for protection of fields below */
+	struct mutex mutex;
+	/** handler for video node */
+	struct video_device vfd;
+	/** queue for output buffers */
+	struct vb2_queue vb_queue;
+	/** current image format */
+	const struct mxr_format *fmt;
+	/** current geometry of image */
+	struct mxr_geometry geo;
+};
+
+/** description of mixers output interface */
+struct mxr_output {
+	/** name of output */
+	char name[32];
+	/** output subdev */
+	struct v4l2_subdev *sd;
+	/** cookie used for configuration of registers */
+	int cookie;
+};
+
+/** specify source of output subdevs */
+struct mxr_output_conf {
+	/** name of output (connector) */
+	char *output_name;
+	/** name of module that generates output subdev */
+	char *module_name;
+	/** cookie need for mixer HW */
+	int cookie;
+};
+
+struct clk;
+struct regulator;
+
+/** auxiliary resources used my mixer */
+struct mxr_resources {
+	/** interrupt index */
+	int irq;
+	/** pointer to Mixer registers */
+	void __iomem *mxr_regs;
+	/** pointer to Video Processor registers */
+	void __iomem *vp_regs;
+	/** other resources, should used under mxr_device.mutex */
+	struct clk *mixer;
+	struct clk *vp;
+	struct clk *sclk_mixer;
+	struct clk *sclk_hdmi;
+	struct clk *sclk_dac;
+};
+
+/* event flags used  */
+enum mxr_devide_flags {
+	MXR_EVENT_VSYNC = 0,
+	MXR_EVENT_TOP = 1,
+};
+
+/** drivers instance */
+struct mxr_device {
+	/** master device */
+	struct device *dev;
+	/** state of each layer */
+	struct mxr_layer *layer[MXR_MAX_LAYERS];
+	/** state of each output */
+	struct mxr_output *output[MXR_MAX_OUTPUTS];
+	/** number of registered outputs */
+	int output_cnt;
+
+	/* video resources */
+
+	/** V4L2 device */
+	struct v4l2_device v4l2_dev;
+	/** context of allocator */
+	void *alloc_ctx;
+	/** event wait queue */
+	wait_queue_head_t event_queue;
+	/** state flags */
+	unsigned long event_flags;
+
+	/** spinlock for protection of registers */
+	spinlock_t reg_slock;
+
+	/** mutex for protection of fields below */
+	struct mutex mutex;
+	/** number of entities depndant on output configuration */
+	int n_output;
+	/** number of users that do streaming */
+	int n_streamer;
+	/** index of current output */
+	int current_output;
+	/** auxiliary resources used my mixer */
+	struct mxr_resources res;
+};
+
+/** transform device structure into mixer device */
+static inline struct mxr_device *to_mdev(struct device *dev)
+{
+	struct v4l2_device *vdev = dev_get_drvdata(dev);
+	return container_of(vdev, struct mxr_device, v4l2_dev);
+}
+
+/** get current output data, should be called under mdev's mutex */
+static inline struct mxr_output *to_output(struct mxr_device *mdev)
+{
+	return mdev->output[mdev->current_output];
+}
+
+/** get current output subdev, should be called under mdev's mutex */
+static inline struct v4l2_subdev *to_outsd(struct mxr_device *mdev)
+{
+	struct mxr_output *out = to_output(mdev);
+	return out ? out->sd : NULL;
+}
+
+/** forward declaration for mixer platform data */
+struct mxr_platform_data;
+
+/** acquiring common video resources */
+int mxr_acquire_video(struct mxr_device *mdev,
+	struct mxr_output_conf *output_cont, int output_count);
+
+/** releasing common video resources */
+void mxr_release_video(struct mxr_device *mdev);
+
+struct mxr_layer *mxr_graph_layer_create(struct mxr_device *mdev, int idx);
+struct mxr_layer *mxr_vp_layer_create(struct mxr_device *mdev, int idx);
+struct mxr_layer *mxr_base_layer_create(struct mxr_device *mdev,
+	int idx, char *name, struct mxr_layer_ops *ops);
+
+void mxr_base_layer_release(struct mxr_layer *layer);
+void mxr_layer_release(struct mxr_layer *layer);
+
+int mxr_base_layer_register(struct mxr_layer *layer);
+void mxr_base_layer_unregister(struct mxr_layer *layer);
+
+unsigned long mxr_get_plane_size(const struct mxr_block *blk,
+	unsigned int width, unsigned int height);
+
+/** adds new consumer for mixer's power */
+int __must_check mxr_power_get(struct mxr_device *mdev);
+/** removes consumer for mixer's power */
+void mxr_power_put(struct mxr_device *mdev);
+/** add new client for output configuration */
+void mxr_output_get(struct mxr_device *mdev);
+/** removes new client for output configuration */
+void mxr_output_put(struct mxr_device *mdev);
+/** add new client for streaming */
+void mxr_streamer_get(struct mxr_device *mdev);
+/** removes new client for streaming */
+void mxr_streamer_put(struct mxr_device *mdev);
+/** returns format of data delivared to current output */
+void mxr_get_mbus_fmt(struct mxr_device *mdev,
+	struct v4l2_mbus_framefmt *mbus_fmt);
+
+/* Debug */
+
+#define mxr_err(mdev, fmt, ...)  dev_err(mdev->dev, fmt, ##__VA_ARGS__)
+#define mxr_warn(mdev, fmt, ...) dev_warn(mdev->dev, fmt, ##__VA_ARGS__)
+#define mxr_info(mdev, fmt, ...) dev_info(mdev->dev, fmt, ##__VA_ARGS__)
+
+#ifdef CONFIG_VIDEO_SAMSUNG_S5P_MIXER_DEBUG
+	#define mxr_dbg(mdev, fmt, ...)  dev_dbg(mdev->dev, fmt, ##__VA_ARGS__)
+#else
+	#define mxr_dbg(mdev, fmt, ...)  do { (void) mdev; } while (0)
+#endif
+
+/* accessing Mixer's and Video Processor's registers */
+
+void mxr_vsync_set_update(struct mxr_device *mdev, int en);
+void mxr_reg_reset(struct mxr_device *mdev);
+irqreturn_t mxr_irq_handler(int irq, void *dev_data);
+void mxr_reg_s_output(struct mxr_device *mdev, int cookie);
+void mxr_reg_streamon(struct mxr_device *mdev);
+void mxr_reg_streamoff(struct mxr_device *mdev);
+int mxr_reg_wait4vsync(struct mxr_device *mdev);
+void mxr_reg_set_mbus_fmt(struct mxr_device *mdev,
+	struct v4l2_mbus_framefmt *fmt);
+void mxr_reg_graph_layer_stream(struct mxr_device *mdev, int idx, int en);
+void mxr_reg_graph_buffer(struct mxr_device *mdev, int idx, dma_addr_t addr);
+void mxr_reg_graph_format(struct mxr_device *mdev, int idx,
+	const struct mxr_format *fmt, const struct mxr_geometry *geo);
+
+void mxr_reg_vp_layer_stream(struct mxr_device *mdev, int en);
+void mxr_reg_vp_buffer(struct mxr_device *mdev,
+	dma_addr_t luma_addr[2], dma_addr_t chroma_addr[2]);
+void mxr_reg_vp_format(struct mxr_device *mdev,
+	const struct mxr_format *fmt, const struct mxr_geometry *geo);
+void mxr_reg_dump(struct mxr_device *mdev);
+
+#endif /* SAMSUNG_MIXER_H */
+
diff --git a/drivers/media/platform/s5p-tv/mixer_drv.c b/drivers/media/platform/s5p-tv/mixer_drv.c
new file mode 100644
index 0000000..5ef6777
--- /dev/null
+++ b/drivers/media/platform/s5p-tv/mixer_drv.c
@@ -0,0 +1,527 @@
+/*
+ * Samsung TV Mixer driver
+ *
+ * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
+ *
+ * Tomasz Stanislawski, <t.stanislaws@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published
+ * by the Free Software Foundiation. either version 2 of the License,
+ * or (at your option) any later version
+ */
+
+#include "mixer.h"
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/fb.h>
+#include <linux/delay.h>
+#include <linux/pm_runtime.h>
+#include <linux/clk.h>
+
+MODULE_AUTHOR("Tomasz Stanislawski, <t.stanislaws@samsung.com>");
+MODULE_DESCRIPTION("Samsung MIXER");
+MODULE_LICENSE("GPL");
+
+/* --------- DRIVER PARAMETERS ---------- */
+
+static struct mxr_output_conf mxr_output_conf[] = {
+	{
+		.output_name = "S5P HDMI connector",
+		.module_name = "s5p-hdmi",
+		.cookie = 1,
+	},
+	{
+		.output_name = "S5P SDO connector",
+		.module_name = "s5p-sdo",
+		.cookie = 0,
+	},
+};
+
+void mxr_get_mbus_fmt(struct mxr_device *mdev,
+	struct v4l2_mbus_framefmt *mbus_fmt)
+{
+	struct v4l2_subdev *sd;
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	int ret;
+
+	mutex_lock(&mdev->mutex);
+	sd = to_outsd(mdev);
+	ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &fmt);
+	*mbus_fmt = fmt.format;
+	WARN(ret, "failed to get mbus_fmt for output %s\n", sd->name);
+	mutex_unlock(&mdev->mutex);
+}
+
+void mxr_streamer_get(struct mxr_device *mdev)
+{
+	mutex_lock(&mdev->mutex);
+	++mdev->n_streamer;
+	mxr_dbg(mdev, "%s(%d)\n", __func__, mdev->n_streamer);
+	if (mdev->n_streamer == 1) {
+		struct v4l2_subdev *sd = to_outsd(mdev);
+		struct v4l2_subdev_format fmt = {
+			.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		};
+		struct v4l2_mbus_framefmt *mbus_fmt = &fmt.format;
+		struct mxr_resources *res = &mdev->res;
+		int ret;
+
+		if (to_output(mdev)->cookie == 0)
+			clk_set_parent(res->sclk_mixer, res->sclk_dac);
+		else
+			clk_set_parent(res->sclk_mixer, res->sclk_hdmi);
+		mxr_reg_s_output(mdev, to_output(mdev)->cookie);
+
+		ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &fmt);
+		WARN(ret, "failed to get mbus_fmt for output %s\n", sd->name);
+		ret = v4l2_subdev_call(sd, video, s_stream, 1);
+		WARN(ret, "starting stream failed for output %s\n", sd->name);
+
+		mxr_reg_set_mbus_fmt(mdev, mbus_fmt);
+		mxr_reg_streamon(mdev);
+		ret = mxr_reg_wait4vsync(mdev);
+		WARN(ret, "failed to get vsync (%d) from output\n", ret);
+	}
+	mutex_unlock(&mdev->mutex);
+	mxr_reg_dump(mdev);
+	/* FIXME: what to do when streaming fails? */
+}
+
+void mxr_streamer_put(struct mxr_device *mdev)
+{
+	mutex_lock(&mdev->mutex);
+	--mdev->n_streamer;
+	mxr_dbg(mdev, "%s(%d)\n", __func__, mdev->n_streamer);
+	if (mdev->n_streamer == 0) {
+		int ret;
+		struct v4l2_subdev *sd = to_outsd(mdev);
+
+		mxr_reg_streamoff(mdev);
+		/* vsync applies Mixer setup */
+		ret = mxr_reg_wait4vsync(mdev);
+		WARN(ret, "failed to get vsync (%d) from output\n", ret);
+		ret = v4l2_subdev_call(sd, video, s_stream, 0);
+		WARN(ret, "stopping stream failed for output %s\n", sd->name);
+	}
+	WARN(mdev->n_streamer < 0, "negative number of streamers (%d)\n",
+		mdev->n_streamer);
+	mutex_unlock(&mdev->mutex);
+	mxr_reg_dump(mdev);
+}
+
+void mxr_output_get(struct mxr_device *mdev)
+{
+	mutex_lock(&mdev->mutex);
+	++mdev->n_output;
+	mxr_dbg(mdev, "%s(%d)\n", __func__, mdev->n_output);
+	/* turn on auxiliary driver */
+	if (mdev->n_output == 1)
+		v4l2_subdev_call(to_outsd(mdev), core, s_power, 1);
+	mutex_unlock(&mdev->mutex);
+}
+
+void mxr_output_put(struct mxr_device *mdev)
+{
+	mutex_lock(&mdev->mutex);
+	--mdev->n_output;
+	mxr_dbg(mdev, "%s(%d)\n", __func__, mdev->n_output);
+	/* turn on auxiliary driver */
+	if (mdev->n_output == 0)
+		v4l2_subdev_call(to_outsd(mdev), core, s_power, 0);
+	WARN(mdev->n_output < 0, "negative number of output users (%d)\n",
+		mdev->n_output);
+	mutex_unlock(&mdev->mutex);
+}
+
+int mxr_power_get(struct mxr_device *mdev)
+{
+	int ret = pm_runtime_get_sync(mdev->dev);
+
+	/* returning 1 means that power is already enabled,
+	 * so zero success be returned */
+	if (IS_ERR_VALUE(ret))
+		return ret;
+	return 0;
+}
+
+void mxr_power_put(struct mxr_device *mdev)
+{
+	pm_runtime_put_sync(mdev->dev);
+}
+
+/* --------- RESOURCE MANAGEMENT -------------*/
+
+static int mxr_acquire_plat_resources(struct mxr_device *mdev,
+				      struct platform_device *pdev)
+{
+	struct resource *res;
+	int ret;
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "mxr");
+	if (res == NULL) {
+		mxr_err(mdev, "get memory resource failed.\n");
+		ret = -ENXIO;
+		goto fail;
+	}
+
+	mdev->res.mxr_regs = ioremap(res->start, resource_size(res));
+	if (mdev->res.mxr_regs == NULL) {
+		mxr_err(mdev, "register mapping failed.\n");
+		ret = -ENXIO;
+		goto fail;
+	}
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "vp");
+	if (res == NULL) {
+		mxr_err(mdev, "get memory resource failed.\n");
+		ret = -ENXIO;
+		goto fail_mxr_regs;
+	}
+
+	mdev->res.vp_regs = ioremap(res->start, resource_size(res));
+	if (mdev->res.vp_regs == NULL) {
+		mxr_err(mdev, "register mapping failed.\n");
+		ret = -ENXIO;
+		goto fail_mxr_regs;
+	}
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_IRQ, "irq");
+	if (res == NULL) {
+		mxr_err(mdev, "get interrupt resource failed.\n");
+		ret = -ENXIO;
+		goto fail_vp_regs;
+	}
+
+	ret = request_irq(res->start, mxr_irq_handler, 0, "s5p-mixer", mdev);
+	if (ret) {
+		mxr_err(mdev, "request interrupt failed.\n");
+		goto fail_vp_regs;
+	}
+	mdev->res.irq = res->start;
+
+	return 0;
+
+fail_vp_regs:
+	iounmap(mdev->res.vp_regs);
+
+fail_mxr_regs:
+	iounmap(mdev->res.mxr_regs);
+
+fail:
+	return ret;
+}
+
+static void mxr_resource_clear_clocks(struct mxr_resources *res)
+{
+	res->mixer	= ERR_PTR(-EINVAL);
+	res->vp		= ERR_PTR(-EINVAL);
+	res->sclk_mixer	= ERR_PTR(-EINVAL);
+	res->sclk_hdmi	= ERR_PTR(-EINVAL);
+	res->sclk_dac	= ERR_PTR(-EINVAL);
+}
+
+static void mxr_release_plat_resources(struct mxr_device *mdev)
+{
+	free_irq(mdev->res.irq, mdev);
+	iounmap(mdev->res.vp_regs);
+	iounmap(mdev->res.mxr_regs);
+}
+
+static void mxr_release_clocks(struct mxr_device *mdev)
+{
+	struct mxr_resources *res = &mdev->res;
+
+	if (!IS_ERR(res->sclk_dac))
+		clk_put(res->sclk_dac);
+	if (!IS_ERR(res->sclk_hdmi))
+		clk_put(res->sclk_hdmi);
+	if (!IS_ERR(res->sclk_mixer))
+		clk_put(res->sclk_mixer);
+	if (!IS_ERR(res->vp))
+		clk_put(res->vp);
+	if (!IS_ERR(res->mixer))
+		clk_put(res->mixer);
+}
+
+static int mxr_acquire_clocks(struct mxr_device *mdev)
+{
+	struct mxr_resources *res = &mdev->res;
+	struct device *dev = mdev->dev;
+
+	mxr_resource_clear_clocks(res);
+
+	res->mixer = clk_get(dev, "mixer");
+	if (IS_ERR(res->mixer)) {
+		mxr_err(mdev, "failed to get clock 'mixer'\n");
+		goto fail;
+	}
+	res->vp = clk_get(dev, "vp");
+	if (IS_ERR(res->vp)) {
+		mxr_err(mdev, "failed to get clock 'vp'\n");
+		goto fail;
+	}
+	res->sclk_mixer = clk_get(dev, "sclk_mixer");
+	if (IS_ERR(res->sclk_mixer)) {
+		mxr_err(mdev, "failed to get clock 'sclk_mixer'\n");
+		goto fail;
+	}
+	res->sclk_hdmi = clk_get(dev, "sclk_hdmi");
+	if (IS_ERR(res->sclk_hdmi)) {
+		mxr_err(mdev, "failed to get clock 'sclk_hdmi'\n");
+		goto fail;
+	}
+	res->sclk_dac = clk_get(dev, "sclk_dac");
+	if (IS_ERR(res->sclk_dac)) {
+		mxr_err(mdev, "failed to get clock 'sclk_dac'\n");
+		goto fail;
+	}
+
+	return 0;
+fail:
+	mxr_release_clocks(mdev);
+	return -ENODEV;
+}
+
+static int mxr_acquire_resources(struct mxr_device *mdev,
+				 struct platform_device *pdev)
+{
+	int ret;
+	ret = mxr_acquire_plat_resources(mdev, pdev);
+
+	if (ret)
+		goto fail;
+
+	ret = mxr_acquire_clocks(mdev);
+	if (ret)
+		goto fail_plat;
+
+	mxr_info(mdev, "resources acquired\n");
+	return 0;
+
+fail_plat:
+	mxr_release_plat_resources(mdev);
+fail:
+	mxr_err(mdev, "resources acquire failed\n");
+	return ret;
+}
+
+static void mxr_release_resources(struct mxr_device *mdev)
+{
+	mxr_release_clocks(mdev);
+	mxr_release_plat_resources(mdev);
+	memset(&mdev->res, 0, sizeof(mdev->res));
+	mxr_resource_clear_clocks(&mdev->res);
+}
+
+static void mxr_release_layers(struct mxr_device *mdev)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(mdev->layer); ++i)
+		if (mdev->layer[i])
+			mxr_layer_release(mdev->layer[i]);
+}
+
+static int mxr_acquire_layers(struct mxr_device *mdev,
+			      struct mxr_platform_data *pdata)
+{
+	mdev->layer[0] = mxr_graph_layer_create(mdev, 0);
+	mdev->layer[1] = mxr_graph_layer_create(mdev, 1);
+	mdev->layer[2] = mxr_vp_layer_create(mdev, 0);
+
+	if (!mdev->layer[0] || !mdev->layer[1] || !mdev->layer[2]) {
+		mxr_err(mdev, "failed to acquire layers\n");
+		goto fail;
+	}
+
+	return 0;
+
+fail:
+	mxr_release_layers(mdev);
+	return -ENODEV;
+}
+
+/* ---------- POWER MANAGEMENT ----------- */
+
+static int mxr_runtime_resume(struct device *dev)
+{
+	struct mxr_device *mdev = to_mdev(dev);
+	struct mxr_resources *res = &mdev->res;
+	int ret;
+
+	mxr_dbg(mdev, "resume - start\n");
+	mutex_lock(&mdev->mutex);
+	/* turn clocks on */
+	ret = clk_prepare_enable(res->mixer);
+	if (ret < 0) {
+		dev_err(mdev->dev, "clk_prepare_enable(mixer) failed\n");
+		goto fail;
+	}
+	ret = clk_prepare_enable(res->vp);
+	if (ret < 0) {
+		dev_err(mdev->dev, "clk_prepare_enable(vp) failed\n");
+		goto fail_mixer;
+	}
+	ret = clk_prepare_enable(res->sclk_mixer);
+	if (ret < 0) {
+		dev_err(mdev->dev, "clk_prepare_enable(sclk_mixer) failed\n");
+		goto fail_vp;
+	}
+	/* apply default configuration */
+	mxr_reg_reset(mdev);
+	mxr_dbg(mdev, "resume - finished\n");
+
+	mutex_unlock(&mdev->mutex);
+	return 0;
+
+fail_vp:
+	clk_disable_unprepare(res->vp);
+fail_mixer:
+	clk_disable_unprepare(res->mixer);
+fail:
+	mutex_unlock(&mdev->mutex);
+	dev_err(mdev->dev, "resume failed\n");
+	return ret;
+}
+
+static int mxr_runtime_suspend(struct device *dev)
+{
+	struct mxr_device *mdev = to_mdev(dev);
+	struct mxr_resources *res = &mdev->res;
+	mxr_dbg(mdev, "suspend - start\n");
+	mutex_lock(&mdev->mutex);
+	/* turn clocks off */
+	clk_disable_unprepare(res->sclk_mixer);
+	clk_disable_unprepare(res->vp);
+	clk_disable_unprepare(res->mixer);
+	mutex_unlock(&mdev->mutex);
+	mxr_dbg(mdev, "suspend - finished\n");
+	return 0;
+}
+
+static const struct dev_pm_ops mxr_pm_ops = {
+	.runtime_suspend = mxr_runtime_suspend,
+	.runtime_resume	 = mxr_runtime_resume,
+};
+
+/* --------- DRIVER INITIALIZATION ---------- */
+
+static int mxr_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct mxr_platform_data *pdata = dev->platform_data;
+	struct mxr_device *mdev;
+	int ret;
+
+	/* mdev does not exist yet so no mxr_dbg is used */
+	dev_info(dev, "probe start\n");
+
+	mdev = kzalloc(sizeof(*mdev), GFP_KERNEL);
+	if (!mdev) {
+		dev_err(dev, "not enough memory.\n");
+		ret = -ENOMEM;
+		goto fail;
+	}
+
+	/* setup pointer to master device */
+	mdev->dev = dev;
+
+	mutex_init(&mdev->mutex);
+	spin_lock_init(&mdev->reg_slock);
+	init_waitqueue_head(&mdev->event_queue);
+
+	/* acquire resources: regs, irqs, clocks, regulators */
+	ret = mxr_acquire_resources(mdev, pdev);
+	if (ret)
+		goto fail_mem;
+
+	/* configure resources for video output */
+	ret = mxr_acquire_video(mdev, mxr_output_conf,
+		ARRAY_SIZE(mxr_output_conf));
+	if (ret)
+		goto fail_resources;
+
+	/* configure layers */
+	ret = mxr_acquire_layers(mdev, pdata);
+	if (ret)
+		goto fail_video;
+
+	pm_runtime_enable(dev);
+
+	mxr_info(mdev, "probe successful\n");
+	return 0;
+
+fail_video:
+	mxr_release_video(mdev);
+
+fail_resources:
+	mxr_release_resources(mdev);
+
+fail_mem:
+	kfree(mdev);
+
+fail:
+	dev_info(dev, "probe failed\n");
+	return ret;
+}
+
+static int mxr_remove(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct mxr_device *mdev = to_mdev(dev);
+
+	pm_runtime_disable(dev);
+
+	mxr_release_layers(mdev);
+	mxr_release_video(mdev);
+	mxr_release_resources(mdev);
+
+	kfree(mdev);
+
+	dev_info(dev, "remove successful\n");
+	return 0;
+}
+
+static struct platform_driver mxr_driver __refdata = {
+	.probe = mxr_probe,
+	.remove = mxr_remove,
+	.driver = {
+		.name = MXR_DRIVER_NAME,
+		.pm = &mxr_pm_ops,
+	}
+};
+
+static int __init mxr_init(void)
+{
+	int i, ret;
+	static const char banner[] __initconst =
+		"Samsung TV Mixer driver, "
+		"(c) 2010-2011 Samsung Electronics Co., Ltd.\n";
+	pr_info("%s\n", banner);
+
+	/* Loading auxiliary modules */
+	for (i = 0; i < ARRAY_SIZE(mxr_output_conf); ++i)
+		request_module(mxr_output_conf[i].module_name);
+
+	ret = platform_driver_register(&mxr_driver);
+	if (ret != 0) {
+		pr_err("s5p-tv: registration of MIXER driver failed\n");
+		return -ENXIO;
+	}
+
+	return 0;
+}
+module_init(mxr_init);
+
+static void __exit mxr_exit(void)
+{
+	platform_driver_unregister(&mxr_driver);
+}
+module_exit(mxr_exit);
diff --git a/drivers/media/platform/s5p-tv/mixer_grp_layer.c b/drivers/media/platform/s5p-tv/mixer_grp_layer.c
new file mode 100644
index 0000000..db3163b
--- /dev/null
+++ b/drivers/media/platform/s5p-tv/mixer_grp_layer.c
@@ -0,0 +1,270 @@
+/*
+ * Samsung TV Mixer driver
+ *
+ * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
+ *
+ * Tomasz Stanislawski, <t.stanislaws@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published
+ * by the Free Software Foundiation. either version 2 of the License,
+ * or (at your option) any later version
+ */
+
+#include "mixer.h"
+
+#include <media/videobuf2-dma-contig.h>
+
+/* FORMAT DEFINITIONS */
+
+static const struct mxr_format mxr_fb_fmt_rgb565 = {
+	.name = "RGB565",
+	.fourcc = V4L2_PIX_FMT_RGB565,
+	.colorspace = V4L2_COLORSPACE_SRGB,
+	.num_planes = 1,
+	.plane = {
+		{ .width = 1, .height = 1, .size = 2 },
+	},
+	.num_subframes = 1,
+	.cookie = 4,
+};
+
+static const struct mxr_format mxr_fb_fmt_argb1555 = {
+	.name = "ARGB1555",
+	.num_planes = 1,
+	.fourcc = V4L2_PIX_FMT_RGB555,
+	.colorspace = V4L2_COLORSPACE_SRGB,
+	.plane = {
+		{ .width = 1, .height = 1, .size = 2 },
+	},
+	.num_subframes = 1,
+	.cookie = 5,
+};
+
+static const struct mxr_format mxr_fb_fmt_argb4444 = {
+	.name = "ARGB4444",
+	.num_planes = 1,
+	.fourcc = V4L2_PIX_FMT_RGB444,
+	.colorspace = V4L2_COLORSPACE_SRGB,
+	.plane = {
+		{ .width = 1, .height = 1, .size = 2 },
+	},
+	.num_subframes = 1,
+	.cookie = 6,
+};
+
+static const struct mxr_format mxr_fb_fmt_argb8888 = {
+	.name = "ARGB8888",
+	.fourcc = V4L2_PIX_FMT_BGR32,
+	.colorspace = V4L2_COLORSPACE_SRGB,
+	.num_planes = 1,
+	.plane = {
+		{ .width = 1, .height = 1, .size = 4 },
+	},
+	.num_subframes = 1,
+	.cookie = 7,
+};
+
+static const struct mxr_format *mxr_graph_format[] = {
+	&mxr_fb_fmt_rgb565,
+	&mxr_fb_fmt_argb1555,
+	&mxr_fb_fmt_argb4444,
+	&mxr_fb_fmt_argb8888,
+};
+
+/* AUXILIARY CALLBACKS */
+
+static void mxr_graph_layer_release(struct mxr_layer *layer)
+{
+	mxr_base_layer_unregister(layer);
+	mxr_base_layer_release(layer);
+}
+
+static void mxr_graph_buffer_set(struct mxr_layer *layer,
+	struct mxr_buffer *buf)
+{
+	dma_addr_t addr = 0;
+
+	if (buf)
+		addr = vb2_dma_contig_plane_dma_addr(&buf->vb.vb2_buf, 0);
+	mxr_reg_graph_buffer(layer->mdev, layer->idx, addr);
+}
+
+static void mxr_graph_stream_set(struct mxr_layer *layer, int en)
+{
+	mxr_reg_graph_layer_stream(layer->mdev, layer->idx, en);
+}
+
+static void mxr_graph_format_set(struct mxr_layer *layer)
+{
+	mxr_reg_graph_format(layer->mdev, layer->idx,
+		layer->fmt, &layer->geo);
+}
+
+static inline unsigned int closest(unsigned int x, unsigned int a,
+	unsigned int b, unsigned long flags)
+{
+	unsigned int mid = (a + b) / 2;
+
+	/* choosing closest value with constraints according to table:
+	 * -------------+-----+-----+-----+-------+
+	 * flags	|  0  |  LE |  GE | LE|GE |
+	 * -------------+-----+-----+-----+-------+
+	 * x <= a	|  a  |  a  |  a  |   a   |
+	 * a < x <= mid	|  a  |  a  |  b  |   a   |
+	 * mid < x < b	|  b  |  a  |  b  |   b   |
+	 * b <= x	|  b  |  b  |  b  |   b   |
+	 * -------------+-----+-----+-----+-------+
+	 */
+
+	/* remove all non-constraint flags */
+	flags &= V4L2_SEL_FLAG_LE | V4L2_SEL_FLAG_GE;
+
+	if (x <= a)
+		return  a;
+	if (x >= b)
+		return b;
+	if (flags == V4L2_SEL_FLAG_LE)
+		return a;
+	if (flags == V4L2_SEL_FLAG_GE)
+		return b;
+	if (x <= mid)
+		return a;
+	return b;
+}
+
+static inline unsigned int do_center(unsigned int center,
+	unsigned int size, unsigned int upper, unsigned int flags)
+{
+	unsigned int lower;
+
+	if (flags & MXR_NO_OFFSET)
+		return 0;
+
+	lower = center - min(center, size / 2);
+	return min(lower, upper - size);
+}
+
+static void mxr_graph_fix_geometry(struct mxr_layer *layer,
+	enum mxr_geometry_stage stage, unsigned long flags)
+{
+	struct mxr_geometry *geo = &layer->geo;
+	struct mxr_crop *src = &geo->src;
+	struct mxr_crop *dst = &geo->dst;
+	unsigned int x_center, y_center;
+
+	switch (stage) {
+
+	case MXR_GEOMETRY_SINK: /* nothing to be fixed here */
+		flags = 0;
+		/* fall through */
+
+	case MXR_GEOMETRY_COMPOSE:
+		/* remember center of the area */
+		x_center = dst->x_offset + dst->width / 2;
+		y_center = dst->y_offset + dst->height / 2;
+		/* round up/down to 2 multiple depending on flags */
+		if (flags & V4L2_SEL_FLAG_LE) {
+			dst->width = round_down(dst->width, 2);
+			dst->height = round_down(dst->height, 2);
+		} else {
+			dst->width = round_up(dst->width, 2);
+			dst->height = round_up(dst->height, 2);
+		}
+		/* assure that compose rect is inside display area */
+		dst->width = min(dst->width, dst->full_width);
+		dst->height = min(dst->height, dst->full_height);
+
+		/* ensure that compose is reachable using 2x scaling */
+		dst->width = min(dst->width, 2 * src->full_width);
+		dst->height = min(dst->height, 2 * src->full_height);
+
+		/* setup offsets */
+		dst->x_offset = do_center(x_center, dst->width,
+			dst->full_width, flags);
+		dst->y_offset = do_center(y_center, dst->height,
+			dst->full_height, flags);
+		flags = 0;
+		/* fall through */
+
+	case MXR_GEOMETRY_CROP:
+		/* remember center of the area */
+		x_center = src->x_offset + src->width / 2;
+		y_center = src->y_offset + src->height / 2;
+		/* ensure that cropping area lies inside the buffer */
+		if (src->full_width < dst->width)
+			src->width = dst->width / 2;
+		else
+			src->width = closest(src->width, dst->width / 2,
+				dst->width, flags);
+
+		if (src->width == dst->width)
+			geo->x_ratio = 0;
+		else
+			geo->x_ratio = 1;
+
+		if (src->full_height < dst->height)
+			src->height = dst->height / 2;
+		else
+			src->height = closest(src->height, dst->height / 2,
+				dst->height, flags);
+
+		if (src->height == dst->height)
+			geo->y_ratio = 0;
+		else
+			geo->y_ratio = 1;
+
+		/* setup offsets */
+		src->x_offset = do_center(x_center, src->width,
+			src->full_width, flags);
+		src->y_offset = do_center(y_center, src->height,
+			src->full_height, flags);
+		flags = 0;
+		/* fall through */
+	case MXR_GEOMETRY_SOURCE:
+		src->full_width = clamp_val(src->full_width,
+			src->width + src->x_offset, 32767);
+		src->full_height = clamp_val(src->full_height,
+			src->height + src->y_offset, 2047);
+	}
+}
+
+/* PUBLIC API */
+
+struct mxr_layer *mxr_graph_layer_create(struct mxr_device *mdev, int idx)
+{
+	struct mxr_layer *layer;
+	int ret;
+	struct mxr_layer_ops ops = {
+		.release = mxr_graph_layer_release,
+		.buffer_set = mxr_graph_buffer_set,
+		.stream_set = mxr_graph_stream_set,
+		.format_set = mxr_graph_format_set,
+		.fix_geometry = mxr_graph_fix_geometry,
+	};
+	char name[32];
+
+	sprintf(name, "graph%d", idx);
+
+	layer = mxr_base_layer_create(mdev, idx, name, &ops);
+	if (layer == NULL) {
+		mxr_err(mdev, "failed to initialize layer(%d) base\n", idx);
+		goto fail;
+	}
+
+	layer->fmt_array = mxr_graph_format;
+	layer->fmt_array_size = ARRAY_SIZE(mxr_graph_format);
+
+	ret = mxr_base_layer_register(layer);
+	if (ret)
+		goto fail_layer;
+
+	return layer;
+
+fail_layer:
+	mxr_base_layer_release(layer);
+
+fail:
+	return NULL;
+}
+
diff --git a/drivers/media/platform/s5p-tv/mixer_reg.c b/drivers/media/platform/s5p-tv/mixer_reg.c
new file mode 100644
index 0000000..a0ec14a
--- /dev/null
+++ b/drivers/media/platform/s5p-tv/mixer_reg.c
@@ -0,0 +1,551 @@
+/*
+ * Samsung TV Mixer driver
+ *
+ * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
+ *
+ * Tomasz Stanislawski, <t.stanislaws@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published
+ * by the Free Software Foundiation. either version 2 of the License,
+ * or (at your option) any later version
+ */
+
+#include "mixer.h"
+#include "regs-mixer.h"
+#include "regs-vp.h"
+
+#include <linux/delay.h>
+
+/* Register access subroutines */
+
+static inline u32 vp_read(struct mxr_device *mdev, u32 reg_id)
+{
+	return readl(mdev->res.vp_regs + reg_id);
+}
+
+static inline void vp_write(struct mxr_device *mdev, u32 reg_id, u32 val)
+{
+	writel(val, mdev->res.vp_regs + reg_id);
+}
+
+static inline void vp_write_mask(struct mxr_device *mdev, u32 reg_id,
+	u32 val, u32 mask)
+{
+	u32 old = vp_read(mdev, reg_id);
+
+	val = (val & mask) | (old & ~mask);
+	writel(val, mdev->res.vp_regs + reg_id);
+}
+
+static inline u32 mxr_read(struct mxr_device *mdev, u32 reg_id)
+{
+	return readl(mdev->res.mxr_regs + reg_id);
+}
+
+static inline void mxr_write(struct mxr_device *mdev, u32 reg_id, u32 val)
+{
+	writel(val, mdev->res.mxr_regs + reg_id);
+}
+
+static inline void mxr_write_mask(struct mxr_device *mdev, u32 reg_id,
+	u32 val, u32 mask)
+{
+	u32 old = mxr_read(mdev, reg_id);
+
+	val = (val & mask) | (old & ~mask);
+	writel(val, mdev->res.mxr_regs + reg_id);
+}
+
+void mxr_vsync_set_update(struct mxr_device *mdev, int en)
+{
+	/* block update on vsync */
+	mxr_write_mask(mdev, MXR_STATUS, en ? MXR_STATUS_SYNC_ENABLE : 0,
+		MXR_STATUS_SYNC_ENABLE);
+	vp_write(mdev, VP_SHADOW_UPDATE, en ? VP_SHADOW_UPDATE_ENABLE : 0);
+}
+
+static void __mxr_reg_vp_reset(struct mxr_device *mdev)
+{
+	int tries = 100;
+
+	vp_write(mdev, VP_SRESET, VP_SRESET_PROCESSING);
+	for (tries = 100; tries; --tries) {
+		/* waiting until VP_SRESET_PROCESSING is 0 */
+		if (~vp_read(mdev, VP_SRESET) & VP_SRESET_PROCESSING)
+			break;
+		mdelay(10);
+	}
+	WARN(tries == 0, "failed to reset Video Processor\n");
+}
+
+static void mxr_reg_vp_default_filter(struct mxr_device *mdev);
+
+void mxr_reg_reset(struct mxr_device *mdev)
+{
+	unsigned long flags;
+	u32 val; /* value stored to register */
+
+	spin_lock_irqsave(&mdev->reg_slock, flags);
+	mxr_vsync_set_update(mdev, MXR_DISABLE);
+
+	/* set output in RGB888 mode */
+	mxr_write(mdev, MXR_CFG, MXR_CFG_OUT_RGB888);
+
+	/* 16 beat burst in DMA */
+	mxr_write_mask(mdev, MXR_STATUS, MXR_STATUS_16_BURST,
+		MXR_STATUS_BURST_MASK);
+
+	/* setting default layer priority: layer1 > video > layer0
+	 * because typical usage scenario would be
+	 * layer0 - framebuffer
+	 * video - video overlay
+	 * layer1 - OSD
+	 */
+	val  = MXR_LAYER_CFG_GRP0_VAL(1);
+	val |= MXR_LAYER_CFG_VP_VAL(2);
+	val |= MXR_LAYER_CFG_GRP1_VAL(3);
+	mxr_write(mdev, MXR_LAYER_CFG, val);
+
+	/* use dark gray background color */
+	mxr_write(mdev, MXR_BG_COLOR0, 0x808080);
+	mxr_write(mdev, MXR_BG_COLOR1, 0x808080);
+	mxr_write(mdev, MXR_BG_COLOR2, 0x808080);
+
+	/* setting graphical layers */
+
+	val  = MXR_GRP_CFG_COLOR_KEY_DISABLE; /* no blank key */
+	val |= MXR_GRP_CFG_BLEND_PRE_MUL; /* premul mode */
+	val |= MXR_GRP_CFG_ALPHA_VAL(0xff); /* non-transparent alpha */
+
+	/* the same configuration for both layers */
+	mxr_write(mdev, MXR_GRAPHIC_CFG(0), val);
+	mxr_write(mdev, MXR_GRAPHIC_CFG(1), val);
+
+	/* configuration of Video Processor Registers */
+	__mxr_reg_vp_reset(mdev);
+	mxr_reg_vp_default_filter(mdev);
+
+	/* enable all interrupts */
+	mxr_write_mask(mdev, MXR_INT_EN, ~0, MXR_INT_EN_ALL);
+
+	mxr_vsync_set_update(mdev, MXR_ENABLE);
+	spin_unlock_irqrestore(&mdev->reg_slock, flags);
+}
+
+void mxr_reg_graph_format(struct mxr_device *mdev, int idx,
+	const struct mxr_format *fmt, const struct mxr_geometry *geo)
+{
+	u32 val;
+	unsigned long flags;
+
+	spin_lock_irqsave(&mdev->reg_slock, flags);
+	mxr_vsync_set_update(mdev, MXR_DISABLE);
+
+	/* setup format */
+	mxr_write_mask(mdev, MXR_GRAPHIC_CFG(idx),
+		MXR_GRP_CFG_FORMAT_VAL(fmt->cookie), MXR_GRP_CFG_FORMAT_MASK);
+
+	/* setup geometry */
+	mxr_write(mdev, MXR_GRAPHIC_SPAN(idx), geo->src.full_width);
+	val  = MXR_GRP_WH_WIDTH(geo->src.width);
+	val |= MXR_GRP_WH_HEIGHT(geo->src.height);
+	val |= MXR_GRP_WH_H_SCALE(geo->x_ratio);
+	val |= MXR_GRP_WH_V_SCALE(geo->y_ratio);
+	mxr_write(mdev, MXR_GRAPHIC_WH(idx), val);
+
+	/* setup offsets in source image */
+	val  = MXR_GRP_SXY_SX(geo->src.x_offset);
+	val |= MXR_GRP_SXY_SY(geo->src.y_offset);
+	mxr_write(mdev, MXR_GRAPHIC_SXY(idx), val);
+
+	/* setup offsets in display image */
+	val  = MXR_GRP_DXY_DX(geo->dst.x_offset);
+	val |= MXR_GRP_DXY_DY(geo->dst.y_offset);
+	mxr_write(mdev, MXR_GRAPHIC_DXY(idx), val);
+
+	mxr_vsync_set_update(mdev, MXR_ENABLE);
+	spin_unlock_irqrestore(&mdev->reg_slock, flags);
+}
+
+void mxr_reg_vp_format(struct mxr_device *mdev,
+	const struct mxr_format *fmt, const struct mxr_geometry *geo)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&mdev->reg_slock, flags);
+	mxr_vsync_set_update(mdev, MXR_DISABLE);
+
+	vp_write_mask(mdev, VP_MODE, fmt->cookie, VP_MODE_FMT_MASK);
+
+	/* setting size of input image */
+	vp_write(mdev, VP_IMG_SIZE_Y, VP_IMG_HSIZE(geo->src.full_width) |
+		VP_IMG_VSIZE(geo->src.full_height));
+	/* chroma height has to reduced by 2 to avoid chroma distorions */
+	vp_write(mdev, VP_IMG_SIZE_C, VP_IMG_HSIZE(geo->src.full_width) |
+		VP_IMG_VSIZE(geo->src.full_height / 2));
+
+	vp_write(mdev, VP_SRC_WIDTH, geo->src.width);
+	vp_write(mdev, VP_SRC_HEIGHT, geo->src.height);
+	vp_write(mdev, VP_SRC_H_POSITION,
+		VP_SRC_H_POSITION_VAL(geo->src.x_offset));
+	vp_write(mdev, VP_SRC_V_POSITION, geo->src.y_offset);
+
+	vp_write(mdev, VP_DST_WIDTH, geo->dst.width);
+	vp_write(mdev, VP_DST_H_POSITION, geo->dst.x_offset);
+	if (geo->dst.field == V4L2_FIELD_INTERLACED) {
+		vp_write(mdev, VP_DST_HEIGHT, geo->dst.height / 2);
+		vp_write(mdev, VP_DST_V_POSITION, geo->dst.y_offset / 2);
+	} else {
+		vp_write(mdev, VP_DST_HEIGHT, geo->dst.height);
+		vp_write(mdev, VP_DST_V_POSITION, geo->dst.y_offset);
+	}
+
+	vp_write(mdev, VP_H_RATIO, geo->x_ratio);
+	vp_write(mdev, VP_V_RATIO, geo->y_ratio);
+
+	vp_write(mdev, VP_ENDIAN_MODE, VP_ENDIAN_MODE_LITTLE);
+
+	mxr_vsync_set_update(mdev, MXR_ENABLE);
+	spin_unlock_irqrestore(&mdev->reg_slock, flags);
+
+}
+
+void mxr_reg_graph_buffer(struct mxr_device *mdev, int idx, dma_addr_t addr)
+{
+	u32 val = addr ? ~0 : 0;
+	unsigned long flags;
+
+	spin_lock_irqsave(&mdev->reg_slock, flags);
+	mxr_vsync_set_update(mdev, MXR_DISABLE);
+
+	if (idx == 0)
+		mxr_write_mask(mdev, MXR_CFG, val, MXR_CFG_GRP0_ENABLE);
+	else
+		mxr_write_mask(mdev, MXR_CFG, val, MXR_CFG_GRP1_ENABLE);
+	mxr_write(mdev, MXR_GRAPHIC_BASE(idx), addr);
+
+	mxr_vsync_set_update(mdev, MXR_ENABLE);
+	spin_unlock_irqrestore(&mdev->reg_slock, flags);
+}
+
+void mxr_reg_vp_buffer(struct mxr_device *mdev,
+	dma_addr_t luma_addr[2], dma_addr_t chroma_addr[2])
+{
+	u32 val = luma_addr[0] ? ~0 : 0;
+	unsigned long flags;
+
+	spin_lock_irqsave(&mdev->reg_slock, flags);
+	mxr_vsync_set_update(mdev, MXR_DISABLE);
+
+	mxr_write_mask(mdev, MXR_CFG, val, MXR_CFG_VP_ENABLE);
+	vp_write_mask(mdev, VP_ENABLE, val, VP_ENABLE_ON);
+	/* TODO: fix tiled mode */
+	vp_write(mdev, VP_TOP_Y_PTR, luma_addr[0]);
+	vp_write(mdev, VP_TOP_C_PTR, chroma_addr[0]);
+	vp_write(mdev, VP_BOT_Y_PTR, luma_addr[1]);
+	vp_write(mdev, VP_BOT_C_PTR, chroma_addr[1]);
+
+	mxr_vsync_set_update(mdev, MXR_ENABLE);
+	spin_unlock_irqrestore(&mdev->reg_slock, flags);
+}
+
+static void mxr_irq_layer_handle(struct mxr_layer *layer)
+{
+	struct list_head *head = &layer->enq_list;
+	struct mxr_buffer *done;
+
+	/* skip non-existing layer */
+	if (layer == NULL)
+		return;
+
+	spin_lock(&layer->enq_slock);
+	if (layer->state == MXR_LAYER_IDLE)
+		goto done;
+
+	done = layer->shadow_buf;
+	layer->shadow_buf = layer->update_buf;
+
+	if (list_empty(head)) {
+		if (layer->state != MXR_LAYER_STREAMING)
+			layer->update_buf = NULL;
+	} else {
+		struct mxr_buffer *next;
+		next = list_first_entry(head, struct mxr_buffer, list);
+		list_del(&next->list);
+		layer->update_buf = next;
+	}
+
+	layer->ops.buffer_set(layer, layer->update_buf);
+
+	if (done && done != layer->shadow_buf)
+		vb2_buffer_done(&done->vb.vb2_buf, VB2_BUF_STATE_DONE);
+
+done:
+	spin_unlock(&layer->enq_slock);
+}
+
+irqreturn_t mxr_irq_handler(int irq, void *dev_data)
+{
+	struct mxr_device *mdev = dev_data;
+	u32 i, val;
+
+	spin_lock(&mdev->reg_slock);
+	val = mxr_read(mdev, MXR_INT_STATUS);
+
+	/* wake up process waiting for VSYNC */
+	if (val & MXR_INT_STATUS_VSYNC) {
+		set_bit(MXR_EVENT_VSYNC, &mdev->event_flags);
+		/* toggle TOP field event if working in interlaced mode */
+		if (~mxr_read(mdev, MXR_CFG) & MXR_CFG_SCAN_PROGRASSIVE)
+			change_bit(MXR_EVENT_TOP, &mdev->event_flags);
+		wake_up(&mdev->event_queue);
+		/* vsync interrupt use different bit for read and clear */
+		val &= ~MXR_INT_STATUS_VSYNC;
+		val |= MXR_INT_CLEAR_VSYNC;
+	}
+
+	/* clear interrupts */
+	mxr_write(mdev, MXR_INT_STATUS, val);
+
+	spin_unlock(&mdev->reg_slock);
+	/* leave on non-vsync event */
+	if (~val & MXR_INT_CLEAR_VSYNC)
+		return IRQ_HANDLED;
+	/* skip layer update on bottom field */
+	if (!test_bit(MXR_EVENT_TOP, &mdev->event_flags))
+		return IRQ_HANDLED;
+	for (i = 0; i < MXR_MAX_LAYERS; ++i)
+		mxr_irq_layer_handle(mdev->layer[i]);
+	return IRQ_HANDLED;
+}
+
+void mxr_reg_s_output(struct mxr_device *mdev, int cookie)
+{
+	u32 val;
+
+	val = cookie == 0 ? MXR_CFG_DST_SDO : MXR_CFG_DST_HDMI;
+	mxr_write_mask(mdev, MXR_CFG, val, MXR_CFG_DST_MASK);
+}
+
+void mxr_reg_streamon(struct mxr_device *mdev)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&mdev->reg_slock, flags);
+	/* single write -> no need to block vsync update */
+
+	/* start MIXER */
+	mxr_write_mask(mdev, MXR_STATUS, ~0, MXR_STATUS_REG_RUN);
+	set_bit(MXR_EVENT_TOP, &mdev->event_flags);
+
+	spin_unlock_irqrestore(&mdev->reg_slock, flags);
+}
+
+void mxr_reg_streamoff(struct mxr_device *mdev)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&mdev->reg_slock, flags);
+	/* single write -> no need to block vsync update */
+
+	/* stop MIXER */
+	mxr_write_mask(mdev, MXR_STATUS, 0, MXR_STATUS_REG_RUN);
+
+	spin_unlock_irqrestore(&mdev->reg_slock, flags);
+}
+
+int mxr_reg_wait4vsync(struct mxr_device *mdev)
+{
+	long time_left;
+
+	clear_bit(MXR_EVENT_VSYNC, &mdev->event_flags);
+	/* TODO: consider adding interruptible */
+	time_left = wait_event_timeout(mdev->event_queue,
+			test_bit(MXR_EVENT_VSYNC, &mdev->event_flags),
+				 msecs_to_jiffies(1000));
+	if (time_left > 0)
+		return 0;
+	mxr_warn(mdev, "no vsync detected - timeout\n");
+	return -ETIME;
+}
+
+void mxr_reg_set_mbus_fmt(struct mxr_device *mdev,
+	struct v4l2_mbus_framefmt *fmt)
+{
+	u32 val = 0;
+	unsigned long flags;
+
+	spin_lock_irqsave(&mdev->reg_slock, flags);
+	mxr_vsync_set_update(mdev, MXR_DISABLE);
+
+	/* selecting colorspace accepted by output */
+	if (fmt->colorspace == V4L2_COLORSPACE_JPEG)
+		val |= MXR_CFG_OUT_YUV444;
+	else
+		val |= MXR_CFG_OUT_RGB888;
+
+	/* choosing between interlace and progressive mode */
+	if (fmt->field == V4L2_FIELD_INTERLACED)
+		val |= MXR_CFG_SCAN_INTERLACE;
+	else
+		val |= MXR_CFG_SCAN_PROGRASSIVE;
+
+	/* choosing between porper HD and SD mode */
+	if (fmt->height == 480)
+		val |= MXR_CFG_SCAN_NTSC | MXR_CFG_SCAN_SD;
+	else if (fmt->height == 576)
+		val |= MXR_CFG_SCAN_PAL | MXR_CFG_SCAN_SD;
+	else if (fmt->height == 720)
+		val |= MXR_CFG_SCAN_HD_720 | MXR_CFG_SCAN_HD;
+	else if (fmt->height == 1080)
+		val |= MXR_CFG_SCAN_HD_1080 | MXR_CFG_SCAN_HD;
+	else
+		WARN(1, "unrecognized mbus height %u!\n", fmt->height);
+
+	mxr_write_mask(mdev, MXR_CFG, val, MXR_CFG_SCAN_MASK |
+		MXR_CFG_OUT_MASK);
+
+	val = (fmt->field == V4L2_FIELD_INTERLACED) ? ~0 : 0;
+	vp_write_mask(mdev, VP_MODE, val,
+		VP_MODE_LINE_SKIP | VP_MODE_FIELD_ID_AUTO_TOGGLING);
+
+	mxr_vsync_set_update(mdev, MXR_ENABLE);
+	spin_unlock_irqrestore(&mdev->reg_slock, flags);
+}
+
+void mxr_reg_graph_layer_stream(struct mxr_device *mdev, int idx, int en)
+{
+	/* no extra actions need to be done */
+}
+
+void mxr_reg_vp_layer_stream(struct mxr_device *mdev, int en)
+{
+	/* no extra actions need to be done */
+}
+
+static const u8 filter_y_horiz_tap8[] = {
+	0,	-1,	-1,	-1,	-1,	-1,	-1,	-1,
+	-1,	-1,	-1,	-1,	-1,	0,	0,	0,
+	0,	2,	4,	5,	6,	6,	6,	6,
+	6,	5,	5,	4,	3,	2,	1,	1,
+	0,	-6,	-12,	-16,	-18,	-20,	-21,	-20,
+	-20,	-18,	-16,	-13,	-10,	-8,	-5,	-2,
+	127,	126,	125,	121,	114,	107,	99,	89,
+	79,	68,	57,	46,	35,	25,	16,	8,
+};
+
+static const u8 filter_y_vert_tap4[] = {
+	0,	-3,	-6,	-8,	-8,	-8,	-8,	-7,
+	-6,	-5,	-4,	-3,	-2,	-1,	-1,	0,
+	127,	126,	124,	118,	111,	102,	92,	81,
+	70,	59,	48,	37,	27,	19,	11,	5,
+	0,	5,	11,	19,	27,	37,	48,	59,
+	70,	81,	92,	102,	111,	118,	124,	126,
+	0,	0,	-1,	-1,	-2,	-3,	-4,	-5,
+	-6,	-7,	-8,	-8,	-8,	-8,	-6,	-3,
+};
+
+static const u8 filter_cr_horiz_tap4[] = {
+	0,	-3,	-6,	-8,	-8,	-8,	-8,	-7,
+	-6,	-5,	-4,	-3,	-2,	-1,	-1,	0,
+	127,	126,	124,	118,	111,	102,	92,	81,
+	70,	59,	48,	37,	27,	19,	11,	5,
+};
+
+static inline void mxr_reg_vp_filter_set(struct mxr_device *mdev,
+	int reg_id, const u8 *data, unsigned int size)
+{
+	/* assure 4-byte align */
+	BUG_ON(size & 3);
+	for (; size; size -= 4, reg_id += 4, data += 4) {
+		u32 val = (data[0] << 24) |  (data[1] << 16) |
+			(data[2] << 8) | data[3];
+		vp_write(mdev, reg_id, val);
+	}
+}
+
+static void mxr_reg_vp_default_filter(struct mxr_device *mdev)
+{
+	mxr_reg_vp_filter_set(mdev, VP_POLY8_Y0_LL,
+		filter_y_horiz_tap8, sizeof(filter_y_horiz_tap8));
+	mxr_reg_vp_filter_set(mdev, VP_POLY4_Y0_LL,
+		filter_y_vert_tap4, sizeof(filter_y_vert_tap4));
+	mxr_reg_vp_filter_set(mdev, VP_POLY4_C0_LL,
+		filter_cr_horiz_tap4, sizeof(filter_cr_horiz_tap4));
+}
+
+static void mxr_reg_mxr_dump(struct mxr_device *mdev)
+{
+#define DUMPREG(reg_id) \
+do { \
+	mxr_dbg(mdev, #reg_id " = %08x\n", \
+		(u32)readl(mdev->res.mxr_regs + reg_id)); \
+} while (0)
+
+	DUMPREG(MXR_STATUS);
+	DUMPREG(MXR_CFG);
+	DUMPREG(MXR_INT_EN);
+	DUMPREG(MXR_INT_STATUS);
+
+	DUMPREG(MXR_LAYER_CFG);
+	DUMPREG(MXR_VIDEO_CFG);
+
+	DUMPREG(MXR_GRAPHIC0_CFG);
+	DUMPREG(MXR_GRAPHIC0_BASE);
+	DUMPREG(MXR_GRAPHIC0_SPAN);
+	DUMPREG(MXR_GRAPHIC0_WH);
+	DUMPREG(MXR_GRAPHIC0_SXY);
+	DUMPREG(MXR_GRAPHIC0_DXY);
+
+	DUMPREG(MXR_GRAPHIC1_CFG);
+	DUMPREG(MXR_GRAPHIC1_BASE);
+	DUMPREG(MXR_GRAPHIC1_SPAN);
+	DUMPREG(MXR_GRAPHIC1_WH);
+	DUMPREG(MXR_GRAPHIC1_SXY);
+	DUMPREG(MXR_GRAPHIC1_DXY);
+#undef DUMPREG
+}
+
+static void mxr_reg_vp_dump(struct mxr_device *mdev)
+{
+#define DUMPREG(reg_id) \
+do { \
+	mxr_dbg(mdev, #reg_id " = %08x\n", \
+		(u32) readl(mdev->res.vp_regs + reg_id)); \
+} while (0)
+
+
+	DUMPREG(VP_ENABLE);
+	DUMPREG(VP_SRESET);
+	DUMPREG(VP_SHADOW_UPDATE);
+	DUMPREG(VP_FIELD_ID);
+	DUMPREG(VP_MODE);
+	DUMPREG(VP_IMG_SIZE_Y);
+	DUMPREG(VP_IMG_SIZE_C);
+	DUMPREG(VP_PER_RATE_CTRL);
+	DUMPREG(VP_TOP_Y_PTR);
+	DUMPREG(VP_BOT_Y_PTR);
+	DUMPREG(VP_TOP_C_PTR);
+	DUMPREG(VP_BOT_C_PTR);
+	DUMPREG(VP_ENDIAN_MODE);
+	DUMPREG(VP_SRC_H_POSITION);
+	DUMPREG(VP_SRC_V_POSITION);
+	DUMPREG(VP_SRC_WIDTH);
+	DUMPREG(VP_SRC_HEIGHT);
+	DUMPREG(VP_DST_H_POSITION);
+	DUMPREG(VP_DST_V_POSITION);
+	DUMPREG(VP_DST_WIDTH);
+	DUMPREG(VP_DST_HEIGHT);
+	DUMPREG(VP_H_RATIO);
+	DUMPREG(VP_V_RATIO);
+
+#undef DUMPREG
+}
+
+void mxr_reg_dump(struct mxr_device *mdev)
+{
+	mxr_reg_mxr_dump(mdev);
+	mxr_reg_vp_dump(mdev);
+}
+
diff --git a/drivers/media/platform/s5p-tv/mixer_video.c b/drivers/media/platform/s5p-tv/mixer_video.c
new file mode 100644
index 0000000..dc1c679
--- /dev/null
+++ b/drivers/media/platform/s5p-tv/mixer_video.c
@@ -0,0 +1,1139 @@
+/*
+ * Samsung TV Mixer driver
+ *
+ * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
+ *
+ * Tomasz Stanislawski, <t.stanislaws@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published
+ * by the Free Software Foundation. either version 2 of the License,
+ * or (at your option) any later version
+ */
+
+#define pr_fmt(fmt) "s5p-tv (mixer): " fmt
+
+#include "mixer.h"
+
+#include <media/v4l2-ioctl.h>
+#include <linux/videodev2.h>
+#include <linux/mm.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/timer.h>
+#include <media/videobuf2-dma-contig.h>
+
+static int find_reg_callback(struct device *dev, void *p)
+{
+	struct v4l2_subdev **sd = p;
+
+	*sd = dev_get_drvdata(dev);
+	/* non-zero value stops iteration */
+	return 1;
+}
+
+static struct v4l2_subdev *find_and_register_subdev(
+	struct mxr_device *mdev, char *module_name)
+{
+	struct device_driver *drv;
+	struct v4l2_subdev *sd = NULL;
+	int ret;
+
+	/* TODO: add waiting until probe is finished */
+	drv = driver_find(module_name, &platform_bus_type);
+	if (!drv) {
+		mxr_warn(mdev, "module %s is missing\n", module_name);
+		return NULL;
+	}
+	/* driver refcnt is increased, it is safe to iterate over devices */
+	ret = driver_for_each_device(drv, NULL, &sd, find_reg_callback);
+	/* ret == 0 means that find_reg_callback was never executed */
+	if (sd == NULL) {
+		mxr_warn(mdev, "module %s provides no subdev!\n", module_name);
+		goto done;
+	}
+	/* v4l2_device_register_subdev detects if sd is NULL */
+	ret = v4l2_device_register_subdev(&mdev->v4l2_dev, sd);
+	if (ret) {
+		mxr_warn(mdev, "failed to register subdev %s\n", sd->name);
+		sd = NULL;
+	}
+
+done:
+	return sd;
+}
+
+int mxr_acquire_video(struct mxr_device *mdev,
+		      struct mxr_output_conf *output_conf, int output_count)
+{
+	struct device *dev = mdev->dev;
+	struct v4l2_device *v4l2_dev = &mdev->v4l2_dev;
+	int i;
+	int ret = 0;
+	struct v4l2_subdev *sd;
+
+	strlcpy(v4l2_dev->name, dev_name(mdev->dev), sizeof(v4l2_dev->name));
+	/* prepare context for V4L2 device */
+	ret = v4l2_device_register(dev, v4l2_dev);
+	if (ret) {
+		mxr_err(mdev, "could not register v4l2 device.\n");
+		goto fail;
+	}
+
+	mdev->alloc_ctx = vb2_dma_contig_init_ctx(mdev->dev);
+	if (IS_ERR(mdev->alloc_ctx)) {
+		mxr_err(mdev, "could not acquire vb2 allocator\n");
+		ret = PTR_ERR(mdev->alloc_ctx);
+		goto fail_v4l2_dev;
+	}
+
+	/* registering outputs */
+	mdev->output_cnt = 0;
+	for (i = 0; i < output_count; ++i) {
+		struct mxr_output_conf *conf = &output_conf[i];
+		struct mxr_output *out;
+
+		sd = find_and_register_subdev(mdev, conf->module_name);
+		/* trying to register next output */
+		if (sd == NULL)
+			continue;
+		out = kzalloc(sizeof(*out), GFP_KERNEL);
+		if (out == NULL) {
+			mxr_err(mdev, "no memory for '%s'\n",
+				conf->output_name);
+			ret = -ENOMEM;
+			/* registered subdevs are removed in fail_v4l2_dev */
+			goto fail_output;
+		}
+		strlcpy(out->name, conf->output_name, sizeof(out->name));
+		out->sd = sd;
+		out->cookie = conf->cookie;
+		mdev->output[mdev->output_cnt++] = out;
+		mxr_info(mdev, "added output '%s' from module '%s'\n",
+			conf->output_name, conf->module_name);
+		/* checking if maximal number of outputs is reached */
+		if (mdev->output_cnt >= MXR_MAX_OUTPUTS)
+			break;
+	}
+
+	if (mdev->output_cnt == 0) {
+		mxr_err(mdev, "failed to register any output\n");
+		ret = -ENODEV;
+		/* skipping fail_output because there is nothing to free */
+		goto fail_vb2_allocator;
+	}
+
+	return 0;
+
+fail_output:
+	/* kfree is NULL-safe */
+	for (i = 0; i < mdev->output_cnt; ++i)
+		kfree(mdev->output[i]);
+	memset(mdev->output, 0, sizeof(mdev->output));
+
+fail_vb2_allocator:
+	/* freeing allocator context */
+	vb2_dma_contig_cleanup_ctx(mdev->alloc_ctx);
+
+fail_v4l2_dev:
+	/* NOTE: automatically unregister all subdevs */
+	v4l2_device_unregister(v4l2_dev);
+
+fail:
+	return ret;
+}
+
+void mxr_release_video(struct mxr_device *mdev)
+{
+	int i;
+
+	/* kfree is NULL-safe */
+	for (i = 0; i < mdev->output_cnt; ++i)
+		kfree(mdev->output[i]);
+
+	vb2_dma_contig_cleanup_ctx(mdev->alloc_ctx);
+	v4l2_device_unregister(&mdev->v4l2_dev);
+}
+
+static int mxr_querycap(struct file *file, void *priv,
+	struct v4l2_capability *cap)
+{
+	struct mxr_layer *layer = video_drvdata(file);
+
+	mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
+
+	strlcpy(cap->driver, MXR_DRIVER_NAME, sizeof(cap->driver));
+	strlcpy(cap->card, layer->vfd.name, sizeof(cap->card));
+	sprintf(cap->bus_info, "%d", layer->idx);
+	cap->device_caps = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_OUTPUT_MPLANE;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+
+	return 0;
+}
+
+static void mxr_geometry_dump(struct mxr_device *mdev, struct mxr_geometry *geo)
+{
+	mxr_dbg(mdev, "src.full_size = (%u, %u)\n",
+		geo->src.full_width, geo->src.full_height);
+	mxr_dbg(mdev, "src.size = (%u, %u)\n",
+		geo->src.width, geo->src.height);
+	mxr_dbg(mdev, "src.offset = (%u, %u)\n",
+		geo->src.x_offset, geo->src.y_offset);
+	mxr_dbg(mdev, "dst.full_size = (%u, %u)\n",
+		geo->dst.full_width, geo->dst.full_height);
+	mxr_dbg(mdev, "dst.size = (%u, %u)\n",
+		geo->dst.width, geo->dst.height);
+	mxr_dbg(mdev, "dst.offset = (%u, %u)\n",
+		geo->dst.x_offset, geo->dst.y_offset);
+	mxr_dbg(mdev, "ratio = (%u, %u)\n",
+		geo->x_ratio, geo->y_ratio);
+}
+
+static void mxr_layer_default_geo(struct mxr_layer *layer)
+{
+	struct mxr_device *mdev = layer->mdev;
+	struct v4l2_mbus_framefmt mbus_fmt;
+
+	memset(&layer->geo, 0, sizeof(layer->geo));
+
+	mxr_get_mbus_fmt(mdev, &mbus_fmt);
+
+	layer->geo.dst.full_width = mbus_fmt.width;
+	layer->geo.dst.full_height = mbus_fmt.height;
+	layer->geo.dst.width = layer->geo.dst.full_width;
+	layer->geo.dst.height = layer->geo.dst.full_height;
+	layer->geo.dst.field = mbus_fmt.field;
+
+	layer->geo.src.full_width = mbus_fmt.width;
+	layer->geo.src.full_height = mbus_fmt.height;
+	layer->geo.src.width = layer->geo.src.full_width;
+	layer->geo.src.height = layer->geo.src.full_height;
+
+	mxr_geometry_dump(mdev, &layer->geo);
+	layer->ops.fix_geometry(layer, MXR_GEOMETRY_SINK, 0);
+	mxr_geometry_dump(mdev, &layer->geo);
+}
+
+static void mxr_layer_update_output(struct mxr_layer *layer)
+{
+	struct mxr_device *mdev = layer->mdev;
+	struct v4l2_mbus_framefmt mbus_fmt;
+
+	mxr_get_mbus_fmt(mdev, &mbus_fmt);
+	/* checking if update is needed */
+	if (layer->geo.dst.full_width == mbus_fmt.width &&
+		layer->geo.dst.full_height == mbus_fmt.width)
+		return;
+
+	layer->geo.dst.full_width = mbus_fmt.width;
+	layer->geo.dst.full_height = mbus_fmt.height;
+	layer->geo.dst.field = mbus_fmt.field;
+	layer->ops.fix_geometry(layer, MXR_GEOMETRY_SINK, 0);
+
+	mxr_geometry_dump(mdev, &layer->geo);
+}
+
+static const struct mxr_format *find_format_by_fourcc(
+	struct mxr_layer *layer, unsigned long fourcc);
+static const struct mxr_format *find_format_by_index(
+	struct mxr_layer *layer, unsigned long index);
+
+static int mxr_enum_fmt(struct file *file, void  *priv,
+	struct v4l2_fmtdesc *f)
+{
+	struct mxr_layer *layer = video_drvdata(file);
+	struct mxr_device *mdev = layer->mdev;
+	const struct mxr_format *fmt;
+
+	mxr_dbg(mdev, "%s\n", __func__);
+	fmt = find_format_by_index(layer, f->index);
+	if (fmt == NULL)
+		return -EINVAL;
+
+	strlcpy(f->description, fmt->name, sizeof(f->description));
+	f->pixelformat = fmt->fourcc;
+
+	return 0;
+}
+
+static unsigned int divup(unsigned int divident, unsigned int divisor)
+{
+	return (divident + divisor - 1) / divisor;
+}
+
+unsigned long mxr_get_plane_size(const struct mxr_block *blk,
+	unsigned int width, unsigned int height)
+{
+	unsigned int bl_width = divup(width, blk->width);
+	unsigned int bl_height = divup(height, blk->height);
+
+	return bl_width * bl_height * blk->size;
+}
+
+static void mxr_mplane_fill(struct v4l2_plane_pix_format *planes,
+	const struct mxr_format *fmt, u32 width, u32 height)
+{
+	int i;
+
+	/* checking if nothing to fill */
+	if (!planes)
+		return;
+
+	memset(planes, 0, sizeof(*planes) * fmt->num_subframes);
+	for (i = 0; i < fmt->num_planes; ++i) {
+		struct v4l2_plane_pix_format *plane = planes
+			+ fmt->plane2subframe[i];
+		const struct mxr_block *blk = &fmt->plane[i];
+		u32 bl_width = divup(width, blk->width);
+		u32 bl_height = divup(height, blk->height);
+		u32 sizeimage = bl_width * bl_height * blk->size;
+		u32 bytesperline = bl_width * blk->size / blk->height;
+
+		plane->sizeimage += sizeimage;
+		plane->bytesperline = max(plane->bytesperline, bytesperline);
+	}
+}
+
+static int mxr_g_fmt(struct file *file, void *priv,
+			     struct v4l2_format *f)
+{
+	struct mxr_layer *layer = video_drvdata(file);
+	struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp;
+
+	mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
+
+	pix->width = layer->geo.src.full_width;
+	pix->height = layer->geo.src.full_height;
+	pix->field = V4L2_FIELD_NONE;
+	pix->pixelformat = layer->fmt->fourcc;
+	pix->colorspace = layer->fmt->colorspace;
+	mxr_mplane_fill(pix->plane_fmt, layer->fmt, pix->width, pix->height);
+
+	return 0;
+}
+
+static int mxr_s_fmt(struct file *file, void *priv,
+	struct v4l2_format *f)
+{
+	struct mxr_layer *layer = video_drvdata(file);
+	const struct mxr_format *fmt;
+	struct v4l2_pix_format_mplane *pix;
+	struct mxr_device *mdev = layer->mdev;
+	struct mxr_geometry *geo = &layer->geo;
+
+	mxr_dbg(mdev, "%s:%d\n", __func__, __LINE__);
+
+	pix = &f->fmt.pix_mp;
+	fmt = find_format_by_fourcc(layer, pix->pixelformat);
+	if (fmt == NULL) {
+		mxr_warn(mdev, "not recognized fourcc: %08x\n",
+			pix->pixelformat);
+		return -EINVAL;
+	}
+	layer->fmt = fmt;
+	/* set source size to highest accepted value */
+	geo->src.full_width = max(geo->dst.full_width, pix->width);
+	geo->src.full_height = max(geo->dst.full_height, pix->height);
+	layer->ops.fix_geometry(layer, MXR_GEOMETRY_SOURCE, 0);
+	mxr_geometry_dump(mdev, &layer->geo);
+	/* set cropping to total visible screen */
+	geo->src.width = pix->width;
+	geo->src.height = pix->height;
+	geo->src.x_offset = 0;
+	geo->src.y_offset = 0;
+	/* assure consistency of geometry */
+	layer->ops.fix_geometry(layer, MXR_GEOMETRY_CROP, MXR_NO_OFFSET);
+	mxr_geometry_dump(mdev, &layer->geo);
+	/* set full size to lowest possible value */
+	geo->src.full_width = 0;
+	geo->src.full_height = 0;
+	layer->ops.fix_geometry(layer, MXR_GEOMETRY_SOURCE, 0);
+	mxr_geometry_dump(mdev, &layer->geo);
+
+	/* returning results */
+	mxr_g_fmt(file, priv, f);
+
+	return 0;
+}
+
+static int mxr_g_selection(struct file *file, void *fh,
+	struct v4l2_selection *s)
+{
+	struct mxr_layer *layer = video_drvdata(file);
+	struct mxr_geometry *geo = &layer->geo;
+
+	mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
+
+	if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT &&
+		s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		return -EINVAL;
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_CROP:
+		s->r.left = geo->src.x_offset;
+		s->r.top = geo->src.y_offset;
+		s->r.width = geo->src.width;
+		s->r.height = geo->src.height;
+		break;
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		s->r.left = 0;
+		s->r.top = 0;
+		s->r.width = geo->src.full_width;
+		s->r.height = geo->src.full_height;
+		break;
+	case V4L2_SEL_TGT_COMPOSE:
+	case V4L2_SEL_TGT_COMPOSE_PADDED:
+		s->r.left = geo->dst.x_offset;
+		s->r.top = geo->dst.y_offset;
+		s->r.width = geo->dst.width;
+		s->r.height = geo->dst.height;
+		break;
+	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+		s->r.left = 0;
+		s->r.top = 0;
+		s->r.width = geo->dst.full_width;
+		s->r.height = geo->dst.full_height;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* returns 1 if rectangle 'a' is inside 'b' */
+static int mxr_is_rect_inside(struct v4l2_rect *a, struct v4l2_rect *b)
+{
+	if (a->left < b->left)
+		return 0;
+	if (a->top < b->top)
+		return 0;
+	if (a->left + a->width > b->left + b->width)
+		return 0;
+	if (a->top + a->height > b->top + b->height)
+		return 0;
+	return 1;
+}
+
+static int mxr_s_selection(struct file *file, void *fh,
+	struct v4l2_selection *s)
+{
+	struct mxr_layer *layer = video_drvdata(file);
+	struct mxr_geometry *geo = &layer->geo;
+	struct mxr_crop *target = NULL;
+	enum mxr_geometry_stage stage;
+	struct mxr_geometry tmp;
+	struct v4l2_rect res;
+
+	memset(&res, 0, sizeof(res));
+
+	mxr_dbg(layer->mdev, "%s: rect: %dx%d@%d,%d\n", __func__,
+		s->r.width, s->r.height, s->r.left, s->r.top);
+
+	if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT &&
+		s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		return -EINVAL;
+
+	switch (s->target) {
+	/* ignore read-only targets */
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		res.width = geo->src.full_width;
+		res.height = geo->src.full_height;
+		break;
+
+	/* ignore read-only targets */
+	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+		res.width = geo->dst.full_width;
+		res.height = geo->dst.full_height;
+		break;
+
+	case V4L2_SEL_TGT_CROP:
+		target = &geo->src;
+		stage = MXR_GEOMETRY_CROP;
+		break;
+	case V4L2_SEL_TGT_COMPOSE:
+	case V4L2_SEL_TGT_COMPOSE_PADDED:
+		target = &geo->dst;
+		stage = MXR_GEOMETRY_COMPOSE;
+		break;
+	default:
+		return -EINVAL;
+	}
+	/* apply change and update geometry if needed */
+	if (target) {
+		/* backup current geometry if setup fails */
+		memcpy(&tmp, geo, sizeof(tmp));
+
+		/* apply requested selection */
+		target->x_offset = s->r.left;
+		target->y_offset = s->r.top;
+		target->width = s->r.width;
+		target->height = s->r.height;
+
+		layer->ops.fix_geometry(layer, stage, s->flags);
+
+		/* retrieve update selection rectangle */
+		res.left = target->x_offset;
+		res.top = target->y_offset;
+		res.width = target->width;
+		res.height = target->height;
+
+		mxr_geometry_dump(layer->mdev, &layer->geo);
+	}
+
+	/* checking if the rectangle satisfies constraints */
+	if ((s->flags & V4L2_SEL_FLAG_LE) && !mxr_is_rect_inside(&res, &s->r))
+		goto fail;
+	if ((s->flags & V4L2_SEL_FLAG_GE) && !mxr_is_rect_inside(&s->r, &res))
+		goto fail;
+
+	/* return result rectangle */
+	s->r = res;
+
+	return 0;
+fail:
+	/* restore old geometry, which is not touched if target is NULL */
+	if (target)
+		memcpy(geo, &tmp, sizeof(tmp));
+	return -ERANGE;
+}
+
+static int mxr_enum_dv_timings(struct file *file, void *fh,
+	struct v4l2_enum_dv_timings *timings)
+{
+	struct mxr_layer *layer = video_drvdata(file);
+	struct mxr_device *mdev = layer->mdev;
+	int ret;
+
+	timings->pad = 0;
+
+	/* lock protects from changing sd_out */
+	mutex_lock(&mdev->mutex);
+	ret = v4l2_subdev_call(to_outsd(mdev), pad, enum_dv_timings, timings);
+	mutex_unlock(&mdev->mutex);
+
+	return ret ? -EINVAL : 0;
+}
+
+static int mxr_s_dv_timings(struct file *file, void *fh,
+	struct v4l2_dv_timings *timings)
+{
+	struct mxr_layer *layer = video_drvdata(file);
+	struct mxr_device *mdev = layer->mdev;
+	int ret;
+
+	/* lock protects from changing sd_out */
+	mutex_lock(&mdev->mutex);
+
+	/* timings change cannot be done while there is an entity
+	 * dependent on output configuration
+	 */
+	if (mdev->n_output > 0) {
+		mutex_unlock(&mdev->mutex);
+		return -EBUSY;
+	}
+
+	ret = v4l2_subdev_call(to_outsd(mdev), video, s_dv_timings, timings);
+
+	mutex_unlock(&mdev->mutex);
+
+	mxr_layer_update_output(layer);
+
+	/* any failure should return EINVAL according to V4L2 doc */
+	return ret ? -EINVAL : 0;
+}
+
+static int mxr_g_dv_timings(struct file *file, void *fh,
+	struct v4l2_dv_timings *timings)
+{
+	struct mxr_layer *layer = video_drvdata(file);
+	struct mxr_device *mdev = layer->mdev;
+	int ret;
+
+	/* lock protects from changing sd_out */
+	mutex_lock(&mdev->mutex);
+	ret = v4l2_subdev_call(to_outsd(mdev), video, g_dv_timings, timings);
+	mutex_unlock(&mdev->mutex);
+
+	return ret ? -EINVAL : 0;
+}
+
+static int mxr_dv_timings_cap(struct file *file, void *fh,
+	struct v4l2_dv_timings_cap *cap)
+{
+	struct mxr_layer *layer = video_drvdata(file);
+	struct mxr_device *mdev = layer->mdev;
+	int ret;
+
+	cap->pad = 0;
+
+	/* lock protects from changing sd_out */
+	mutex_lock(&mdev->mutex);
+	ret = v4l2_subdev_call(to_outsd(mdev), pad, dv_timings_cap, cap);
+	mutex_unlock(&mdev->mutex);
+
+	return ret ? -EINVAL : 0;
+}
+
+static int mxr_s_std(struct file *file, void *fh, v4l2_std_id norm)
+{
+	struct mxr_layer *layer = video_drvdata(file);
+	struct mxr_device *mdev = layer->mdev;
+	int ret;
+
+	/* lock protects from changing sd_out */
+	mutex_lock(&mdev->mutex);
+
+	/* standard change cannot be done while there is an entity
+	 * dependent on output configuration
+	 */
+	if (mdev->n_output > 0) {
+		mutex_unlock(&mdev->mutex);
+		return -EBUSY;
+	}
+
+	ret = v4l2_subdev_call(to_outsd(mdev), video, s_std_output, norm);
+
+	mutex_unlock(&mdev->mutex);
+
+	mxr_layer_update_output(layer);
+
+	return ret ? -EINVAL : 0;
+}
+
+static int mxr_g_std(struct file *file, void *fh, v4l2_std_id *norm)
+{
+	struct mxr_layer *layer = video_drvdata(file);
+	struct mxr_device *mdev = layer->mdev;
+	int ret;
+
+	/* lock protects from changing sd_out */
+	mutex_lock(&mdev->mutex);
+	ret = v4l2_subdev_call(to_outsd(mdev), video, g_std_output, norm);
+	mutex_unlock(&mdev->mutex);
+
+	return ret ? -EINVAL : 0;
+}
+
+static int mxr_enum_output(struct file *file, void *fh, struct v4l2_output *a)
+{
+	struct mxr_layer *layer = video_drvdata(file);
+	struct mxr_device *mdev = layer->mdev;
+	struct mxr_output *out;
+	struct v4l2_subdev *sd;
+
+	if (a->index >= mdev->output_cnt)
+		return -EINVAL;
+	out = mdev->output[a->index];
+	BUG_ON(out == NULL);
+	sd = out->sd;
+	strlcpy(a->name, out->name, sizeof(a->name));
+
+	/* try to obtain supported tv norms */
+	v4l2_subdev_call(sd, video, g_tvnorms_output, &a->std);
+	a->capabilities = 0;
+	if (sd->ops->video && sd->ops->video->s_dv_timings)
+		a->capabilities |= V4L2_OUT_CAP_DV_TIMINGS;
+	if (sd->ops->video && sd->ops->video->s_std_output)
+		a->capabilities |= V4L2_OUT_CAP_STD;
+	a->type = V4L2_OUTPUT_TYPE_ANALOG;
+
+	return 0;
+}
+
+static int mxr_s_output(struct file *file, void *fh, unsigned int i)
+{
+	struct video_device *vfd = video_devdata(file);
+	struct mxr_layer *layer = video_drvdata(file);
+	struct mxr_device *mdev = layer->mdev;
+
+	if (i >= mdev->output_cnt || mdev->output[i] == NULL)
+		return -EINVAL;
+
+	mutex_lock(&mdev->mutex);
+	if (mdev->n_output > 0) {
+		mutex_unlock(&mdev->mutex);
+		return -EBUSY;
+	}
+	mdev->current_output = i;
+	vfd->tvnorms = 0;
+	v4l2_subdev_call(to_outsd(mdev), video, g_tvnorms_output,
+		&vfd->tvnorms);
+	mutex_unlock(&mdev->mutex);
+
+	/* update layers geometry */
+	mxr_layer_update_output(layer);
+
+	mxr_dbg(mdev, "tvnorms = %08llx\n", vfd->tvnorms);
+
+	return 0;
+}
+
+static int mxr_g_output(struct file *file, void *fh, unsigned int *p)
+{
+	struct mxr_layer *layer = video_drvdata(file);
+	struct mxr_device *mdev = layer->mdev;
+
+	mutex_lock(&mdev->mutex);
+	*p = mdev->current_output;
+	mutex_unlock(&mdev->mutex);
+
+	return 0;
+}
+
+static int mxr_reqbufs(struct file *file, void *priv,
+			  struct v4l2_requestbuffers *p)
+{
+	struct mxr_layer *layer = video_drvdata(file);
+
+	mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
+	return vb2_reqbufs(&layer->vb_queue, p);
+}
+
+static int mxr_querybuf(struct file *file, void *priv, struct v4l2_buffer *p)
+{
+	struct mxr_layer *layer = video_drvdata(file);
+
+	mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
+	return vb2_querybuf(&layer->vb_queue, p);
+}
+
+static int mxr_qbuf(struct file *file, void *priv, struct v4l2_buffer *p)
+{
+	struct mxr_layer *layer = video_drvdata(file);
+
+	mxr_dbg(layer->mdev, "%s:%d(%d)\n", __func__, __LINE__, p->index);
+	return vb2_qbuf(&layer->vb_queue, p);
+}
+
+static int mxr_dqbuf(struct file *file, void *priv, struct v4l2_buffer *p)
+{
+	struct mxr_layer *layer = video_drvdata(file);
+
+	mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
+	return vb2_dqbuf(&layer->vb_queue, p, file->f_flags & O_NONBLOCK);
+}
+
+static int mxr_expbuf(struct file *file, void *priv,
+	struct v4l2_exportbuffer *eb)
+{
+	struct mxr_layer *layer = video_drvdata(file);
+
+	mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
+	return vb2_expbuf(&layer->vb_queue, eb);
+}
+
+static int mxr_streamon(struct file *file, void *priv, enum v4l2_buf_type i)
+{
+	struct mxr_layer *layer = video_drvdata(file);
+
+	mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
+	return vb2_streamon(&layer->vb_queue, i);
+}
+
+static int mxr_streamoff(struct file *file, void *priv, enum v4l2_buf_type i)
+{
+	struct mxr_layer *layer = video_drvdata(file);
+
+	mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
+	return vb2_streamoff(&layer->vb_queue, i);
+}
+
+static const struct v4l2_ioctl_ops mxr_ioctl_ops = {
+	.vidioc_querycap = mxr_querycap,
+	/* format handling */
+	.vidioc_enum_fmt_vid_out_mplane = mxr_enum_fmt,
+	.vidioc_s_fmt_vid_out_mplane = mxr_s_fmt,
+	.vidioc_g_fmt_vid_out_mplane = mxr_g_fmt,
+	/* buffer control */
+	.vidioc_reqbufs = mxr_reqbufs,
+	.vidioc_querybuf = mxr_querybuf,
+	.vidioc_qbuf = mxr_qbuf,
+	.vidioc_dqbuf = mxr_dqbuf,
+	.vidioc_expbuf = mxr_expbuf,
+	/* Streaming control */
+	.vidioc_streamon = mxr_streamon,
+	.vidioc_streamoff = mxr_streamoff,
+	/* DV Timings functions */
+	.vidioc_enum_dv_timings = mxr_enum_dv_timings,
+	.vidioc_s_dv_timings = mxr_s_dv_timings,
+	.vidioc_g_dv_timings = mxr_g_dv_timings,
+	.vidioc_dv_timings_cap = mxr_dv_timings_cap,
+	/* analog TV standard functions */
+	.vidioc_s_std = mxr_s_std,
+	.vidioc_g_std = mxr_g_std,
+	/* Output handling */
+	.vidioc_enum_output = mxr_enum_output,
+	.vidioc_s_output = mxr_s_output,
+	.vidioc_g_output = mxr_g_output,
+	/* selection ioctls */
+	.vidioc_g_selection = mxr_g_selection,
+	.vidioc_s_selection = mxr_s_selection,
+};
+
+static int mxr_video_open(struct file *file)
+{
+	struct mxr_layer *layer = video_drvdata(file);
+	struct mxr_device *mdev = layer->mdev;
+	int ret = 0;
+
+	mxr_dbg(mdev, "%s:%d\n", __func__, __LINE__);
+	if (mutex_lock_interruptible(&layer->mutex))
+		return -ERESTARTSYS;
+	/* assure device probe is finished */
+	wait_for_device_probe();
+	/* creating context for file descriptor */
+	ret = v4l2_fh_open(file);
+	if (ret) {
+		mxr_err(mdev, "v4l2_fh_open failed\n");
+		goto unlock;
+	}
+
+	/* leaving if layer is already initialized */
+	if (!v4l2_fh_is_singular_file(file))
+		goto unlock;
+
+	/* FIXME: should power be enabled on open? */
+	ret = mxr_power_get(mdev);
+	if (ret) {
+		mxr_err(mdev, "power on failed\n");
+		goto fail_fh_open;
+	}
+
+	ret = vb2_queue_init(&layer->vb_queue);
+	if (ret != 0) {
+		mxr_err(mdev, "failed to initialize vb2 queue\n");
+		goto fail_power;
+	}
+	/* set default format, first on the list */
+	layer->fmt = layer->fmt_array[0];
+	/* setup default geometry */
+	mxr_layer_default_geo(layer);
+	mutex_unlock(&layer->mutex);
+
+	return 0;
+
+fail_power:
+	mxr_power_put(mdev);
+
+fail_fh_open:
+	v4l2_fh_release(file);
+
+unlock:
+	mutex_unlock(&layer->mutex);
+
+	return ret;
+}
+
+static unsigned int
+mxr_video_poll(struct file *file, struct poll_table_struct *wait)
+{
+	struct mxr_layer *layer = video_drvdata(file);
+	unsigned int res;
+
+	mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
+
+	mutex_lock(&layer->mutex);
+	res = vb2_poll(&layer->vb_queue, file, wait);
+	mutex_unlock(&layer->mutex);
+	return res;
+}
+
+static int mxr_video_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct mxr_layer *layer = video_drvdata(file);
+	int ret;
+
+	mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
+
+	if (mutex_lock_interruptible(&layer->mutex))
+		return -ERESTARTSYS;
+	ret = vb2_mmap(&layer->vb_queue, vma);
+	mutex_unlock(&layer->mutex);
+	return ret;
+}
+
+static int mxr_video_release(struct file *file)
+{
+	struct mxr_layer *layer = video_drvdata(file);
+
+	mxr_dbg(layer->mdev, "%s:%d\n", __func__, __LINE__);
+	mutex_lock(&layer->mutex);
+	if (v4l2_fh_is_singular_file(file)) {
+		vb2_queue_release(&layer->vb_queue);
+		mxr_power_put(layer->mdev);
+	}
+	v4l2_fh_release(file);
+	mutex_unlock(&layer->mutex);
+	return 0;
+}
+
+static const struct v4l2_file_operations mxr_fops = {
+	.owner = THIS_MODULE,
+	.open = mxr_video_open,
+	.poll = mxr_video_poll,
+	.mmap = mxr_video_mmap,
+	.release = mxr_video_release,
+	.unlocked_ioctl = video_ioctl2,
+};
+
+static int queue_setup(struct vb2_queue *vq, const void *parg,
+	unsigned int *nbuffers, unsigned int *nplanes, unsigned int sizes[],
+	void *alloc_ctxs[])
+{
+	struct mxr_layer *layer = vb2_get_drv_priv(vq);
+	const struct mxr_format *fmt = layer->fmt;
+	int i;
+	struct mxr_device *mdev = layer->mdev;
+	struct v4l2_plane_pix_format planes[3];
+
+	mxr_dbg(mdev, "%s\n", __func__);
+	/* checking if format was configured */
+	if (fmt == NULL)
+		return -EINVAL;
+	mxr_dbg(mdev, "fmt = %s\n", fmt->name);
+	mxr_mplane_fill(planes, fmt, layer->geo.src.full_width,
+		layer->geo.src.full_height);
+
+	*nplanes = fmt->num_subframes;
+	for (i = 0; i < fmt->num_subframes; ++i) {
+		alloc_ctxs[i] = layer->mdev->alloc_ctx;
+		sizes[i] = planes[i].sizeimage;
+		mxr_dbg(mdev, "size[%d] = %08x\n", i, sizes[i]);
+	}
+
+	if (*nbuffers == 0)
+		*nbuffers = 1;
+
+	return 0;
+}
+
+static void buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct mxr_buffer *buffer = container_of(vbuf, struct mxr_buffer, vb);
+	struct mxr_layer *layer = vb2_get_drv_priv(vb->vb2_queue);
+	struct mxr_device *mdev = layer->mdev;
+	unsigned long flags;
+
+	spin_lock_irqsave(&layer->enq_slock, flags);
+	list_add_tail(&buffer->list, &layer->enq_list);
+	spin_unlock_irqrestore(&layer->enq_slock, flags);
+
+	mxr_dbg(mdev, "queuing buffer\n");
+}
+
+static int start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct mxr_layer *layer = vb2_get_drv_priv(vq);
+	struct mxr_device *mdev = layer->mdev;
+	unsigned long flags;
+
+	mxr_dbg(mdev, "%s\n", __func__);
+
+	/* block any changes in output configuration */
+	mxr_output_get(mdev);
+
+	mxr_layer_update_output(layer);
+	layer->ops.format_set(layer);
+	/* enabling layer in hardware */
+	spin_lock_irqsave(&layer->enq_slock, flags);
+	layer->state = MXR_LAYER_STREAMING;
+	spin_unlock_irqrestore(&layer->enq_slock, flags);
+
+	layer->ops.stream_set(layer, MXR_ENABLE);
+	mxr_streamer_get(mdev);
+
+	return 0;
+}
+
+static void mxr_watchdog(unsigned long arg)
+{
+	struct mxr_layer *layer = (struct mxr_layer *) arg;
+	struct mxr_device *mdev = layer->mdev;
+	unsigned long flags;
+
+	mxr_err(mdev, "watchdog fired for layer %s\n", layer->vfd.name);
+
+	spin_lock_irqsave(&layer->enq_slock, flags);
+
+	if (layer->update_buf == layer->shadow_buf)
+		layer->update_buf = NULL;
+	if (layer->update_buf) {
+		vb2_buffer_done(&layer->update_buf->vb.vb2_buf,
+				VB2_BUF_STATE_ERROR);
+		layer->update_buf = NULL;
+	}
+	if (layer->shadow_buf) {
+		vb2_buffer_done(&layer->shadow_buf->vb.vb2_buf,
+				VB2_BUF_STATE_ERROR);
+		layer->shadow_buf = NULL;
+	}
+	spin_unlock_irqrestore(&layer->enq_slock, flags);
+}
+
+static void stop_streaming(struct vb2_queue *vq)
+{
+	struct mxr_layer *layer = vb2_get_drv_priv(vq);
+	struct mxr_device *mdev = layer->mdev;
+	unsigned long flags;
+	struct timer_list watchdog;
+	struct mxr_buffer *buf, *buf_tmp;
+
+	mxr_dbg(mdev, "%s\n", __func__);
+
+	spin_lock_irqsave(&layer->enq_slock, flags);
+
+	/* reset list */
+	layer->state = MXR_LAYER_STREAMING_FINISH;
+
+	/* set all buffer to be done */
+	list_for_each_entry_safe(buf, buf_tmp, &layer->enq_list, list) {
+		list_del(&buf->list);
+		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+	}
+
+	spin_unlock_irqrestore(&layer->enq_slock, flags);
+
+	/* give 1 seconds to complete to complete last buffers */
+	setup_timer_on_stack(&watchdog, mxr_watchdog,
+		(unsigned long)layer);
+	mod_timer(&watchdog, jiffies + msecs_to_jiffies(1000));
+
+	/* wait until all buffers are goes to done state */
+	vb2_wait_for_all_buffers(vq);
+
+	/* stop timer if all synchronization is done */
+	del_timer_sync(&watchdog);
+	destroy_timer_on_stack(&watchdog);
+
+	/* stopping hardware */
+	spin_lock_irqsave(&layer->enq_slock, flags);
+	layer->state = MXR_LAYER_IDLE;
+	spin_unlock_irqrestore(&layer->enq_slock, flags);
+
+	/* disabling layer in hardware */
+	layer->ops.stream_set(layer, MXR_DISABLE);
+	/* remove one streamer */
+	mxr_streamer_put(mdev);
+	/* allow changes in output configuration */
+	mxr_output_put(mdev);
+}
+
+static struct vb2_ops mxr_video_qops = {
+	.queue_setup = queue_setup,
+	.buf_queue = buf_queue,
+	.wait_prepare = vb2_ops_wait_prepare,
+	.wait_finish = vb2_ops_wait_finish,
+	.start_streaming = start_streaming,
+	.stop_streaming = stop_streaming,
+};
+
+/* FIXME: try to put this functions to mxr_base_layer_create */
+int mxr_base_layer_register(struct mxr_layer *layer)
+{
+	struct mxr_device *mdev = layer->mdev;
+	int ret;
+
+	ret = video_register_device(&layer->vfd, VFL_TYPE_GRABBER, -1);
+	if (ret)
+		mxr_err(mdev, "failed to register video device\n");
+	else
+		mxr_info(mdev, "registered layer %s as /dev/video%d\n",
+			layer->vfd.name, layer->vfd.num);
+	return ret;
+}
+
+void mxr_base_layer_unregister(struct mxr_layer *layer)
+{
+	video_unregister_device(&layer->vfd);
+}
+
+void mxr_layer_release(struct mxr_layer *layer)
+{
+	if (layer->ops.release)
+		layer->ops.release(layer);
+}
+
+void mxr_base_layer_release(struct mxr_layer *layer)
+{
+	kfree(layer);
+}
+
+static void mxr_vfd_release(struct video_device *vdev)
+{
+	pr_info("video device release\n");
+}
+
+struct mxr_layer *mxr_base_layer_create(struct mxr_device *mdev,
+	int idx, char *name, struct mxr_layer_ops *ops)
+{
+	struct mxr_layer *layer;
+
+	layer = kzalloc(sizeof(*layer), GFP_KERNEL);
+	if (layer == NULL) {
+		mxr_err(mdev, "not enough memory for layer.\n");
+		goto fail;
+	}
+
+	layer->mdev = mdev;
+	layer->idx = idx;
+	layer->ops = *ops;
+
+	spin_lock_init(&layer->enq_slock);
+	INIT_LIST_HEAD(&layer->enq_list);
+	mutex_init(&layer->mutex);
+
+	layer->vfd = (struct video_device) {
+		.minor = -1,
+		.release = mxr_vfd_release,
+		.fops = &mxr_fops,
+		.vfl_dir = VFL_DIR_TX,
+		.ioctl_ops = &mxr_ioctl_ops,
+	};
+	strlcpy(layer->vfd.name, name, sizeof(layer->vfd.name));
+
+	video_set_drvdata(&layer->vfd, layer);
+	layer->vfd.lock = &layer->mutex;
+	layer->vfd.v4l2_dev = &mdev->v4l2_dev;
+
+	layer->vb_queue = (struct vb2_queue) {
+		.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
+		.io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF,
+		.drv_priv = layer,
+		.buf_struct_size = sizeof(struct mxr_buffer),
+		.ops = &mxr_video_qops,
+		.min_buffers_needed = 1,
+		.mem_ops = &vb2_dma_contig_memops,
+		.lock = &layer->mutex,
+	};
+
+	return layer;
+
+fail:
+	return NULL;
+}
+
+static const struct mxr_format *find_format_by_fourcc(
+	struct mxr_layer *layer, unsigned long fourcc)
+{
+	int i;
+
+	for (i = 0; i < layer->fmt_array_size; ++i)
+		if (layer->fmt_array[i]->fourcc == fourcc)
+			return layer->fmt_array[i];
+	return NULL;
+}
+
+static const struct mxr_format *find_format_by_index(
+	struct mxr_layer *layer, unsigned long index)
+{
+	if (index >= layer->fmt_array_size)
+		return NULL;
+	return layer->fmt_array[index];
+}
+
diff --git a/drivers/media/platform/s5p-tv/mixer_vp_layer.c b/drivers/media/platform/s5p-tv/mixer_vp_layer.c
new file mode 100644
index 0000000..dd002a4
--- /dev/null
+++ b/drivers/media/platform/s5p-tv/mixer_vp_layer.c
@@ -0,0 +1,242 @@
+/*
+ * Samsung TV Mixer driver
+ *
+ * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
+ *
+ * Tomasz Stanislawski, <t.stanislaws@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published
+ * by the Free Software Foundiation. either version 2 of the License,
+ * or (at your option) any later version
+ */
+
+#include "mixer.h"
+
+#include "regs-vp.h"
+
+#include <media/videobuf2-dma-contig.h>
+
+/* FORMAT DEFINITIONS */
+static const struct mxr_format mxr_fmt_nv12 = {
+	.name = "NV12",
+	.fourcc = V4L2_PIX_FMT_NV12,
+	.colorspace = V4L2_COLORSPACE_JPEG,
+	.num_planes = 2,
+	.plane = {
+		{ .width = 1, .height = 1, .size = 1 },
+		{ .width = 2, .height = 2, .size = 2 },
+	},
+	.num_subframes = 1,
+	.cookie = VP_MODE_NV12 | VP_MODE_MEM_LINEAR,
+};
+
+static const struct mxr_format mxr_fmt_nv21 = {
+	.name = "NV21",
+	.fourcc = V4L2_PIX_FMT_NV21,
+	.colorspace = V4L2_COLORSPACE_JPEG,
+	.num_planes = 2,
+	.plane = {
+		{ .width = 1, .height = 1, .size = 1 },
+		{ .width = 2, .height = 2, .size = 2 },
+	},
+	.num_subframes = 1,
+	.cookie = VP_MODE_NV21 | VP_MODE_MEM_LINEAR,
+};
+
+static const struct mxr_format mxr_fmt_nv12m = {
+	.name = "NV12 (mplane)",
+	.fourcc = V4L2_PIX_FMT_NV12M,
+	.colorspace = V4L2_COLORSPACE_JPEG,
+	.num_planes = 2,
+	.plane = {
+		{ .width = 1, .height = 1, .size = 1 },
+		{ .width = 2, .height = 2, .size = 2 },
+	},
+	.num_subframes = 2,
+	.plane2subframe = {0, 1},
+	.cookie = VP_MODE_NV12 | VP_MODE_MEM_LINEAR,
+};
+
+static const struct mxr_format mxr_fmt_nv12mt = {
+	.name = "NV12 tiled (mplane)",
+	.fourcc = V4L2_PIX_FMT_NV12MT,
+	.colorspace = V4L2_COLORSPACE_JPEG,
+	.num_planes = 2,
+	.plane = {
+		{ .width = 128, .height = 32, .size = 4096 },
+		{ .width = 128, .height = 32, .size = 2048 },
+	},
+	.num_subframes = 2,
+	.plane2subframe = {0, 1},
+	.cookie = VP_MODE_NV12 | VP_MODE_MEM_TILED,
+};
+
+static const struct mxr_format *mxr_video_format[] = {
+	&mxr_fmt_nv12,
+	&mxr_fmt_nv21,
+	&mxr_fmt_nv12m,
+	&mxr_fmt_nv12mt,
+};
+
+/* AUXILIARY CALLBACKS */
+
+static void mxr_vp_layer_release(struct mxr_layer *layer)
+{
+	mxr_base_layer_unregister(layer);
+	mxr_base_layer_release(layer);
+}
+
+static void mxr_vp_buffer_set(struct mxr_layer *layer,
+	struct mxr_buffer *buf)
+{
+	dma_addr_t luma_addr[2] = {0, 0};
+	dma_addr_t chroma_addr[2] = {0, 0};
+
+	if (buf == NULL) {
+		mxr_reg_vp_buffer(layer->mdev, luma_addr, chroma_addr);
+		return;
+	}
+	luma_addr[0] = vb2_dma_contig_plane_dma_addr(&buf->vb.vb2_buf, 0);
+	if (layer->fmt->num_subframes == 2) {
+		chroma_addr[0] =
+			vb2_dma_contig_plane_dma_addr(&buf->vb.vb2_buf, 1);
+	} else {
+		/* FIXME: mxr_get_plane_size compute integer division,
+		 * which is slow and should not be performed in interrupt */
+		chroma_addr[0] = luma_addr[0] + mxr_get_plane_size(
+			&layer->fmt->plane[0], layer->geo.src.full_width,
+			layer->geo.src.full_height);
+	}
+	if (layer->fmt->cookie & VP_MODE_MEM_TILED) {
+		luma_addr[1] = luma_addr[0] + 0x40;
+		chroma_addr[1] = chroma_addr[0] + 0x40;
+	} else {
+		luma_addr[1] = luma_addr[0] + layer->geo.src.full_width;
+		chroma_addr[1] = chroma_addr[0];
+	}
+	mxr_reg_vp_buffer(layer->mdev, luma_addr, chroma_addr);
+}
+
+static void mxr_vp_stream_set(struct mxr_layer *layer, int en)
+{
+	mxr_reg_vp_layer_stream(layer->mdev, en);
+}
+
+static void mxr_vp_format_set(struct mxr_layer *layer)
+{
+	mxr_reg_vp_format(layer->mdev, layer->fmt, &layer->geo);
+}
+
+static inline unsigned int do_center(unsigned int center,
+	unsigned int size, unsigned int upper, unsigned int flags)
+{
+	unsigned int lower;
+
+	if (flags & MXR_NO_OFFSET)
+		return 0;
+
+	lower = center - min(center, size / 2);
+	return min(lower, upper - size);
+}
+
+static void mxr_vp_fix_geometry(struct mxr_layer *layer,
+	enum mxr_geometry_stage stage, unsigned long flags)
+{
+	struct mxr_geometry *geo = &layer->geo;
+	struct mxr_crop *src = &geo->src;
+	struct mxr_crop *dst = &geo->dst;
+	unsigned long x_center, y_center;
+
+	switch (stage) {
+
+	case MXR_GEOMETRY_SINK: /* nothing to be fixed here */
+	case MXR_GEOMETRY_COMPOSE:
+		/* remember center of the area */
+		x_center = dst->x_offset + dst->width / 2;
+		y_center = dst->y_offset + dst->height / 2;
+
+		/* ensure that compose is reachable using 16x scaling */
+		dst->width = clamp(dst->width, 8U, 16 * src->full_width);
+		dst->height = clamp(dst->height, 1U, 16 * src->full_height);
+
+		/* setup offsets */
+		dst->x_offset = do_center(x_center, dst->width,
+			dst->full_width, flags);
+		dst->y_offset = do_center(y_center, dst->height,
+			dst->full_height, flags);
+		flags = 0; /* remove possible MXR_NO_OFFSET flag */
+		/* fall through */
+	case MXR_GEOMETRY_CROP:
+		/* remember center of the area */
+		x_center = src->x_offset + src->width / 2;
+		y_center = src->y_offset + src->height / 2;
+
+		/* ensure scaling is between 0.25x .. 16x */
+		src->width = clamp(src->width, round_up(dst->width / 16, 4),
+			dst->width * 4);
+		src->height = clamp(src->height, round_up(dst->height / 16, 4),
+			dst->height * 4);
+
+		/* hardware limits */
+		src->width = clamp(src->width, 32U, 2047U);
+		src->height = clamp(src->height, 4U, 2047U);
+
+		/* setup offsets */
+		src->x_offset = do_center(x_center, src->width,
+			src->full_width, flags);
+		src->y_offset = do_center(y_center, src->height,
+			src->full_height, flags);
+
+		/* setting scaling ratio */
+		geo->x_ratio = (src->width << 16) / dst->width;
+		geo->y_ratio = (src->height << 16) / dst->height;
+		/* fall through */
+
+	case MXR_GEOMETRY_SOURCE:
+		src->full_width = clamp(src->full_width,
+			ALIGN(src->width + src->x_offset, 8), 8192U);
+		src->full_height = clamp(src->full_height,
+			src->height + src->y_offset, 8192U);
+	}
+}
+
+/* PUBLIC API */
+
+struct mxr_layer *mxr_vp_layer_create(struct mxr_device *mdev, int idx)
+{
+	struct mxr_layer *layer;
+	int ret;
+	struct mxr_layer_ops ops = {
+		.release = mxr_vp_layer_release,
+		.buffer_set = mxr_vp_buffer_set,
+		.stream_set = mxr_vp_stream_set,
+		.format_set = mxr_vp_format_set,
+		.fix_geometry = mxr_vp_fix_geometry,
+	};
+	char name[32];
+
+	sprintf(name, "video%d", idx);
+
+	layer = mxr_base_layer_create(mdev, idx, name, &ops);
+	if (layer == NULL) {
+		mxr_err(mdev, "failed to initialize layer(%d) base\n", idx);
+		goto fail;
+	}
+
+	layer->fmt_array = mxr_video_format;
+	layer->fmt_array_size = ARRAY_SIZE(mxr_video_format);
+
+	ret = mxr_base_layer_register(layer);
+	if (ret)
+		goto fail_layer;
+
+	return layer;
+
+fail_layer:
+	mxr_base_layer_release(layer);
+
+fail:
+	return NULL;
+}
+
diff --git a/drivers/media/platform/s5p-tv/regs-hdmi.h b/drivers/media/platform/s5p-tv/regs-hdmi.h
new file mode 100644
index 0000000..a889d1f
--- /dev/null
+++ b/drivers/media/platform/s5p-tv/regs-hdmi.h
@@ -0,0 +1,146 @@
+/* linux/arch/arm/mach-exynos4/include/mach/regs-hdmi.h
+ *
+ * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
+ * http://www.samsung.com/
+ *
+ * HDMI register header file for Samsung TVOUT driver
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+*/
+
+#ifndef SAMSUNG_REGS_HDMI_H
+#define SAMSUNG_REGS_HDMI_H
+
+/*
+ * Register part
+*/
+
+#define HDMI_CTRL_BASE(x)		((x) + 0x00000000)
+#define HDMI_CORE_BASE(x)		((x) + 0x00010000)
+#define HDMI_TG_BASE(x)			((x) + 0x00050000)
+
+/* Control registers */
+#define HDMI_INTC_CON			HDMI_CTRL_BASE(0x0000)
+#define HDMI_INTC_FLAG			HDMI_CTRL_BASE(0x0004)
+#define HDMI_HPD_STATUS			HDMI_CTRL_BASE(0x000C)
+#define HDMI_PHY_RSTOUT			HDMI_CTRL_BASE(0x0014)
+#define HDMI_PHY_VPLL			HDMI_CTRL_BASE(0x0018)
+#define HDMI_PHY_CMU			HDMI_CTRL_BASE(0x001C)
+#define HDMI_CORE_RSTOUT		HDMI_CTRL_BASE(0x0020)
+
+/* Core registers */
+#define HDMI_CON_0			HDMI_CORE_BASE(0x0000)
+#define HDMI_CON_1			HDMI_CORE_BASE(0x0004)
+#define HDMI_CON_2			HDMI_CORE_BASE(0x0008)
+#define HDMI_SYS_STATUS			HDMI_CORE_BASE(0x0010)
+#define HDMI_PHY_STATUS			HDMI_CORE_BASE(0x0014)
+#define HDMI_STATUS_EN			HDMI_CORE_BASE(0x0020)
+#define HDMI_HPD			HDMI_CORE_BASE(0x0030)
+#define HDMI_MODE_SEL			HDMI_CORE_BASE(0x0040)
+#define HDMI_BLUE_SCREEN_0		HDMI_CORE_BASE(0x0050)
+#define HDMI_BLUE_SCREEN_1		HDMI_CORE_BASE(0x0054)
+#define HDMI_BLUE_SCREEN_2		HDMI_CORE_BASE(0x0058)
+#define HDMI_H_BLANK_0			HDMI_CORE_BASE(0x00A0)
+#define HDMI_H_BLANK_1			HDMI_CORE_BASE(0x00A4)
+#define HDMI_V_BLANK_0			HDMI_CORE_BASE(0x00B0)
+#define HDMI_V_BLANK_1			HDMI_CORE_BASE(0x00B4)
+#define HDMI_V_BLANK_2			HDMI_CORE_BASE(0x00B8)
+#define HDMI_H_V_LINE_0			HDMI_CORE_BASE(0x00C0)
+#define HDMI_H_V_LINE_1			HDMI_CORE_BASE(0x00C4)
+#define HDMI_H_V_LINE_2			HDMI_CORE_BASE(0x00C8)
+#define HDMI_VSYNC_POL			HDMI_CORE_BASE(0x00E4)
+#define HDMI_INT_PRO_MODE		HDMI_CORE_BASE(0x00E8)
+#define HDMI_V_BLANK_F_0		HDMI_CORE_BASE(0x0110)
+#define HDMI_V_BLANK_F_1		HDMI_CORE_BASE(0x0114)
+#define HDMI_V_BLANK_F_2		HDMI_CORE_BASE(0x0118)
+#define HDMI_H_SYNC_GEN_0		HDMI_CORE_BASE(0x0120)
+#define HDMI_H_SYNC_GEN_1		HDMI_CORE_BASE(0x0124)
+#define HDMI_H_SYNC_GEN_2		HDMI_CORE_BASE(0x0128)
+#define HDMI_V_SYNC_GEN_1_0		HDMI_CORE_BASE(0x0130)
+#define HDMI_V_SYNC_GEN_1_1		HDMI_CORE_BASE(0x0134)
+#define HDMI_V_SYNC_GEN_1_2		HDMI_CORE_BASE(0x0138)
+#define HDMI_V_SYNC_GEN_2_0		HDMI_CORE_BASE(0x0140)
+#define HDMI_V_SYNC_GEN_2_1		HDMI_CORE_BASE(0x0144)
+#define HDMI_V_SYNC_GEN_2_2		HDMI_CORE_BASE(0x0148)
+#define HDMI_V_SYNC_GEN_3_0		HDMI_CORE_BASE(0x0150)
+#define HDMI_V_SYNC_GEN_3_1		HDMI_CORE_BASE(0x0154)
+#define HDMI_V_SYNC_GEN_3_2		HDMI_CORE_BASE(0x0158)
+#define HDMI_AVI_CON			HDMI_CORE_BASE(0x0300)
+#define HDMI_AVI_BYTE(n)		HDMI_CORE_BASE(0x0320 + 4 * (n))
+#define	HDMI_DC_CONTROL			HDMI_CORE_BASE(0x05C0)
+#define HDMI_VIDEO_PATTERN_GEN		HDMI_CORE_BASE(0x05C4)
+#define HDMI_HPD_GEN			HDMI_CORE_BASE(0x05C8)
+
+/* Timing generator registers */
+#define HDMI_TG_CMD			HDMI_TG_BASE(0x0000)
+#define HDMI_TG_H_FSZ_L			HDMI_TG_BASE(0x0018)
+#define HDMI_TG_H_FSZ_H			HDMI_TG_BASE(0x001C)
+#define HDMI_TG_HACT_ST_L		HDMI_TG_BASE(0x0020)
+#define HDMI_TG_HACT_ST_H		HDMI_TG_BASE(0x0024)
+#define HDMI_TG_HACT_SZ_L		HDMI_TG_BASE(0x0028)
+#define HDMI_TG_HACT_SZ_H		HDMI_TG_BASE(0x002C)
+#define HDMI_TG_V_FSZ_L			HDMI_TG_BASE(0x0030)
+#define HDMI_TG_V_FSZ_H			HDMI_TG_BASE(0x0034)
+#define HDMI_TG_VSYNC_L			HDMI_TG_BASE(0x0038)
+#define HDMI_TG_VSYNC_H			HDMI_TG_BASE(0x003C)
+#define HDMI_TG_VSYNC2_L		HDMI_TG_BASE(0x0040)
+#define HDMI_TG_VSYNC2_H		HDMI_TG_BASE(0x0044)
+#define HDMI_TG_VACT_ST_L		HDMI_TG_BASE(0x0048)
+#define HDMI_TG_VACT_ST_H		HDMI_TG_BASE(0x004C)
+#define HDMI_TG_VACT_SZ_L		HDMI_TG_BASE(0x0050)
+#define HDMI_TG_VACT_SZ_H		HDMI_TG_BASE(0x0054)
+#define HDMI_TG_FIELD_CHG_L		HDMI_TG_BASE(0x0058)
+#define HDMI_TG_FIELD_CHG_H		HDMI_TG_BASE(0x005C)
+#define HDMI_TG_VACT_ST2_L		HDMI_TG_BASE(0x0060)
+#define HDMI_TG_VACT_ST2_H		HDMI_TG_BASE(0x0064)
+#define HDMI_TG_VSYNC_TOP_HDMI_L	HDMI_TG_BASE(0x0078)
+#define HDMI_TG_VSYNC_TOP_HDMI_H	HDMI_TG_BASE(0x007C)
+#define HDMI_TG_VSYNC_BOT_HDMI_L	HDMI_TG_BASE(0x0080)
+#define HDMI_TG_VSYNC_BOT_HDMI_H	HDMI_TG_BASE(0x0084)
+#define HDMI_TG_FIELD_TOP_HDMI_L	HDMI_TG_BASE(0x0088)
+#define HDMI_TG_FIELD_TOP_HDMI_H	HDMI_TG_BASE(0x008C)
+#define HDMI_TG_FIELD_BOT_HDMI_L	HDMI_TG_BASE(0x0090)
+#define HDMI_TG_FIELD_BOT_HDMI_H	HDMI_TG_BASE(0x0094)
+
+/*
+ * Bit definition part
+ */
+
+/* HDMI_INTC_CON */
+#define HDMI_INTC_EN_GLOBAL		(1 << 6)
+#define HDMI_INTC_EN_HPD_PLUG		(1 << 3)
+#define HDMI_INTC_EN_HPD_UNPLUG		(1 << 2)
+
+/* HDMI_INTC_FLAG */
+#define HDMI_INTC_FLAG_HPD_PLUG		(1 << 3)
+#define HDMI_INTC_FLAG_HPD_UNPLUG	(1 << 2)
+
+/* HDMI_PHY_RSTOUT */
+#define HDMI_PHY_SW_RSTOUT		(1 << 0)
+
+/* HDMI_CORE_RSTOUT */
+#define HDMI_CORE_SW_RSTOUT		(1 << 0)
+
+/* HDMI_CON_0 */
+#define HDMI_BLUE_SCR_EN		(1 << 5)
+#define HDMI_EN				(1 << 0)
+
+/* HDMI_CON_2 */
+#define HDMI_DVI_PERAMBLE_EN		(1 << 5)
+#define HDMI_DVI_BAND_EN		(1 << 1)
+
+/* HDMI_PHY_STATUS */
+#define HDMI_PHY_STATUS_READY		(1 << 0)
+
+/* HDMI_MODE_SEL */
+#define HDMI_MODE_HDMI_EN		(1 << 1)
+#define HDMI_MODE_DVI_EN		(1 << 0)
+#define HDMI_MODE_MASK			(3 << 0)
+
+/* HDMI_TG_CMD */
+#define HDMI_TG_FIELD_EN		(1 << 1)
+#define HDMI_TG_EN			(1 << 0)
+
+#endif /* SAMSUNG_REGS_HDMI_H */
diff --git a/drivers/media/platform/s5p-tv/regs-mixer.h b/drivers/media/platform/s5p-tv/regs-mixer.h
new file mode 100644
index 0000000..158abb4
--- /dev/null
+++ b/drivers/media/platform/s5p-tv/regs-mixer.h
@@ -0,0 +1,122 @@
+/*
+ * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
+ * http://www.samsung.com/
+ *
+ * Mixer register header file for Samsung Mixer driver
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+*/
+#ifndef SAMSUNG_REGS_MIXER_H
+#define SAMSUNG_REGS_MIXER_H
+
+/*
+ * Register part
+ */
+#define MXR_STATUS			0x0000
+#define MXR_CFG				0x0004
+#define MXR_INT_EN			0x0008
+#define MXR_INT_STATUS			0x000C
+#define MXR_LAYER_CFG			0x0010
+#define MXR_VIDEO_CFG			0x0014
+#define MXR_GRAPHIC0_CFG		0x0020
+#define MXR_GRAPHIC0_BASE		0x0024
+#define MXR_GRAPHIC0_SPAN		0x0028
+#define MXR_GRAPHIC0_SXY		0x002C
+#define MXR_GRAPHIC0_WH			0x0030
+#define MXR_GRAPHIC0_DXY		0x0034
+#define MXR_GRAPHIC0_BLANK		0x0038
+#define MXR_GRAPHIC1_CFG		0x0040
+#define MXR_GRAPHIC1_BASE		0x0044
+#define MXR_GRAPHIC1_SPAN		0x0048
+#define MXR_GRAPHIC1_SXY		0x004C
+#define MXR_GRAPHIC1_WH			0x0050
+#define MXR_GRAPHIC1_DXY		0x0054
+#define MXR_GRAPHIC1_BLANK		0x0058
+#define MXR_BG_CFG			0x0060
+#define MXR_BG_COLOR0			0x0064
+#define MXR_BG_COLOR1			0x0068
+#define MXR_BG_COLOR2			0x006C
+
+/* for parametrized access to layer registers */
+#define MXR_GRAPHIC_CFG(i)		(0x0020 + (i) * 0x20)
+#define MXR_GRAPHIC_BASE(i)		(0x0024 + (i) * 0x20)
+#define MXR_GRAPHIC_SPAN(i)		(0x0028 + (i) * 0x20)
+#define MXR_GRAPHIC_SXY(i)		(0x002C + (i) * 0x20)
+#define MXR_GRAPHIC_WH(i)		(0x0030 + (i) * 0x20)
+#define MXR_GRAPHIC_DXY(i)		(0x0034 + (i) * 0x20)
+
+/*
+ * Bit definition part
+ */
+
+/* generates mask for range of bits */
+#define MXR_MASK(high_bit, low_bit) \
+	(((2 << ((high_bit) - (low_bit))) - 1) << (low_bit))
+
+#define MXR_MASK_VAL(val, high_bit, low_bit) \
+	(((val) << (low_bit)) & MXR_MASK(high_bit, low_bit))
+
+/* bits for MXR_STATUS */
+#define MXR_STATUS_16_BURST		(1 << 7)
+#define MXR_STATUS_BURST_MASK		(1 << 7)
+#define MXR_STATUS_SYNC_ENABLE		(1 << 2)
+#define MXR_STATUS_REG_RUN		(1 << 0)
+
+/* bits for MXR_CFG */
+#define MXR_CFG_OUT_YUV444		(0 << 8)
+#define MXR_CFG_OUT_RGB888		(1 << 8)
+#define MXR_CFG_OUT_MASK		(1 << 8)
+#define MXR_CFG_DST_SDO			(0 << 7)
+#define MXR_CFG_DST_HDMI		(1 << 7)
+#define MXR_CFG_DST_MASK		(1 << 7)
+#define MXR_CFG_SCAN_HD_720		(0 << 6)
+#define MXR_CFG_SCAN_HD_1080		(1 << 6)
+#define MXR_CFG_GRP1_ENABLE		(1 << 5)
+#define MXR_CFG_GRP0_ENABLE		(1 << 4)
+#define MXR_CFG_VP_ENABLE		(1 << 3)
+#define MXR_CFG_SCAN_INTERLACE		(0 << 2)
+#define MXR_CFG_SCAN_PROGRASSIVE	(1 << 2)
+#define MXR_CFG_SCAN_NTSC		(0 << 1)
+#define MXR_CFG_SCAN_PAL		(1 << 1)
+#define MXR_CFG_SCAN_SD			(0 << 0)
+#define MXR_CFG_SCAN_HD			(1 << 0)
+#define MXR_CFG_SCAN_MASK		0x47
+
+/* bits for MXR_GRAPHICn_CFG */
+#define MXR_GRP_CFG_COLOR_KEY_DISABLE	(1 << 21)
+#define MXR_GRP_CFG_BLEND_PRE_MUL	(1 << 20)
+#define MXR_GRP_CFG_FORMAT_VAL(x)	MXR_MASK_VAL(x, 11, 8)
+#define MXR_GRP_CFG_FORMAT_MASK		MXR_GRP_CFG_FORMAT_VAL(~0)
+#define MXR_GRP_CFG_ALPHA_VAL(x)	MXR_MASK_VAL(x, 7, 0)
+
+/* bits for MXR_GRAPHICn_WH */
+#define MXR_GRP_WH_H_SCALE(x)		MXR_MASK_VAL(x, 28, 28)
+#define MXR_GRP_WH_V_SCALE(x)		MXR_MASK_VAL(x, 12, 12)
+#define MXR_GRP_WH_WIDTH(x)		MXR_MASK_VAL(x, 26, 16)
+#define MXR_GRP_WH_HEIGHT(x)		MXR_MASK_VAL(x, 10, 0)
+
+/* bits for MXR_GRAPHICn_SXY */
+#define MXR_GRP_SXY_SX(x)		MXR_MASK_VAL(x, 26, 16)
+#define MXR_GRP_SXY_SY(x)		MXR_MASK_VAL(x, 10, 0)
+
+/* bits for MXR_GRAPHICn_DXY */
+#define MXR_GRP_DXY_DX(x)		MXR_MASK_VAL(x, 26, 16)
+#define MXR_GRP_DXY_DY(x)		MXR_MASK_VAL(x, 10, 0)
+
+/* bits for MXR_INT_EN */
+#define MXR_INT_EN_VSYNC		(1 << 11)
+#define MXR_INT_EN_ALL			(0x0f << 8)
+
+/* bit for MXR_INT_STATUS */
+#define MXR_INT_CLEAR_VSYNC		(1 << 11)
+#define MXR_INT_STATUS_VSYNC		(1 << 0)
+
+/* bit for MXR_LAYER_CFG */
+#define MXR_LAYER_CFG_GRP1_VAL(x)	MXR_MASK_VAL(x, 11, 8)
+#define MXR_LAYER_CFG_GRP0_VAL(x)	MXR_MASK_VAL(x, 7, 4)
+#define MXR_LAYER_CFG_VP_VAL(x)		MXR_MASK_VAL(x, 3, 0)
+
+#endif /* SAMSUNG_REGS_MIXER_H */
+
diff --git a/drivers/media/platform/s5p-tv/regs-sdo.h b/drivers/media/platform/s5p-tv/regs-sdo.h
new file mode 100644
index 0000000..6f22fbf
--- /dev/null
+++ b/drivers/media/platform/s5p-tv/regs-sdo.h
@@ -0,0 +1,63 @@
+/* drivers/media/platform/s5p-tv/regs-sdo.h
+ *
+ * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com/
+ *
+ * SDO register description file
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef SAMSUNG_REGS_SDO_H
+#define SAMSUNG_REGS_SDO_H
+
+/*
+ * Register part
+ */
+
+#define SDO_CLKCON			0x0000
+#define SDO_CONFIG			0x0008
+#define SDO_VBI				0x0014
+#define SDO_DAC				0x003C
+#define SDO_CCCON			0x0180
+#define SDO_IRQ				0x0280
+#define SDO_IRQMASK			0x0284
+#define SDO_VERSION			0x03D8
+
+/*
+ * Bit definition part
+ */
+
+/* SDO Clock Control Register (SDO_CLKCON) */
+#define SDO_TVOUT_SW_RESET		(1 << 4)
+#define SDO_TVOUT_CLOCK_READY		(1 << 1)
+#define SDO_TVOUT_CLOCK_ON		(1 << 0)
+
+/* SDO Video Standard Configuration Register (SDO_CONFIG) */
+#define SDO_PROGRESSIVE			(1 << 4)
+#define SDO_NTSC_M			0
+#define SDO_PAL_M			1
+#define SDO_PAL_BGHID			2
+#define SDO_PAL_N			3
+#define SDO_PAL_NC			4
+#define SDO_NTSC_443			8
+#define SDO_PAL_60			9
+#define SDO_STANDARD_MASK		0xf
+
+/* SDO VBI Configuration Register (SDO_VBI) */
+#define SDO_CVBS_WSS_INS		(1 << 14)
+#define SDO_CVBS_CLOSED_CAPTION_MASK	(3 << 12)
+
+/* SDO DAC Configuration Register (SDO_DAC) */
+#define SDO_POWER_ON_DAC		(1 << 0)
+
+/* SDO Color Compensation On/Off Control (SDO_CCCON) */
+#define SDO_COMPENSATION_BHS_ADJ_OFF	(1 << 4)
+#define SDO_COMPENSATION_CVBS_COMP_OFF	(1 << 0)
+
+/* SDO Interrupt Request Register (SDO_IRQ) */
+#define SDO_VSYNC_IRQ_PEND		(1 << 0)
+
+#endif /* SAMSUNG_REGS_SDO_H */
diff --git a/drivers/media/platform/s5p-tv/regs-vp.h b/drivers/media/platform/s5p-tv/regs-vp.h
new file mode 100644
index 0000000..6c63984
--- /dev/null
+++ b/drivers/media/platform/s5p-tv/regs-vp.h
@@ -0,0 +1,88 @@
+/*
+ * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com/
+ *
+ * Video processor register header file for Samsung Mixer driver
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef SAMSUNG_REGS_VP_H
+#define SAMSUNG_REGS_VP_H
+
+/*
+ * Register part
+ */
+
+#define VP_ENABLE			0x0000
+#define VP_SRESET			0x0004
+#define VP_SHADOW_UPDATE		0x0008
+#define VP_FIELD_ID			0x000C
+#define VP_MODE				0x0010
+#define VP_IMG_SIZE_Y			0x0014
+#define VP_IMG_SIZE_C			0x0018
+#define VP_PER_RATE_CTRL		0x001C
+#define VP_TOP_Y_PTR			0x0028
+#define VP_BOT_Y_PTR			0x002C
+#define VP_TOP_C_PTR			0x0030
+#define VP_BOT_C_PTR			0x0034
+#define VP_ENDIAN_MODE			0x03CC
+#define VP_SRC_H_POSITION		0x0044
+#define VP_SRC_V_POSITION		0x0048
+#define VP_SRC_WIDTH			0x004C
+#define VP_SRC_HEIGHT			0x0050
+#define VP_DST_H_POSITION		0x0054
+#define VP_DST_V_POSITION		0x0058
+#define VP_DST_WIDTH			0x005C
+#define VP_DST_HEIGHT			0x0060
+#define VP_H_RATIO			0x0064
+#define VP_V_RATIO			0x0068
+#define VP_POLY8_Y0_LL			0x006C
+#define VP_POLY4_Y0_LL			0x00EC
+#define VP_POLY4_C0_LL			0x012C
+
+/*
+ * Bit definition part
+ */
+
+/* generates mask for range of bits */
+
+#define VP_MASK(high_bit, low_bit) \
+	(((2 << ((high_bit) - (low_bit))) - 1) << (low_bit))
+
+#define VP_MASK_VAL(val, high_bit, low_bit) \
+	(((val) << (low_bit)) & VP_MASK(high_bit, low_bit))
+
+ /* VP_ENABLE */
+#define VP_ENABLE_ON			(1 << 0)
+
+/* VP_SRESET */
+#define VP_SRESET_PROCESSING		(1 << 0)
+
+/* VP_SHADOW_UPDATE */
+#define VP_SHADOW_UPDATE_ENABLE		(1 << 0)
+
+/* VP_MODE */
+#define VP_MODE_NV12			(0 << 6)
+#define VP_MODE_NV21			(1 << 6)
+#define VP_MODE_LINE_SKIP		(1 << 5)
+#define VP_MODE_MEM_LINEAR		(0 << 4)
+#define VP_MODE_MEM_TILED		(1 << 4)
+#define VP_MODE_FMT_MASK		(5 << 4)
+#define VP_MODE_FIELD_ID_AUTO_TOGGLING	(1 << 2)
+#define VP_MODE_2D_IPC			(1 << 1)
+
+/* VP_IMG_SIZE_Y */
+/* VP_IMG_SIZE_C */
+#define VP_IMG_HSIZE(x)			VP_MASK_VAL(x, 29, 16)
+#define VP_IMG_VSIZE(x)			VP_MASK_VAL(x, 13, 0)
+
+/* VP_SRC_H_POSITION */
+#define VP_SRC_H_POSITION_VAL(x)	VP_MASK_VAL(x, 14, 4)
+
+/* VP_ENDIAN_MODE */
+#define VP_ENDIAN_MODE_LITTLE		(1 << 0)
+
+#endif /* SAMSUNG_REGS_VP_H */
diff --git a/drivers/media/platform/s5p-tv/sdo_drv.c b/drivers/media/platform/s5p-tv/sdo_drv.c
new file mode 100644
index 0000000..c75d435
--- /dev/null
+++ b/drivers/media/platform/s5p-tv/sdo_drv.c
@@ -0,0 +1,497 @@
+/*
+ * Samsung Standard Definition Output (SDO) driver
+ *
+ * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
+ *
+ * Tomasz Stanislawski, <t.stanislaws@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published
+ * by the Free Software Foundiation. either version 2 of the License,
+ * or (at your option) any later version
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+
+#include <media/v4l2-subdev.h>
+
+#include "regs-sdo.h"
+
+MODULE_AUTHOR("Tomasz Stanislawski, <t.stanislaws@samsung.com>");
+MODULE_DESCRIPTION("Samsung Standard Definition Output (SDO)");
+MODULE_LICENSE("GPL");
+
+#define SDO_DEFAULT_STD	V4L2_STD_PAL
+
+struct sdo_format {
+	v4l2_std_id id;
+	/* all modes are 720 pixels wide */
+	unsigned int height;
+	unsigned int cookie;
+};
+
+struct sdo_device {
+	/** pointer to device parent */
+	struct device *dev;
+	/** base address of SDO registers */
+	void __iomem *regs;
+	/** SDO interrupt */
+	unsigned int irq;
+	/** DAC source clock */
+	struct clk *sclk_dac;
+	/** DAC clock */
+	struct clk *dac;
+	/** DAC physical interface */
+	struct clk *dacphy;
+	/** clock for control of VPLL */
+	struct clk *fout_vpll;
+	/** vpll rate before sdo stream was on */
+	unsigned long vpll_rate;
+	/** regulator for SDO IP power */
+	struct regulator *vdac;
+	/** regulator for SDO plug detection */
+	struct regulator *vdet;
+	/** subdev used as device interface */
+	struct v4l2_subdev sd;
+	/** current format */
+	const struct sdo_format *fmt;
+};
+
+static inline struct sdo_device *sd_to_sdev(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct sdo_device, sd);
+}
+
+static inline
+void sdo_write_mask(struct sdo_device *sdev, u32 reg_id, u32 value, u32 mask)
+{
+	u32 old = readl(sdev->regs + reg_id);
+	value = (value & mask) | (old & ~mask);
+	writel(value, sdev->regs + reg_id);
+}
+
+static inline
+void sdo_write(struct sdo_device *sdev, u32 reg_id, u32 value)
+{
+	writel(value, sdev->regs + reg_id);
+}
+
+static inline
+u32 sdo_read(struct sdo_device *sdev, u32 reg_id)
+{
+	return readl(sdev->regs + reg_id);
+}
+
+static irqreturn_t sdo_irq_handler(int irq, void *dev_data)
+{
+	struct sdo_device *sdev = dev_data;
+
+	/* clear interrupt */
+	sdo_write_mask(sdev, SDO_IRQ, ~0, SDO_VSYNC_IRQ_PEND);
+	return IRQ_HANDLED;
+}
+
+static void sdo_reg_debug(struct sdo_device *sdev)
+{
+#define DBGREG(reg_id) \
+	dev_info(sdev->dev, #reg_id " = %08x\n", \
+		sdo_read(sdev, reg_id))
+
+	DBGREG(SDO_CLKCON);
+	DBGREG(SDO_CONFIG);
+	DBGREG(SDO_VBI);
+	DBGREG(SDO_DAC);
+	DBGREG(SDO_IRQ);
+	DBGREG(SDO_IRQMASK);
+	DBGREG(SDO_VERSION);
+}
+
+static const struct sdo_format sdo_format[] = {
+	{ V4L2_STD_PAL_N,	.height = 576, .cookie = SDO_PAL_N },
+	{ V4L2_STD_PAL_Nc,	.height = 576, .cookie = SDO_PAL_NC },
+	{ V4L2_STD_PAL_M,	.height = 480, .cookie = SDO_PAL_M },
+	{ V4L2_STD_PAL_60,	.height = 480, .cookie = SDO_PAL_60 },
+	{ V4L2_STD_NTSC_443,	.height = 480, .cookie = SDO_NTSC_443 },
+	{ V4L2_STD_PAL,		.height = 576, .cookie = SDO_PAL_BGHID },
+	{ V4L2_STD_NTSC_M,	.height = 480, .cookie = SDO_NTSC_M },
+};
+
+static const struct sdo_format *sdo_find_format(v4l2_std_id id)
+{
+	int i;
+	for (i = 0; i < ARRAY_SIZE(sdo_format); ++i)
+		if (sdo_format[i].id & id)
+			return &sdo_format[i];
+	return NULL;
+}
+
+static int sdo_g_tvnorms_output(struct v4l2_subdev *sd, v4l2_std_id *std)
+{
+	*std = V4L2_STD_NTSC_M | V4L2_STD_PAL_M | V4L2_STD_PAL |
+		V4L2_STD_PAL_N | V4L2_STD_PAL_Nc |
+		V4L2_STD_NTSC_443 | V4L2_STD_PAL_60;
+	return 0;
+}
+
+static int sdo_s_std_output(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	struct sdo_device *sdev = sd_to_sdev(sd);
+	const struct sdo_format *fmt;
+	fmt = sdo_find_format(std);
+	if (fmt == NULL)
+		return -EINVAL;
+	sdev->fmt = fmt;
+	return 0;
+}
+
+static int sdo_g_std_output(struct v4l2_subdev *sd, v4l2_std_id *std)
+{
+	*std = sd_to_sdev(sd)->fmt->id;
+	return 0;
+}
+
+static int sdo_get_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+	struct sdo_device *sdev = sd_to_sdev(sd);
+
+	if (!sdev->fmt)
+		return -ENXIO;
+	if (format->pad)
+		return -EINVAL;
+	/* all modes are 720 pixels wide */
+	fmt->width = 720;
+	fmt->height = sdev->fmt->height;
+	fmt->code = MEDIA_BUS_FMT_FIXED;
+	fmt->field = V4L2_FIELD_INTERLACED;
+	fmt->colorspace = V4L2_COLORSPACE_JPEG;
+	return 0;
+}
+
+static int sdo_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct sdo_device *sdev = sd_to_sdev(sd);
+	struct device *dev = sdev->dev;
+	int ret;
+
+	dev_info(dev, "sdo_s_power(%d)\n", on);
+
+	if (on)
+		ret = pm_runtime_get_sync(dev);
+	else
+		ret = pm_runtime_put_sync(dev);
+
+	/* only values < 0 indicate errors */
+	return ret < 0 ? ret : 0;
+}
+
+static int sdo_streamon(struct sdo_device *sdev)
+{
+	int ret;
+
+	/* set proper clock for Timing Generator */
+	sdev->vpll_rate = clk_get_rate(sdev->fout_vpll);
+	ret = clk_set_rate(sdev->fout_vpll, 54000000);
+	if (ret < 0) {
+		dev_err(sdev->dev, "Failed to set vpll rate\n");
+		return ret;
+	}
+	dev_info(sdev->dev, "fout_vpll.rate = %lu\n",
+	clk_get_rate(sdev->fout_vpll));
+	/* enable clock in SDO */
+	sdo_write_mask(sdev, SDO_CLKCON, ~0, SDO_TVOUT_CLOCK_ON);
+	ret = clk_prepare_enable(sdev->dacphy);
+	if (ret < 0) {
+		dev_err(sdev->dev, "clk_prepare_enable(dacphy) failed\n");
+		goto fail;
+	}
+	/* enable DAC */
+	sdo_write_mask(sdev, SDO_DAC, ~0, SDO_POWER_ON_DAC);
+	sdo_reg_debug(sdev);
+	return 0;
+
+fail:
+	sdo_write_mask(sdev, SDO_CLKCON, 0, SDO_TVOUT_CLOCK_ON);
+	clk_set_rate(sdev->fout_vpll, sdev->vpll_rate);
+	return ret;
+}
+
+static int sdo_streamoff(struct sdo_device *sdev)
+{
+	int tries;
+
+	sdo_write_mask(sdev, SDO_DAC, 0, SDO_POWER_ON_DAC);
+	clk_disable_unprepare(sdev->dacphy);
+	sdo_write_mask(sdev, SDO_CLKCON, 0, SDO_TVOUT_CLOCK_ON);
+	for (tries = 100; tries; --tries) {
+		if (sdo_read(sdev, SDO_CLKCON) & SDO_TVOUT_CLOCK_READY)
+			break;
+		mdelay(1);
+	}
+	if (tries == 0)
+		dev_err(sdev->dev, "failed to stop streaming\n");
+	clk_set_rate(sdev->fout_vpll, sdev->vpll_rate);
+	return tries ? 0 : -EIO;
+}
+
+static int sdo_s_stream(struct v4l2_subdev *sd, int on)
+{
+	struct sdo_device *sdev = sd_to_sdev(sd);
+	return on ? sdo_streamon(sdev) : sdo_streamoff(sdev);
+}
+
+static const struct v4l2_subdev_core_ops sdo_sd_core_ops = {
+	.s_power = sdo_s_power,
+};
+
+static const struct v4l2_subdev_video_ops sdo_sd_video_ops = {
+	.s_std_output = sdo_s_std_output,
+	.g_std_output = sdo_g_std_output,
+	.g_tvnorms_output = sdo_g_tvnorms_output,
+	.s_stream = sdo_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops sdo_sd_pad_ops = {
+	.get_fmt = sdo_get_fmt,
+};
+
+static const struct v4l2_subdev_ops sdo_sd_ops = {
+	.core = &sdo_sd_core_ops,
+	.video = &sdo_sd_video_ops,
+	.pad = &sdo_sd_pad_ops,
+};
+
+static int sdo_runtime_suspend(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct sdo_device *sdev = sd_to_sdev(sd);
+
+	dev_info(dev, "suspend\n");
+	regulator_disable(sdev->vdet);
+	regulator_disable(sdev->vdac);
+	clk_disable_unprepare(sdev->sclk_dac);
+	return 0;
+}
+
+static int sdo_runtime_resume(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct sdo_device *sdev = sd_to_sdev(sd);
+	int ret;
+
+	dev_info(dev, "resume\n");
+
+	ret = clk_prepare_enable(sdev->sclk_dac);
+	if (ret < 0)
+		return ret;
+
+	ret = regulator_enable(sdev->vdac);
+	if (ret < 0)
+		goto dac_clk_dis;
+
+	ret = regulator_enable(sdev->vdet);
+	if (ret < 0)
+		goto vdac_r_dis;
+
+	/* software reset */
+	sdo_write_mask(sdev, SDO_CLKCON, ~0, SDO_TVOUT_SW_RESET);
+	mdelay(10);
+	sdo_write_mask(sdev, SDO_CLKCON, 0, SDO_TVOUT_SW_RESET);
+
+	/* setting TV mode */
+	sdo_write_mask(sdev, SDO_CONFIG, sdev->fmt->cookie, SDO_STANDARD_MASK);
+	/* XXX: forcing interlaced mode using undocumented bit */
+	sdo_write_mask(sdev, SDO_CONFIG, 0, SDO_PROGRESSIVE);
+	/* turn all VBI off */
+	sdo_write_mask(sdev, SDO_VBI, 0, SDO_CVBS_WSS_INS |
+		SDO_CVBS_CLOSED_CAPTION_MASK);
+	/* turn all post processing off */
+	sdo_write_mask(sdev, SDO_CCCON, ~0, SDO_COMPENSATION_BHS_ADJ_OFF |
+		SDO_COMPENSATION_CVBS_COMP_OFF);
+	sdo_reg_debug(sdev);
+	return 0;
+
+vdac_r_dis:
+	regulator_disable(sdev->vdac);
+dac_clk_dis:
+	clk_disable_unprepare(sdev->sclk_dac);
+	return ret;
+}
+
+static const struct dev_pm_ops sdo_pm_ops = {
+	.runtime_suspend = sdo_runtime_suspend,
+	.runtime_resume	 = sdo_runtime_resume,
+};
+
+static int sdo_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct sdo_device *sdev;
+	struct resource *res;
+	int ret = 0;
+	struct clk *sclk_vpll;
+
+	dev_info(dev, "probe start\n");
+	sdev = devm_kzalloc(&pdev->dev, sizeof(*sdev), GFP_KERNEL);
+	if (!sdev) {
+		dev_err(dev, "not enough memory.\n");
+		ret = -ENOMEM;
+		goto fail;
+	}
+	sdev->dev = dev;
+
+	/* mapping registers */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res == NULL) {
+		dev_err(dev, "get memory resource failed.\n");
+		ret = -ENXIO;
+		goto fail;
+	}
+
+	sdev->regs = devm_ioremap(&pdev->dev, res->start, resource_size(res));
+	if (sdev->regs == NULL) {
+		dev_err(dev, "register mapping failed.\n");
+		ret = -ENXIO;
+		goto fail;
+	}
+
+	/* acquiring interrupt */
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (res == NULL) {
+		dev_err(dev, "get interrupt resource failed.\n");
+		ret = -ENXIO;
+		goto fail;
+	}
+	ret = devm_request_irq(&pdev->dev, res->start, sdo_irq_handler, 0,
+			       "s5p-sdo", sdev);
+	if (ret) {
+		dev_err(dev, "request interrupt failed.\n");
+		goto fail;
+	}
+	sdev->irq = res->start;
+
+	/* acquire clocks */
+	sdev->sclk_dac = clk_get(dev, "sclk_dac");
+	if (IS_ERR(sdev->sclk_dac)) {
+		dev_err(dev, "failed to get clock 'sclk_dac'\n");
+		ret = PTR_ERR(sdev->sclk_dac);
+		goto fail;
+	}
+	sdev->dac = clk_get(dev, "dac");
+	if (IS_ERR(sdev->dac)) {
+		dev_err(dev, "failed to get clock 'dac'\n");
+		ret = PTR_ERR(sdev->dac);
+		goto fail_sclk_dac;
+	}
+	sdev->dacphy = clk_get(dev, "dacphy");
+	if (IS_ERR(sdev->dacphy)) {
+		dev_err(dev, "failed to get clock 'dacphy'\n");
+		ret = PTR_ERR(sdev->dacphy);
+		goto fail_dac;
+	}
+	sclk_vpll = clk_get(dev, "sclk_vpll");
+	if (IS_ERR(sclk_vpll)) {
+		dev_err(dev, "failed to get clock 'sclk_vpll'\n");
+		ret = PTR_ERR(sclk_vpll);
+		goto fail_dacphy;
+	}
+	clk_set_parent(sdev->sclk_dac, sclk_vpll);
+	clk_put(sclk_vpll);
+	sdev->fout_vpll = clk_get(dev, "fout_vpll");
+	if (IS_ERR(sdev->fout_vpll)) {
+		dev_err(dev, "failed to get clock 'fout_vpll'\n");
+		ret = PTR_ERR(sdev->fout_vpll);
+		goto fail_dacphy;
+	}
+	dev_info(dev, "fout_vpll.rate = %lu\n", clk_get_rate(sclk_vpll));
+
+	/* acquire regulator */
+	sdev->vdac = devm_regulator_get(dev, "vdd33a_dac");
+	if (IS_ERR(sdev->vdac)) {
+		dev_err(dev, "failed to get regulator 'vdac'\n");
+		ret = PTR_ERR(sdev->vdac);
+		goto fail_fout_vpll;
+	}
+	sdev->vdet = devm_regulator_get(dev, "vdet");
+	if (IS_ERR(sdev->vdet)) {
+		dev_err(dev, "failed to get regulator 'vdet'\n");
+		ret = PTR_ERR(sdev->vdet);
+		goto fail_fout_vpll;
+	}
+
+	/* enable gate for dac clock, because mixer uses it */
+	ret = clk_prepare_enable(sdev->dac);
+	if (ret < 0) {
+		dev_err(dev, "clk_prepare_enable(dac) failed\n");
+		goto fail_fout_vpll;
+	}
+
+	/* configure power management */
+	pm_runtime_enable(dev);
+
+	/* configuration of interface subdevice */
+	v4l2_subdev_init(&sdev->sd, &sdo_sd_ops);
+	sdev->sd.owner = THIS_MODULE;
+	strlcpy(sdev->sd.name, "s5p-sdo", sizeof(sdev->sd.name));
+
+	/* set default format */
+	sdev->fmt = sdo_find_format(SDO_DEFAULT_STD);
+	BUG_ON(sdev->fmt == NULL);
+
+	/* keeping subdev in device's private for use by other drivers */
+	dev_set_drvdata(dev, &sdev->sd);
+
+	dev_info(dev, "probe succeeded\n");
+	return 0;
+
+fail_fout_vpll:
+	clk_put(sdev->fout_vpll);
+fail_dacphy:
+	clk_put(sdev->dacphy);
+fail_dac:
+	clk_put(sdev->dac);
+fail_sclk_dac:
+	clk_put(sdev->sclk_dac);
+fail:
+	dev_info(dev, "probe failed\n");
+	return ret;
+}
+
+static int sdo_remove(struct platform_device *pdev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(&pdev->dev);
+	struct sdo_device *sdev = sd_to_sdev(sd);
+
+	pm_runtime_disable(&pdev->dev);
+	clk_disable_unprepare(sdev->dac);
+	clk_put(sdev->fout_vpll);
+	clk_put(sdev->dacphy);
+	clk_put(sdev->dac);
+	clk_put(sdev->sclk_dac);
+
+	dev_info(&pdev->dev, "remove successful\n");
+	return 0;
+}
+
+static struct platform_driver sdo_driver __refdata = {
+	.probe = sdo_probe,
+	.remove = sdo_remove,
+	.driver = {
+		.name = "s5p-sdo",
+		.pm = &sdo_pm_ops,
+	}
+};
+
+module_platform_driver(sdo_driver);
diff --git a/drivers/media/platform/s5p-tv/sii9234_drv.c b/drivers/media/platform/s5p-tv/sii9234_drv.c
new file mode 100644
index 0000000..8d17131
--- /dev/null
+++ b/drivers/media/platform/s5p-tv/sii9234_drv.c
@@ -0,0 +1,407 @@
+/*
+ * Samsung MHL interface driver
+ *
+ * Copyright (C) 2011 Samsung Electronics Co.Ltd
+ * Author: Tomasz Stanislawski <t.stanislaws@samsung.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ */
+
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/freezer.h>
+#include <linux/gpio.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/kthread.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <linux/regulator/machine.h>
+#include <linux/slab.h>
+
+#include <media/sii9234.h>
+#include <media/v4l2-subdev.h>
+
+MODULE_AUTHOR("Tomasz Stanislawski <t.stanislaws@samsung.com>");
+MODULE_DESCRIPTION("Samsung MHL interface driver");
+MODULE_LICENSE("GPL");
+
+struct sii9234_context {
+	struct i2c_client *client;
+	struct regulator *power;
+	int gpio_n_reset;
+	struct v4l2_subdev sd;
+};
+
+static inline struct sii9234_context *sd_to_context(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct sii9234_context, sd);
+}
+
+static inline int sii9234_readb(struct i2c_client *client, int addr)
+{
+	return i2c_smbus_read_byte_data(client, addr);
+}
+
+static inline int sii9234_writeb(struct i2c_client *client, int addr, int value)
+{
+	return i2c_smbus_write_byte_data(client, addr, value);
+}
+
+static inline int sii9234_writeb_mask(struct i2c_client *client, int addr,
+	int value, int mask)
+{
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(client, addr);
+	if (ret < 0)
+		return ret;
+	ret = (ret & ~mask) | (value & mask);
+	return i2c_smbus_write_byte_data(client, addr, ret);
+}
+
+static inline int sii9234_readb_idx(struct i2c_client *client, int addr)
+{
+	int ret;
+	ret = i2c_smbus_write_byte_data(client, 0xbc, addr >> 8);
+	if (ret < 0)
+		return ret;
+	ret = i2c_smbus_write_byte_data(client, 0xbd, addr & 0xff);
+	if (ret < 0)
+		return ret;
+	return i2c_smbus_read_byte_data(client, 0xbe);
+}
+
+static inline int sii9234_writeb_idx(struct i2c_client *client, int addr,
+	int value)
+{
+	int ret;
+	ret = i2c_smbus_write_byte_data(client, 0xbc, addr >> 8);
+	if (ret < 0)
+		return ret;
+	ret = i2c_smbus_write_byte_data(client, 0xbd, addr & 0xff);
+	if (ret < 0)
+		return ret;
+	ret = i2c_smbus_write_byte_data(client, 0xbe, value);
+	return ret;
+}
+
+static inline int sii9234_writeb_idx_mask(struct i2c_client *client, int addr,
+	int value, int mask)
+{
+	int ret;
+
+	ret = sii9234_readb_idx(client, addr);
+	if (ret < 0)
+		return ret;
+	ret = (ret & ~mask) | (value & mask);
+	return sii9234_writeb_idx(client, addr, ret);
+}
+
+static int sii9234_reset(struct sii9234_context *ctx)
+{
+	struct i2c_client *client = ctx->client;
+	struct device *dev = &client->dev;
+	int ret, tries;
+
+	gpio_direction_output(ctx->gpio_n_reset, 1);
+	mdelay(1);
+	gpio_direction_output(ctx->gpio_n_reset, 0);
+	mdelay(1);
+	gpio_direction_output(ctx->gpio_n_reset, 1);
+	mdelay(1);
+
+	/* going to TTPI mode */
+	ret = sii9234_writeb(client, 0xc7, 0);
+	if (ret < 0) {
+		dev_err(dev, "failed to set TTPI mode\n");
+		return ret;
+	}
+	for (tries = 0; tries < 100 ; ++tries) {
+		ret = sii9234_readb(client, 0x1b);
+		if (ret > 0)
+			break;
+		if (ret < 0) {
+			dev_err(dev, "failed to reset device\n");
+			return -EIO;
+		}
+		mdelay(1);
+	}
+	if (tries == 100) {
+		dev_err(dev, "maximal number of tries reached\n");
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int sii9234_verify_version(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	int family, rev, tpi_rev, dev_id, sub_id, hdcp, id;
+
+	family = sii9234_readb(client, 0x1b);
+	rev = sii9234_readb(client, 0x1c) & 0x0f;
+	tpi_rev = sii9234_readb(client, 0x1d) & 0x7f;
+	dev_id = sii9234_readb_idx(client, 0x0103);
+	sub_id = sii9234_readb_idx(client, 0x0102);
+	hdcp = sii9234_readb(client, 0x30);
+
+	if (family < 0 || rev < 0 || tpi_rev < 0 || dev_id < 0 ||
+		sub_id < 0 || hdcp < 0) {
+		dev_err(dev, "failed to read chip's version\n");
+		return -EIO;
+	}
+
+	id = (dev_id << 8) | sub_id;
+
+	dev_info(dev, "chip: SiL%02x family: %02x, rev: %02x\n",
+		id, family, rev);
+	dev_info(dev, "tpi_rev:%02x, hdcp: %02x\n", tpi_rev, hdcp);
+	if (id != 0x9234) {
+		dev_err(dev, "not supported chip\n");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static u8 data[][3] = {
+/* setup from driver created by doonsoo45.kim */
+	{ 0x01, 0x05, 0x04 }, /* Enable Auto soft reset on SCDT = 0 */
+	{ 0x01, 0x08, 0x35 }, /* Power Up TMDS Tx Core */
+	{ 0x01, 0x0d, 0x1c }, /* HDMI Transcode mode enable */
+	{ 0x01, 0x2b, 0x01 }, /* Enable HDCP Compliance workaround */
+	{ 0x01, 0x79, 0x40 }, /* daniel test...MHL_INT */
+	{ 0x01, 0x80, 0x34 }, /* Enable Rx PLL Clock Value */
+	{ 0x01, 0x90, 0x27 }, /* Enable CBUS discovery */
+	{ 0x01, 0x91, 0xe5 }, /* Skip RGND detection */
+	{ 0x01, 0x92, 0x46 }, /* Force MHD mode */
+	{ 0x01, 0x93, 0xdc }, /* Disable CBUS pull-up during RGND measurement */
+	{ 0x01, 0x94, 0x66 }, /* 1.8V CBUS VTH & GND threshold */
+	{ 0x01, 0x95, 0x31 }, /* RGND block & single discovery attempt */
+	{ 0x01, 0x96, 0x22 }, /* use 1K and 2K setting */
+	{ 0x01, 0xa0, 0x10 }, /* SIMG: Term mode */
+	{ 0x01, 0xa1, 0xfc }, /* Disable internal Mobile HD driver */
+	{ 0x01, 0xa3, 0xfa }, /* SIMG: Output Swing  default EB, 3x Clk Mult */
+	{ 0x01, 0xa5, 0x80 }, /* SIMG: RGND Hysterisis, 3x mode for Beast */
+	{ 0x01, 0xa6, 0x0c }, /* SIMG: Swing Offset */
+	{ 0x02, 0x3d, 0x3f }, /* Power up CVCC 1.2V core */
+	{ 0x03, 0x00, 0x00 }, /* SIMG: correcting HW default */
+	{ 0x03, 0x11, 0x01 }, /* Enable TxPLL Clock */
+	{ 0x03, 0x12, 0x15 }, /* Enable Tx Clock Path & Equalizer */
+	{ 0x03, 0x13, 0x60 }, /* SIMG: Set termination value */
+	{ 0x03, 0x14, 0xf0 }, /* SIMG: Change CKDT level */
+	{ 0x03, 0x17, 0x07 }, /* SIMG: PLL Calrefsel */
+	{ 0x03, 0x1a, 0x20 }, /* VCO Cal */
+	{ 0x03, 0x22, 0xe0 }, /* SIMG: Auto EQ */
+	{ 0x03, 0x23, 0xc0 }, /* SIMG: Auto EQ */
+	{ 0x03, 0x24, 0xa0 }, /* SIMG: Auto EQ */
+	{ 0x03, 0x25, 0x80 }, /* SIMG: Auto EQ */
+	{ 0x03, 0x26, 0x60 }, /* SIMG: Auto EQ */
+	{ 0x03, 0x27, 0x40 }, /* SIMG: Auto EQ */
+	{ 0x03, 0x28, 0x20 }, /* SIMG: Auto EQ */
+	{ 0x03, 0x29, 0x00 }, /* SIMG: Auto EQ */
+	{ 0x03, 0x31, 0x0b }, /* SIMG: Rx PLL BW value from I2C BW ~ 4MHz */
+	{ 0x03, 0x45, 0x06 }, /* SIMG: DPLL Mode */
+	{ 0x03, 0x4b, 0x06 }, /* SIMG: Correcting HW default */
+	{ 0x03, 0x4c, 0xa0 }, /* Manual zone control */
+	{ 0x03, 0x4d, 0x02 }, /* SIMG: PLL Mode Value (order is important) */
+};
+
+static int sii9234_set_internal(struct sii9234_context *ctx)
+{
+	struct i2c_client *client = ctx->client;
+	int i, ret;
+
+	for (i = 0; i < ARRAY_SIZE(data); ++i) {
+		int addr = (data[i][0] << 8) | data[i][1];
+		ret = sii9234_writeb_idx(client, addr, data[i][2]);
+		if (ret < 0)
+			return ret;
+	}
+	return 0;
+}
+
+static int sii9234_runtime_suspend(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct sii9234_context *ctx = sd_to_context(sd);
+	struct i2c_client *client = ctx->client;
+
+	dev_info(dev, "suspend start\n");
+
+	sii9234_writeb_mask(client, 0x1e, 3, 3);
+	regulator_disable(ctx->power);
+
+	return 0;
+}
+
+static int sii9234_runtime_resume(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct sii9234_context *ctx = sd_to_context(sd);
+	struct i2c_client *client = ctx->client;
+	int ret;
+
+	dev_info(dev, "resume start\n");
+	ret = regulator_enable(ctx->power);
+	if (ret < 0)
+		return ret;
+
+	ret = sii9234_reset(ctx);
+	if (ret)
+		goto fail;
+
+	/* enable tpi */
+	ret = sii9234_writeb_mask(client, 0x1e, 1, 0);
+	if (ret < 0)
+		goto fail;
+	ret = sii9234_set_internal(ctx);
+	if (ret < 0)
+		goto fail;
+
+	return 0;
+
+fail:
+	dev_err(dev, "failed to resume\n");
+	regulator_disable(ctx->power);
+
+	return ret;
+}
+
+static const struct dev_pm_ops sii9234_pm_ops = {
+	.runtime_suspend = sii9234_runtime_suspend,
+	.runtime_resume	 = sii9234_runtime_resume,
+};
+
+static int sii9234_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct sii9234_context *ctx = sd_to_context(sd);
+	int ret;
+
+	if (on)
+		ret = pm_runtime_get_sync(&ctx->client->dev);
+	else
+		ret = pm_runtime_put(&ctx->client->dev);
+	/* only values < 0 indicate errors */
+	return ret < 0 ? ret : 0;
+}
+
+static int sii9234_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct sii9234_context *ctx = sd_to_context(sd);
+
+	/* (dis/en)able TDMS output */
+	sii9234_writeb_mask(ctx->client, 0x1a, enable ? 0 : ~0 , 1 << 4);
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops sii9234_core_ops = {
+	.s_power =  sii9234_s_power,
+};
+
+static const struct v4l2_subdev_video_ops sii9234_video_ops = {
+	.s_stream =  sii9234_s_stream,
+};
+
+static const struct v4l2_subdev_ops sii9234_ops = {
+	.core = &sii9234_core_ops,
+	.video = &sii9234_video_ops,
+};
+
+static int sii9234_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	struct device *dev = &client->dev;
+	struct sii9234_platform_data *pdata = dev->platform_data;
+	struct sii9234_context *ctx;
+	int ret;
+
+	ctx = devm_kzalloc(&client->dev, sizeof(*ctx), GFP_KERNEL);
+	if (!ctx) {
+		dev_err(dev, "out of memory\n");
+		ret = -ENOMEM;
+		goto fail;
+	}
+	ctx->client = client;
+
+	ctx->power = devm_regulator_get(dev, "hdmi-en");
+	if (IS_ERR(ctx->power)) {
+		dev_err(dev, "failed to acquire regulator hdmi-en\n");
+		return PTR_ERR(ctx->power);
+	}
+
+	ctx->gpio_n_reset = pdata->gpio_n_reset;
+	ret = devm_gpio_request(dev, ctx->gpio_n_reset, "MHL_RST");
+	if (ret) {
+		dev_err(dev, "failed to acquire MHL_RST gpio\n");
+		return ret;
+	}
+
+	v4l2_i2c_subdev_init(&ctx->sd, client, &sii9234_ops);
+
+	pm_runtime_enable(dev);
+
+	/* enable device */
+	ret = pm_runtime_get_sync(dev);
+	if (ret)
+		goto fail_pm;
+
+	/* verify chip version */
+	ret = sii9234_verify_version(client);
+	if (ret)
+		goto fail_pm_get;
+
+	/* stop processing */
+	pm_runtime_put(dev);
+
+	dev_info(dev, "probe successful\n");
+
+	return 0;
+
+fail_pm_get:
+	pm_runtime_put_sync(dev);
+
+fail_pm:
+	pm_runtime_disable(dev);
+
+fail:
+	dev_err(dev, "probe failed\n");
+
+	return ret;
+}
+
+static int sii9234_remove(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+
+	pm_runtime_disable(dev);
+
+	dev_info(dev, "remove successful\n");
+
+	return 0;
+}
+
+
+static const struct i2c_device_id sii9234_id[] = {
+	{ "SII9234", 0 },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(i2c, sii9234_id);
+static struct i2c_driver sii9234_driver = {
+	.driver = {
+		.name	= "sii9234",
+		.pm = &sii9234_pm_ops,
+	},
+	.probe		= sii9234_probe,
+	.remove		= sii9234_remove,
+	.id_table = sii9234_id,
+};
+
+module_i2c_driver(sii9234_driver);
diff --git a/drivers/media/platform/sh_veu.c b/drivers/media/platform/sh_veu.c
new file mode 100644
index 0000000..d6ab33e
--- /dev/null
+++ b/drivers/media/platform/sh_veu.c
@@ -0,0 +1,1235 @@
+/*
+ * sh-mobile VEU mem2mem driver
+ *
+ * Copyright (C) 2012 Renesas Electronics Corporation
+ * Author: Guennadi Liakhovetski, <g.liakhovetski@gmx.de>
+ * Copyright (C) 2008 Magnus Damm
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the version 2 of the GNU General Public License as
+ * published by the Free Software Foundation
+ */
+
+#include <linux/err.h>
+#include <linux/fs.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <linux/videodev2.h>
+
+#include <media/v4l2-dev.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/v4l2-image-sizes.h>
+#include <media/videobuf2-dma-contig.h>
+
+#define VEU_STR 0x00 /* start register */
+#define VEU_SWR 0x10 /* src: line length */
+#define VEU_SSR 0x14 /* src: image size */
+#define VEU_SAYR 0x18 /* src: y/rgb plane address */
+#define VEU_SACR 0x1c /* src: c plane address */
+#define VEU_BSSR 0x20 /* bundle mode register */
+#define VEU_EDWR 0x30 /* dst: line length */
+#define VEU_DAYR 0x34 /* dst: y/rgb plane address */
+#define VEU_DACR 0x38 /* dst: c plane address */
+#define VEU_TRCR 0x50 /* transform control */
+#define VEU_RFCR 0x54 /* resize scale */
+#define VEU_RFSR 0x58 /* resize clip */
+#define VEU_ENHR 0x5c /* enhance */
+#define VEU_FMCR 0x70 /* filter mode */
+#define VEU_VTCR 0x74 /* lowpass vertical */
+#define VEU_HTCR 0x78 /* lowpass horizontal */
+#define VEU_APCR 0x80 /* color match */
+#define VEU_ECCR 0x84 /* color replace */
+#define VEU_AFXR 0x90 /* fixed mode */
+#define VEU_SWPR 0x94 /* swap */
+#define VEU_EIER 0xa0 /* interrupt mask */
+#define VEU_EVTR 0xa4 /* interrupt event */
+#define VEU_STAR 0xb0 /* status */
+#define VEU_BSRR 0xb4 /* reset */
+
+#define VEU_MCR00 0x200 /* color conversion matrix coefficient 00 */
+#define VEU_MCR01 0x204 /* color conversion matrix coefficient 01 */
+#define VEU_MCR02 0x208 /* color conversion matrix coefficient 02 */
+#define VEU_MCR10 0x20c /* color conversion matrix coefficient 10 */
+#define VEU_MCR11 0x210 /* color conversion matrix coefficient 11 */
+#define VEU_MCR12 0x214 /* color conversion matrix coefficient 12 */
+#define VEU_MCR20 0x218 /* color conversion matrix coefficient 20 */
+#define VEU_MCR21 0x21c /* color conversion matrix coefficient 21 */
+#define VEU_MCR22 0x220 /* color conversion matrix coefficient 22 */
+#define VEU_COFFR 0x224 /* color conversion offset */
+#define VEU_CBR   0x228 /* color conversion clip */
+
+/*
+ * 4092x4092 max size is the normal case. In some cases it can be reduced to
+ * 2048x2048, in other cases it can be 4092x8188 or even 8188x8188.
+ */
+#define MAX_W 4092
+#define MAX_H 4092
+#define MIN_W 8
+#define MIN_H 8
+#define ALIGN_W 4
+
+/* 3 buffers of 2048 x 1536 - 3 megapixels @ 16bpp */
+#define VIDEO_MEM_LIMIT ALIGN(2048 * 1536 * 2 * 3, 1024 * 1024)
+
+#define MEM2MEM_DEF_TRANSLEN 1
+
+struct sh_veu_dev;
+
+struct sh_veu_file {
+	struct sh_veu_dev *veu_dev;
+	bool cfg_needed;
+};
+
+struct sh_veu_format {
+	char *name;
+	u32 fourcc;
+	unsigned int depth;
+	unsigned int ydepth;
+};
+
+/* video data format */
+struct sh_veu_vfmt {
+	/* Replace with v4l2_rect */
+	struct v4l2_rect		frame;
+	unsigned int			bytesperline;
+	unsigned int			offset_y;
+	unsigned int			offset_c;
+	const struct sh_veu_format	*fmt;
+};
+
+struct sh_veu_dev {
+	struct v4l2_device v4l2_dev;
+	struct video_device vdev;
+	struct v4l2_m2m_dev *m2m_dev;
+	struct device *dev;
+	struct v4l2_m2m_ctx *m2m_ctx;
+	struct sh_veu_vfmt vfmt_out;
+	struct sh_veu_vfmt vfmt_in;
+	/* Only single user per direction so far */
+	struct sh_veu_file *capture;
+	struct sh_veu_file *output;
+	struct mutex fop_lock;
+	void __iomem *base;
+	struct vb2_alloc_ctx *alloc_ctx;
+	spinlock_t lock;
+	bool is_2h;
+	unsigned int xaction;
+	bool aborting;
+};
+
+enum sh_veu_fmt_idx {
+	SH_VEU_FMT_NV12,
+	SH_VEU_FMT_NV16,
+	SH_VEU_FMT_NV24,
+	SH_VEU_FMT_RGB332,
+	SH_VEU_FMT_RGB444,
+	SH_VEU_FMT_RGB565,
+	SH_VEU_FMT_RGB666,
+	SH_VEU_FMT_RGB24,
+};
+
+#define DEFAULT_IN_WIDTH	VGA_WIDTH
+#define DEFAULT_IN_HEIGHT	VGA_HEIGHT
+#define DEFAULT_IN_FMTIDX	SH_VEU_FMT_NV12
+#define DEFAULT_OUT_WIDTH	VGA_WIDTH
+#define DEFAULT_OUT_HEIGHT	VGA_HEIGHT
+#define DEFAULT_OUT_FMTIDX	SH_VEU_FMT_RGB565
+
+/*
+ * Alignment: Y-plane should be 4-byte aligned for NV12 and NV16, and 8-byte
+ * aligned for NV24.
+ */
+static const struct sh_veu_format sh_veu_fmt[] = {
+	[SH_VEU_FMT_NV12]   = { .ydepth = 8, .depth = 12, .name = "NV12", .fourcc = V4L2_PIX_FMT_NV12 },
+	[SH_VEU_FMT_NV16]   = { .ydepth = 8, .depth = 16, .name = "NV16", .fourcc = V4L2_PIX_FMT_NV16 },
+	[SH_VEU_FMT_NV24]   = { .ydepth = 8, .depth = 24, .name = "NV24", .fourcc = V4L2_PIX_FMT_NV24 },
+	[SH_VEU_FMT_RGB332] = { .ydepth = 8, .depth = 8, .name = "RGB332", .fourcc = V4L2_PIX_FMT_RGB332 },
+	[SH_VEU_FMT_RGB444] = { .ydepth = 16, .depth = 16, .name = "RGB444", .fourcc = V4L2_PIX_FMT_RGB444 },
+	[SH_VEU_FMT_RGB565] = { .ydepth = 16, .depth = 16, .name = "RGB565", .fourcc = V4L2_PIX_FMT_RGB565 },
+	[SH_VEU_FMT_RGB666] = { .ydepth = 32, .depth = 32, .name = "BGR666", .fourcc = V4L2_PIX_FMT_BGR666 },
+	[SH_VEU_FMT_RGB24]  = { .ydepth = 24, .depth = 24, .name = "RGB24", .fourcc = V4L2_PIX_FMT_RGB24 },
+};
+
+#define DEFAULT_IN_VFMT (struct sh_veu_vfmt){						\
+	.frame = {									\
+		.width = VGA_WIDTH,							\
+		.height = VGA_HEIGHT,							\
+	},										\
+	.bytesperline = (VGA_WIDTH * sh_veu_fmt[DEFAULT_IN_FMTIDX].ydepth) >> 3,	\
+	.fmt = &sh_veu_fmt[DEFAULT_IN_FMTIDX],						\
+}
+
+#define DEFAULT_OUT_VFMT (struct sh_veu_vfmt){						\
+	.frame = {									\
+		.width = VGA_WIDTH,							\
+		.height = VGA_HEIGHT,							\
+	},										\
+	.bytesperline = (VGA_WIDTH * sh_veu_fmt[DEFAULT_OUT_FMTIDX].ydepth) >> 3,	\
+	.fmt = &sh_veu_fmt[DEFAULT_OUT_FMTIDX],						\
+}
+
+/*
+ * TODO: add support for further output formats:
+ *	SH_VEU_FMT_NV12,
+ *	SH_VEU_FMT_NV16,
+ *	SH_VEU_FMT_NV24,
+ *	SH_VEU_FMT_RGB332,
+ *	SH_VEU_FMT_RGB444,
+ *	SH_VEU_FMT_RGB666,
+ *	SH_VEU_FMT_RGB24,
+ */
+
+static const int sh_veu_fmt_out[] = {
+	SH_VEU_FMT_RGB565,
+};
+
+/*
+ * TODO: add support for further input formats:
+ *	SH_VEU_FMT_NV16,
+ *	SH_VEU_FMT_NV24,
+ *	SH_VEU_FMT_RGB565,
+ *	SH_VEU_FMT_RGB666,
+ *	SH_VEU_FMT_RGB24,
+ */
+static const int sh_veu_fmt_in[] = {
+	SH_VEU_FMT_NV12,
+};
+
+static enum v4l2_colorspace sh_veu_4cc2cspace(u32 fourcc)
+{
+	switch (fourcc) {
+	default:
+		BUG();
+	case V4L2_PIX_FMT_NV12:
+	case V4L2_PIX_FMT_NV16:
+	case V4L2_PIX_FMT_NV24:
+		return V4L2_COLORSPACE_SMPTE170M;
+	case V4L2_PIX_FMT_RGB332:
+	case V4L2_PIX_FMT_RGB444:
+	case V4L2_PIX_FMT_RGB565:
+	case V4L2_PIX_FMT_BGR666:
+	case V4L2_PIX_FMT_RGB24:
+		return V4L2_COLORSPACE_SRGB;
+	}
+}
+
+static u32 sh_veu_reg_read(struct sh_veu_dev *veu, unsigned int reg)
+{
+	return ioread32(veu->base + reg);
+}
+
+static void sh_veu_reg_write(struct sh_veu_dev *veu, unsigned int reg,
+			     u32 value)
+{
+	iowrite32(value, veu->base + reg);
+}
+
+		/* ========== mem2mem callbacks ========== */
+
+static void sh_veu_job_abort(void *priv)
+{
+	struct sh_veu_dev *veu = priv;
+
+	/* Will cancel the transaction in the next interrupt handler */
+	veu->aborting = true;
+}
+
+static void sh_veu_process(struct sh_veu_dev *veu,
+			   struct vb2_buffer *src_buf,
+			   struct vb2_buffer *dst_buf)
+{
+	dma_addr_t addr = vb2_dma_contig_plane_dma_addr(dst_buf, 0);
+
+	sh_veu_reg_write(veu, VEU_DAYR, addr + veu->vfmt_out.offset_y);
+	sh_veu_reg_write(veu, VEU_DACR, veu->vfmt_out.offset_c ?
+			 addr + veu->vfmt_out.offset_c : 0);
+	dev_dbg(veu->dev, "%s(): dst base %lx, y: %x, c: %x\n", __func__,
+		(unsigned long)addr,
+		veu->vfmt_out.offset_y, veu->vfmt_out.offset_c);
+
+	addr = vb2_dma_contig_plane_dma_addr(src_buf, 0);
+	sh_veu_reg_write(veu, VEU_SAYR, addr + veu->vfmt_in.offset_y);
+	sh_veu_reg_write(veu, VEU_SACR, veu->vfmt_in.offset_c ?
+			 addr + veu->vfmt_in.offset_c : 0);
+	dev_dbg(veu->dev, "%s(): src base %lx, y: %x, c: %x\n", __func__,
+		(unsigned long)addr,
+		veu->vfmt_in.offset_y, veu->vfmt_in.offset_c);
+
+	sh_veu_reg_write(veu, VEU_STR, 1);
+
+	sh_veu_reg_write(veu, VEU_EIER, 1); /* enable interrupt in VEU */
+}
+
+/**
+ * sh_veu_device_run() - prepares and starts the device
+ *
+ * This will be called by the framework when it decides to schedule a particular
+ * instance.
+ */
+static void sh_veu_device_run(void *priv)
+{
+	struct sh_veu_dev *veu = priv;
+	struct vb2_buffer *src_buf, *dst_buf;
+
+	src_buf = v4l2_m2m_next_src_buf(veu->m2m_ctx);
+	dst_buf = v4l2_m2m_next_dst_buf(veu->m2m_ctx);
+
+	if (src_buf && dst_buf)
+		sh_veu_process(veu, src_buf, dst_buf);
+}
+
+		/* ========== video ioctls ========== */
+
+static bool sh_veu_is_streamer(struct sh_veu_dev *veu, struct sh_veu_file *veu_file,
+			       enum v4l2_buf_type type)
+{
+	return (type == V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+		veu_file == veu->capture) ||
+		(type == V4L2_BUF_TYPE_VIDEO_OUTPUT &&
+		 veu_file == veu->output);
+}
+
+static int sh_veu_queue_init(void *priv, struct vb2_queue *src_vq,
+			     struct vb2_queue *dst_vq);
+
+/*
+ * It is not unusual to have video nodes open()ed multiple times. While some
+ * V4L2 operations are non-intrusive, like querying formats and various
+ * parameters, others, like setting formats, starting and stopping streaming,
+ * queuing and dequeuing buffers, directly affect hardware configuration and /
+ * or execution. This function verifies availability of the requested interface
+ * and, if available, reserves it for the requesting user.
+ */
+static int sh_veu_stream_init(struct sh_veu_dev *veu, struct sh_veu_file *veu_file,
+			      enum v4l2_buf_type type)
+{
+	struct sh_veu_file **stream;
+
+	switch (type) {
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		stream = &veu->capture;
+		break;
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+		stream = &veu->output;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (*stream == veu_file)
+		return 0;
+
+	if (*stream)
+		return -EBUSY;
+
+	*stream = veu_file;
+
+	return 0;
+}
+
+static int sh_veu_context_init(struct sh_veu_dev *veu)
+{
+	if (veu->m2m_ctx)
+		return 0;
+
+	veu->m2m_ctx = v4l2_m2m_ctx_init(veu->m2m_dev, veu,
+					 sh_veu_queue_init);
+
+	return PTR_ERR_OR_ZERO(veu->m2m_ctx);
+}
+
+static int sh_veu_querycap(struct file *file, void *priv,
+			   struct v4l2_capability *cap)
+{
+	strlcpy(cap->driver, "sh-veu", sizeof(cap->driver));
+	strlcpy(cap->card, "sh-mobile VEU", sizeof(cap->card));
+	strlcpy(cap->bus_info, "platform:sh-veu", sizeof(cap->bus_info));
+	cap->device_caps = V4L2_CAP_VIDEO_M2M | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+
+	return 0;
+}
+
+static int sh_veu_enum_fmt(struct v4l2_fmtdesc *f, const int *fmt, int fmt_num)
+{
+	if (f->index >= fmt_num)
+		return -EINVAL;
+
+	strlcpy(f->description, sh_veu_fmt[fmt[f->index]].name, sizeof(f->description));
+	f->pixelformat = sh_veu_fmt[fmt[f->index]].fourcc;
+	return 0;
+}
+
+static int sh_veu_enum_fmt_vid_cap(struct file *file, void *priv,
+				   struct v4l2_fmtdesc *f)
+{
+	return sh_veu_enum_fmt(f, sh_veu_fmt_out, ARRAY_SIZE(sh_veu_fmt_out));
+}
+
+static int sh_veu_enum_fmt_vid_out(struct file *file, void *priv,
+				   struct v4l2_fmtdesc *f)
+{
+	return sh_veu_enum_fmt(f, sh_veu_fmt_in, ARRAY_SIZE(sh_veu_fmt_in));
+}
+
+static struct sh_veu_vfmt *sh_veu_get_vfmt(struct sh_veu_dev *veu,
+					   enum v4l2_buf_type type)
+{
+	switch (type) {
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		return &veu->vfmt_out;
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+		return &veu->vfmt_in;
+	default:
+		return NULL;
+	}
+}
+
+static int sh_veu_g_fmt(struct sh_veu_file *veu_file, struct v4l2_format *f)
+{
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct sh_veu_dev *veu = veu_file->veu_dev;
+	struct sh_veu_vfmt *vfmt;
+
+	vfmt = sh_veu_get_vfmt(veu, f->type);
+
+	pix->width		= vfmt->frame.width;
+	pix->height		= vfmt->frame.height;
+	pix->field		= V4L2_FIELD_NONE;
+	pix->pixelformat	= vfmt->fmt->fourcc;
+	pix->colorspace		= sh_veu_4cc2cspace(pix->pixelformat);
+	pix->bytesperline	= vfmt->bytesperline;
+	pix->sizeimage		= vfmt->bytesperline * pix->height *
+		vfmt->fmt->depth / vfmt->fmt->ydepth;
+	dev_dbg(veu->dev, "%s(): type: %d, size %u @ %ux%u, fmt %x\n", __func__,
+		f->type, pix->sizeimage, pix->width, pix->height, pix->pixelformat);
+
+	return 0;
+}
+
+static int sh_veu_g_fmt_vid_out(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	return sh_veu_g_fmt(priv, f);
+}
+
+static int sh_veu_g_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	return sh_veu_g_fmt(priv, f);
+}
+
+static int sh_veu_try_fmt(struct v4l2_format *f, const struct sh_veu_format *fmt)
+{
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	unsigned int y_bytes_used;
+
+	/*
+	 * V4L2 specification suggests, that the driver should correct the
+	 * format struct if any of the dimensions is unsupported
+	 */
+	switch (pix->field) {
+	default:
+	case V4L2_FIELD_ANY:
+		pix->field = V4L2_FIELD_NONE;
+		/* fall through: continue handling V4L2_FIELD_NONE */
+	case V4L2_FIELD_NONE:
+		break;
+	}
+
+	v4l_bound_align_image(&pix->width, MIN_W, MAX_W, ALIGN_W,
+			      &pix->height, MIN_H, MAX_H, 0, 0);
+
+	y_bytes_used = (pix->width * fmt->ydepth) >> 3;
+
+	if (pix->bytesperline < y_bytes_used)
+		pix->bytesperline = y_bytes_used;
+	pix->sizeimage = pix->height * pix->bytesperline * fmt->depth / fmt->ydepth;
+
+	pix->pixelformat	= fmt->fourcc;
+	pix->colorspace		= sh_veu_4cc2cspace(pix->pixelformat);
+
+	pr_debug("%s(): type: %d, size %u\n", __func__, f->type, pix->sizeimage);
+
+	return 0;
+}
+
+static const struct sh_veu_format *sh_veu_find_fmt(const struct v4l2_format *f)
+{
+	const int *fmt;
+	int i, n, dflt;
+
+	pr_debug("%s(%d;%d)\n", __func__, f->type, f->fmt.pix.field);
+
+	switch (f->type) {
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		fmt = sh_veu_fmt_out;
+		n = ARRAY_SIZE(sh_veu_fmt_out);
+		dflt = DEFAULT_OUT_FMTIDX;
+		break;
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+	default:
+		fmt = sh_veu_fmt_in;
+		n = ARRAY_SIZE(sh_veu_fmt_in);
+		dflt = DEFAULT_IN_FMTIDX;
+		break;
+	}
+
+	for (i = 0; i < n; i++)
+		if (sh_veu_fmt[fmt[i]].fourcc == f->fmt.pix.pixelformat)
+			return &sh_veu_fmt[fmt[i]];
+
+	return &sh_veu_fmt[dflt];
+}
+
+static int sh_veu_try_fmt_vid_cap(struct file *file, void *priv,
+				  struct v4l2_format *f)
+{
+	const struct sh_veu_format *fmt;
+
+	fmt = sh_veu_find_fmt(f);
+	if (!fmt)
+		/* wrong buffer type */
+		return -EINVAL;
+
+	return sh_veu_try_fmt(f, fmt);
+}
+
+static int sh_veu_try_fmt_vid_out(struct file *file, void *priv,
+				  struct v4l2_format *f)
+{
+	const struct sh_veu_format *fmt;
+
+	fmt = sh_veu_find_fmt(f);
+	if (!fmt)
+		/* wrong buffer type */
+		return -EINVAL;
+
+	return sh_veu_try_fmt(f, fmt);
+}
+
+static void sh_veu_colour_offset(struct sh_veu_dev *veu, struct sh_veu_vfmt *vfmt)
+{
+	/* dst_left and dst_top validity will be verified in CROP / COMPOSE */
+	unsigned int left = vfmt->frame.left & ~0x03;
+	unsigned int top = vfmt->frame.top;
+	dma_addr_t offset = ((left * veu->vfmt_out.fmt->depth) >> 3) +
+		top * veu->vfmt_out.bytesperline;
+	unsigned int y_line;
+
+	vfmt->offset_y = offset;
+
+	switch (vfmt->fmt->fourcc) {
+	case V4L2_PIX_FMT_NV12:
+	case V4L2_PIX_FMT_NV16:
+	case V4L2_PIX_FMT_NV24:
+		y_line = ALIGN(vfmt->frame.width, 16);
+		vfmt->offset_c = offset + y_line * vfmt->frame.height;
+		break;
+	case V4L2_PIX_FMT_RGB332:
+	case V4L2_PIX_FMT_RGB444:
+	case V4L2_PIX_FMT_RGB565:
+	case V4L2_PIX_FMT_BGR666:
+	case V4L2_PIX_FMT_RGB24:
+		vfmt->offset_c = 0;
+		break;
+	default:
+		BUG();
+	}
+}
+
+static int sh_veu_s_fmt(struct sh_veu_file *veu_file, struct v4l2_format *f)
+{
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct sh_veu_dev *veu = veu_file->veu_dev;
+	struct sh_veu_vfmt *vfmt;
+	struct vb2_queue *vq;
+	int ret = sh_veu_context_init(veu);
+	if (ret < 0)
+		return ret;
+
+	vq = v4l2_m2m_get_vq(veu->m2m_ctx, f->type);
+	if (!vq)
+		return -EINVAL;
+
+	if (vb2_is_busy(vq)) {
+		v4l2_err(&veu_file->veu_dev->v4l2_dev, "%s queue busy\n", __func__);
+		return -EBUSY;
+	}
+
+	vfmt = sh_veu_get_vfmt(veu, f->type);
+	/* called after try_fmt(), hence vfmt != NULL. Implicit BUG_ON() below */
+
+	vfmt->fmt		= sh_veu_find_fmt(f);
+	/* vfmt->fmt != NULL following the same argument as above */
+	vfmt->frame.width	= pix->width;
+	vfmt->frame.height	= pix->height;
+	vfmt->bytesperline	= pix->bytesperline;
+
+	sh_veu_colour_offset(veu, vfmt);
+
+	/*
+	 * We could also verify and require configuration only if any parameters
+	 * actually have changed, but it is unlikely, that the user requests the
+	 * same configuration several times without closing the device.
+	 */
+	veu_file->cfg_needed = true;
+
+	dev_dbg(veu->dev,
+		"Setting format for type %d, wxh: %dx%d, fmt: %x\n",
+		f->type, pix->width, pix->height, vfmt->fmt->fourcc);
+
+	return 0;
+}
+
+static int sh_veu_s_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	int ret = sh_veu_try_fmt_vid_cap(file, priv, f);
+	if (ret)
+		return ret;
+
+	return sh_veu_s_fmt(priv, f);
+}
+
+static int sh_veu_s_fmt_vid_out(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	int ret = sh_veu_try_fmt_vid_out(file, priv, f);
+	if (ret)
+		return ret;
+
+	return sh_veu_s_fmt(priv, f);
+}
+
+static int sh_veu_reqbufs(struct file *file, void *priv,
+			  struct v4l2_requestbuffers *reqbufs)
+{
+	struct sh_veu_file *veu_file = priv;
+	struct sh_veu_dev *veu = veu_file->veu_dev;
+	int ret = sh_veu_context_init(veu);
+	if (ret < 0)
+		return ret;
+
+	ret = sh_veu_stream_init(veu, veu_file, reqbufs->type);
+	if (ret < 0)
+		return ret;
+
+	return v4l2_m2m_reqbufs(file, veu->m2m_ctx, reqbufs);
+}
+
+static int sh_veu_querybuf(struct file *file, void *priv,
+			   struct v4l2_buffer *buf)
+{
+	struct sh_veu_file *veu_file = priv;
+
+	if (!sh_veu_is_streamer(veu_file->veu_dev, veu_file, buf->type))
+		return -EBUSY;
+
+	return v4l2_m2m_querybuf(file, veu_file->veu_dev->m2m_ctx, buf);
+}
+
+static int sh_veu_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
+{
+	struct sh_veu_file *veu_file = priv;
+
+	dev_dbg(veu_file->veu_dev->dev, "%s(%d)\n", __func__, buf->type);
+	if (!sh_veu_is_streamer(veu_file->veu_dev, veu_file, buf->type))
+		return -EBUSY;
+
+	return v4l2_m2m_qbuf(file, veu_file->veu_dev->m2m_ctx, buf);
+}
+
+static int sh_veu_dqbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
+{
+	struct sh_veu_file *veu_file = priv;
+
+	dev_dbg(veu_file->veu_dev->dev, "%s(%d)\n", __func__, buf->type);
+	if (!sh_veu_is_streamer(veu_file->veu_dev, veu_file, buf->type))
+		return -EBUSY;
+
+	return v4l2_m2m_dqbuf(file, veu_file->veu_dev->m2m_ctx, buf);
+}
+
+static void sh_veu_calc_scale(struct sh_veu_dev *veu,
+			      int size_in, int size_out, int crop_out,
+			      u32 *mant, u32 *frac, u32 *rep)
+{
+	u32 fixpoint;
+
+	/* calculate FRAC and MANT */
+	*rep = *mant = *frac = 0;
+
+	if (size_in == size_out) {
+		if (crop_out != size_out)
+			*mant = 1; /* needed for cropping */
+		return;
+	}
+
+	/* VEU2H special upscale */
+	if (veu->is_2h && size_out > size_in) {
+		u32 fixpoint = (4096 * size_in) / size_out;
+		*mant = fixpoint / 4096;
+		*frac = (fixpoint - (*mant * 4096)) & ~0x07;
+
+		switch (*frac) {
+		case 0x800:
+			*rep = 1;
+			break;
+		case 0x400:
+			*rep = 3;
+			break;
+		case 0x200:
+			*rep = 7;
+			break;
+		}
+		if (*rep)
+			return;
+	}
+
+	fixpoint = (4096 * (size_in - 1)) / (size_out + 1);
+	*mant = fixpoint / 4096;
+	*frac = fixpoint - (*mant * 4096);
+
+	if (*frac & 0x07) {
+		/*
+		 * FIXME: do we really have to round down twice in the
+		 * up-scaling case?
+		 */
+		*frac &= ~0x07;
+		if (size_out > size_in)
+			*frac -= 8; /* round down if scaling up */
+		else
+			*frac += 8; /* round up if scaling down */
+	}
+}
+
+static unsigned long sh_veu_scale_v(struct sh_veu_dev *veu,
+				    int size_in, int size_out, int crop_out)
+{
+	u32 mant, frac, value, rep;
+
+	sh_veu_calc_scale(veu, size_in, size_out, crop_out, &mant, &frac, &rep);
+
+	/* set scale */
+	value = (sh_veu_reg_read(veu, VEU_RFCR) & ~0xffff0000) |
+		(((mant << 12) | frac) << 16);
+
+	sh_veu_reg_write(veu, VEU_RFCR, value);
+
+	/* set clip */
+	value = (sh_veu_reg_read(veu, VEU_RFSR) & ~0xffff0000) |
+		(((rep << 12) | crop_out) << 16);
+
+	sh_veu_reg_write(veu, VEU_RFSR, value);
+
+	return ALIGN((size_in * crop_out) / size_out, 4);
+}
+
+static unsigned long sh_veu_scale_h(struct sh_veu_dev *veu,
+				    int size_in, int size_out, int crop_out)
+{
+	u32 mant, frac, value, rep;
+
+	sh_veu_calc_scale(veu, size_in, size_out, crop_out, &mant, &frac, &rep);
+
+	/* set scale */
+	value = (sh_veu_reg_read(veu, VEU_RFCR) & ~0xffff) |
+		(mant << 12) | frac;
+
+	sh_veu_reg_write(veu, VEU_RFCR, value);
+
+	/* set clip */
+	value = (sh_veu_reg_read(veu, VEU_RFSR) & ~0xffff) |
+		(rep << 12) | crop_out;
+
+	sh_veu_reg_write(veu, VEU_RFSR, value);
+
+	return ALIGN((size_in * crop_out) / size_out, 4);
+}
+
+static void sh_veu_configure(struct sh_veu_dev *veu)
+{
+	u32 src_width, src_stride, src_height;
+	u32 dst_width, dst_stride, dst_height;
+	u32 real_w, real_h;
+
+	/* reset VEU */
+	sh_veu_reg_write(veu, VEU_BSRR, 0x100);
+
+	src_width = veu->vfmt_in.frame.width;
+	src_height = veu->vfmt_in.frame.height;
+	src_stride = ALIGN(veu->vfmt_in.frame.width, 16);
+
+	dst_width = real_w = veu->vfmt_out.frame.width;
+	dst_height = real_h = veu->vfmt_out.frame.height;
+	/* Datasheet is unclear - whether it's always number of bytes or not */
+	dst_stride = veu->vfmt_out.bytesperline;
+
+	/*
+	 * So far real_w == dst_width && real_h == dst_height, but it wasn't
+	 * necessarily the case in the original vidix driver, so, it may change
+	 * here in the future too.
+	 */
+	src_width = sh_veu_scale_h(veu, src_width, real_w, dst_width);
+	src_height = sh_veu_scale_v(veu, src_height, real_h, dst_height);
+
+	sh_veu_reg_write(veu, VEU_SWR, src_stride);
+	sh_veu_reg_write(veu, VEU_SSR, src_width | (src_height << 16));
+	sh_veu_reg_write(veu, VEU_BSSR, 0); /* not using bundle mode */
+
+	sh_veu_reg_write(veu, VEU_EDWR, dst_stride);
+	sh_veu_reg_write(veu, VEU_DACR, 0); /* unused for RGB */
+
+	sh_veu_reg_write(veu, VEU_SWPR, 0x67);
+	sh_veu_reg_write(veu, VEU_TRCR, (6 << 16) | (0 << 14) | 2 | 4);
+
+	if (veu->is_2h) {
+		sh_veu_reg_write(veu, VEU_MCR00, 0x0cc5);
+		sh_veu_reg_write(veu, VEU_MCR01, 0x0950);
+		sh_veu_reg_write(veu, VEU_MCR02, 0x0000);
+
+		sh_veu_reg_write(veu, VEU_MCR10, 0x397f);
+		sh_veu_reg_write(veu, VEU_MCR11, 0x0950);
+		sh_veu_reg_write(veu, VEU_MCR12, 0x3ccd);
+
+		sh_veu_reg_write(veu, VEU_MCR20, 0x0000);
+		sh_veu_reg_write(veu, VEU_MCR21, 0x0950);
+		sh_veu_reg_write(veu, VEU_MCR22, 0x1023);
+
+		sh_veu_reg_write(veu, VEU_COFFR, 0x00800010);
+	}
+}
+
+static int sh_veu_streamon(struct file *file, void *priv,
+			   enum v4l2_buf_type type)
+{
+	struct sh_veu_file *veu_file = priv;
+
+	if (!sh_veu_is_streamer(veu_file->veu_dev, veu_file, type))
+		return -EBUSY;
+
+	if (veu_file->cfg_needed) {
+		struct sh_veu_dev *veu = veu_file->veu_dev;
+		veu_file->cfg_needed = false;
+		sh_veu_configure(veu_file->veu_dev);
+		veu->xaction = 0;
+		veu->aborting = false;
+	}
+
+	return v4l2_m2m_streamon(file, veu_file->veu_dev->m2m_ctx, type);
+}
+
+static int sh_veu_streamoff(struct file *file, void *priv,
+			    enum v4l2_buf_type type)
+{
+	struct sh_veu_file *veu_file = priv;
+
+	if (!sh_veu_is_streamer(veu_file->veu_dev, veu_file, type))
+		return -EBUSY;
+
+	return v4l2_m2m_streamoff(file, veu_file->veu_dev->m2m_ctx, type);
+}
+
+static const struct v4l2_ioctl_ops sh_veu_ioctl_ops = {
+	.vidioc_querycap	= sh_veu_querycap,
+
+	.vidioc_enum_fmt_vid_cap = sh_veu_enum_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap	= sh_veu_g_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap	= sh_veu_try_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap	= sh_veu_s_fmt_vid_cap,
+
+	.vidioc_enum_fmt_vid_out = sh_veu_enum_fmt_vid_out,
+	.vidioc_g_fmt_vid_out	= sh_veu_g_fmt_vid_out,
+	.vidioc_try_fmt_vid_out	= sh_veu_try_fmt_vid_out,
+	.vidioc_s_fmt_vid_out	= sh_veu_s_fmt_vid_out,
+
+	.vidioc_reqbufs		= sh_veu_reqbufs,
+	.vidioc_querybuf	= sh_veu_querybuf,
+
+	.vidioc_qbuf		= sh_veu_qbuf,
+	.vidioc_dqbuf		= sh_veu_dqbuf,
+
+	.vidioc_streamon	= sh_veu_streamon,
+	.vidioc_streamoff	= sh_veu_streamoff,
+};
+
+		/* ========== Queue operations ========== */
+
+static int sh_veu_queue_setup(struct vb2_queue *vq,
+			      const void *parg,
+			      unsigned int *nbuffers, unsigned int *nplanes,
+			      unsigned int sizes[], void *alloc_ctxs[])
+{
+	const struct v4l2_format *f = parg;
+	struct sh_veu_dev *veu = vb2_get_drv_priv(vq);
+	struct sh_veu_vfmt *vfmt;
+	unsigned int size, count = *nbuffers;
+
+	if (f) {
+		const struct v4l2_pix_format *pix = &f->fmt.pix;
+		const struct sh_veu_format *fmt = sh_veu_find_fmt(f);
+		struct v4l2_format ftmp = *f;
+
+		if (fmt->fourcc != pix->pixelformat)
+			return -EINVAL;
+		sh_veu_try_fmt(&ftmp, fmt);
+		if (ftmp.fmt.pix.width != pix->width ||
+		    ftmp.fmt.pix.height != pix->height)
+			return -EINVAL;
+		size = pix->bytesperline ? pix->bytesperline * pix->height * fmt->depth / fmt->ydepth :
+			pix->width * pix->height * fmt->depth / fmt->ydepth;
+	} else {
+		vfmt = sh_veu_get_vfmt(veu, vq->type);
+		size = vfmt->bytesperline * vfmt->frame.height * vfmt->fmt->depth / vfmt->fmt->ydepth;
+	}
+
+	if (count < 2)
+		*nbuffers = count = 2;
+
+	if (size * count > VIDEO_MEM_LIMIT) {
+		count = VIDEO_MEM_LIMIT / size;
+		*nbuffers = count;
+	}
+
+	*nplanes = 1;
+	sizes[0] = size;
+	alloc_ctxs[0] = veu->alloc_ctx;
+
+	dev_dbg(veu->dev, "get %d buffer(s) of size %d each.\n", count, size);
+
+	return 0;
+}
+
+static int sh_veu_buf_prepare(struct vb2_buffer *vb)
+{
+	struct sh_veu_dev *veu = vb2_get_drv_priv(vb->vb2_queue);
+	struct sh_veu_vfmt *vfmt;
+	unsigned int sizeimage;
+
+	vfmt = sh_veu_get_vfmt(veu, vb->vb2_queue->type);
+	sizeimage = vfmt->bytesperline * vfmt->frame.height *
+		vfmt->fmt->depth / vfmt->fmt->ydepth;
+
+	if (vb2_plane_size(vb, 0) < sizeimage) {
+		dev_dbg(veu->dev, "%s data will not fit into plane (%lu < %u)\n",
+			__func__, vb2_plane_size(vb, 0), sizeimage);
+		return -EINVAL;
+	}
+
+	vb2_set_plane_payload(vb, 0, sizeimage);
+
+	return 0;
+}
+
+static void sh_veu_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct sh_veu_dev *veu = vb2_get_drv_priv(vb->vb2_queue);
+	dev_dbg(veu->dev, "%s(%d)\n", __func__, vb->type);
+	v4l2_m2m_buf_queue(veu->m2m_ctx, vbuf);
+}
+
+static const struct vb2_ops sh_veu_qops = {
+	.queue_setup	 = sh_veu_queue_setup,
+	.buf_prepare	 = sh_veu_buf_prepare,
+	.buf_queue	 = sh_veu_buf_queue,
+	.wait_prepare	 = vb2_ops_wait_prepare,
+	.wait_finish	 = vb2_ops_wait_finish,
+};
+
+static int sh_veu_queue_init(void *priv, struct vb2_queue *src_vq,
+			     struct vb2_queue *dst_vq)
+{
+	struct sh_veu_dev *veu = priv;
+	int ret;
+
+	memset(src_vq, 0, sizeof(*src_vq));
+	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	src_vq->io_modes = VB2_MMAP | VB2_USERPTR;
+	src_vq->drv_priv = veu;
+	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	src_vq->ops = &sh_veu_qops;
+	src_vq->mem_ops = &vb2_dma_contig_memops;
+	src_vq->lock = &veu->fop_lock;
+	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+
+	ret = vb2_queue_init(src_vq);
+	if (ret < 0)
+		return ret;
+
+	memset(dst_vq, 0, sizeof(*dst_vq));
+	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	dst_vq->io_modes = VB2_MMAP | VB2_USERPTR;
+	dst_vq->drv_priv = veu;
+	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	dst_vq->ops = &sh_veu_qops;
+	dst_vq->mem_ops = &vb2_dma_contig_memops;
+	dst_vq->lock = &veu->fop_lock;
+	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+
+	return vb2_queue_init(dst_vq);
+}
+
+		/* ========== File operations ========== */
+
+static int sh_veu_open(struct file *file)
+{
+	struct sh_veu_dev *veu = video_drvdata(file);
+	struct sh_veu_file *veu_file;
+
+	veu_file = kzalloc(sizeof(*veu_file), GFP_KERNEL);
+	if (!veu_file)
+		return -ENOMEM;
+
+	veu_file->veu_dev = veu;
+	veu_file->cfg_needed = true;
+
+	file->private_data = veu_file;
+
+	pm_runtime_get_sync(veu->dev);
+
+	dev_dbg(veu->dev, "Created instance %p\n", veu_file);
+
+	return 0;
+}
+
+static int sh_veu_release(struct file *file)
+{
+	struct sh_veu_dev *veu = video_drvdata(file);
+	struct sh_veu_file *veu_file = file->private_data;
+
+	dev_dbg(veu->dev, "Releasing instance %p\n", veu_file);
+
+	if (veu_file == veu->capture) {
+		veu->capture = NULL;
+		vb2_queue_release(v4l2_m2m_get_vq(veu->m2m_ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE));
+	}
+
+	if (veu_file == veu->output) {
+		veu->output = NULL;
+		vb2_queue_release(v4l2_m2m_get_vq(veu->m2m_ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT));
+	}
+
+	if (!veu->output && !veu->capture && veu->m2m_ctx) {
+		v4l2_m2m_ctx_release(veu->m2m_ctx);
+		veu->m2m_ctx = NULL;
+	}
+
+	pm_runtime_put(veu->dev);
+
+	kfree(veu_file);
+
+	return 0;
+}
+
+static unsigned int sh_veu_poll(struct file *file,
+				struct poll_table_struct *wait)
+{
+	struct sh_veu_file *veu_file = file->private_data;
+
+	return v4l2_m2m_poll(file, veu_file->veu_dev->m2m_ctx, wait);
+}
+
+static int sh_veu_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct sh_veu_file *veu_file = file->private_data;
+
+	return v4l2_m2m_mmap(file, veu_file->veu_dev->m2m_ctx, vma);
+}
+
+static const struct v4l2_file_operations sh_veu_fops = {
+	.owner		= THIS_MODULE,
+	.open		= sh_veu_open,
+	.release	= sh_veu_release,
+	.poll		= sh_veu_poll,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap		= sh_veu_mmap,
+};
+
+static const struct video_device sh_veu_videodev = {
+	.name		= "sh-veu",
+	.fops		= &sh_veu_fops,
+	.ioctl_ops	= &sh_veu_ioctl_ops,
+	.minor		= -1,
+	.release	= video_device_release_empty,
+	.vfl_dir	= VFL_DIR_M2M,
+};
+
+static const struct v4l2_m2m_ops sh_veu_m2m_ops = {
+	.device_run	= sh_veu_device_run,
+	.job_abort	= sh_veu_job_abort,
+};
+
+static irqreturn_t sh_veu_bh(int irq, void *dev_id)
+{
+	struct sh_veu_dev *veu = dev_id;
+
+	if (veu->xaction == MEM2MEM_DEF_TRANSLEN || veu->aborting) {
+		v4l2_m2m_job_finish(veu->m2m_dev, veu->m2m_ctx);
+		veu->xaction = 0;
+	} else {
+		sh_veu_device_run(veu);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t sh_veu_isr(int irq, void *dev_id)
+{
+	struct sh_veu_dev *veu = dev_id;
+	struct vb2_v4l2_buffer *dst;
+	struct vb2_v4l2_buffer *src;
+	u32 status = sh_veu_reg_read(veu, VEU_EVTR);
+
+	/* bundle read mode not used */
+	if (!(status & 1))
+		return IRQ_NONE;
+
+	/* disable interrupt in VEU */
+	sh_veu_reg_write(veu, VEU_EIER, 0);
+	/* halt operation */
+	sh_veu_reg_write(veu, VEU_STR, 0);
+	/* ack int, write 0 to clear bits */
+	sh_veu_reg_write(veu, VEU_EVTR, status & ~1);
+
+	/* conversion completed */
+	dst = v4l2_m2m_dst_buf_remove(veu->m2m_ctx);
+	src = v4l2_m2m_src_buf_remove(veu->m2m_ctx);
+	if (!src || !dst)
+		return IRQ_NONE;
+
+	dst->timestamp = src->timestamp;
+	dst->flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+	dst->flags |=
+		src->flags & V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+	dst->timecode = src->timecode;
+
+	spin_lock(&veu->lock);
+	v4l2_m2m_buf_done(src, VB2_BUF_STATE_DONE);
+	v4l2_m2m_buf_done(dst, VB2_BUF_STATE_DONE);
+	spin_unlock(&veu->lock);
+
+	veu->xaction++;
+
+	return IRQ_WAKE_THREAD;
+}
+
+static int sh_veu_probe(struct platform_device *pdev)
+{
+	struct sh_veu_dev *veu;
+	struct resource *reg_res;
+	struct video_device *vdev;
+	int irq, ret;
+
+	reg_res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	irq = platform_get_irq(pdev, 0);
+
+	if (!reg_res || irq <= 0) {
+		dev_err(&pdev->dev, "Insufficient VEU platform information.\n");
+		return -ENODEV;
+	}
+
+	veu = devm_kzalloc(&pdev->dev, sizeof(*veu), GFP_KERNEL);
+	if (!veu)
+		return -ENOMEM;
+
+	veu->is_2h = resource_size(reg_res) == 0x22c;
+
+	veu->base = devm_ioremap_resource(&pdev->dev, reg_res);
+	if (IS_ERR(veu->base))
+		return PTR_ERR(veu->base);
+
+	ret = devm_request_threaded_irq(&pdev->dev, irq, sh_veu_isr, sh_veu_bh,
+					0, "veu", veu);
+	if (ret < 0)
+		return ret;
+
+	ret = v4l2_device_register(&pdev->dev, &veu->v4l2_dev);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Error registering v4l2 device\n");
+		return ret;
+	}
+
+	vdev = &veu->vdev;
+
+	veu->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
+	if (IS_ERR(veu->alloc_ctx)) {
+		ret = PTR_ERR(veu->alloc_ctx);
+		goto einitctx;
+	}
+
+	*vdev = sh_veu_videodev;
+	vdev->v4l2_dev = &veu->v4l2_dev;
+	spin_lock_init(&veu->lock);
+	mutex_init(&veu->fop_lock);
+	vdev->lock = &veu->fop_lock;
+
+	video_set_drvdata(vdev, veu);
+
+	veu->dev	= &pdev->dev;
+	veu->vfmt_out	= DEFAULT_OUT_VFMT;
+	veu->vfmt_in	= DEFAULT_IN_VFMT;
+
+	veu->m2m_dev = v4l2_m2m_init(&sh_veu_m2m_ops);
+	if (IS_ERR(veu->m2m_dev)) {
+		ret = PTR_ERR(veu->m2m_dev);
+		v4l2_err(&veu->v4l2_dev, "Failed to init mem2mem device: %d\n", ret);
+		goto em2minit;
+	}
+
+	pm_runtime_enable(&pdev->dev);
+	pm_runtime_resume(&pdev->dev);
+
+	ret = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
+	pm_runtime_suspend(&pdev->dev);
+	if (ret < 0)
+		goto evidreg;
+
+	return ret;
+
+evidreg:
+	pm_runtime_disable(&pdev->dev);
+	v4l2_m2m_release(veu->m2m_dev);
+em2minit:
+	vb2_dma_contig_cleanup_ctx(veu->alloc_ctx);
+einitctx:
+	v4l2_device_unregister(&veu->v4l2_dev);
+	return ret;
+}
+
+static int sh_veu_remove(struct platform_device *pdev)
+{
+	struct v4l2_device *v4l2_dev = platform_get_drvdata(pdev);
+	struct sh_veu_dev *veu = container_of(v4l2_dev,
+					      struct sh_veu_dev, v4l2_dev);
+
+	video_unregister_device(&veu->vdev);
+	pm_runtime_disable(&pdev->dev);
+	v4l2_m2m_release(veu->m2m_dev);
+	vb2_dma_contig_cleanup_ctx(veu->alloc_ctx);
+	v4l2_device_unregister(&veu->v4l2_dev);
+
+	return 0;
+}
+
+static struct platform_driver __refdata sh_veu_pdrv = {
+	.remove		= sh_veu_remove,
+	.driver		= {
+		.name	= "sh_veu",
+	},
+};
+
+module_platform_driver_probe(sh_veu_pdrv, sh_veu_probe);
+
+MODULE_DESCRIPTION("sh-mobile VEU mem2mem driver");
+MODULE_AUTHOR("Guennadi Liakhovetski, <g.liakhovetski@gmx.de>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/platform/sh_vou.c b/drivers/media/platform/sh_vou.c
new file mode 100644
index 0000000..2231f89
--- /dev/null
+++ b/drivers/media/platform/sh_vou.c
@@ -0,0 +1,1389 @@
+/*
+ * SuperH Video Output Unit (VOU) driver
+ *
+ * Copyright (C) 2010, Guennadi Liakhovetski <g.liakhovetski@gmx.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/dma-mapping.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <linux/module.h>
+
+#include <media/sh_vou.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-mediabus.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/videobuf2-dma-contig.h>
+
+/* Mirror addresses are not available for all registers */
+#define VOUER	0
+#define VOUCR	4
+#define VOUSTR	8
+#define VOUVCR	0xc
+#define VOUISR	0x10
+#define VOUBCR	0x14
+#define VOUDPR	0x18
+#define VOUDSR	0x1c
+#define VOUVPR	0x20
+#define VOUIR	0x24
+#define VOUSRR	0x28
+#define VOUMSR	0x2c
+#define VOUHIR	0x30
+#define VOUDFR	0x34
+#define VOUAD1R	0x38
+#define VOUAD2R	0x3c
+#define VOUAIR	0x40
+#define VOUSWR	0x44
+#define VOURCR	0x48
+#define VOURPR	0x50
+
+enum sh_vou_status {
+	SH_VOU_IDLE,
+	SH_VOU_INITIALISING,
+	SH_VOU_RUNNING,
+};
+
+#define VOU_MIN_IMAGE_WIDTH	16
+#define VOU_MAX_IMAGE_WIDTH	720
+#define VOU_MIN_IMAGE_HEIGHT	16
+
+struct sh_vou_buffer {
+	struct vb2_v4l2_buffer vb;
+	struct list_head list;
+};
+
+static inline struct
+sh_vou_buffer *to_sh_vou_buffer(struct vb2_v4l2_buffer *vb2)
+{
+	return container_of(vb2, struct sh_vou_buffer, vb);
+}
+
+struct sh_vou_device {
+	struct v4l2_device v4l2_dev;
+	struct video_device vdev;
+	struct sh_vou_pdata *pdata;
+	spinlock_t lock;
+	void __iomem *base;
+	/* State information */
+	struct v4l2_pix_format pix;
+	struct v4l2_rect rect;
+	struct list_head buf_list;
+	v4l2_std_id std;
+	int pix_idx;
+	struct vb2_queue queue;
+	struct vb2_alloc_ctx *alloc_ctx;
+	struct sh_vou_buffer *active;
+	enum sh_vou_status status;
+	unsigned sequence;
+	struct mutex fop_lock;
+};
+
+/* Register access routines for sides A, B and mirror addresses */
+static void sh_vou_reg_a_write(struct sh_vou_device *vou_dev, unsigned int reg,
+			       u32 value)
+{
+	__raw_writel(value, vou_dev->base + reg);
+}
+
+static void sh_vou_reg_ab_write(struct sh_vou_device *vou_dev, unsigned int reg,
+				u32 value)
+{
+	__raw_writel(value, vou_dev->base + reg);
+	__raw_writel(value, vou_dev->base + reg + 0x1000);
+}
+
+static void sh_vou_reg_m_write(struct sh_vou_device *vou_dev, unsigned int reg,
+			       u32 value)
+{
+	__raw_writel(value, vou_dev->base + reg + 0x2000);
+}
+
+static u32 sh_vou_reg_a_read(struct sh_vou_device *vou_dev, unsigned int reg)
+{
+	return __raw_readl(vou_dev->base + reg);
+}
+
+static void sh_vou_reg_a_set(struct sh_vou_device *vou_dev, unsigned int reg,
+			     u32 value, u32 mask)
+{
+	u32 old = __raw_readl(vou_dev->base + reg);
+
+	value = (value & mask) | (old & ~mask);
+	__raw_writel(value, vou_dev->base + reg);
+}
+
+static void sh_vou_reg_b_set(struct sh_vou_device *vou_dev, unsigned int reg,
+			     u32 value, u32 mask)
+{
+	sh_vou_reg_a_set(vou_dev, reg + 0x1000, value, mask);
+}
+
+static void sh_vou_reg_ab_set(struct sh_vou_device *vou_dev, unsigned int reg,
+			      u32 value, u32 mask)
+{
+	sh_vou_reg_a_set(vou_dev, reg, value, mask);
+	sh_vou_reg_b_set(vou_dev, reg, value, mask);
+}
+
+struct sh_vou_fmt {
+	u32		pfmt;
+	char		*desc;
+	unsigned char	bpp;
+	unsigned char	bpl;
+	unsigned char	rgb;
+	unsigned char	yf;
+	unsigned char	pkf;
+};
+
+/* Further pixel formats can be added */
+static struct sh_vou_fmt vou_fmt[] = {
+	{
+		.pfmt	= V4L2_PIX_FMT_NV12,
+		.bpp	= 12,
+		.bpl	= 1,
+		.desc	= "YVU420 planar",
+		.yf	= 0,
+		.rgb	= 0,
+	},
+	{
+		.pfmt	= V4L2_PIX_FMT_NV16,
+		.bpp	= 16,
+		.bpl	= 1,
+		.desc	= "YVYU planar",
+		.yf	= 1,
+		.rgb	= 0,
+	},
+	{
+		.pfmt	= V4L2_PIX_FMT_RGB24,
+		.bpp	= 24,
+		.bpl	= 3,
+		.desc	= "RGB24",
+		.pkf	= 2,
+		.rgb	= 1,
+	},
+	{
+		.pfmt	= V4L2_PIX_FMT_RGB565,
+		.bpp	= 16,
+		.bpl	= 2,
+		.desc	= "RGB565",
+		.pkf	= 3,
+		.rgb	= 1,
+	},
+	{
+		.pfmt	= V4L2_PIX_FMT_RGB565X,
+		.bpp	= 16,
+		.bpl	= 2,
+		.desc	= "RGB565 byteswapped",
+		.pkf	= 3,
+		.rgb	= 1,
+	},
+};
+
+static void sh_vou_schedule_next(struct sh_vou_device *vou_dev,
+				 struct vb2_v4l2_buffer *vbuf)
+{
+	dma_addr_t addr1, addr2;
+
+	addr1 = vb2_dma_contig_plane_dma_addr(&vbuf->vb2_buf, 0);
+	switch (vou_dev->pix.pixelformat) {
+	case V4L2_PIX_FMT_NV12:
+	case V4L2_PIX_FMT_NV16:
+		addr2 = addr1 + vou_dev->pix.width * vou_dev->pix.height;
+		break;
+	default:
+		addr2 = 0;
+	}
+
+	sh_vou_reg_m_write(vou_dev, VOUAD1R, addr1);
+	sh_vou_reg_m_write(vou_dev, VOUAD2R, addr2);
+}
+
+static void sh_vou_stream_config(struct sh_vou_device *vou_dev)
+{
+	unsigned int row_coeff;
+#ifdef __LITTLE_ENDIAN
+	u32 dataswap = 7;
+#else
+	u32 dataswap = 0;
+#endif
+
+	switch (vou_dev->pix.pixelformat) {
+	default:
+	case V4L2_PIX_FMT_NV12:
+	case V4L2_PIX_FMT_NV16:
+		row_coeff = 1;
+		break;
+	case V4L2_PIX_FMT_RGB565:
+		dataswap ^= 1;
+	case V4L2_PIX_FMT_RGB565X:
+		row_coeff = 2;
+		break;
+	case V4L2_PIX_FMT_RGB24:
+		row_coeff = 3;
+		break;
+	}
+
+	sh_vou_reg_a_write(vou_dev, VOUSWR, dataswap);
+	sh_vou_reg_ab_write(vou_dev, VOUAIR, vou_dev->pix.width * row_coeff);
+}
+
+/* Locking: caller holds fop_lock mutex */
+static int sh_vou_queue_setup(struct vb2_queue *vq, const void *parg,
+		       unsigned int *nbuffers, unsigned int *nplanes,
+		       unsigned int sizes[], void *alloc_ctxs[])
+{
+	const struct v4l2_format *fmt = parg;
+	struct sh_vou_device *vou_dev = vb2_get_drv_priv(vq);
+	struct v4l2_pix_format *pix = &vou_dev->pix;
+	int bytes_per_line = vou_fmt[vou_dev->pix_idx].bpp * pix->width / 8;
+
+	dev_dbg(vou_dev->v4l2_dev.dev, "%s()\n", __func__);
+
+	if (fmt && fmt->fmt.pix.sizeimage < pix->height * bytes_per_line)
+		return -EINVAL;
+	*nplanes = 1;
+	sizes[0] = fmt ? fmt->fmt.pix.sizeimage : pix->height * bytes_per_line;
+	alloc_ctxs[0] = vou_dev->alloc_ctx;
+	return 0;
+}
+
+static int sh_vou_buf_prepare(struct vb2_buffer *vb)
+{
+	struct sh_vou_device *vou_dev = vb2_get_drv_priv(vb->vb2_queue);
+	struct v4l2_pix_format *pix = &vou_dev->pix;
+	unsigned bytes_per_line = vou_fmt[vou_dev->pix_idx].bpp * pix->width / 8;
+	unsigned size = pix->height * bytes_per_line;
+
+	dev_dbg(vou_dev->v4l2_dev.dev, "%s()\n", __func__);
+
+	if (vb2_plane_size(vb, 0) < size) {
+		/* User buffer too small */
+		dev_warn(vou_dev->v4l2_dev.dev, "buffer too small (%lu < %u)\n",
+			 vb2_plane_size(vb, 0), size);
+		return -EINVAL;
+	}
+
+	vb2_set_plane_payload(vb, 0, size);
+	return 0;
+}
+
+/* Locking: caller holds fop_lock mutex and vq->irqlock spinlock */
+static void sh_vou_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct sh_vou_device *vou_dev = vb2_get_drv_priv(vb->vb2_queue);
+	struct sh_vou_buffer *shbuf = to_sh_vou_buffer(vbuf);
+	unsigned long flags;
+
+	spin_lock_irqsave(&vou_dev->lock, flags);
+	list_add_tail(&shbuf->list, &vou_dev->buf_list);
+	spin_unlock_irqrestore(&vou_dev->lock, flags);
+}
+
+static int sh_vou_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct sh_vou_device *vou_dev = vb2_get_drv_priv(vq);
+	struct sh_vou_buffer *buf, *node;
+	int ret;
+
+	vou_dev->sequence = 0;
+	ret = v4l2_device_call_until_err(&vou_dev->v4l2_dev, 0,
+					 video, s_stream, 1);
+	if (ret < 0 && ret != -ENOIOCTLCMD) {
+		list_for_each_entry_safe(buf, node, &vou_dev->buf_list, list) {
+			vb2_buffer_done(&buf->vb.vb2_buf,
+					VB2_BUF_STATE_QUEUED);
+			list_del(&buf->list);
+		}
+		vou_dev->active = NULL;
+		return ret;
+	}
+
+	buf = list_entry(vou_dev->buf_list.next, struct sh_vou_buffer, list);
+
+	vou_dev->active = buf;
+
+	/* Start from side A: we use mirror addresses, so, set B */
+	sh_vou_reg_a_write(vou_dev, VOURPR, 1);
+	dev_dbg(vou_dev->v4l2_dev.dev, "%s: first buffer status 0x%x\n",
+		__func__, sh_vou_reg_a_read(vou_dev, VOUSTR));
+	sh_vou_schedule_next(vou_dev, &buf->vb);
+
+	buf = list_entry(buf->list.next, struct sh_vou_buffer, list);
+
+	/* Second buffer - initialise register side B */
+	sh_vou_reg_a_write(vou_dev, VOURPR, 0);
+	sh_vou_schedule_next(vou_dev, &buf->vb);
+
+	/* Register side switching with frame VSYNC */
+	sh_vou_reg_a_write(vou_dev, VOURCR, 5);
+
+	sh_vou_stream_config(vou_dev);
+	/* Enable End-of-Frame (VSYNC) interrupts */
+	sh_vou_reg_a_write(vou_dev, VOUIR, 0x10004);
+
+	/* Two buffers on the queue - activate the hardware */
+	vou_dev->status = SH_VOU_RUNNING;
+	sh_vou_reg_a_write(vou_dev, VOUER, 0x107);
+	return 0;
+}
+
+static void sh_vou_stop_streaming(struct vb2_queue *vq)
+{
+	struct sh_vou_device *vou_dev = vb2_get_drv_priv(vq);
+	struct sh_vou_buffer *buf, *node;
+	unsigned long flags;
+
+	v4l2_device_call_until_err(&vou_dev->v4l2_dev, 0,
+					 video, s_stream, 0);
+	/* disable output */
+	sh_vou_reg_a_set(vou_dev, VOUER, 0, 1);
+	/* ...but the current frame will complete */
+	sh_vou_reg_a_set(vou_dev, VOUIR, 0, 0x30000);
+	msleep(50);
+	spin_lock_irqsave(&vou_dev->lock, flags);
+	list_for_each_entry_safe(buf, node, &vou_dev->buf_list, list) {
+		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+		list_del(&buf->list);
+	}
+	vou_dev->active = NULL;
+	spin_unlock_irqrestore(&vou_dev->lock, flags);
+}
+
+static struct vb2_ops sh_vou_qops = {
+	.queue_setup		= sh_vou_queue_setup,
+	.buf_prepare		= sh_vou_buf_prepare,
+	.buf_queue		= sh_vou_buf_queue,
+	.start_streaming	= sh_vou_start_streaming,
+	.stop_streaming		= sh_vou_stop_streaming,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
+};
+
+/* Video IOCTLs */
+static int sh_vou_querycap(struct file *file, void  *priv,
+			   struct v4l2_capability *cap)
+{
+	struct sh_vou_device *vou_dev = video_drvdata(file);
+
+	dev_dbg(vou_dev->v4l2_dev.dev, "%s()\n", __func__);
+
+	strlcpy(cap->card, "SuperH VOU", sizeof(cap->card));
+	strlcpy(cap->driver, "sh-vou", sizeof(cap->driver));
+	strlcpy(cap->bus_info, "platform:sh-vou", sizeof(cap->bus_info));
+	cap->device_caps = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_READWRITE |
+			   V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	return 0;
+}
+
+/* Enumerate formats, that the device can accept from the user */
+static int sh_vou_enum_fmt_vid_out(struct file *file, void  *priv,
+				   struct v4l2_fmtdesc *fmt)
+{
+	struct sh_vou_device *vou_dev = video_drvdata(file);
+
+	if (fmt->index >= ARRAY_SIZE(vou_fmt))
+		return -EINVAL;
+
+	dev_dbg(vou_dev->v4l2_dev.dev, "%s()\n", __func__);
+
+	fmt->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	strlcpy(fmt->description, vou_fmt[fmt->index].desc,
+		sizeof(fmt->description));
+	fmt->pixelformat = vou_fmt[fmt->index].pfmt;
+
+	return 0;
+}
+
+static int sh_vou_g_fmt_vid_out(struct file *file, void *priv,
+				struct v4l2_format *fmt)
+{
+	struct sh_vou_device *vou_dev = video_drvdata(file);
+
+	dev_dbg(vou_dev->v4l2_dev.dev, "%s()\n", __func__);
+
+	fmt->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	fmt->fmt.pix = vou_dev->pix;
+
+	return 0;
+}
+
+static const unsigned char vou_scale_h_num[] = {1, 9, 2, 9, 4};
+static const unsigned char vou_scale_h_den[] = {1, 8, 1, 4, 1};
+static const unsigned char vou_scale_h_fld[] = {0, 2, 1, 3};
+static const unsigned char vou_scale_v_num[] = {1, 2, 4};
+static const unsigned char vou_scale_v_den[] = {1, 1, 1};
+static const unsigned char vou_scale_v_fld[] = {0, 1};
+
+static void sh_vou_configure_geometry(struct sh_vou_device *vou_dev,
+				      int pix_idx, int w_idx, int h_idx)
+{
+	struct sh_vou_fmt *fmt = vou_fmt + pix_idx;
+	unsigned int black_left, black_top, width_max,
+		frame_in_height, frame_out_height, frame_out_top;
+	struct v4l2_rect *rect = &vou_dev->rect;
+	struct v4l2_pix_format *pix = &vou_dev->pix;
+	u32 vouvcr = 0, dsr_h, dsr_v;
+
+	if (vou_dev->std & V4L2_STD_525_60) {
+		width_max = 858;
+		/* height_max = 262; */
+	} else {
+		width_max = 864;
+		/* height_max = 312; */
+	}
+
+	frame_in_height = pix->height / 2;
+	frame_out_height = rect->height / 2;
+	frame_out_top = rect->top / 2;
+
+	/*
+	 * Cropping scheme: max useful image is 720x480, and the total video
+	 * area is 858x525 (NTSC) or 864x625 (PAL). AK8813 / 8814 starts
+	 * sampling data beginning with fixed 276th (NTSC) / 288th (PAL) clock,
+	 * of which the first 33 / 25 clocks HSYNC must be held active. This
+	 * has to be configured in CR[HW]. 1 pixel equals 2 clock periods.
+	 * This gives CR[HW] = 16 / 12, VPR[HVP] = 138 / 144, which gives
+	 * exactly 858 - 138 = 864 - 144 = 720! We call the out-of-display area,
+	 * beyond DSR, specified on the left and top by the VPR register "black
+	 * pixels" and out-of-image area (DPR) "background pixels." We fix VPR
+	 * at 138 / 144 : 20, because that's the HSYNC timing, that our first
+	 * client requires, and that's exactly what leaves us 720 pixels for the
+	 * image; we leave VPR[VVP] at default 20 for now, because the client
+	 * doesn't seem to have any special requirements for it. Otherwise we
+	 * could also set it to max - 240 = 22 / 72. Thus VPR depends only on
+	 * the selected standard, and DPR and DSR are selected according to
+	 * cropping. Q: how does the client detect the first valid line? Does
+	 * HSYNC stay inactive during invalid (black) lines?
+	 */
+	black_left = width_max - VOU_MAX_IMAGE_WIDTH;
+	black_top = 20;
+
+	dsr_h = rect->width + rect->left;
+	dsr_v = frame_out_height + frame_out_top;
+
+	dev_dbg(vou_dev->v4l2_dev.dev,
+		"image %ux%u, black %u:%u, offset %u:%u, display %ux%u\n",
+		pix->width, frame_in_height, black_left, black_top,
+		rect->left, frame_out_top, dsr_h, dsr_v);
+
+	/* VOUISR height - half of a frame height in frame mode */
+	sh_vou_reg_ab_write(vou_dev, VOUISR, (pix->width << 16) | frame_in_height);
+	sh_vou_reg_ab_write(vou_dev, VOUVPR, (black_left << 16) | black_top);
+	sh_vou_reg_ab_write(vou_dev, VOUDPR, (rect->left << 16) | frame_out_top);
+	sh_vou_reg_ab_write(vou_dev, VOUDSR, (dsr_h << 16) | dsr_v);
+
+	/*
+	 * if necessary, we could set VOUHIR to
+	 * max(black_left + dsr_h, width_max) here
+	 */
+
+	if (w_idx)
+		vouvcr |= (1 << 15) | (vou_scale_h_fld[w_idx - 1] << 4);
+	if (h_idx)
+		vouvcr |= (1 << 14) | vou_scale_v_fld[h_idx - 1];
+
+	dev_dbg(vou_dev->v4l2_dev.dev, "%s: scaling 0x%x\n", fmt->desc, vouvcr);
+
+	/* To produce a colour bar for testing set bit 23 of VOUVCR */
+	sh_vou_reg_ab_write(vou_dev, VOUVCR, vouvcr);
+	sh_vou_reg_ab_write(vou_dev, VOUDFR,
+			    fmt->pkf | (fmt->yf << 8) | (fmt->rgb << 16));
+}
+
+struct sh_vou_geometry {
+	struct v4l2_rect output;
+	unsigned int in_width;
+	unsigned int in_height;
+	int scale_idx_h;
+	int scale_idx_v;
+};
+
+/*
+ * Find input geometry, that we can use to produce output, closest to the
+ * requested rectangle, using VOU scaling
+ */
+static void vou_adjust_input(struct sh_vou_geometry *geo, v4l2_std_id std)
+{
+	/* The compiler cannot know, that best and idx will indeed be set */
+	unsigned int best_err = UINT_MAX, best = 0, img_height_max;
+	int i, idx = 0;
+
+	if (std & V4L2_STD_525_60)
+		img_height_max = 480;
+	else
+		img_height_max = 576;
+
+	/* Image width must be a multiple of 4 */
+	v4l_bound_align_image(&geo->in_width,
+			      VOU_MIN_IMAGE_WIDTH, VOU_MAX_IMAGE_WIDTH, 2,
+			      &geo->in_height,
+			      VOU_MIN_IMAGE_HEIGHT, img_height_max, 1, 0);
+
+	/* Select scales to come as close as possible to the output image */
+	for (i = ARRAY_SIZE(vou_scale_h_num) - 1; i >= 0; i--) {
+		unsigned int err;
+		unsigned int found = geo->output.width * vou_scale_h_den[i] /
+			vou_scale_h_num[i];
+
+		if (found > VOU_MAX_IMAGE_WIDTH)
+			/* scales increase */
+			break;
+
+		err = abs(found - geo->in_width);
+		if (err < best_err) {
+			best_err = err;
+			idx = i;
+			best = found;
+		}
+		if (!err)
+			break;
+	}
+
+	geo->in_width = best;
+	geo->scale_idx_h = idx;
+
+	best_err = UINT_MAX;
+
+	/* This loop can be replaced with one division */
+	for (i = ARRAY_SIZE(vou_scale_v_num) - 1; i >= 0; i--) {
+		unsigned int err;
+		unsigned int found = geo->output.height * vou_scale_v_den[i] /
+			vou_scale_v_num[i];
+
+		if (found > img_height_max)
+			/* scales increase */
+			break;
+
+		err = abs(found - geo->in_height);
+		if (err < best_err) {
+			best_err = err;
+			idx = i;
+			best = found;
+		}
+		if (!err)
+			break;
+	}
+
+	geo->in_height = best;
+	geo->scale_idx_v = idx;
+}
+
+/*
+ * Find output geometry, that we can produce, using VOU scaling, closest to
+ * the requested rectangle
+ */
+static void vou_adjust_output(struct sh_vou_geometry *geo, v4l2_std_id std)
+{
+	unsigned int best_err = UINT_MAX, best = geo->in_width,
+		width_max, height_max, img_height_max;
+	int i, idx_h = 0, idx_v = 0;
+
+	if (std & V4L2_STD_525_60) {
+		width_max = 858;
+		height_max = 262 * 2;
+		img_height_max = 480;
+	} else {
+		width_max = 864;
+		height_max = 312 * 2;
+		img_height_max = 576;
+	}
+
+	/* Select scales to come as close as possible to the output image */
+	for (i = 0; i < ARRAY_SIZE(vou_scale_h_num); i++) {
+		unsigned int err;
+		unsigned int found = geo->in_width * vou_scale_h_num[i] /
+			vou_scale_h_den[i];
+
+		if (found > VOU_MAX_IMAGE_WIDTH)
+			/* scales increase */
+			break;
+
+		err = abs(found - geo->output.width);
+		if (err < best_err) {
+			best_err = err;
+			idx_h = i;
+			best = found;
+		}
+		if (!err)
+			break;
+	}
+
+	geo->output.width = best;
+	geo->scale_idx_h = idx_h;
+	if (geo->output.left + best > width_max)
+		geo->output.left = width_max - best;
+
+	pr_debug("%s(): W %u * %u/%u = %u\n", __func__, geo->in_width,
+		 vou_scale_h_num[idx_h], vou_scale_h_den[idx_h], best);
+
+	best_err = UINT_MAX;
+
+	/* This loop can be replaced with one division */
+	for (i = 0; i < ARRAY_SIZE(vou_scale_v_num); i++) {
+		unsigned int err;
+		unsigned int found = geo->in_height * vou_scale_v_num[i] /
+			vou_scale_v_den[i];
+
+		if (found > img_height_max)
+			/* scales increase */
+			break;
+
+		err = abs(found - geo->output.height);
+		if (err < best_err) {
+			best_err = err;
+			idx_v = i;
+			best = found;
+		}
+		if (!err)
+			break;
+	}
+
+	geo->output.height = best;
+	geo->scale_idx_v = idx_v;
+	if (geo->output.top + best > height_max)
+		geo->output.top = height_max - best;
+
+	pr_debug("%s(): H %u * %u/%u = %u\n", __func__, geo->in_height,
+		 vou_scale_v_num[idx_v], vou_scale_v_den[idx_v], best);
+}
+
+static int sh_vou_try_fmt_vid_out(struct file *file, void *priv,
+				  struct v4l2_format *fmt)
+{
+	struct sh_vou_device *vou_dev = video_drvdata(file);
+	struct v4l2_pix_format *pix = &fmt->fmt.pix;
+	unsigned int img_height_max;
+	int pix_idx;
+
+	dev_dbg(vou_dev->v4l2_dev.dev, "%s()\n", __func__);
+
+	pix->field = V4L2_FIELD_INTERLACED;
+	pix->colorspace = V4L2_COLORSPACE_SMPTE170M;
+	pix->ycbcr_enc = pix->quantization = 0;
+
+	for (pix_idx = 0; pix_idx < ARRAY_SIZE(vou_fmt); pix_idx++)
+		if (vou_fmt[pix_idx].pfmt == pix->pixelformat)
+			break;
+
+	if (pix_idx == ARRAY_SIZE(vou_fmt))
+		return -EINVAL;
+
+	if (vou_dev->std & V4L2_STD_525_60)
+		img_height_max = 480;
+	else
+		img_height_max = 576;
+
+	v4l_bound_align_image(&pix->width,
+			      VOU_MIN_IMAGE_WIDTH, VOU_MAX_IMAGE_WIDTH, 2,
+			      &pix->height,
+			      VOU_MIN_IMAGE_HEIGHT, img_height_max, 1, 0);
+	pix->bytesperline = pix->width * vou_fmt[pix_idx].bpl;
+	pix->sizeimage = pix->height * ((pix->width * vou_fmt[pix_idx].bpp) >> 3);
+
+	return 0;
+}
+
+static int sh_vou_set_fmt_vid_out(struct sh_vou_device *vou_dev,
+				struct v4l2_pix_format *pix)
+{
+	unsigned int img_height_max;
+	struct sh_vou_geometry geo;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		/* Revisit: is this the correct code? */
+		.format.code = MEDIA_BUS_FMT_YUYV8_2X8,
+		.format.field = V4L2_FIELD_INTERLACED,
+		.format.colorspace = V4L2_COLORSPACE_SMPTE170M,
+	};
+	struct v4l2_mbus_framefmt *mbfmt = &format.format;
+	int pix_idx;
+	int ret;
+
+	if (vb2_is_busy(&vou_dev->queue))
+		return -EBUSY;
+
+	for (pix_idx = 0; pix_idx < ARRAY_SIZE(vou_fmt); pix_idx++)
+		if (vou_fmt[pix_idx].pfmt == pix->pixelformat)
+			break;
+
+	geo.in_width = pix->width;
+	geo.in_height = pix->height;
+	geo.output = vou_dev->rect;
+
+	vou_adjust_output(&geo, vou_dev->std);
+
+	mbfmt->width = geo.output.width;
+	mbfmt->height = geo.output.height;
+	ret = v4l2_device_call_until_err(&vou_dev->v4l2_dev, 0, pad,
+					 set_fmt, NULL, &format);
+	/* Must be implemented, so, don't check for -ENOIOCTLCMD */
+	if (ret < 0)
+		return ret;
+
+	dev_dbg(vou_dev->v4l2_dev.dev, "%s(): %ux%u -> %ux%u\n", __func__,
+		geo.output.width, geo.output.height, mbfmt->width, mbfmt->height);
+
+	if (vou_dev->std & V4L2_STD_525_60)
+		img_height_max = 480;
+	else
+		img_height_max = 576;
+
+	/* Sanity checks */
+	if ((unsigned)mbfmt->width > VOU_MAX_IMAGE_WIDTH ||
+	    (unsigned)mbfmt->height > img_height_max ||
+	    mbfmt->code != MEDIA_BUS_FMT_YUYV8_2X8)
+		return -EIO;
+
+	if (mbfmt->width != geo.output.width ||
+	    mbfmt->height != geo.output.height) {
+		geo.output.width = mbfmt->width;
+		geo.output.height = mbfmt->height;
+
+		vou_adjust_input(&geo, vou_dev->std);
+	}
+
+	/* We tried to preserve output rectangle, but it could have changed */
+	vou_dev->rect = geo.output;
+	pix->width = geo.in_width;
+	pix->height = geo.in_height;
+
+	dev_dbg(vou_dev->v4l2_dev.dev, "%s(): %ux%u\n", __func__,
+		pix->width, pix->height);
+
+	vou_dev->pix_idx = pix_idx;
+
+	vou_dev->pix = *pix;
+
+	sh_vou_configure_geometry(vou_dev, pix_idx,
+				  geo.scale_idx_h, geo.scale_idx_v);
+
+	return 0;
+}
+
+static int sh_vou_s_fmt_vid_out(struct file *file, void *priv,
+				struct v4l2_format *fmt)
+{
+	struct sh_vou_device *vou_dev = video_drvdata(file);
+	int ret = sh_vou_try_fmt_vid_out(file, priv, fmt);
+
+	if (ret)
+		return ret;
+	return sh_vou_set_fmt_vid_out(vou_dev, &fmt->fmt.pix);
+}
+
+static int sh_vou_enum_output(struct file *file, void *fh,
+			      struct v4l2_output *a)
+{
+	struct sh_vou_device *vou_dev = video_drvdata(file);
+
+	if (a->index)
+		return -EINVAL;
+	strlcpy(a->name, "Video Out", sizeof(a->name));
+	a->type = V4L2_OUTPUT_TYPE_ANALOG;
+	a->std = vou_dev->vdev.tvnorms;
+	return 0;
+}
+
+static int sh_vou_g_output(struct file *file, void *fh, unsigned int *i)
+{
+	*i = 0;
+	return 0;
+}
+
+static int sh_vou_s_output(struct file *file, void *fh, unsigned int i)
+{
+	return i ? -EINVAL : 0;
+}
+
+static u32 sh_vou_ntsc_mode(enum sh_vou_bus_fmt bus_fmt)
+{
+	switch (bus_fmt) {
+	default:
+		pr_warning("%s(): Invalid bus-format code %d, using default 8-bit\n",
+			   __func__, bus_fmt);
+	case SH_VOU_BUS_8BIT:
+		return 1;
+	case SH_VOU_BUS_16BIT:
+		return 0;
+	case SH_VOU_BUS_BT656:
+		return 3;
+	}
+}
+
+static int sh_vou_s_std(struct file *file, void *priv, v4l2_std_id std_id)
+{
+	struct sh_vou_device *vou_dev = video_drvdata(file);
+	int ret;
+
+	dev_dbg(vou_dev->v4l2_dev.dev, "%s(): 0x%llx\n", __func__, std_id);
+
+	if (std_id == vou_dev->std)
+		return 0;
+
+	if (vb2_is_busy(&vou_dev->queue))
+		return -EBUSY;
+
+	ret = v4l2_device_call_until_err(&vou_dev->v4l2_dev, 0, video,
+					 s_std_output, std_id);
+	/* Shall we continue, if the subdev doesn't support .s_std_output()? */
+	if (ret < 0 && ret != -ENOIOCTLCMD)
+		return ret;
+
+	vou_dev->rect.top = vou_dev->rect.left = 0;
+	vou_dev->rect.width = VOU_MAX_IMAGE_WIDTH;
+	if (std_id & V4L2_STD_525_60) {
+		sh_vou_reg_ab_set(vou_dev, VOUCR,
+			sh_vou_ntsc_mode(vou_dev->pdata->bus_fmt) << 29, 7 << 29);
+		vou_dev->rect.height = 480;
+	} else {
+		sh_vou_reg_ab_set(vou_dev, VOUCR, 5 << 29, 7 << 29);
+		vou_dev->rect.height = 576;
+	}
+
+	vou_dev->pix.width = vou_dev->rect.width;
+	vou_dev->pix.height = vou_dev->rect.height;
+	vou_dev->pix.bytesperline =
+		vou_dev->pix.width * vou_fmt[vou_dev->pix_idx].bpl;
+	vou_dev->pix.sizeimage = vou_dev->pix.height *
+		((vou_dev->pix.width * vou_fmt[vou_dev->pix_idx].bpp) >> 3);
+	vou_dev->std = std_id;
+	sh_vou_set_fmt_vid_out(vou_dev, &vou_dev->pix);
+
+	return 0;
+}
+
+static int sh_vou_g_std(struct file *file, void *priv, v4l2_std_id *std)
+{
+	struct sh_vou_device *vou_dev = video_drvdata(file);
+
+	dev_dbg(vou_dev->v4l2_dev.dev, "%s()\n", __func__);
+
+	*std = vou_dev->std;
+
+	return 0;
+}
+
+static int sh_vou_log_status(struct file *file, void *priv)
+{
+	struct sh_vou_device *vou_dev = video_drvdata(file);
+
+	pr_info("VOUER:   0x%08x\n", sh_vou_reg_a_read(vou_dev, VOUER));
+	pr_info("VOUCR:   0x%08x\n", sh_vou_reg_a_read(vou_dev, VOUCR));
+	pr_info("VOUSTR:  0x%08x\n", sh_vou_reg_a_read(vou_dev, VOUSTR));
+	pr_info("VOUVCR:  0x%08x\n", sh_vou_reg_a_read(vou_dev, VOUVCR));
+	pr_info("VOUISR:  0x%08x\n", sh_vou_reg_a_read(vou_dev, VOUISR));
+	pr_info("VOUBCR:  0x%08x\n", sh_vou_reg_a_read(vou_dev, VOUBCR));
+	pr_info("VOUDPR:  0x%08x\n", sh_vou_reg_a_read(vou_dev, VOUDPR));
+	pr_info("VOUDSR:  0x%08x\n", sh_vou_reg_a_read(vou_dev, VOUDSR));
+	pr_info("VOUVPR:  0x%08x\n", sh_vou_reg_a_read(vou_dev, VOUVPR));
+	pr_info("VOUIR:   0x%08x\n", sh_vou_reg_a_read(vou_dev, VOUIR));
+	pr_info("VOUSRR:  0x%08x\n", sh_vou_reg_a_read(vou_dev, VOUSRR));
+	pr_info("VOUMSR:  0x%08x\n", sh_vou_reg_a_read(vou_dev, VOUMSR));
+	pr_info("VOUHIR:  0x%08x\n", sh_vou_reg_a_read(vou_dev, VOUHIR));
+	pr_info("VOUDFR:  0x%08x\n", sh_vou_reg_a_read(vou_dev, VOUDFR));
+	pr_info("VOUAD1R: 0x%08x\n", sh_vou_reg_a_read(vou_dev, VOUAD1R));
+	pr_info("VOUAD2R: 0x%08x\n", sh_vou_reg_a_read(vou_dev, VOUAD2R));
+	pr_info("VOUAIR:  0x%08x\n", sh_vou_reg_a_read(vou_dev, VOUAIR));
+	pr_info("VOUSWR:  0x%08x\n", sh_vou_reg_a_read(vou_dev, VOUSWR));
+	pr_info("VOURCR:  0x%08x\n", sh_vou_reg_a_read(vou_dev, VOURCR));
+	pr_info("VOURPR:  0x%08x\n", sh_vou_reg_a_read(vou_dev, VOURPR));
+	return 0;
+}
+
+static int sh_vou_g_selection(struct file *file, void *fh,
+			      struct v4l2_selection *sel)
+{
+	struct sh_vou_device *vou_dev = video_drvdata(file);
+
+	if (sel->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		return -EINVAL;
+	switch (sel->target) {
+	case V4L2_SEL_TGT_COMPOSE:
+		sel->r = vou_dev->rect;
+		break;
+	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = VOU_MAX_IMAGE_WIDTH;
+		if (vou_dev->std & V4L2_STD_525_60)
+			sel->r.height = 480;
+		else
+			sel->r.height = 576;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/* Assume a dull encoder, do all the work ourselves. */
+static int sh_vou_s_selection(struct file *file, void *fh,
+			      struct v4l2_selection *sel)
+{
+	struct v4l2_rect *rect = &sel->r;
+	struct sh_vou_device *vou_dev = video_drvdata(file);
+	struct v4l2_crop sd_crop = {.type = V4L2_BUF_TYPE_VIDEO_OUTPUT};
+	struct v4l2_pix_format *pix = &vou_dev->pix;
+	struct sh_vou_geometry geo;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		/* Revisit: is this the correct code? */
+		.format.code = MEDIA_BUS_FMT_YUYV8_2X8,
+		.format.field = V4L2_FIELD_INTERLACED,
+		.format.colorspace = V4L2_COLORSPACE_SMPTE170M,
+	};
+	unsigned int img_height_max;
+	int ret;
+
+	if (sel->type != V4L2_BUF_TYPE_VIDEO_OUTPUT ||
+	    sel->target != V4L2_SEL_TGT_COMPOSE)
+		return -EINVAL;
+
+	if (vb2_is_busy(&vou_dev->queue))
+		return -EBUSY;
+
+	if (vou_dev->std & V4L2_STD_525_60)
+		img_height_max = 480;
+	else
+		img_height_max = 576;
+
+	v4l_bound_align_image(&rect->width,
+			      VOU_MIN_IMAGE_WIDTH, VOU_MAX_IMAGE_WIDTH, 1,
+			      &rect->height,
+			      VOU_MIN_IMAGE_HEIGHT, img_height_max, 1, 0);
+
+	if (rect->width + rect->left > VOU_MAX_IMAGE_WIDTH)
+		rect->left = VOU_MAX_IMAGE_WIDTH - rect->width;
+
+	if (rect->height + rect->top > img_height_max)
+		rect->top = img_height_max - rect->height;
+
+	geo.output = *rect;
+	geo.in_width = pix->width;
+	geo.in_height = pix->height;
+
+	/* Configure the encoder one-to-one, position at 0, ignore errors */
+	sd_crop.c.width = geo.output.width;
+	sd_crop.c.height = geo.output.height;
+	/*
+	 * We first issue a S_CROP, so that the subsequent S_FMT delivers the
+	 * final encoder configuration.
+	 */
+	v4l2_device_call_until_err(&vou_dev->v4l2_dev, 0, video,
+				   s_crop, &sd_crop);
+	format.format.width = geo.output.width;
+	format.format.height = geo.output.height;
+	ret = v4l2_device_call_until_err(&vou_dev->v4l2_dev, 0, pad,
+					 set_fmt, NULL, &format);
+	/* Must be implemented, so, don't check for -ENOIOCTLCMD */
+	if (ret < 0)
+		return ret;
+
+	/* Sanity checks */
+	if ((unsigned)format.format.width > VOU_MAX_IMAGE_WIDTH ||
+	    (unsigned)format.format.height > img_height_max ||
+	    format.format.code != MEDIA_BUS_FMT_YUYV8_2X8)
+		return -EIO;
+
+	geo.output.width = format.format.width;
+	geo.output.height = format.format.height;
+
+	/*
+	 * No down-scaling. According to the API, current call has precedence:
+	 * http://v4l2spec.bytesex.org/spec/x1904.htm#AEN1954 paragraph two.
+	 */
+	vou_adjust_input(&geo, vou_dev->std);
+
+	/* We tried to preserve output rectangle, but it could have changed */
+	vou_dev->rect = geo.output;
+	pix->width = geo.in_width;
+	pix->height = geo.in_height;
+
+	sh_vou_configure_geometry(vou_dev, vou_dev->pix_idx,
+				  geo.scale_idx_h, geo.scale_idx_v);
+
+	return 0;
+}
+
+static irqreturn_t sh_vou_isr(int irq, void *dev_id)
+{
+	struct sh_vou_device *vou_dev = dev_id;
+	static unsigned long j;
+	struct sh_vou_buffer *vb;
+	static int cnt;
+	u32 irq_status = sh_vou_reg_a_read(vou_dev, VOUIR), masked;
+	u32 vou_status = sh_vou_reg_a_read(vou_dev, VOUSTR);
+
+	if (!(irq_status & 0x300)) {
+		if (printk_timed_ratelimit(&j, 500))
+			dev_warn(vou_dev->v4l2_dev.dev, "IRQ status 0x%x!\n",
+				 irq_status);
+		return IRQ_NONE;
+	}
+
+	spin_lock(&vou_dev->lock);
+	if (!vou_dev->active || list_empty(&vou_dev->buf_list)) {
+		if (printk_timed_ratelimit(&j, 500))
+			dev_warn(vou_dev->v4l2_dev.dev,
+				 "IRQ without active buffer: %x!\n", irq_status);
+		/* Just ack: buf_release will disable further interrupts */
+		sh_vou_reg_a_set(vou_dev, VOUIR, 0, 0x300);
+		spin_unlock(&vou_dev->lock);
+		return IRQ_HANDLED;
+	}
+
+	masked = ~(0x300 & irq_status) & irq_status & 0x30304;
+	dev_dbg(vou_dev->v4l2_dev.dev,
+		"IRQ status 0x%x -> 0x%x, VOU status 0x%x, cnt %d\n",
+		irq_status, masked, vou_status, cnt);
+
+	cnt++;
+	/* side = vou_status & 0x10000; */
+
+	/* Clear only set interrupts */
+	sh_vou_reg_a_write(vou_dev, VOUIR, masked);
+
+	vb = vou_dev->active;
+	if (list_is_singular(&vb->list)) {
+		/* Keep cycling while no next buffer is available */
+		sh_vou_schedule_next(vou_dev, &vb->vb);
+		spin_unlock(&vou_dev->lock);
+		return IRQ_HANDLED;
+	}
+
+	list_del(&vb->list);
+
+	v4l2_get_timestamp(&vb->vb.timestamp);
+	vb->vb.sequence = vou_dev->sequence++;
+	vb->vb.field = V4L2_FIELD_INTERLACED;
+	vb2_buffer_done(&vb->vb.vb2_buf, VB2_BUF_STATE_DONE);
+
+	vou_dev->active = list_entry(vou_dev->buf_list.next,
+				     struct sh_vou_buffer, list);
+
+	if (list_is_singular(&vou_dev->buf_list)) {
+		/* Keep cycling while no next buffer is available */
+		sh_vou_schedule_next(vou_dev, &vou_dev->active->vb);
+	} else {
+		struct sh_vou_buffer *new = list_entry(vou_dev->active->list.next,
+						struct sh_vou_buffer, list);
+		sh_vou_schedule_next(vou_dev, &new->vb);
+	}
+
+	spin_unlock(&vou_dev->lock);
+
+	return IRQ_HANDLED;
+}
+
+static int sh_vou_hw_init(struct sh_vou_device *vou_dev)
+{
+	struct sh_vou_pdata *pdata = vou_dev->pdata;
+	u32 voucr = sh_vou_ntsc_mode(pdata->bus_fmt) << 29;
+	int i = 100;
+
+	/* Disable all IRQs */
+	sh_vou_reg_a_write(vou_dev, VOUIR, 0);
+
+	/* Reset VOU interfaces - registers unaffected */
+	sh_vou_reg_a_write(vou_dev, VOUSRR, 0x101);
+	while (--i && (sh_vou_reg_a_read(vou_dev, VOUSRR) & 0x101))
+		udelay(1);
+
+	if (!i)
+		return -ETIMEDOUT;
+
+	dev_dbg(vou_dev->v4l2_dev.dev, "Reset took %dus\n", 100 - i);
+
+	if (pdata->flags & SH_VOU_PCLK_FALLING)
+		voucr |= 1 << 28;
+	if (pdata->flags & SH_VOU_HSYNC_LOW)
+		voucr |= 1 << 27;
+	if (pdata->flags & SH_VOU_VSYNC_LOW)
+		voucr |= 1 << 26;
+	sh_vou_reg_ab_set(vou_dev, VOUCR, voucr, 0xfc000000);
+
+	/* Manual register side switching at first */
+	sh_vou_reg_a_write(vou_dev, VOURCR, 4);
+	/* Default - fixed HSYNC length, can be made configurable is required */
+	sh_vou_reg_ab_write(vou_dev, VOUMSR, 0x800000);
+
+	sh_vou_set_fmt_vid_out(vou_dev, &vou_dev->pix);
+
+	return 0;
+}
+
+/* File operations */
+static int sh_vou_open(struct file *file)
+{
+	struct sh_vou_device *vou_dev = video_drvdata(file);
+	int err;
+
+	if (mutex_lock_interruptible(&vou_dev->fop_lock))
+		return -ERESTARTSYS;
+
+	err = v4l2_fh_open(file);
+	if (err)
+		goto done_open;
+	if (v4l2_fh_is_singular_file(file) &&
+	    vou_dev->status == SH_VOU_INITIALISING) {
+		/* First open */
+		pm_runtime_get_sync(vou_dev->v4l2_dev.dev);
+		err = sh_vou_hw_init(vou_dev);
+		if (err < 0) {
+			pm_runtime_put(vou_dev->v4l2_dev.dev);
+			v4l2_fh_release(file);
+		} else {
+			vou_dev->status = SH_VOU_IDLE;
+		}
+	}
+done_open:
+	mutex_unlock(&vou_dev->fop_lock);
+	return err;
+}
+
+static int sh_vou_release(struct file *file)
+{
+	struct sh_vou_device *vou_dev = video_drvdata(file);
+	bool is_last;
+
+	mutex_lock(&vou_dev->fop_lock);
+	is_last = v4l2_fh_is_singular_file(file);
+	_vb2_fop_release(file, NULL);
+	if (is_last) {
+		/* Last close */
+		vou_dev->status = SH_VOU_INITIALISING;
+		sh_vou_reg_a_set(vou_dev, VOUER, 0, 0x101);
+		pm_runtime_put(vou_dev->v4l2_dev.dev);
+	}
+	mutex_unlock(&vou_dev->fop_lock);
+	return 0;
+}
+
+/* sh_vou display ioctl operations */
+static const struct v4l2_ioctl_ops sh_vou_ioctl_ops = {
+	.vidioc_querycap        	= sh_vou_querycap,
+	.vidioc_enum_fmt_vid_out	= sh_vou_enum_fmt_vid_out,
+	.vidioc_g_fmt_vid_out		= sh_vou_g_fmt_vid_out,
+	.vidioc_s_fmt_vid_out		= sh_vou_s_fmt_vid_out,
+	.vidioc_try_fmt_vid_out		= sh_vou_try_fmt_vid_out,
+	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
+	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
+	.vidioc_querybuf		= vb2_ioctl_querybuf,
+	.vidioc_qbuf			= vb2_ioctl_qbuf,
+	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
+	.vidioc_prepare_buf		= vb2_ioctl_prepare_buf,
+	.vidioc_streamon		= vb2_ioctl_streamon,
+	.vidioc_streamoff		= vb2_ioctl_streamoff,
+	.vidioc_expbuf			= vb2_ioctl_expbuf,
+	.vidioc_g_output		= sh_vou_g_output,
+	.vidioc_s_output		= sh_vou_s_output,
+	.vidioc_enum_output		= sh_vou_enum_output,
+	.vidioc_s_std			= sh_vou_s_std,
+	.vidioc_g_std			= sh_vou_g_std,
+	.vidioc_g_selection		= sh_vou_g_selection,
+	.vidioc_s_selection		= sh_vou_s_selection,
+	.vidioc_log_status		= sh_vou_log_status,
+};
+
+static const struct v4l2_file_operations sh_vou_fops = {
+	.owner		= THIS_MODULE,
+	.open		= sh_vou_open,
+	.release	= sh_vou_release,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap		= vb2_fop_mmap,
+	.poll		= vb2_fop_poll,
+	.write		= vb2_fop_write,
+};
+
+static const struct video_device sh_vou_video_template = {
+	.name		= "sh_vou",
+	.fops		= &sh_vou_fops,
+	.ioctl_ops	= &sh_vou_ioctl_ops,
+	.tvnorms	= V4L2_STD_525_60, /* PAL only supported in 8-bit non-bt656 mode */
+	.vfl_dir	= VFL_DIR_TX,
+};
+
+static int sh_vou_probe(struct platform_device *pdev)
+{
+	struct sh_vou_pdata *vou_pdata = pdev->dev.platform_data;
+	struct v4l2_rect *rect;
+	struct v4l2_pix_format *pix;
+	struct i2c_adapter *i2c_adap;
+	struct video_device *vdev;
+	struct sh_vou_device *vou_dev;
+	struct resource *reg_res;
+	struct v4l2_subdev *subdev;
+	struct vb2_queue *q;
+	int irq, ret;
+
+	reg_res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	irq = platform_get_irq(pdev, 0);
+
+	if (!vou_pdata || !reg_res || irq <= 0) {
+		dev_err(&pdev->dev, "Insufficient VOU platform information.\n");
+		return -ENODEV;
+	}
+
+	vou_dev = devm_kzalloc(&pdev->dev, sizeof(*vou_dev), GFP_KERNEL);
+	if (!vou_dev)
+		return -ENOMEM;
+
+	INIT_LIST_HEAD(&vou_dev->buf_list);
+	spin_lock_init(&vou_dev->lock);
+	mutex_init(&vou_dev->fop_lock);
+	vou_dev->pdata = vou_pdata;
+	vou_dev->status = SH_VOU_INITIALISING;
+	vou_dev->pix_idx = 1;
+
+	rect = &vou_dev->rect;
+	pix = &vou_dev->pix;
+
+	/* Fill in defaults */
+	vou_dev->std		= V4L2_STD_NTSC_M;
+	rect->left		= 0;
+	rect->top		= 0;
+	rect->width		= VOU_MAX_IMAGE_WIDTH;
+	rect->height		= 480;
+	pix->width		= VOU_MAX_IMAGE_WIDTH;
+	pix->height		= 480;
+	pix->pixelformat	= V4L2_PIX_FMT_NV16;
+	pix->field		= V4L2_FIELD_INTERLACED;
+	pix->bytesperline	= VOU_MAX_IMAGE_WIDTH;
+	pix->sizeimage		= VOU_MAX_IMAGE_WIDTH * 2 * 480;
+	pix->colorspace		= V4L2_COLORSPACE_SMPTE170M;
+
+	vou_dev->base = devm_ioremap_resource(&pdev->dev, reg_res);
+	if (IS_ERR(vou_dev->base))
+		return PTR_ERR(vou_dev->base);
+
+	ret = devm_request_irq(&pdev->dev, irq, sh_vou_isr, 0, "vou", vou_dev);
+	if (ret < 0)
+		return ret;
+
+	ret = v4l2_device_register(&pdev->dev, &vou_dev->v4l2_dev);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Error registering v4l2 device\n");
+		return ret;
+	}
+
+	vdev = &vou_dev->vdev;
+	*vdev = sh_vou_video_template;
+	if (vou_pdata->bus_fmt == SH_VOU_BUS_8BIT)
+		vdev->tvnorms |= V4L2_STD_PAL;
+	vdev->v4l2_dev = &vou_dev->v4l2_dev;
+	vdev->release = video_device_release_empty;
+	vdev->lock = &vou_dev->fop_lock;
+
+	video_set_drvdata(vdev, vou_dev);
+
+	/* Initialize the vb2 queue */
+	q = &vou_dev->queue;
+	q->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	q->io_modes = VB2_MMAP | VB2_DMABUF | VB2_WRITE;
+	q->drv_priv = vou_dev;
+	q->buf_struct_size = sizeof(struct sh_vou_buffer);
+	q->ops = &sh_vou_qops;
+	q->mem_ops = &vb2_dma_contig_memops;
+	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	q->min_buffers_needed = 2;
+	q->lock = &vou_dev->fop_lock;
+	ret = vb2_queue_init(q);
+	if (ret)
+		goto einitctx;
+
+	vou_dev->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
+	if (IS_ERR(vou_dev->alloc_ctx)) {
+		dev_err(&pdev->dev, "Can't allocate buffer context");
+		ret = PTR_ERR(vou_dev->alloc_ctx);
+		goto einitctx;
+	}
+	vdev->queue = q;
+	INIT_LIST_HEAD(&vou_dev->buf_list);
+
+	pm_runtime_enable(&pdev->dev);
+	pm_runtime_resume(&pdev->dev);
+
+	i2c_adap = i2c_get_adapter(vou_pdata->i2c_adap);
+	if (!i2c_adap) {
+		ret = -ENODEV;
+		goto ei2cgadap;
+	}
+
+	ret = sh_vou_hw_init(vou_dev);
+	if (ret < 0)
+		goto ereset;
+
+	subdev = v4l2_i2c_new_subdev_board(&vou_dev->v4l2_dev, i2c_adap,
+			vou_pdata->board_info, NULL);
+	if (!subdev) {
+		ret = -ENOMEM;
+		goto ei2cnd;
+	}
+
+	ret = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
+	if (ret < 0)
+		goto evregdev;
+
+	return 0;
+
+evregdev:
+ei2cnd:
+ereset:
+	i2c_put_adapter(i2c_adap);
+ei2cgadap:
+	vb2_dma_contig_cleanup_ctx(vou_dev->alloc_ctx);
+einitctx:
+	pm_runtime_disable(&pdev->dev);
+	v4l2_device_unregister(&vou_dev->v4l2_dev);
+	return ret;
+}
+
+static int sh_vou_remove(struct platform_device *pdev)
+{
+	struct v4l2_device *v4l2_dev = platform_get_drvdata(pdev);
+	struct sh_vou_device *vou_dev = container_of(v4l2_dev,
+						struct sh_vou_device, v4l2_dev);
+	struct v4l2_subdev *sd = list_entry(v4l2_dev->subdevs.next,
+					    struct v4l2_subdev, list);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	pm_runtime_disable(&pdev->dev);
+	video_unregister_device(&vou_dev->vdev);
+	i2c_put_adapter(client->adapter);
+	vb2_dma_contig_cleanup_ctx(vou_dev->alloc_ctx);
+	v4l2_device_unregister(&vou_dev->v4l2_dev);
+	return 0;
+}
+
+static struct platform_driver __refdata sh_vou = {
+	.remove  = sh_vou_remove,
+	.driver  = {
+		.name	= "sh-vou",
+	},
+};
+
+module_platform_driver_probe(sh_vou, sh_vou_probe);
+
+MODULE_DESCRIPTION("SuperH VOU driver");
+MODULE_AUTHOR("Guennadi Liakhovetski <g.liakhovetski@gmx.de>");
+MODULE_LICENSE("GPL v2");
+MODULE_VERSION("0.1.0");
+MODULE_ALIAS("platform:sh-vou");
diff --git a/drivers/media/platform/soc_camera/Kconfig b/drivers/media/platform/soc_camera/Kconfig
new file mode 100644
index 0000000..f2776cd
--- /dev/null
+++ b/drivers/media/platform/soc_camera/Kconfig
@@ -0,0 +1,91 @@
+config SOC_CAMERA
+	tristate "SoC camera support"
+	depends on VIDEO_V4L2 && HAS_DMA && I2C
+	select VIDEOBUF_GEN
+	select VIDEOBUF2_CORE
+	help
+	  SoC Camera is a common API to several cameras, not connecting
+	  over a bus like PCI or USB. For example some i2c camera connected
+	  directly to the data bus of an SoC.
+
+config SOC_CAMERA_SCALE_CROP
+	tristate
+
+config SOC_CAMERA_PLATFORM
+	tristate "platform camera support"
+	depends on SOC_CAMERA
+	help
+	  This is a generic SoC camera platform driver, useful for testing
+
+config VIDEO_MX3
+	tristate "i.MX3x Camera Sensor Interface driver"
+	depends on VIDEO_DEV && MX3_IPU && SOC_CAMERA
+	depends on MX3_IPU || COMPILE_TEST
+	depends on HAS_DMA
+	select VIDEOBUF2_DMA_CONTIG
+	---help---
+	  This is a v4l2 driver for the i.MX3x Camera Sensor Interface
+
+config VIDEO_PXA27x
+	tristate "PXA27x Quick Capture Interface driver"
+	depends on VIDEO_DEV && PXA27x && SOC_CAMERA
+	select VIDEOBUF_DMA_SG
+	---help---
+	  This is a v4l2 driver for the PXA27x Quick Capture Interface
+
+config VIDEO_RCAR_VIN
+	tristate "R-Car Video Input (VIN) support"
+	depends on VIDEO_DEV && SOC_CAMERA
+	depends on ARCH_SHMOBILE || COMPILE_TEST
+	depends on HAS_DMA
+	select VIDEOBUF2_DMA_CONTIG
+	select SOC_CAMERA_SCALE_CROP
+	---help---
+	  This is a v4l2 driver for the R-Car VIN Interface
+
+config VIDEO_SH_MOBILE_CSI2
+	tristate "SuperH Mobile MIPI CSI-2 Interface driver"
+	depends on VIDEO_DEV && SOC_CAMERA && HAVE_CLK
+	depends on ARCH_SHMOBILE || SUPERH || COMPILE_TEST
+	---help---
+	  This is a v4l2 driver for the SuperH MIPI CSI-2 Interface
+
+config VIDEO_SH_MOBILE_CEU
+	tristate "SuperH Mobile CEU Interface driver"
+	depends on VIDEO_DEV && SOC_CAMERA && HAS_DMA && HAVE_CLK
+	depends on ARCH_SHMOBILE || SUPERH || COMPILE_TEST
+	depends on HAS_DMA
+	select VIDEOBUF2_DMA_CONTIG
+	select SOC_CAMERA_SCALE_CROP
+	---help---
+	  This is a v4l2 driver for the SuperH Mobile CEU Interface
+
+config VIDEO_OMAP1
+	tristate "OMAP1 Camera Interface driver"
+	depends on VIDEO_DEV && SOC_CAMERA
+	depends on ARCH_OMAP1
+	depends on HAS_DMA
+	select VIDEOBUF_DMA_CONTIG
+	select VIDEOBUF_DMA_SG
+	---help---
+	  This is a v4l2 driver for the TI OMAP1 camera interface
+
+config VIDEO_MX2
+	tristate "i.MX27 Camera Sensor Interface driver"
+	depends on VIDEO_DEV && SOC_CAMERA
+	depends on SOC_IMX27 || COMPILE_TEST
+	depends on HAS_DMA
+	select VIDEOBUF2_DMA_CONTIG
+	---help---
+	  This is a v4l2 driver for the i.MX27 Camera Sensor Interface
+
+config VIDEO_ATMEL_ISI
+	tristate "ATMEL Image Sensor Interface (ISI) support"
+	depends on VIDEO_DEV && SOC_CAMERA
+	depends on ARCH_AT91 || COMPILE_TEST
+	depends on HAS_DMA
+	select VIDEOBUF2_DMA_CONTIG
+	---help---
+	  This module makes the ATMEL Image Sensor Interface available
+	  as a v4l2 device.
+
diff --git a/drivers/media/platform/soc_camera/Makefile b/drivers/media/platform/soc_camera/Makefile
new file mode 100644
index 0000000..2826382
--- /dev/null
+++ b/drivers/media/platform/soc_camera/Makefile
@@ -0,0 +1,16 @@
+obj-$(CONFIG_SOC_CAMERA)		+= soc_camera.o soc_mediabus.o
+obj-$(CONFIG_SOC_CAMERA_SCALE_CROP)	+= soc_scale_crop.o
+
+# a platform subdevice driver stub, allowing to support cameras by adding a
+# couple of callback functions to the board code
+obj-$(CONFIG_SOC_CAMERA_PLATFORM)	+= soc_camera_platform.o
+
+# soc-camera host drivers have to be linked after camera drivers
+obj-$(CONFIG_VIDEO_ATMEL_ISI)		+= atmel-isi.o
+obj-$(CONFIG_VIDEO_MX2)			+= mx2_camera.o
+obj-$(CONFIG_VIDEO_MX3)			+= mx3_camera.o
+obj-$(CONFIG_VIDEO_OMAP1)		+= omap1_camera.o
+obj-$(CONFIG_VIDEO_PXA27x)		+= pxa_camera.o
+obj-$(CONFIG_VIDEO_SH_MOBILE_CEU)	+= sh_mobile_ceu_camera.o
+obj-$(CONFIG_VIDEO_SH_MOBILE_CSI2)	+= sh_mobile_csi2.o
+obj-$(CONFIG_VIDEO_RCAR_VIN)		+= rcar_vin.o
diff --git a/drivers/media/platform/soc_camera/atmel-isi.c b/drivers/media/platform/soc_camera/atmel-isi.c
new file mode 100644
index 0000000..454f68f
--- /dev/null
+++ b/drivers/media/platform/soc_camera/atmel-isi.c
@@ -0,0 +1,1096 @@
+/*
+ * Copyright (c) 2011 Atmel Corporation
+ * Josh Wu, <josh.wu@atmel.com>
+ *
+ * Based on previous work by Lars Haring, <lars.haring@atmel.com>
+ * and Sedji Gaouaou
+ * Based on the bttv driver for Bt848 with respective copyright holders
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/clk.h>
+#include <linux/completion.h>
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+
+#include <media/soc_camera.h>
+#include <media/soc_mediabus.h>
+#include <media/v4l2-of.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "atmel-isi.h"
+
+#define MAX_BUFFER_NUM			32
+#define MAX_SUPPORT_WIDTH		2048
+#define MAX_SUPPORT_HEIGHT		2048
+#define VID_LIMIT_BYTES			(16 * 1024 * 1024)
+#define MIN_FRAME_RATE			15
+#define FRAME_INTERVAL_MILLI_SEC	(1000 / MIN_FRAME_RATE)
+
+/* Frame buffer descriptor */
+struct fbd {
+	/* Physical address of the frame buffer */
+	u32 fb_address;
+	/* DMA Control Register(only in HISI2) */
+	u32 dma_ctrl;
+	/* Physical address of the next fbd */
+	u32 next_fbd_address;
+};
+
+static void set_dma_ctrl(struct fbd *fb_desc, u32 ctrl)
+{
+	fb_desc->dma_ctrl = ctrl;
+}
+
+struct isi_dma_desc {
+	struct list_head list;
+	struct fbd *p_fbd;
+	dma_addr_t fbd_phys;
+};
+
+/* Frame buffer data */
+struct frame_buffer {
+	struct vb2_v4l2_buffer vb;
+	struct isi_dma_desc *p_dma_desc;
+	struct list_head list;
+};
+
+struct atmel_isi {
+	/* Protects the access of variables shared with the ISR */
+	spinlock_t			lock;
+	void __iomem			*regs;
+
+	int				sequence;
+
+	struct vb2_alloc_ctx		*alloc_ctx;
+
+	/* Allocate descriptors for dma buffer use */
+	struct fbd			*p_fb_descriptors;
+	dma_addr_t			fb_descriptors_phys;
+	struct				list_head dma_desc_head;
+	struct isi_dma_desc		dma_desc[MAX_BUFFER_NUM];
+
+	struct completion		complete;
+	/* ISI peripherial clock */
+	struct clk			*pclk;
+	unsigned int			irq;
+
+	struct isi_platform_data	pdata;
+	u16				width_flags;	/* max 12 bits */
+
+	struct list_head		video_buffer_list;
+	struct frame_buffer		*active;
+
+	struct soc_camera_host		soc_host;
+};
+
+static void isi_writel(struct atmel_isi *isi, u32 reg, u32 val)
+{
+	writel(val, isi->regs + reg);
+}
+static u32 isi_readl(struct atmel_isi *isi, u32 reg)
+{
+	return readl(isi->regs + reg);
+}
+
+static void configure_geometry(struct atmel_isi *isi, u32 width,
+			u32 height, u32 code)
+{
+	u32 cfg2;
+
+	/* According to sensor's output format to set cfg2 */
+	switch (code) {
+	default:
+	/* Grey */
+	case MEDIA_BUS_FMT_Y8_1X8:
+		cfg2 = ISI_CFG2_GRAYSCALE | ISI_CFG2_COL_SPACE_YCbCr;
+		break;
+	/* YUV */
+	case MEDIA_BUS_FMT_VYUY8_2X8:
+		cfg2 = ISI_CFG2_YCC_SWAP_MODE_3 | ISI_CFG2_COL_SPACE_YCbCr;
+		break;
+	case MEDIA_BUS_FMT_UYVY8_2X8:
+		cfg2 = ISI_CFG2_YCC_SWAP_MODE_2 | ISI_CFG2_COL_SPACE_YCbCr;
+		break;
+	case MEDIA_BUS_FMT_YVYU8_2X8:
+		cfg2 = ISI_CFG2_YCC_SWAP_MODE_1 | ISI_CFG2_COL_SPACE_YCbCr;
+		break;
+	case MEDIA_BUS_FMT_YUYV8_2X8:
+		cfg2 = ISI_CFG2_YCC_SWAP_DEFAULT | ISI_CFG2_COL_SPACE_YCbCr;
+		break;
+	/* RGB, TODO */
+	}
+
+	isi_writel(isi, ISI_CTRL, ISI_CTRL_DIS);
+	/* Set width */
+	cfg2 |= ((width - 1) << ISI_CFG2_IM_HSIZE_OFFSET) &
+			ISI_CFG2_IM_HSIZE_MASK;
+	/* Set height */
+	cfg2 |= ((height - 1) << ISI_CFG2_IM_VSIZE_OFFSET)
+			& ISI_CFG2_IM_VSIZE_MASK;
+	isi_writel(isi, ISI_CFG2, cfg2);
+}
+
+static bool is_supported(struct soc_camera_device *icd,
+		const u32 pixformat)
+{
+	switch (pixformat) {
+	/* YUV, including grey */
+	case V4L2_PIX_FMT_GREY:
+	case V4L2_PIX_FMT_YUYV:
+	case V4L2_PIX_FMT_UYVY:
+	case V4L2_PIX_FMT_YVYU:
+	case V4L2_PIX_FMT_VYUY:
+		return true;
+	/* RGB, TODO */
+	default:
+		return false;
+	}
+}
+
+static irqreturn_t atmel_isi_handle_streaming(struct atmel_isi *isi)
+{
+	if (isi->active) {
+		struct vb2_v4l2_buffer *vbuf = &isi->active->vb;
+		struct frame_buffer *buf = isi->active;
+
+		list_del_init(&buf->list);
+		v4l2_get_timestamp(&vbuf->timestamp);
+		vbuf->sequence = isi->sequence++;
+		vb2_buffer_done(&vbuf->vb2_buf, VB2_BUF_STATE_DONE);
+	}
+
+	if (list_empty(&isi->video_buffer_list)) {
+		isi->active = NULL;
+	} else {
+		/* start next dma frame. */
+		isi->active = list_entry(isi->video_buffer_list.next,
+					struct frame_buffer, list);
+		isi_writel(isi, ISI_DMA_C_DSCR,
+			(u32)isi->active->p_dma_desc->fbd_phys);
+		isi_writel(isi, ISI_DMA_C_CTRL,
+			ISI_DMA_CTRL_FETCH | ISI_DMA_CTRL_DONE);
+		isi_writel(isi, ISI_DMA_CHER, ISI_DMA_CHSR_C_CH);
+	}
+	return IRQ_HANDLED;
+}
+
+/* ISI interrupt service routine */
+static irqreturn_t isi_interrupt(int irq, void *dev_id)
+{
+	struct atmel_isi *isi = dev_id;
+	u32 status, mask, pending;
+	irqreturn_t ret = IRQ_NONE;
+
+	spin_lock(&isi->lock);
+
+	status = isi_readl(isi, ISI_STATUS);
+	mask = isi_readl(isi, ISI_INTMASK);
+	pending = status & mask;
+
+	if (pending & ISI_CTRL_SRST) {
+		complete(&isi->complete);
+		isi_writel(isi, ISI_INTDIS, ISI_CTRL_SRST);
+		ret = IRQ_HANDLED;
+	} else if (pending & ISI_CTRL_DIS) {
+		complete(&isi->complete);
+		isi_writel(isi, ISI_INTDIS, ISI_CTRL_DIS);
+		ret = IRQ_HANDLED;
+	} else {
+		if (likely(pending & ISI_SR_CXFR_DONE))
+			ret = atmel_isi_handle_streaming(isi);
+	}
+
+	spin_unlock(&isi->lock);
+	return ret;
+}
+
+#define	WAIT_ISI_RESET		1
+#define	WAIT_ISI_DISABLE	0
+static int atmel_isi_wait_status(struct atmel_isi *isi, int wait_reset)
+{
+	unsigned long timeout;
+	/*
+	 * The reset or disable will only succeed if we have a
+	 * pixel clock from the camera.
+	 */
+	init_completion(&isi->complete);
+
+	if (wait_reset) {
+		isi_writel(isi, ISI_INTEN, ISI_CTRL_SRST);
+		isi_writel(isi, ISI_CTRL, ISI_CTRL_SRST);
+	} else {
+		isi_writel(isi, ISI_INTEN, ISI_CTRL_DIS);
+		isi_writel(isi, ISI_CTRL, ISI_CTRL_DIS);
+	}
+
+	timeout = wait_for_completion_timeout(&isi->complete,
+			msecs_to_jiffies(500));
+	if (timeout == 0)
+		return -ETIMEDOUT;
+
+	return 0;
+}
+
+/* ------------------------------------------------------------------
+	Videobuf operations
+   ------------------------------------------------------------------*/
+static int queue_setup(struct vb2_queue *vq, const void *parg,
+				unsigned int *nbuffers, unsigned int *nplanes,
+				unsigned int sizes[], void *alloc_ctxs[])
+{
+	struct soc_camera_device *icd = soc_camera_from_vb2q(vq);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct atmel_isi *isi = ici->priv;
+	unsigned long size;
+
+	size = icd->sizeimage;
+
+	if (!*nbuffers || *nbuffers > MAX_BUFFER_NUM)
+		*nbuffers = MAX_BUFFER_NUM;
+
+	if (size * *nbuffers > VID_LIMIT_BYTES)
+		*nbuffers = VID_LIMIT_BYTES / size;
+
+	*nplanes = 1;
+	sizes[0] = size;
+	alloc_ctxs[0] = isi->alloc_ctx;
+
+	isi->sequence = 0;
+	isi->active = NULL;
+
+	dev_dbg(icd->parent, "%s, count=%d, size=%ld\n", __func__,
+		*nbuffers, size);
+
+	return 0;
+}
+
+static int buffer_init(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct frame_buffer *buf = container_of(vbuf, struct frame_buffer, vb);
+
+	buf->p_dma_desc = NULL;
+	INIT_LIST_HEAD(&buf->list);
+
+	return 0;
+}
+
+static int buffer_prepare(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue);
+	struct frame_buffer *buf = container_of(vbuf, struct frame_buffer, vb);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct atmel_isi *isi = ici->priv;
+	unsigned long size;
+	struct isi_dma_desc *desc;
+
+	size = icd->sizeimage;
+
+	if (vb2_plane_size(vb, 0) < size) {
+		dev_err(icd->parent, "%s data will not fit into plane (%lu < %lu)\n",
+				__func__, vb2_plane_size(vb, 0), size);
+		return -EINVAL;
+	}
+
+	vb2_set_plane_payload(vb, 0, size);
+
+	if (!buf->p_dma_desc) {
+		if (list_empty(&isi->dma_desc_head)) {
+			dev_err(icd->parent, "Not enough dma descriptors.\n");
+			return -EINVAL;
+		} else {
+			/* Get an available descriptor */
+			desc = list_entry(isi->dma_desc_head.next,
+						struct isi_dma_desc, list);
+			/* Delete the descriptor since now it is used */
+			list_del_init(&desc->list);
+
+			/* Initialize the dma descriptor */
+			desc->p_fbd->fb_address =
+					vb2_dma_contig_plane_dma_addr(vb, 0);
+			desc->p_fbd->next_fbd_address = 0;
+			set_dma_ctrl(desc->p_fbd, ISI_DMA_CTRL_WB);
+
+			buf->p_dma_desc = desc;
+		}
+	}
+	return 0;
+}
+
+static void buffer_cleanup(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct atmel_isi *isi = ici->priv;
+	struct frame_buffer *buf = container_of(vbuf, struct frame_buffer, vb);
+
+	/* This descriptor is available now and we add to head list */
+	if (buf->p_dma_desc)
+		list_add(&buf->p_dma_desc->list, &isi->dma_desc_head);
+}
+
+static void start_dma(struct atmel_isi *isi, struct frame_buffer *buffer)
+{
+	u32 ctrl, cfg1;
+
+	cfg1 = isi_readl(isi, ISI_CFG1);
+	/* Enable irq: cxfr for the codec path, pxfr for the preview path */
+	isi_writel(isi, ISI_INTEN,
+			ISI_SR_CXFR_DONE | ISI_SR_PXFR_DONE);
+
+	/* Check if already in a frame */
+	if (isi_readl(isi, ISI_STATUS) & ISI_CTRL_CDC) {
+		dev_err(isi->soc_host.icd->parent, "Already in frame handling.\n");
+		return;
+	}
+
+	isi_writel(isi, ISI_DMA_C_DSCR, (u32)buffer->p_dma_desc->fbd_phys);
+	isi_writel(isi, ISI_DMA_C_CTRL, ISI_DMA_CTRL_FETCH | ISI_DMA_CTRL_DONE);
+	isi_writel(isi, ISI_DMA_CHER, ISI_DMA_CHSR_C_CH);
+
+	cfg1 &= ~ISI_CFG1_FRATE_DIV_MASK;
+	/* Enable linked list */
+	cfg1 |= isi->pdata.frate | ISI_CFG1_DISCR;
+
+	/* Enable codec path and ISI */
+	ctrl = ISI_CTRL_CDC | ISI_CTRL_EN;
+	isi_writel(isi, ISI_CTRL, ctrl);
+	isi_writel(isi, ISI_CFG1, cfg1);
+}
+
+static void buffer_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct atmel_isi *isi = ici->priv;
+	struct frame_buffer *buf = container_of(vbuf, struct frame_buffer, vb);
+	unsigned long flags = 0;
+
+	spin_lock_irqsave(&isi->lock, flags);
+	list_add_tail(&buf->list, &isi->video_buffer_list);
+
+	if (isi->active == NULL) {
+		isi->active = buf;
+		if (vb2_is_streaming(vb->vb2_queue))
+			start_dma(isi, buf);
+	}
+	spin_unlock_irqrestore(&isi->lock, flags);
+}
+
+static int start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct soc_camera_device *icd = soc_camera_from_vb2q(vq);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct atmel_isi *isi = ici->priv;
+	int ret;
+
+	pm_runtime_get_sync(ici->v4l2_dev.dev);
+
+	/* Reset ISI */
+	ret = atmel_isi_wait_status(isi, WAIT_ISI_RESET);
+	if (ret < 0) {
+		dev_err(icd->parent, "Reset ISI timed out\n");
+		pm_runtime_put(ici->v4l2_dev.dev);
+		return ret;
+	}
+	/* Disable all interrupts */
+	isi_writel(isi, ISI_INTDIS, (u32)~0UL);
+
+	configure_geometry(isi, icd->user_width, icd->user_height,
+				icd->current_fmt->code);
+
+	spin_lock_irq(&isi->lock);
+	/* Clear any pending interrupt */
+	isi_readl(isi, ISI_STATUS);
+
+	if (count)
+		start_dma(isi, isi->active);
+	spin_unlock_irq(&isi->lock);
+
+	return 0;
+}
+
+/* abort streaming and wait for last buffer */
+static void stop_streaming(struct vb2_queue *vq)
+{
+	struct soc_camera_device *icd = soc_camera_from_vb2q(vq);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct atmel_isi *isi = ici->priv;
+	struct frame_buffer *buf, *node;
+	int ret = 0;
+	unsigned long timeout;
+
+	spin_lock_irq(&isi->lock);
+	isi->active = NULL;
+	/* Release all active buffers */
+	list_for_each_entry_safe(buf, node, &isi->video_buffer_list, list) {
+		list_del_init(&buf->list);
+		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+	}
+	spin_unlock_irq(&isi->lock);
+
+	timeout = jiffies + FRAME_INTERVAL_MILLI_SEC * HZ;
+	/* Wait until the end of the current frame. */
+	while ((isi_readl(isi, ISI_STATUS) & ISI_CTRL_CDC) &&
+			time_before(jiffies, timeout))
+		msleep(1);
+
+	if (time_after(jiffies, timeout))
+		dev_err(icd->parent,
+			"Timeout waiting for finishing codec request\n");
+
+	/* Disable interrupts */
+	isi_writel(isi, ISI_INTDIS,
+			ISI_SR_CXFR_DONE | ISI_SR_PXFR_DONE);
+
+	/* Disable ISI and wait for it is done */
+	ret = atmel_isi_wait_status(isi, WAIT_ISI_DISABLE);
+	if (ret < 0)
+		dev_err(icd->parent, "Disable ISI timed out\n");
+
+	pm_runtime_put(ici->v4l2_dev.dev);
+}
+
+static struct vb2_ops isi_video_qops = {
+	.queue_setup		= queue_setup,
+	.buf_init		= buffer_init,
+	.buf_prepare		= buffer_prepare,
+	.buf_cleanup		= buffer_cleanup,
+	.buf_queue		= buffer_queue,
+	.start_streaming	= start_streaming,
+	.stop_streaming		= stop_streaming,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
+};
+
+/* ------------------------------------------------------------------
+	SOC camera operations for the device
+   ------------------------------------------------------------------*/
+static int isi_camera_init_videobuf(struct vb2_queue *q,
+				     struct soc_camera_device *icd)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+
+	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	q->io_modes = VB2_MMAP;
+	q->drv_priv = icd;
+	q->buf_struct_size = sizeof(struct frame_buffer);
+	q->ops = &isi_video_qops;
+	q->mem_ops = &vb2_dma_contig_memops;
+	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	q->lock = &ici->host_lock;
+
+	return vb2_queue_init(q);
+}
+
+static int isi_camera_set_fmt(struct soc_camera_device *icd,
+			      struct v4l2_format *f)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	const struct soc_camera_format_xlate *xlate;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_mbus_framefmt *mf = &format.format;
+	int ret;
+
+	/* check with atmel-isi support format, if not support use YUYV */
+	if (!is_supported(icd, pix->pixelformat))
+		pix->pixelformat = V4L2_PIX_FMT_YUYV;
+
+	xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
+	if (!xlate) {
+		dev_warn(icd->parent, "Format %x not found\n",
+			 pix->pixelformat);
+		return -EINVAL;
+	}
+
+	dev_dbg(icd->parent, "Plan to set format %dx%d\n",
+			pix->width, pix->height);
+
+	mf->width	= pix->width;
+	mf->height	= pix->height;
+	mf->field	= pix->field;
+	mf->colorspace	= pix->colorspace;
+	mf->code	= xlate->code;
+
+	ret = v4l2_subdev_call(sd, pad, set_fmt, NULL, &format);
+	if (ret < 0)
+		return ret;
+
+	if (mf->code != xlate->code)
+		return -EINVAL;
+
+	pix->width		= mf->width;
+	pix->height		= mf->height;
+	pix->field		= mf->field;
+	pix->colorspace		= mf->colorspace;
+	icd->current_fmt	= xlate;
+
+	dev_dbg(icd->parent, "Finally set format %dx%d\n",
+		pix->width, pix->height);
+
+	return ret;
+}
+
+static int isi_camera_try_fmt(struct soc_camera_device *icd,
+			      struct v4l2_format *f)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	const struct soc_camera_format_xlate *xlate;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct v4l2_subdev_pad_config pad_cfg;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+	};
+	struct v4l2_mbus_framefmt *mf = &format.format;
+	u32 pixfmt = pix->pixelformat;
+	int ret;
+
+	/* check with atmel-isi support format, if not support use YUYV */
+	if (!is_supported(icd, pix->pixelformat))
+		pix->pixelformat = V4L2_PIX_FMT_YUYV;
+
+	xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
+	if (pixfmt && !xlate) {
+		dev_warn(icd->parent, "Format %x not found\n", pixfmt);
+		return -EINVAL;
+	}
+
+	/* limit to Atmel ISI hardware capabilities */
+	if (pix->height > MAX_SUPPORT_HEIGHT)
+		pix->height = MAX_SUPPORT_HEIGHT;
+	if (pix->width > MAX_SUPPORT_WIDTH)
+		pix->width = MAX_SUPPORT_WIDTH;
+
+	/* limit to sensor capabilities */
+	mf->width	= pix->width;
+	mf->height	= pix->height;
+	mf->field	= pix->field;
+	mf->colorspace	= pix->colorspace;
+	mf->code	= xlate->code;
+
+	ret = v4l2_subdev_call(sd, pad, set_fmt, &pad_cfg, &format);
+	if (ret < 0)
+		return ret;
+
+	pix->width	= mf->width;
+	pix->height	= mf->height;
+	pix->colorspace	= mf->colorspace;
+
+	switch (mf->field) {
+	case V4L2_FIELD_ANY:
+		pix->field = V4L2_FIELD_NONE;
+		break;
+	case V4L2_FIELD_NONE:
+		break;
+	default:
+		dev_err(icd->parent, "Field type %d unsupported.\n",
+			mf->field);
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static const struct soc_mbus_pixelfmt isi_camera_formats[] = {
+	{
+		.fourcc			= V4L2_PIX_FMT_YUYV,
+		.name			= "Packed YUV422 16 bit",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+};
+
+/* This will be corrected as we get more formats */
+static bool isi_camera_packing_supported(const struct soc_mbus_pixelfmt *fmt)
+{
+	return	fmt->packing == SOC_MBUS_PACKING_NONE ||
+		(fmt->bits_per_sample == 8 &&
+		 fmt->packing == SOC_MBUS_PACKING_2X8_PADHI) ||
+		(fmt->bits_per_sample > 8 &&
+		 fmt->packing == SOC_MBUS_PACKING_EXTEND16);
+}
+
+#define ISI_BUS_PARAM (V4L2_MBUS_MASTER |	\
+		V4L2_MBUS_HSYNC_ACTIVE_HIGH |	\
+		V4L2_MBUS_HSYNC_ACTIVE_LOW |	\
+		V4L2_MBUS_VSYNC_ACTIVE_HIGH |	\
+		V4L2_MBUS_VSYNC_ACTIVE_LOW |	\
+		V4L2_MBUS_PCLK_SAMPLE_RISING |	\
+		V4L2_MBUS_PCLK_SAMPLE_FALLING |	\
+		V4L2_MBUS_DATA_ACTIVE_HIGH)
+
+static int isi_camera_try_bus_param(struct soc_camera_device *icd,
+				    unsigned char buswidth)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct atmel_isi *isi = ici->priv;
+	struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
+	unsigned long common_flags;
+	int ret;
+
+	ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
+	if (!ret) {
+		common_flags = soc_mbus_config_compatible(&cfg,
+							  ISI_BUS_PARAM);
+		if (!common_flags) {
+			dev_warn(icd->parent,
+				 "Flags incompatible: camera 0x%x, host 0x%x\n",
+				 cfg.flags, ISI_BUS_PARAM);
+			return -EINVAL;
+		}
+	} else if (ret != -ENOIOCTLCMD) {
+		return ret;
+	}
+
+	if ((1 << (buswidth - 1)) & isi->width_flags)
+		return 0;
+	return -EINVAL;
+}
+
+
+static int isi_camera_get_formats(struct soc_camera_device *icd,
+				  unsigned int idx,
+				  struct soc_camera_format_xlate *xlate)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	int formats = 0, ret;
+	/* sensor format */
+	struct v4l2_subdev_mbus_code_enum code = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.index = idx,
+	};
+	/* soc camera host format */
+	const struct soc_mbus_pixelfmt *fmt;
+
+	ret = v4l2_subdev_call(sd, pad, enum_mbus_code, NULL, &code);
+	if (ret < 0)
+		/* No more formats */
+		return 0;
+
+	fmt = soc_mbus_get_fmtdesc(code.code);
+	if (!fmt) {
+		dev_err(icd->parent,
+			"Invalid format code #%u: %d\n", idx, code.code);
+		return 0;
+	}
+
+	/* This also checks support for the requested bits-per-sample */
+	ret = isi_camera_try_bus_param(icd, fmt->bits_per_sample);
+	if (ret < 0) {
+		dev_err(icd->parent,
+			"Fail to try the bus parameters.\n");
+		return 0;
+	}
+
+	switch (code.code) {
+	case MEDIA_BUS_FMT_UYVY8_2X8:
+	case MEDIA_BUS_FMT_VYUY8_2X8:
+	case MEDIA_BUS_FMT_YUYV8_2X8:
+	case MEDIA_BUS_FMT_YVYU8_2X8:
+		formats++;
+		if (xlate) {
+			xlate->host_fmt	= &isi_camera_formats[0];
+			xlate->code	= code.code;
+			xlate++;
+			dev_dbg(icd->parent, "Providing format %s using code %d\n",
+				isi_camera_formats[0].name, code.code);
+		}
+		break;
+	default:
+		if (!isi_camera_packing_supported(fmt))
+			return 0;
+		if (xlate)
+			dev_dbg(icd->parent,
+				"Providing format %s in pass-through mode\n",
+				fmt->name);
+	}
+
+	/* Generic pass-through */
+	formats++;
+	if (xlate) {
+		xlate->host_fmt	= fmt;
+		xlate->code	= code.code;
+		xlate++;
+	}
+
+	return formats;
+}
+
+static int isi_camera_add_device(struct soc_camera_device *icd)
+{
+	dev_dbg(icd->parent, "Atmel ISI Camera driver attached to camera %d\n",
+		 icd->devnum);
+
+	return 0;
+}
+
+static void isi_camera_remove_device(struct soc_camera_device *icd)
+{
+	dev_dbg(icd->parent, "Atmel ISI Camera driver detached from camera %d\n",
+		 icd->devnum);
+}
+
+static unsigned int isi_camera_poll(struct file *file, poll_table *pt)
+{
+	struct soc_camera_device *icd = file->private_data;
+
+	return vb2_poll(&icd->vb2_vidq, file, pt);
+}
+
+static int isi_camera_querycap(struct soc_camera_host *ici,
+			       struct v4l2_capability *cap)
+{
+	strcpy(cap->driver, "atmel-isi");
+	strcpy(cap->card, "Atmel Image Sensor Interface");
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+
+	return 0;
+}
+
+static int isi_camera_set_bus_param(struct soc_camera_device *icd)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct atmel_isi *isi = ici->priv;
+	struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
+	unsigned long common_flags;
+	int ret;
+	u32 cfg1 = 0;
+
+	ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
+	if (!ret) {
+		common_flags = soc_mbus_config_compatible(&cfg,
+							  ISI_BUS_PARAM);
+		if (!common_flags) {
+			dev_warn(icd->parent,
+				 "Flags incompatible: camera 0x%x, host 0x%x\n",
+				 cfg.flags, ISI_BUS_PARAM);
+			return -EINVAL;
+		}
+	} else if (ret != -ENOIOCTLCMD) {
+		return ret;
+	} else {
+		common_flags = ISI_BUS_PARAM;
+	}
+	dev_dbg(icd->parent, "Flags cam: 0x%x host: 0x%x common: 0x%lx\n",
+		cfg.flags, ISI_BUS_PARAM, common_flags);
+
+	/* Make choises, based on platform preferences */
+	if ((common_flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH) &&
+	    (common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)) {
+		if (isi->pdata.hsync_act_low)
+			common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_HIGH;
+		else
+			common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_LOW;
+	}
+
+	if ((common_flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH) &&
+	    (common_flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)) {
+		if (isi->pdata.vsync_act_low)
+			common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_HIGH;
+		else
+			common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_LOW;
+	}
+
+	if ((common_flags & V4L2_MBUS_PCLK_SAMPLE_RISING) &&
+	    (common_flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)) {
+		if (isi->pdata.pclk_act_falling)
+			common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_RISING;
+		else
+			common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_FALLING;
+	}
+
+	cfg.flags = common_flags;
+	ret = v4l2_subdev_call(sd, video, s_mbus_config, &cfg);
+	if (ret < 0 && ret != -ENOIOCTLCMD) {
+		dev_dbg(icd->parent, "camera s_mbus_config(0x%lx) returned %d\n",
+			common_flags, ret);
+		return ret;
+	}
+
+	/* set bus param for ISI */
+	if (common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)
+		cfg1 |= ISI_CFG1_HSYNC_POL_ACTIVE_LOW;
+	if (common_flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
+		cfg1 |= ISI_CFG1_VSYNC_POL_ACTIVE_LOW;
+	if (common_flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
+		cfg1 |= ISI_CFG1_PIXCLK_POL_ACTIVE_FALLING;
+
+	dev_dbg(icd->parent, "vsync active %s, hsync active %s, sampling on pix clock %s edge\n",
+		common_flags & V4L2_MBUS_VSYNC_ACTIVE_LOW ? "low" : "high",
+		common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW ? "low" : "high",
+		common_flags & V4L2_MBUS_PCLK_SAMPLE_FALLING ? "falling" : "rising");
+
+	if (isi->pdata.has_emb_sync)
+		cfg1 |= ISI_CFG1_EMB_SYNC;
+	if (isi->pdata.full_mode)
+		cfg1 |= ISI_CFG1_FULL_MODE;
+
+	cfg1 |= ISI_CFG1_THMASK_BEATS_16;
+
+	/* Enable PM and peripheral clock before operate isi registers */
+	pm_runtime_get_sync(ici->v4l2_dev.dev);
+
+	isi_writel(isi, ISI_CTRL, ISI_CTRL_DIS);
+	isi_writel(isi, ISI_CFG1, cfg1);
+
+	pm_runtime_put(ici->v4l2_dev.dev);
+
+	return 0;
+}
+
+static struct soc_camera_host_ops isi_soc_camera_host_ops = {
+	.owner		= THIS_MODULE,
+	.add		= isi_camera_add_device,
+	.remove		= isi_camera_remove_device,
+	.set_fmt	= isi_camera_set_fmt,
+	.try_fmt	= isi_camera_try_fmt,
+	.get_formats	= isi_camera_get_formats,
+	.init_videobuf2	= isi_camera_init_videobuf,
+	.poll		= isi_camera_poll,
+	.querycap	= isi_camera_querycap,
+	.set_bus_param	= isi_camera_set_bus_param,
+};
+
+/* -----------------------------------------------------------------------*/
+static int atmel_isi_remove(struct platform_device *pdev)
+{
+	struct soc_camera_host *soc_host = to_soc_camera_host(&pdev->dev);
+	struct atmel_isi *isi = container_of(soc_host,
+					struct atmel_isi, soc_host);
+
+	soc_camera_host_unregister(soc_host);
+	vb2_dma_contig_cleanup_ctx(isi->alloc_ctx);
+	dma_free_coherent(&pdev->dev,
+			sizeof(struct fbd) * MAX_BUFFER_NUM,
+			isi->p_fb_descriptors,
+			isi->fb_descriptors_phys);
+	pm_runtime_disable(&pdev->dev);
+
+	return 0;
+}
+
+static int atmel_isi_parse_dt(struct atmel_isi *isi,
+			struct platform_device *pdev)
+{
+	struct device_node *np= pdev->dev.of_node;
+	struct v4l2_of_endpoint ep;
+	int err;
+
+	/* Default settings for ISI */
+	isi->pdata.full_mode = 1;
+	isi->pdata.frate = ISI_CFG1_FRATE_CAPTURE_ALL;
+
+	np = of_graph_get_next_endpoint(np, NULL);
+	if (!np) {
+		dev_err(&pdev->dev, "Could not find the endpoint\n");
+		return -EINVAL;
+	}
+
+	err = v4l2_of_parse_endpoint(np, &ep);
+	of_node_put(np);
+	if (err) {
+		dev_err(&pdev->dev, "Could not parse the endpoint\n");
+		return err;
+	}
+
+	switch (ep.bus.parallel.bus_width) {
+	case 8:
+		isi->pdata.data_width_flags = ISI_DATAWIDTH_8;
+		break;
+	case 10:
+		isi->pdata.data_width_flags =
+				ISI_DATAWIDTH_8 | ISI_DATAWIDTH_10;
+		break;
+	default:
+		dev_err(&pdev->dev, "Unsupported bus width: %d\n",
+				ep.bus.parallel.bus_width);
+		return -EINVAL;
+	}
+
+	if (ep.bus.parallel.flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)
+		isi->pdata.hsync_act_low = true;
+	if (ep.bus.parallel.flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
+		isi->pdata.vsync_act_low = true;
+	if (ep.bus.parallel.flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
+		isi->pdata.pclk_act_falling = true;
+
+	if (ep.bus_type == V4L2_MBUS_BT656)
+		isi->pdata.has_emb_sync = true;
+
+	return 0;
+}
+
+static int atmel_isi_probe(struct platform_device *pdev)
+{
+	unsigned int irq;
+	struct atmel_isi *isi;
+	struct resource *regs;
+	int ret, i;
+	struct soc_camera_host *soc_host;
+
+	isi = devm_kzalloc(&pdev->dev, sizeof(struct atmel_isi), GFP_KERNEL);
+	if (!isi) {
+		dev_err(&pdev->dev, "Can't allocate interface!\n");
+		return -ENOMEM;
+	}
+
+	isi->pclk = devm_clk_get(&pdev->dev, "isi_clk");
+	if (IS_ERR(isi->pclk))
+		return PTR_ERR(isi->pclk);
+
+	ret = atmel_isi_parse_dt(isi, pdev);
+	if (ret)
+		return ret;
+
+	isi->active = NULL;
+	spin_lock_init(&isi->lock);
+	INIT_LIST_HEAD(&isi->video_buffer_list);
+	INIT_LIST_HEAD(&isi->dma_desc_head);
+
+	isi->p_fb_descriptors = dma_alloc_coherent(&pdev->dev,
+				sizeof(struct fbd) * MAX_BUFFER_NUM,
+				&isi->fb_descriptors_phys,
+				GFP_KERNEL);
+	if (!isi->p_fb_descriptors) {
+		dev_err(&pdev->dev, "Can't allocate descriptors!\n");
+		return -ENOMEM;
+	}
+
+	for (i = 0; i < MAX_BUFFER_NUM; i++) {
+		isi->dma_desc[i].p_fbd = isi->p_fb_descriptors + i;
+		isi->dma_desc[i].fbd_phys = isi->fb_descriptors_phys +
+					i * sizeof(struct fbd);
+		list_add(&isi->dma_desc[i].list, &isi->dma_desc_head);
+	}
+
+	isi->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
+	if (IS_ERR(isi->alloc_ctx)) {
+		ret = PTR_ERR(isi->alloc_ctx);
+		goto err_alloc_ctx;
+	}
+
+	regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	isi->regs = devm_ioremap_resource(&pdev->dev, regs);
+	if (IS_ERR(isi->regs)) {
+		ret = PTR_ERR(isi->regs);
+		goto err_ioremap;
+	}
+
+	if (isi->pdata.data_width_flags & ISI_DATAWIDTH_8)
+		isi->width_flags = 1 << 7;
+	if (isi->pdata.data_width_flags & ISI_DATAWIDTH_10)
+		isi->width_flags |= 1 << 9;
+
+	irq = platform_get_irq(pdev, 0);
+	if (IS_ERR_VALUE(irq)) {
+		ret = irq;
+		goto err_req_irq;
+	}
+
+	ret = devm_request_irq(&pdev->dev, irq, isi_interrupt, 0, "isi", isi);
+	if (ret) {
+		dev_err(&pdev->dev, "Unable to request irq %d\n", irq);
+		goto err_req_irq;
+	}
+	isi->irq = irq;
+
+	soc_host		= &isi->soc_host;
+	soc_host->drv_name	= "isi-camera";
+	soc_host->ops		= &isi_soc_camera_host_ops;
+	soc_host->priv		= isi;
+	soc_host->v4l2_dev.dev	= &pdev->dev;
+	soc_host->nr		= pdev->id;
+
+	pm_suspend_ignore_children(&pdev->dev, true);
+	pm_runtime_enable(&pdev->dev);
+
+	ret = soc_camera_host_register(soc_host);
+	if (ret) {
+		dev_err(&pdev->dev, "Unable to register soc camera host\n");
+		goto err_register_soc_camera_host;
+	}
+	return 0;
+
+err_register_soc_camera_host:
+	pm_runtime_disable(&pdev->dev);
+err_req_irq:
+err_ioremap:
+	vb2_dma_contig_cleanup_ctx(isi->alloc_ctx);
+err_alloc_ctx:
+	dma_free_coherent(&pdev->dev,
+			sizeof(struct fbd) * MAX_BUFFER_NUM,
+			isi->p_fb_descriptors,
+			isi->fb_descriptors_phys);
+
+	return ret;
+}
+
+#ifdef CONFIG_PM
+static int atmel_isi_runtime_suspend(struct device *dev)
+{
+	struct soc_camera_host *soc_host = to_soc_camera_host(dev);
+	struct atmel_isi *isi = container_of(soc_host,
+					struct atmel_isi, soc_host);
+
+	clk_disable_unprepare(isi->pclk);
+
+	return 0;
+}
+static int atmel_isi_runtime_resume(struct device *dev)
+{
+	struct soc_camera_host *soc_host = to_soc_camera_host(dev);
+	struct atmel_isi *isi = container_of(soc_host,
+					struct atmel_isi, soc_host);
+
+	return clk_prepare_enable(isi->pclk);
+}
+#endif /* CONFIG_PM */
+
+static const struct dev_pm_ops atmel_isi_dev_pm_ops = {
+	SET_RUNTIME_PM_OPS(atmel_isi_runtime_suspend,
+				atmel_isi_runtime_resume, NULL)
+};
+
+static const struct of_device_id atmel_isi_of_match[] = {
+	{ .compatible = "atmel,at91sam9g45-isi" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, atmel_isi_of_match);
+
+static struct platform_driver atmel_isi_driver = {
+	.remove		= atmel_isi_remove,
+	.driver		= {
+		.name = "atmel_isi",
+		.of_match_table = of_match_ptr(atmel_isi_of_match),
+		.pm	= &atmel_isi_dev_pm_ops,
+	},
+};
+
+module_platform_driver_probe(atmel_isi_driver, atmel_isi_probe);
+
+MODULE_AUTHOR("Josh Wu <josh.wu@atmel.com>");
+MODULE_DESCRIPTION("The V4L2 driver for Atmel Linux");
+MODULE_LICENSE("GPL");
+MODULE_SUPPORTED_DEVICE("video");
diff --git a/drivers/media/platform/soc_camera/atmel-isi.h b/drivers/media/platform/soc_camera/atmel-isi.h
new file mode 100644
index 0000000..5acc771
--- /dev/null
+++ b/drivers/media/platform/soc_camera/atmel-isi.h
@@ -0,0 +1,128 @@
+/*
+ * Register definitions for the Atmel Image Sensor Interface.
+ *
+ * Copyright (C) 2011 Atmel Corporation
+ * Josh Wu, <josh.wu@atmel.com>
+ *
+ * Based on previous work by Lars Haring, <lars.haring@atmel.com>
+ * and Sedji Gaouaou
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#ifndef __ATMEL_ISI_H__
+#define __ATMEL_ISI_H__
+
+#include <linux/types.h>
+
+/* ISI_V2 register offsets */
+#define ISI_CFG1				0x0000
+#define ISI_CFG2				0x0004
+#define ISI_PSIZE				0x0008
+#define ISI_PDECF				0x000c
+#define ISI_Y2R_SET0				0x0010
+#define ISI_Y2R_SET1				0x0014
+#define ISI_R2Y_SET0				0x0018
+#define ISI_R2Y_SET1				0x001C
+#define ISI_R2Y_SET2				0x0020
+#define ISI_CTRL				0x0024
+#define ISI_STATUS				0x0028
+#define ISI_INTEN				0x002C
+#define ISI_INTDIS				0x0030
+#define ISI_INTMASK				0x0034
+#define ISI_DMA_CHER				0x0038
+#define ISI_DMA_CHDR				0x003C
+#define ISI_DMA_CHSR				0x0040
+#define ISI_DMA_P_ADDR				0x0044
+#define ISI_DMA_P_CTRL				0x0048
+#define ISI_DMA_P_DSCR				0x004C
+#define ISI_DMA_C_ADDR				0x0050
+#define ISI_DMA_C_CTRL				0x0054
+#define ISI_DMA_C_DSCR				0x0058
+
+/* Bitfields in CFG1 */
+#define ISI_CFG1_HSYNC_POL_ACTIVE_LOW		(1 << 2)
+#define ISI_CFG1_VSYNC_POL_ACTIVE_LOW		(1 << 3)
+#define ISI_CFG1_PIXCLK_POL_ACTIVE_FALLING	(1 << 4)
+#define ISI_CFG1_EMB_SYNC			(1 << 6)
+#define ISI_CFG1_CRC_SYNC			(1 << 7)
+/* Constants for FRATE(ISI_V2) */
+#define		ISI_CFG1_FRATE_CAPTURE_ALL	(0 << 8)
+#define		ISI_CFG1_FRATE_DIV_2		(1 << 8)
+#define		ISI_CFG1_FRATE_DIV_3		(2 << 8)
+#define		ISI_CFG1_FRATE_DIV_4		(3 << 8)
+#define		ISI_CFG1_FRATE_DIV_5		(4 << 8)
+#define		ISI_CFG1_FRATE_DIV_6		(5 << 8)
+#define		ISI_CFG1_FRATE_DIV_7		(6 << 8)
+#define		ISI_CFG1_FRATE_DIV_8		(7 << 8)
+#define		ISI_CFG1_FRATE_DIV_MASK		(7 << 8)
+#define ISI_CFG1_DISCR				(1 << 11)
+#define ISI_CFG1_FULL_MODE			(1 << 12)
+/* Definition for THMASK(ISI_V2) */
+#define		ISI_CFG1_THMASK_BEATS_4		(0 << 13)
+#define		ISI_CFG1_THMASK_BEATS_8		(1 << 13)
+#define		ISI_CFG1_THMASK_BEATS_16	(2 << 13)
+
+/* Bitfields in CFG2 */
+#define ISI_CFG2_GRAYSCALE			(1 << 13)
+#define ISI_CFG2_COL_SPACE_YCbCr		(0 << 15)
+#define ISI_CFG2_COL_SPACE_RGB			(1 << 15)
+/* Constants for YCC_SWAP(ISI_V2) */
+#define		ISI_CFG2_YCC_SWAP_DEFAULT	(0 << 28)
+#define		ISI_CFG2_YCC_SWAP_MODE_1	(1 << 28)
+#define		ISI_CFG2_YCC_SWAP_MODE_2	(2 << 28)
+#define		ISI_CFG2_YCC_SWAP_MODE_3	(3 << 28)
+#define		ISI_CFG2_YCC_SWAP_MODE_MASK	(3 << 28)
+#define ISI_CFG2_IM_VSIZE_OFFSET		0
+#define ISI_CFG2_IM_HSIZE_OFFSET		16
+#define ISI_CFG2_IM_VSIZE_MASK		(0x7FF << ISI_CFG2_IM_VSIZE_OFFSET)
+#define ISI_CFG2_IM_HSIZE_MASK		(0x7FF << ISI_CFG2_IM_HSIZE_OFFSET)
+
+/* Bitfields in CTRL */
+/* Also using in SR(ISI_V2) */
+#define ISI_CTRL_EN				(1 << 0)
+#define ISI_CTRL_CDC				(1 << 8)
+/* Also using in SR/IER/IDR/IMR(ISI_V2) */
+#define ISI_CTRL_DIS				(1 << 1)
+#define ISI_CTRL_SRST				(1 << 2)
+
+/* Bitfields in SR */
+#define ISI_SR_SIP				(1 << 19)
+/* Also using in SR/IER/IDR/IMR */
+#define ISI_SR_VSYNC				(1 << 10)
+#define ISI_SR_PXFR_DONE			(1 << 16)
+#define ISI_SR_CXFR_DONE			(1 << 17)
+#define ISI_SR_P_OVR				(1 << 24)
+#define ISI_SR_C_OVR				(1 << 25)
+#define ISI_SR_CRC_ERR				(1 << 26)
+#define ISI_SR_FR_OVR				(1 << 27)
+
+/* Bitfields in DMA_C_CTRL & in DMA_P_CTRL */
+#define ISI_DMA_CTRL_FETCH			(1 << 0)
+#define ISI_DMA_CTRL_WB				(1 << 1)
+#define ISI_DMA_CTRL_IEN			(1 << 2)
+#define ISI_DMA_CTRL_DONE			(1 << 3)
+
+/* Bitfields in DMA_CHSR/CHER/CHDR */
+#define ISI_DMA_CHSR_P_CH			(1 << 0)
+#define ISI_DMA_CHSR_C_CH			(1 << 1)
+
+/* Definition for isi_platform_data */
+#define ISI_DATAWIDTH_8				0x01
+#define ISI_DATAWIDTH_10			0x02
+
+struct v4l2_async_subdev;
+
+struct isi_platform_data {
+	u8 has_emb_sync;
+	u8 hsync_act_low;
+	u8 vsync_act_low;
+	u8 pclk_act_falling;
+	u8 full_mode;
+	u32 data_width_flags;
+	/* Using for ISI_CFG1 */
+	u32 frate;
+};
+
+#endif /* __ATMEL_ISI_H__ */
diff --git a/drivers/media/platform/soc_camera/mx2_camera.c b/drivers/media/platform/soc_camera/mx2_camera.c
new file mode 100644
index 0000000..1f28d21
--- /dev/null
+++ b/drivers/media/platform/soc_camera/mx2_camera.c
@@ -0,0 +1,1642 @@
+/*
+ * V4L2 Driver for i.MX27 camera host
+ *
+ * Copyright (C) 2008, Sascha Hauer, Pengutronix
+ * Copyright (C) 2010, Baruch Siach, Orex Computed Radiography
+ * Copyright (C) 2012, Javier Martin, Vista Silicon S.L.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/io.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/dma-mapping.h>
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/gcd.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/math64.h>
+#include <linux/mm.h>
+#include <linux/moduleparam.h>
+#include <linux/time.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+
+#include <media/v4l2-common.h>
+#include <media/v4l2-dev.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/videobuf2-dma-contig.h>
+#include <media/soc_camera.h>
+#include <media/soc_mediabus.h>
+
+#include <linux/videodev2.h>
+
+#include <linux/platform_data/camera-mx2.h>
+
+#include <asm/dma.h>
+
+#define MX2_CAM_DRV_NAME "mx2-camera"
+#define MX2_CAM_VERSION "0.0.6"
+#define MX2_CAM_DRIVER_DESCRIPTION "i.MX2x_Camera"
+
+/* reset values */
+#define CSICR1_RESET_VAL	0x40000800
+#define CSICR2_RESET_VAL	0x0
+#define CSICR3_RESET_VAL	0x0
+
+/* csi control reg 1 */
+#define CSICR1_SWAP16_EN	(1 << 31)
+#define CSICR1_EXT_VSYNC	(1 << 30)
+#define CSICR1_EOF_INTEN	(1 << 29)
+#define CSICR1_PRP_IF_EN	(1 << 28)
+#define CSICR1_CCIR_MODE	(1 << 27)
+#define CSICR1_COF_INTEN	(1 << 26)
+#define CSICR1_SF_OR_INTEN	(1 << 25)
+#define CSICR1_RF_OR_INTEN	(1 << 24)
+#define CSICR1_STATFF_LEVEL	(3 << 22)
+#define CSICR1_STATFF_INTEN	(1 << 21)
+#define CSICR1_RXFF_LEVEL(l)	(((l) & 3) << 19)
+#define CSICR1_RXFF_INTEN	(1 << 18)
+#define CSICR1_SOF_POL		(1 << 17)
+#define CSICR1_SOF_INTEN	(1 << 16)
+#define CSICR1_MCLKDIV(d)	(((d) & 0xF) << 12)
+#define CSICR1_HSYNC_POL	(1 << 11)
+#define CSICR1_CCIR_EN		(1 << 10)
+#define CSICR1_MCLKEN		(1 << 9)
+#define CSICR1_FCC		(1 << 8)
+#define CSICR1_PACK_DIR		(1 << 7)
+#define CSICR1_CLR_STATFIFO	(1 << 6)
+#define CSICR1_CLR_RXFIFO	(1 << 5)
+#define CSICR1_GCLK_MODE	(1 << 4)
+#define CSICR1_INV_DATA		(1 << 3)
+#define CSICR1_INV_PCLK		(1 << 2)
+#define CSICR1_REDGE		(1 << 1)
+#define CSICR1_FMT_MASK		(CSICR1_PACK_DIR | CSICR1_SWAP16_EN)
+
+#define SHIFT_STATFF_LEVEL	22
+#define SHIFT_RXFF_LEVEL	19
+#define SHIFT_MCLKDIV		12
+
+#define SHIFT_FRMCNT		16
+
+#define CSICR1			0x00
+#define CSICR2			0x04
+#define CSISR			0x08
+#define CSISTATFIFO		0x0c
+#define CSIRFIFO		0x10
+#define CSIRXCNT		0x14
+#define CSICR3			0x1c
+#define CSIDMASA_STATFIFO	0x20
+#define CSIDMATA_STATFIFO	0x24
+#define CSIDMASA_FB1		0x28
+#define CSIDMASA_FB2		0x2c
+#define CSIFBUF_PARA		0x30
+#define CSIIMAG_PARA		0x34
+
+/* EMMA PrP */
+#define PRP_CNTL			0x00
+#define PRP_INTR_CNTL			0x04
+#define PRP_INTRSTATUS			0x08
+#define PRP_SOURCE_Y_PTR		0x0c
+#define PRP_SOURCE_CB_PTR		0x10
+#define PRP_SOURCE_CR_PTR		0x14
+#define PRP_DEST_RGB1_PTR		0x18
+#define PRP_DEST_RGB2_PTR		0x1c
+#define PRP_DEST_Y_PTR			0x20
+#define PRP_DEST_CB_PTR			0x24
+#define PRP_DEST_CR_PTR			0x28
+#define PRP_SRC_FRAME_SIZE		0x2c
+#define PRP_DEST_CH1_LINE_STRIDE	0x30
+#define PRP_SRC_PIXEL_FORMAT_CNTL	0x34
+#define PRP_CH1_PIXEL_FORMAT_CNTL	0x38
+#define PRP_CH1_OUT_IMAGE_SIZE		0x3c
+#define PRP_CH2_OUT_IMAGE_SIZE		0x40
+#define PRP_SRC_LINE_STRIDE		0x44
+#define PRP_CSC_COEF_012		0x48
+#define PRP_CSC_COEF_345		0x4c
+#define PRP_CSC_COEF_678		0x50
+#define PRP_CH1_RZ_HORI_COEF1		0x54
+#define PRP_CH1_RZ_HORI_COEF2		0x58
+#define PRP_CH1_RZ_HORI_VALID		0x5c
+#define PRP_CH1_RZ_VERT_COEF1		0x60
+#define PRP_CH1_RZ_VERT_COEF2		0x64
+#define PRP_CH1_RZ_VERT_VALID		0x68
+#define PRP_CH2_RZ_HORI_COEF1		0x6c
+#define PRP_CH2_RZ_HORI_COEF2		0x70
+#define PRP_CH2_RZ_HORI_VALID		0x74
+#define PRP_CH2_RZ_VERT_COEF1		0x78
+#define PRP_CH2_RZ_VERT_COEF2		0x7c
+#define PRP_CH2_RZ_VERT_VALID		0x80
+
+#define PRP_CNTL_CH1EN		(1 << 0)
+#define PRP_CNTL_CH2EN		(1 << 1)
+#define PRP_CNTL_CSIEN		(1 << 2)
+#define PRP_CNTL_DATA_IN_YUV420	(0 << 3)
+#define PRP_CNTL_DATA_IN_YUV422	(1 << 3)
+#define PRP_CNTL_DATA_IN_RGB16	(2 << 3)
+#define PRP_CNTL_DATA_IN_RGB32	(3 << 3)
+#define PRP_CNTL_CH1_OUT_RGB8	(0 << 5)
+#define PRP_CNTL_CH1_OUT_RGB16	(1 << 5)
+#define PRP_CNTL_CH1_OUT_RGB32	(2 << 5)
+#define PRP_CNTL_CH1_OUT_YUV422	(3 << 5)
+#define PRP_CNTL_CH2_OUT_YUV420	(0 << 7)
+#define PRP_CNTL_CH2_OUT_YUV422 (1 << 7)
+#define PRP_CNTL_CH2_OUT_YUV444	(2 << 7)
+#define PRP_CNTL_CH1_LEN	(1 << 9)
+#define PRP_CNTL_CH2_LEN	(1 << 10)
+#define PRP_CNTL_SKIP_FRAME	(1 << 11)
+#define PRP_CNTL_SWRST		(1 << 12)
+#define PRP_CNTL_CLKEN		(1 << 13)
+#define PRP_CNTL_WEN		(1 << 14)
+#define PRP_CNTL_CH1BYP		(1 << 15)
+#define PRP_CNTL_IN_TSKIP(x)	((x) << 16)
+#define PRP_CNTL_CH1_TSKIP(x)	((x) << 19)
+#define PRP_CNTL_CH2_TSKIP(x)	((x) << 22)
+#define PRP_CNTL_INPUT_FIFO_LEVEL(x)	((x) << 25)
+#define PRP_CNTL_RZ_FIFO_LEVEL(x)	((x) << 27)
+#define PRP_CNTL_CH2B1EN	(1 << 29)
+#define PRP_CNTL_CH2B2EN	(1 << 30)
+#define PRP_CNTL_CH2FEN		(1 << 31)
+
+/* IRQ Enable and status register */
+#define PRP_INTR_RDERR		(1 << 0)
+#define PRP_INTR_CH1WERR	(1 << 1)
+#define PRP_INTR_CH2WERR	(1 << 2)
+#define PRP_INTR_CH1FC		(1 << 3)
+#define PRP_INTR_CH2FC		(1 << 5)
+#define PRP_INTR_LBOVF		(1 << 7)
+#define PRP_INTR_CH2OVF		(1 << 8)
+
+/* Resizing registers */
+#define PRP_RZ_VALID_TBL_LEN(x)	((x) << 24)
+#define PRP_RZ_VALID_BILINEAR	(1 << 31)
+
+#define MAX_VIDEO_MEM	16
+
+#define RESIZE_NUM_MIN	1
+#define RESIZE_NUM_MAX	20
+#define BC_COEF		3
+#define SZ_COEF		(1 << BC_COEF)
+
+#define RESIZE_DIR_H	0
+#define RESIZE_DIR_V	1
+
+#define RESIZE_ALGO_BILINEAR 0
+#define RESIZE_ALGO_AVERAGING 1
+
+struct mx2_prp_cfg {
+	int channel;
+	u32 in_fmt;
+	u32 out_fmt;
+	u32 src_pixel;
+	u32 ch1_pixel;
+	u32 irq_flags;
+	u32 csicr1;
+};
+
+/* prp resizing parameters */
+struct emma_prp_resize {
+	int		algo; /* type of algorithm used */
+	int		len; /* number of coefficients */
+	unsigned char	s[RESIZE_NUM_MAX]; /* table of coefficients */
+};
+
+/* prp configuration for a client-host fmt pair */
+struct mx2_fmt_cfg {
+	u32	in_fmt;
+	u32				out_fmt;
+	struct mx2_prp_cfg		cfg;
+};
+
+struct mx2_buf_internal {
+	struct list_head	queue;
+	int			bufnum;
+	bool			discard;
+};
+
+/* buffer for one video frame */
+struct mx2_buffer {
+	/* common v4l buffer stuff -- must be first */
+	struct vb2_v4l2_buffer vb;
+	struct mx2_buf_internal		internal;
+};
+
+enum mx2_camera_type {
+	IMX27_CAMERA,
+};
+
+struct mx2_camera_dev {
+	struct device		*dev;
+	struct soc_camera_host	soc_host;
+	struct clk		*clk_emma_ahb, *clk_emma_ipg;
+	struct clk		*clk_csi_ahb, *clk_csi_per;
+
+	void __iomem		*base_csi, *base_emma;
+
+	struct mx2_camera_platform_data *pdata;
+	unsigned long		platform_flags;
+
+	struct list_head	capture;
+	struct list_head	active_bufs;
+	struct list_head	discard;
+
+	spinlock_t		lock;
+
+	int			dma;
+	struct mx2_buffer	*active;
+	struct mx2_buffer	*fb1_active;
+	struct mx2_buffer	*fb2_active;
+
+	u32			csicr1;
+	enum mx2_camera_type	devtype;
+
+	struct mx2_buf_internal buf_discard[2];
+	void			*discard_buffer;
+	dma_addr_t		discard_buffer_dma;
+	size_t			discard_size;
+	struct mx2_fmt_cfg	*emma_prp;
+	struct emma_prp_resize	resizing[2];
+	unsigned int		s_width, s_height;
+	u32			frame_count;
+	struct vb2_alloc_ctx	*alloc_ctx;
+};
+
+static struct platform_device_id mx2_camera_devtype[] = {
+	{
+		.name = "imx27-camera",
+		.driver_data = IMX27_CAMERA,
+	}, {
+		/* sentinel */
+	}
+};
+MODULE_DEVICE_TABLE(platform, mx2_camera_devtype);
+
+static struct mx2_buffer *mx2_ibuf_to_buf(struct mx2_buf_internal *int_buf)
+{
+	return container_of(int_buf, struct mx2_buffer, internal);
+}
+
+static struct mx2_fmt_cfg mx27_emma_prp_table[] = {
+	/*
+	 * This is a generic configuration which is valid for most
+	 * prp input-output format combinations.
+	 * We set the incoming and outgoing pixelformat to a
+	 * 16 Bit wide format and adjust the bytesperline
+	 * accordingly. With this configuration the inputdata
+	 * will not be changed by the emma and could be any type
+	 * of 16 Bit Pixelformat.
+	 */
+	{
+		.in_fmt		= 0,
+		.out_fmt	= 0,
+		.cfg		= {
+			.channel	= 1,
+			.in_fmt		= PRP_CNTL_DATA_IN_RGB16,
+			.out_fmt	= PRP_CNTL_CH1_OUT_RGB16,
+			.src_pixel	= 0x2ca00565, /* RGB565 */
+			.ch1_pixel	= 0x2ca00565, /* RGB565 */
+			.irq_flags	= PRP_INTR_RDERR | PRP_INTR_CH1WERR |
+						PRP_INTR_CH1FC | PRP_INTR_LBOVF,
+			.csicr1		= 0,
+		}
+	},
+	{
+		.in_fmt		= MEDIA_BUS_FMT_UYVY8_2X8,
+		.out_fmt	= V4L2_PIX_FMT_YUYV,
+		.cfg		= {
+			.channel	= 1,
+			.in_fmt		= PRP_CNTL_DATA_IN_YUV422,
+			.out_fmt	= PRP_CNTL_CH1_OUT_YUV422,
+			.src_pixel	= 0x22000888, /* YUV422 (YUYV) */
+			.ch1_pixel	= 0x62000888, /* YUV422 (YUYV) */
+			.irq_flags	= PRP_INTR_RDERR | PRP_INTR_CH1WERR |
+						PRP_INTR_CH1FC | PRP_INTR_LBOVF,
+			.csicr1		= CSICR1_SWAP16_EN,
+		}
+	},
+	{
+		.in_fmt		= MEDIA_BUS_FMT_YUYV8_2X8,
+		.out_fmt	= V4L2_PIX_FMT_YUYV,
+		.cfg		= {
+			.channel	= 1,
+			.in_fmt		= PRP_CNTL_DATA_IN_YUV422,
+			.out_fmt	= PRP_CNTL_CH1_OUT_YUV422,
+			.src_pixel	= 0x22000888, /* YUV422 (YUYV) */
+			.ch1_pixel	= 0x62000888, /* YUV422 (YUYV) */
+			.irq_flags	= PRP_INTR_RDERR | PRP_INTR_CH1WERR |
+						PRP_INTR_CH1FC | PRP_INTR_LBOVF,
+			.csicr1		= CSICR1_PACK_DIR,
+		}
+	},
+	{
+		.in_fmt		= MEDIA_BUS_FMT_YUYV8_2X8,
+		.out_fmt	= V4L2_PIX_FMT_YUV420,
+		.cfg		= {
+			.channel	= 2,
+			.in_fmt		= PRP_CNTL_DATA_IN_YUV422,
+			.out_fmt	= PRP_CNTL_CH2_OUT_YUV420,
+			.src_pixel	= 0x22000888, /* YUV422 (YUYV) */
+			.irq_flags	= PRP_INTR_RDERR | PRP_INTR_CH2WERR |
+					PRP_INTR_CH2FC | PRP_INTR_LBOVF |
+					PRP_INTR_CH2OVF,
+			.csicr1		= CSICR1_PACK_DIR,
+		}
+	},
+	{
+		.in_fmt		= MEDIA_BUS_FMT_UYVY8_2X8,
+		.out_fmt	= V4L2_PIX_FMT_YUV420,
+		.cfg		= {
+			.channel	= 2,
+			.in_fmt		= PRP_CNTL_DATA_IN_YUV422,
+			.out_fmt	= PRP_CNTL_CH2_OUT_YUV420,
+			.src_pixel	= 0x22000888, /* YUV422 (YUYV) */
+			.irq_flags	= PRP_INTR_RDERR | PRP_INTR_CH2WERR |
+					PRP_INTR_CH2FC | PRP_INTR_LBOVF |
+					PRP_INTR_CH2OVF,
+			.csicr1		= CSICR1_SWAP16_EN,
+		}
+	},
+};
+
+static struct mx2_fmt_cfg *mx27_emma_prp_get_format(u32 in_fmt, u32 out_fmt)
+{
+	int i;
+
+	for (i = 1; i < ARRAY_SIZE(mx27_emma_prp_table); i++)
+		if ((mx27_emma_prp_table[i].in_fmt == in_fmt) &&
+				(mx27_emma_prp_table[i].out_fmt == out_fmt)) {
+			return &mx27_emma_prp_table[i];
+		}
+	/* If no match return the most generic configuration */
+	return &mx27_emma_prp_table[0];
+};
+
+static void mx27_update_emma_buf(struct mx2_camera_dev *pcdev,
+				 unsigned long phys, int bufnum)
+{
+	struct mx2_fmt_cfg *prp = pcdev->emma_prp;
+
+	if (prp->cfg.channel == 1) {
+		writel(phys, pcdev->base_emma +
+				PRP_DEST_RGB1_PTR + 4 * bufnum);
+	} else {
+		writel(phys, pcdev->base_emma +
+			PRP_DEST_Y_PTR - 0x14 * bufnum);
+		if (prp->out_fmt == V4L2_PIX_FMT_YUV420) {
+			u32 imgsize = pcdev->soc_host.icd->user_height *
+					pcdev->soc_host.icd->user_width;
+
+			writel(phys + imgsize, pcdev->base_emma +
+				PRP_DEST_CB_PTR - 0x14 * bufnum);
+			writel(phys + ((5 * imgsize) / 4), pcdev->base_emma +
+				PRP_DEST_CR_PTR - 0x14 * bufnum);
+		}
+	}
+}
+
+static void mx2_camera_deactivate(struct mx2_camera_dev *pcdev)
+{
+	clk_disable_unprepare(pcdev->clk_csi_ahb);
+	clk_disable_unprepare(pcdev->clk_csi_per);
+	writel(0, pcdev->base_csi + CSICR1);
+	writel(0, pcdev->base_emma + PRP_CNTL);
+}
+
+static int mx2_camera_add_device(struct soc_camera_device *icd)
+{
+	dev_info(icd->parent, "Camera driver attached to camera %d\n",
+		 icd->devnum);
+
+	return 0;
+}
+
+static void mx2_camera_remove_device(struct soc_camera_device *icd)
+{
+	dev_info(icd->parent, "Camera driver detached from camera %d\n",
+		 icd->devnum);
+}
+
+/*
+ * The following two functions absolutely depend on the fact, that
+ * there can be only one camera on mx2 camera sensor interface
+ */
+static int mx2_camera_clock_start(struct soc_camera_host *ici)
+{
+	struct mx2_camera_dev *pcdev = ici->priv;
+	int ret;
+	u32 csicr1;
+
+	ret = clk_prepare_enable(pcdev->clk_csi_ahb);
+	if (ret < 0)
+		return ret;
+
+	ret = clk_prepare_enable(pcdev->clk_csi_per);
+	if (ret < 0)
+		goto exit_csi_ahb;
+
+	csicr1 = CSICR1_MCLKEN | CSICR1_PRP_IF_EN | CSICR1_FCC |
+		CSICR1_RXFF_LEVEL(0);
+
+	pcdev->csicr1 = csicr1;
+	writel(pcdev->csicr1, pcdev->base_csi + CSICR1);
+
+	pcdev->frame_count = 0;
+
+	return 0;
+
+exit_csi_ahb:
+	clk_disable_unprepare(pcdev->clk_csi_ahb);
+
+	return ret;
+}
+
+static void mx2_camera_clock_stop(struct soc_camera_host *ici)
+{
+	struct mx2_camera_dev *pcdev = ici->priv;
+
+	mx2_camera_deactivate(pcdev);
+}
+
+/*
+ *  Videobuf operations
+ */
+static int mx2_videobuf_setup(struct vb2_queue *vq,
+			const void *parg,
+			unsigned int *count, unsigned int *num_planes,
+			unsigned int sizes[], void *alloc_ctxs[])
+{
+	const struct v4l2_format *fmt = parg;
+	struct soc_camera_device *icd = soc_camera_from_vb2q(vq);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct mx2_camera_dev *pcdev = ici->priv;
+
+	dev_dbg(icd->parent, "count=%d, size=%d\n", *count, sizes[0]);
+
+	/* TODO: support for VIDIOC_CREATE_BUFS not ready */
+	if (fmt != NULL)
+		return -ENOTTY;
+
+	alloc_ctxs[0] = pcdev->alloc_ctx;
+
+	sizes[0] = icd->sizeimage;
+
+	if (0 == *count)
+		*count = 32;
+	if (!*num_planes &&
+	    sizes[0] * *count > MAX_VIDEO_MEM * 1024 * 1024)
+		*count = (MAX_VIDEO_MEM * 1024 * 1024) / sizes[0];
+
+	*num_planes = 1;
+
+	return 0;
+}
+
+static int mx2_videobuf_prepare(struct vb2_buffer *vb)
+{
+	struct soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue);
+	int ret = 0;
+
+	dev_dbg(icd->parent, "%s (vb=0x%p) 0x%p %lu\n", __func__,
+		vb, vb2_plane_vaddr(vb, 0), vb2_get_plane_payload(vb, 0));
+
+#ifdef DEBUG
+	/*
+	 * This can be useful if you want to see if we actually fill
+	 * the buffer with something
+	 */
+	memset((void *)vb2_plane_vaddr(vb, 0),
+	       0xaa, vb2_get_plane_payload(vb, 0));
+#endif
+
+	vb2_set_plane_payload(vb, 0, icd->sizeimage);
+	if (vb2_plane_vaddr(vb, 0) &&
+	    vb2_get_plane_payload(vb, 0) > vb2_plane_size(vb, 0)) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	return 0;
+
+out:
+	return ret;
+}
+
+static void mx2_videobuf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue);
+	struct soc_camera_host *ici =
+		to_soc_camera_host(icd->parent);
+	struct mx2_camera_dev *pcdev = ici->priv;
+	struct mx2_buffer *buf = container_of(vbuf, struct mx2_buffer, vb);
+	unsigned long flags;
+
+	dev_dbg(icd->parent, "%s (vb=0x%p) 0x%p %lu\n", __func__,
+		vb, vb2_plane_vaddr(vb, 0), vb2_get_plane_payload(vb, 0));
+
+	spin_lock_irqsave(&pcdev->lock, flags);
+
+	list_add_tail(&buf->internal.queue, &pcdev->capture);
+
+	spin_unlock_irqrestore(&pcdev->lock, flags);
+}
+
+static void mx27_camera_emma_buf_init(struct soc_camera_device *icd,
+		int bytesperline)
+{
+	struct soc_camera_host *ici =
+		to_soc_camera_host(icd->parent);
+	struct mx2_camera_dev *pcdev = ici->priv;
+	struct mx2_fmt_cfg *prp = pcdev->emma_prp;
+
+	writel((pcdev->s_width << 16) | pcdev->s_height,
+	       pcdev->base_emma + PRP_SRC_FRAME_SIZE);
+	writel(prp->cfg.src_pixel,
+	       pcdev->base_emma + PRP_SRC_PIXEL_FORMAT_CNTL);
+	if (prp->cfg.channel == 1) {
+		writel((icd->user_width << 16) | icd->user_height,
+			pcdev->base_emma + PRP_CH1_OUT_IMAGE_SIZE);
+		writel(bytesperline,
+			pcdev->base_emma + PRP_DEST_CH1_LINE_STRIDE);
+		writel(prp->cfg.ch1_pixel,
+			pcdev->base_emma + PRP_CH1_PIXEL_FORMAT_CNTL);
+	} else { /* channel 2 */
+		writel((icd->user_width << 16) | icd->user_height,
+			pcdev->base_emma + PRP_CH2_OUT_IMAGE_SIZE);
+	}
+
+	/* Enable interrupts */
+	writel(prp->cfg.irq_flags, pcdev->base_emma + PRP_INTR_CNTL);
+}
+
+static void mx2_prp_resize_commit(struct mx2_camera_dev *pcdev)
+{
+	int dir;
+
+	for (dir = RESIZE_DIR_H; dir <= RESIZE_DIR_V; dir++) {
+		unsigned char *s = pcdev->resizing[dir].s;
+		int len = pcdev->resizing[dir].len;
+		unsigned int coeff[2] = {0, 0};
+		unsigned int valid  = 0;
+		int i;
+
+		if (len == 0)
+			continue;
+
+		for (i = RESIZE_NUM_MAX - 1; i >= 0; i--) {
+			int j;
+
+			j = i > 9 ? 1 : 0;
+			coeff[j] = (coeff[j] << BC_COEF) |
+					(s[i] & (SZ_COEF - 1));
+
+			if (i == 5 || i == 15)
+				coeff[j] <<= 1;
+
+			valid = (valid << 1) | (s[i] >> BC_COEF);
+		}
+
+		valid |= PRP_RZ_VALID_TBL_LEN(len);
+
+		if (pcdev->resizing[dir].algo == RESIZE_ALGO_BILINEAR)
+			valid |= PRP_RZ_VALID_BILINEAR;
+
+		if (pcdev->emma_prp->cfg.channel == 1) {
+			if (dir == RESIZE_DIR_H) {
+				writel(coeff[0], pcdev->base_emma +
+							PRP_CH1_RZ_HORI_COEF1);
+				writel(coeff[1], pcdev->base_emma +
+							PRP_CH1_RZ_HORI_COEF2);
+				writel(valid, pcdev->base_emma +
+							PRP_CH1_RZ_HORI_VALID);
+			} else {
+				writel(coeff[0], pcdev->base_emma +
+							PRP_CH1_RZ_VERT_COEF1);
+				writel(coeff[1], pcdev->base_emma +
+							PRP_CH1_RZ_VERT_COEF2);
+				writel(valid, pcdev->base_emma +
+							PRP_CH1_RZ_VERT_VALID);
+			}
+		} else {
+			if (dir == RESIZE_DIR_H) {
+				writel(coeff[0], pcdev->base_emma +
+							PRP_CH2_RZ_HORI_COEF1);
+				writel(coeff[1], pcdev->base_emma +
+							PRP_CH2_RZ_HORI_COEF2);
+				writel(valid, pcdev->base_emma +
+							PRP_CH2_RZ_HORI_VALID);
+			} else {
+				writel(coeff[0], pcdev->base_emma +
+							PRP_CH2_RZ_VERT_COEF1);
+				writel(coeff[1], pcdev->base_emma +
+							PRP_CH2_RZ_VERT_COEF2);
+				writel(valid, pcdev->base_emma +
+							PRP_CH2_RZ_VERT_VALID);
+			}
+		}
+	}
+}
+
+static int mx2_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct soc_camera_device *icd = soc_camera_from_vb2q(q);
+	struct soc_camera_host *ici =
+		to_soc_camera_host(icd->parent);
+	struct mx2_camera_dev *pcdev = ici->priv;
+	struct mx2_fmt_cfg *prp = pcdev->emma_prp;
+	struct vb2_buffer *vb;
+	struct mx2_buffer *buf;
+	unsigned long phys;
+	int bytesperline;
+	unsigned long flags;
+
+	if (count < 2)
+		return -ENOBUFS;
+
+	spin_lock_irqsave(&pcdev->lock, flags);
+
+	buf = list_first_entry(&pcdev->capture, struct mx2_buffer,
+			       internal.queue);
+	buf->internal.bufnum = 0;
+	vb = &buf->vb.vb2_buf;
+
+	phys = vb2_dma_contig_plane_dma_addr(vb, 0);
+	mx27_update_emma_buf(pcdev, phys, buf->internal.bufnum);
+	list_move_tail(pcdev->capture.next, &pcdev->active_bufs);
+
+	buf = list_first_entry(&pcdev->capture, struct mx2_buffer,
+			       internal.queue);
+	buf->internal.bufnum = 1;
+	vb = &buf->vb.vb2_buf;
+
+	phys = vb2_dma_contig_plane_dma_addr(vb, 0);
+	mx27_update_emma_buf(pcdev, phys, buf->internal.bufnum);
+	list_move_tail(pcdev->capture.next, &pcdev->active_bufs);
+
+	bytesperline = soc_mbus_bytes_per_line(icd->user_width,
+					       icd->current_fmt->host_fmt);
+	if (bytesperline < 0) {
+		spin_unlock_irqrestore(&pcdev->lock, flags);
+		return bytesperline;
+	}
+
+	/*
+	 * I didn't manage to properly enable/disable the prp
+	 * on a per frame basis during running transfers,
+	 * thus we allocate a buffer here and use it to
+	 * discard frames when no buffer is available.
+	 * Feel free to work on this ;)
+	 */
+	pcdev->discard_size = icd->user_height * bytesperline;
+	pcdev->discard_buffer = dma_alloc_coherent(ici->v4l2_dev.dev,
+					pcdev->discard_size,
+					&pcdev->discard_buffer_dma, GFP_ATOMIC);
+	if (!pcdev->discard_buffer) {
+		spin_unlock_irqrestore(&pcdev->lock, flags);
+		return -ENOMEM;
+	}
+
+	pcdev->buf_discard[0].discard = true;
+	list_add_tail(&pcdev->buf_discard[0].queue,
+		      &pcdev->discard);
+
+	pcdev->buf_discard[1].discard = true;
+	list_add_tail(&pcdev->buf_discard[1].queue,
+		      &pcdev->discard);
+
+	mx2_prp_resize_commit(pcdev);
+
+	mx27_camera_emma_buf_init(icd, bytesperline);
+
+	if (prp->cfg.channel == 1) {
+		writel(PRP_CNTL_CH1EN |
+		       PRP_CNTL_CSIEN |
+		       prp->cfg.in_fmt |
+		       prp->cfg.out_fmt |
+		       PRP_CNTL_CH1_LEN |
+		       PRP_CNTL_CH1BYP |
+		       PRP_CNTL_CH1_TSKIP(0) |
+		       PRP_CNTL_IN_TSKIP(0),
+		       pcdev->base_emma + PRP_CNTL);
+	} else {
+		writel(PRP_CNTL_CH2EN |
+		       PRP_CNTL_CSIEN |
+		       prp->cfg.in_fmt |
+		       prp->cfg.out_fmt |
+		       PRP_CNTL_CH2_LEN |
+		       PRP_CNTL_CH2_TSKIP(0) |
+		       PRP_CNTL_IN_TSKIP(0),
+		       pcdev->base_emma + PRP_CNTL);
+	}
+	spin_unlock_irqrestore(&pcdev->lock, flags);
+
+	return 0;
+}
+
+static void mx2_stop_streaming(struct vb2_queue *q)
+{
+	struct soc_camera_device *icd = soc_camera_from_vb2q(q);
+	struct soc_camera_host *ici =
+		to_soc_camera_host(icd->parent);
+	struct mx2_camera_dev *pcdev = ici->priv;
+	struct mx2_fmt_cfg *prp = pcdev->emma_prp;
+	unsigned long flags;
+	void *b;
+	u32 cntl;
+
+	spin_lock_irqsave(&pcdev->lock, flags);
+
+	cntl = readl(pcdev->base_emma + PRP_CNTL);
+	if (prp->cfg.channel == 1) {
+		writel(cntl & ~PRP_CNTL_CH1EN,
+		       pcdev->base_emma + PRP_CNTL);
+	} else {
+		writel(cntl & ~PRP_CNTL_CH2EN,
+		       pcdev->base_emma + PRP_CNTL);
+	}
+	INIT_LIST_HEAD(&pcdev->capture);
+	INIT_LIST_HEAD(&pcdev->active_bufs);
+	INIT_LIST_HEAD(&pcdev->discard);
+
+	b = pcdev->discard_buffer;
+	pcdev->discard_buffer = NULL;
+
+	spin_unlock_irqrestore(&pcdev->lock, flags);
+
+	dma_free_coherent(ici->v4l2_dev.dev,
+			  pcdev->discard_size, b, pcdev->discard_buffer_dma);
+}
+
+static struct vb2_ops mx2_videobuf_ops = {
+	.queue_setup	 = mx2_videobuf_setup,
+	.buf_prepare	 = mx2_videobuf_prepare,
+	.buf_queue	 = mx2_videobuf_queue,
+	.start_streaming = mx2_start_streaming,
+	.stop_streaming	 = mx2_stop_streaming,
+};
+
+static int mx2_camera_init_videobuf(struct vb2_queue *q,
+			      struct soc_camera_device *icd)
+{
+	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	q->io_modes = VB2_MMAP | VB2_USERPTR;
+	q->drv_priv = icd;
+	q->ops = &mx2_videobuf_ops;
+	q->mem_ops = &vb2_dma_contig_memops;
+	q->buf_struct_size = sizeof(struct mx2_buffer);
+	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+
+	return vb2_queue_init(q);
+}
+
+#define MX2_BUS_FLAGS	(V4L2_MBUS_MASTER | \
+			V4L2_MBUS_VSYNC_ACTIVE_HIGH | \
+			V4L2_MBUS_VSYNC_ACTIVE_LOW | \
+			V4L2_MBUS_HSYNC_ACTIVE_HIGH | \
+			V4L2_MBUS_HSYNC_ACTIVE_LOW | \
+			V4L2_MBUS_PCLK_SAMPLE_RISING | \
+			V4L2_MBUS_PCLK_SAMPLE_FALLING | \
+			V4L2_MBUS_DATA_ACTIVE_HIGH | \
+			V4L2_MBUS_DATA_ACTIVE_LOW)
+
+static int mx27_camera_emma_prp_reset(struct mx2_camera_dev *pcdev)
+{
+	int count = 0;
+
+	readl(pcdev->base_emma + PRP_CNTL);
+	writel(PRP_CNTL_SWRST, pcdev->base_emma + PRP_CNTL);
+	while (count++ < 100) {
+		if (!(readl(pcdev->base_emma + PRP_CNTL) & PRP_CNTL_SWRST))
+			return 0;
+		barrier();
+		udelay(1);
+	}
+
+	return -ETIMEDOUT;
+}
+
+static int mx2_camera_set_bus_param(struct soc_camera_device *icd)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct mx2_camera_dev *pcdev = ici->priv;
+	struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
+	unsigned long common_flags;
+	int ret;
+	int bytesperline;
+	u32 csicr1 = pcdev->csicr1;
+
+	ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
+	if (!ret) {
+		common_flags = soc_mbus_config_compatible(&cfg, MX2_BUS_FLAGS);
+		if (!common_flags) {
+			dev_warn(icd->parent,
+				 "Flags incompatible: camera 0x%x, host 0x%x\n",
+				 cfg.flags, MX2_BUS_FLAGS);
+			return -EINVAL;
+		}
+	} else if (ret != -ENOIOCTLCMD) {
+		return ret;
+	} else {
+		common_flags = MX2_BUS_FLAGS;
+	}
+
+	if ((common_flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH) &&
+	    (common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)) {
+		if (pcdev->platform_flags & MX2_CAMERA_HSYNC_HIGH)
+			common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_LOW;
+		else
+			common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_HIGH;
+	}
+
+	if ((common_flags & V4L2_MBUS_PCLK_SAMPLE_RISING) &&
+	    (common_flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)) {
+		if (pcdev->platform_flags & MX2_CAMERA_PCLK_SAMPLE_RISING)
+			common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_FALLING;
+		else
+			common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_RISING;
+	}
+
+	cfg.flags = common_flags;
+	ret = v4l2_subdev_call(sd, video, s_mbus_config, &cfg);
+	if (ret < 0 && ret != -ENOIOCTLCMD) {
+		dev_dbg(icd->parent, "camera s_mbus_config(0x%lx) returned %d\n",
+			common_flags, ret);
+		return ret;
+	}
+
+	csicr1 = (csicr1 & ~CSICR1_FMT_MASK) | pcdev->emma_prp->cfg.csicr1;
+
+	if (common_flags & V4L2_MBUS_PCLK_SAMPLE_RISING)
+		csicr1 |= CSICR1_REDGE;
+	if (common_flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH)
+		csicr1 |= CSICR1_SOF_POL;
+	if (common_flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
+		csicr1 |= CSICR1_HSYNC_POL;
+	if (pcdev->platform_flags & MX2_CAMERA_EXT_VSYNC)
+		csicr1 |= CSICR1_EXT_VSYNC;
+	if (pcdev->platform_flags & MX2_CAMERA_CCIR)
+		csicr1 |= CSICR1_CCIR_EN;
+	if (pcdev->platform_flags & MX2_CAMERA_CCIR_INTERLACE)
+		csicr1 |= CSICR1_CCIR_MODE;
+	if (pcdev->platform_flags & MX2_CAMERA_GATED_CLOCK)
+		csicr1 |= CSICR1_GCLK_MODE;
+	if (pcdev->platform_flags & MX2_CAMERA_INV_DATA)
+		csicr1 |= CSICR1_INV_DATA;
+
+	pcdev->csicr1 = csicr1;
+
+	bytesperline = soc_mbus_bytes_per_line(icd->user_width,
+			icd->current_fmt->host_fmt);
+	if (bytesperline < 0)
+		return bytesperline;
+
+	ret = mx27_camera_emma_prp_reset(pcdev);
+	if (ret)
+		return ret;
+
+	writel(pcdev->csicr1, pcdev->base_csi + CSICR1);
+
+	return 0;
+}
+
+static int mx2_camera_set_crop(struct soc_camera_device *icd,
+				const struct v4l2_crop *a)
+{
+	struct v4l2_crop a_writable = *a;
+	struct v4l2_rect *rect = &a_writable.c;
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_mbus_framefmt *mf = &fmt.format;
+	int ret;
+
+	soc_camera_limit_side(&rect->left, &rect->width, 0, 2, 4096);
+	soc_camera_limit_side(&rect->top, &rect->height, 0, 2, 4096);
+
+	ret = v4l2_subdev_call(sd, video, s_crop, a);
+	if (ret < 0)
+		return ret;
+
+	/* The capture device might have changed its output  */
+	ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &fmt);
+	if (ret < 0)
+		return ret;
+
+	dev_dbg(icd->parent, "Sensor cropped %dx%d\n",
+		mf->width, mf->height);
+
+	icd->user_width		= mf->width;
+	icd->user_height	= mf->height;
+
+	return ret;
+}
+
+static int mx2_camera_get_formats(struct soc_camera_device *icd,
+				  unsigned int idx,
+				  struct soc_camera_format_xlate *xlate)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	const struct soc_mbus_pixelfmt *fmt;
+	struct device *dev = icd->parent;
+	struct v4l2_subdev_mbus_code_enum code = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.index = idx,
+	};
+	int ret, formats = 0;
+
+	ret = v4l2_subdev_call(sd, pad, enum_mbus_code, NULL, &code);
+	if (ret < 0)
+		/* no more formats */
+		return 0;
+
+	fmt = soc_mbus_get_fmtdesc(code.code);
+	if (!fmt) {
+		dev_err(dev, "Invalid format code #%u: %d\n", idx, code.code);
+		return 0;
+	}
+
+	if (code.code == MEDIA_BUS_FMT_YUYV8_2X8 ||
+	    code.code == MEDIA_BUS_FMT_UYVY8_2X8) {
+		formats++;
+		if (xlate) {
+			/*
+			 * CH2 can output YUV420 which is a standard format in
+			 * soc_mediabus.c
+			 */
+			xlate->host_fmt =
+				soc_mbus_get_fmtdesc(MEDIA_BUS_FMT_YUYV8_1_5X8);
+			xlate->code	= code.code;
+			dev_dbg(dev, "Providing host format %s for sensor code %d\n",
+			       xlate->host_fmt->name, code.code);
+			xlate++;
+		}
+	}
+
+	if (code.code == MEDIA_BUS_FMT_UYVY8_2X8) {
+		formats++;
+		if (xlate) {
+			xlate->host_fmt =
+				soc_mbus_get_fmtdesc(MEDIA_BUS_FMT_YUYV8_2X8);
+			xlate->code	= code.code;
+			dev_dbg(dev, "Providing host format %s for sensor code %d\n",
+				xlate->host_fmt->name, code.code);
+			xlate++;
+		}
+	}
+
+	/* Generic pass-trough */
+	formats++;
+	if (xlate) {
+		xlate->host_fmt = fmt;
+		xlate->code	= code.code;
+		xlate++;
+	}
+	return formats;
+}
+
+static int mx2_emmaprp_resize(struct mx2_camera_dev *pcdev,
+			      struct v4l2_mbus_framefmt *mf_in,
+			      struct v4l2_pix_format *pix_out, bool apply)
+{
+	unsigned int num, den;
+	unsigned long m;
+	int i, dir;
+
+	for (dir = RESIZE_DIR_H; dir <= RESIZE_DIR_V; dir++) {
+		struct emma_prp_resize tmprsz;
+		unsigned char *s = tmprsz.s;
+		int len = 0;
+		int in, out;
+
+		if (dir == RESIZE_DIR_H) {
+			in = mf_in->width;
+			out = pix_out->width;
+		} else {
+			in = mf_in->height;
+			out = pix_out->height;
+		}
+
+		if (in < out)
+			return -EINVAL;
+		else if (in == out)
+			continue;
+
+		/* Calculate ratio */
+		m = gcd(in, out);
+		num = in / m;
+		den = out / m;
+		if (num > RESIZE_NUM_MAX)
+			return -EINVAL;
+
+		if ((num >= 2 * den) && (den == 1) &&
+		    (num < 9) && (!(num & 0x01))) {
+			int sum = 0;
+			int j;
+
+			/* Average scaling for >= 2:1 ratios */
+			/* Support can be added for num >=9 and odd values */
+
+			tmprsz.algo = RESIZE_ALGO_AVERAGING;
+			len = num;
+
+			for (i = 0; i < (len / 2); i++)
+				s[i] = 8;
+
+			do {
+				for (i = 0; i < (len / 2); i++) {
+					s[i] = s[i] >> 1;
+					sum = 0;
+					for (j = 0; j < (len / 2); j++)
+						sum += s[j];
+					if (sum == 4)
+						break;
+				}
+			} while (sum != 4);
+
+			for (i = (len / 2); i < len; i++)
+				s[i] = s[len - i - 1];
+
+			s[len - 1] |= SZ_COEF;
+		} else {
+			/* bilinear scaling for < 2:1 ratios */
+			int v; /* overflow counter */
+			int coeff, nxt; /* table output */
+			int in_pos_inc = 2 * den;
+			int out_pos = num;
+			int out_pos_inc = 2 * num;
+			int init_carry = num - den;
+			int carry = init_carry;
+
+			tmprsz.algo = RESIZE_ALGO_BILINEAR;
+			v = den + 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 (len > RESIZE_NUM_MAX)
+					return -EINVAL;
+
+				coeff = ((coeff << BC_COEF) +
+					(in_pos_inc >> 1)) / in_pos_inc;
+
+				if (coeff >= (SZ_COEF - 1))
+					coeff--;
+
+				coeff |= SZ_COEF;
+				s[len] = (unsigned char)coeff;
+				len++;
+
+				for (i = 1; i < nxt; i++) {
+					if (len >= RESIZE_NUM_MAX)
+						return -EINVAL;
+					s[len] = 0;
+					len++;
+				}
+			} while (carry != init_carry);
+		}
+		tmprsz.len = len;
+		if (dir == RESIZE_DIR_H)
+			mf_in->width = pix_out->width;
+		else
+			mf_in->height = pix_out->height;
+
+		if (apply)
+			memcpy(&pcdev->resizing[dir], &tmprsz, sizeof(tmprsz));
+	}
+	return 0;
+}
+
+static int mx2_camera_set_fmt(struct soc_camera_device *icd,
+			       struct v4l2_format *f)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct mx2_camera_dev *pcdev = ici->priv;
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	const struct soc_camera_format_xlate *xlate;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_mbus_framefmt *mf = &format.format;
+	int ret;
+
+	dev_dbg(icd->parent, "%s: requested params: width = %d, height = %d\n",
+		__func__, pix->width, pix->height);
+
+	xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
+	if (!xlate) {
+		dev_warn(icd->parent, "Format %x not found\n",
+				pix->pixelformat);
+		return -EINVAL;
+	}
+
+	mf->width	= pix->width;
+	mf->height	= pix->height;
+	mf->field	= pix->field;
+	mf->colorspace	= pix->colorspace;
+	mf->code	= xlate->code;
+
+	ret = v4l2_subdev_call(sd, pad, set_fmt, NULL, &format);
+	if (ret < 0 && ret != -ENOIOCTLCMD)
+		return ret;
+
+	/* Store width and height returned by the sensor for resizing */
+	pcdev->s_width = mf->width;
+	pcdev->s_height = mf->height;
+	dev_dbg(icd->parent, "%s: sensor params: width = %d, height = %d\n",
+		__func__, pcdev->s_width, pcdev->s_height);
+
+	pcdev->emma_prp = mx27_emma_prp_get_format(xlate->code,
+						   xlate->host_fmt->fourcc);
+
+	memset(pcdev->resizing, 0, sizeof(pcdev->resizing));
+	if ((mf->width != pix->width || mf->height != pix->height) &&
+		pcdev->emma_prp->cfg.in_fmt == PRP_CNTL_DATA_IN_YUV422) {
+		if (mx2_emmaprp_resize(pcdev, mf, pix, true) < 0)
+			dev_dbg(icd->parent, "%s: can't resize\n", __func__);
+	}
+
+	if (mf->code != xlate->code)
+		return -EINVAL;
+
+	pix->width		= mf->width;
+	pix->height		= mf->height;
+	pix->field		= mf->field;
+	pix->colorspace		= mf->colorspace;
+	icd->current_fmt	= xlate;
+
+	dev_dbg(icd->parent, "%s: returned params: width = %d, height = %d\n",
+		__func__, pix->width, pix->height);
+
+	return 0;
+}
+
+static int mx2_camera_try_fmt(struct soc_camera_device *icd,
+				  struct v4l2_format *f)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	const struct soc_camera_format_xlate *xlate;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct v4l2_subdev_pad_config pad_cfg;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+	};
+	struct v4l2_mbus_framefmt *mf = &format.format;
+	__u32 pixfmt = pix->pixelformat;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct mx2_camera_dev *pcdev = ici->priv;
+	struct mx2_fmt_cfg *emma_prp;
+	int ret;
+
+	dev_dbg(icd->parent, "%s: requested params: width = %d, height = %d\n",
+		__func__, pix->width, pix->height);
+
+	xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
+	if (pixfmt && !xlate) {
+		dev_warn(icd->parent, "Format %x not found\n", pixfmt);
+		return -EINVAL;
+	}
+
+	/*
+	 * limit to MX27 hardware capabilities: width must be a multiple of 8 as
+	 * requested by the CSI. (Table 39-2 in the i.MX27 Reference Manual).
+	 */
+	pix->width &= ~0x7;
+
+	/* limit to sensor capabilities */
+	mf->width	= pix->width;
+	mf->height	= pix->height;
+	mf->field	= pix->field;
+	mf->colorspace	= pix->colorspace;
+	mf->code	= xlate->code;
+
+	ret = v4l2_subdev_call(sd, pad, set_fmt, &pad_cfg, &format);
+	if (ret < 0)
+		return ret;
+
+	dev_dbg(icd->parent, "%s: sensor params: width = %d, height = %d\n",
+		__func__, pcdev->s_width, pcdev->s_height);
+
+	/* If the sensor does not support image size try PrP resizing */
+	emma_prp = mx27_emma_prp_get_format(xlate->code,
+					    xlate->host_fmt->fourcc);
+
+	if ((mf->width != pix->width || mf->height != pix->height) &&
+		emma_prp->cfg.in_fmt == PRP_CNTL_DATA_IN_YUV422) {
+		if (mx2_emmaprp_resize(pcdev, mf, pix, false) < 0)
+			dev_dbg(icd->parent, "%s: can't resize\n", __func__);
+	}
+
+	if (mf->field == V4L2_FIELD_ANY)
+		mf->field = V4L2_FIELD_NONE;
+	/*
+	 * Driver supports interlaced images provided they have
+	 * both fields so that they can be processed as if they
+	 * were progressive.
+	 */
+	if (mf->field != V4L2_FIELD_NONE && !V4L2_FIELD_HAS_BOTH(mf->field)) {
+		dev_err(icd->parent, "Field type %d unsupported.\n",
+				mf->field);
+		return -EINVAL;
+	}
+
+	pix->width	= mf->width;
+	pix->height	= mf->height;
+	pix->field	= mf->field;
+	pix->colorspace	= mf->colorspace;
+
+	dev_dbg(icd->parent, "%s: returned params: width = %d, height = %d\n",
+		__func__, pix->width, pix->height);
+
+	return 0;
+}
+
+static int mx2_camera_querycap(struct soc_camera_host *ici,
+			       struct v4l2_capability *cap)
+{
+	/* cap->name is set by the friendly caller:-> */
+	strlcpy(cap->card, MX2_CAM_DRIVER_DESCRIPTION, sizeof(cap->card));
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+
+	return 0;
+}
+
+static unsigned int mx2_camera_poll(struct file *file, poll_table *pt)
+{
+	struct soc_camera_device *icd = file->private_data;
+
+	return vb2_poll(&icd->vb2_vidq, file, pt);
+}
+
+static struct soc_camera_host_ops mx2_soc_camera_host_ops = {
+	.owner		= THIS_MODULE,
+	.add		= mx2_camera_add_device,
+	.remove		= mx2_camera_remove_device,
+	.clock_start	= mx2_camera_clock_start,
+	.clock_stop	= mx2_camera_clock_stop,
+	.set_fmt	= mx2_camera_set_fmt,
+	.set_crop	= mx2_camera_set_crop,
+	.get_formats	= mx2_camera_get_formats,
+	.try_fmt	= mx2_camera_try_fmt,
+	.init_videobuf2	= mx2_camera_init_videobuf,
+	.poll		= mx2_camera_poll,
+	.querycap	= mx2_camera_querycap,
+	.set_bus_param	= mx2_camera_set_bus_param,
+};
+
+static void mx27_camera_frame_done_emma(struct mx2_camera_dev *pcdev,
+		int bufnum, bool err)
+{
+#ifdef DEBUG
+	struct mx2_fmt_cfg *prp = pcdev->emma_prp;
+#endif
+	struct mx2_buf_internal *ibuf;
+	struct mx2_buffer *buf;
+	struct vb2_buffer *vb;
+	struct vb2_v4l2_buffer *vbuf;
+	unsigned long phys;
+
+	ibuf = list_first_entry(&pcdev->active_bufs, struct mx2_buf_internal,
+			       queue);
+
+	BUG_ON(ibuf->bufnum != bufnum);
+
+	if (ibuf->discard) {
+		/*
+		 * Discard buffer must not be returned to user space.
+		 * Just return it to the discard queue.
+		 */
+		list_move_tail(pcdev->active_bufs.next, &pcdev->discard);
+	} else {
+		buf = mx2_ibuf_to_buf(ibuf);
+
+		vb = &buf->vb.vb2_buf;
+		vbuf = to_vb2_v4l2_buffer(vb);
+#ifdef DEBUG
+		phys = vb2_dma_contig_plane_dma_addr(vb, 0);
+		if (prp->cfg.channel == 1) {
+			if (readl(pcdev->base_emma + PRP_DEST_RGB1_PTR +
+				4 * bufnum) != phys) {
+				dev_err(pcdev->dev, "%lx != %x\n", phys,
+					readl(pcdev->base_emma +
+					PRP_DEST_RGB1_PTR + 4 * bufnum));
+			}
+		} else {
+			if (readl(pcdev->base_emma + PRP_DEST_Y_PTR -
+				0x14 * bufnum) != phys) {
+				dev_err(pcdev->dev, "%lx != %x\n", phys,
+					readl(pcdev->base_emma +
+					PRP_DEST_Y_PTR - 0x14 * bufnum));
+			}
+		}
+#endif
+		dev_dbg(pcdev->dev, "%s (vb=0x%p) 0x%p %lu\n", __func__, vb,
+				vb2_plane_vaddr(vb, 0),
+				vb2_get_plane_payload(vb, 0));
+
+		list_del_init(&buf->internal.queue);
+		v4l2_get_timestamp(&vbuf->timestamp);
+		vbuf->sequence = pcdev->frame_count;
+		if (err)
+			vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
+		else
+			vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
+	}
+
+	pcdev->frame_count++;
+
+	if (list_empty(&pcdev->capture)) {
+		if (list_empty(&pcdev->discard)) {
+			dev_warn(pcdev->dev, "%s: trying to access empty discard list\n",
+				 __func__);
+			return;
+		}
+
+		ibuf = list_first_entry(&pcdev->discard,
+					struct mx2_buf_internal, queue);
+		ibuf->bufnum = bufnum;
+
+		list_move_tail(pcdev->discard.next, &pcdev->active_bufs);
+		mx27_update_emma_buf(pcdev, pcdev->discard_buffer_dma, bufnum);
+		return;
+	}
+
+	buf = list_first_entry(&pcdev->capture, struct mx2_buffer,
+			       internal.queue);
+
+	buf->internal.bufnum = bufnum;
+
+	list_move_tail(pcdev->capture.next, &pcdev->active_bufs);
+
+	vb = &buf->vb.vb2_buf;
+
+	phys = vb2_dma_contig_plane_dma_addr(vb, 0);
+	mx27_update_emma_buf(pcdev, phys, bufnum);
+}
+
+static irqreturn_t mx27_camera_emma_irq(int irq_emma, void *data)
+{
+	struct mx2_camera_dev *pcdev = data;
+	unsigned int status = readl(pcdev->base_emma + PRP_INTRSTATUS);
+	struct mx2_buf_internal *ibuf;
+
+	spin_lock(&pcdev->lock);
+
+	if (list_empty(&pcdev->active_bufs)) {
+		dev_warn(pcdev->dev, "%s: called while active list is empty\n",
+			__func__);
+
+		if (!status) {
+			spin_unlock(&pcdev->lock);
+			return IRQ_NONE;
+		}
+	}
+
+	if (status & (1 << 7)) { /* overflow */
+		u32 cntl = readl(pcdev->base_emma + PRP_CNTL);
+		writel(cntl & ~(PRP_CNTL_CH1EN | PRP_CNTL_CH2EN),
+		       pcdev->base_emma + PRP_CNTL);
+		writel(cntl, pcdev->base_emma + PRP_CNTL);
+
+		ibuf = list_first_entry(&pcdev->active_bufs,
+					struct mx2_buf_internal, queue);
+		mx27_camera_frame_done_emma(pcdev,
+					ibuf->bufnum, true);
+
+		status &= ~(1 << 7);
+	} else if (((status & (3 << 5)) == (3 << 5)) ||
+		((status & (3 << 3)) == (3 << 3))) {
+		/*
+		 * Both buffers have triggered, process the one we're expecting
+		 * to first
+		 */
+		ibuf = list_first_entry(&pcdev->active_bufs,
+					struct mx2_buf_internal, queue);
+		mx27_camera_frame_done_emma(pcdev, ibuf->bufnum, false);
+		status &= ~(1 << (6 - ibuf->bufnum)); /* mark processed */
+	} else if ((status & (1 << 6)) || (status & (1 << 4))) {
+		mx27_camera_frame_done_emma(pcdev, 0, false);
+	} else if ((status & (1 << 5)) || (status & (1 << 3))) {
+		mx27_camera_frame_done_emma(pcdev, 1, false);
+	}
+
+	spin_unlock(&pcdev->lock);
+	writel(status, pcdev->base_emma + PRP_INTRSTATUS);
+
+	return IRQ_HANDLED;
+}
+
+static int mx27_camera_emma_init(struct platform_device *pdev)
+{
+	struct mx2_camera_dev *pcdev = platform_get_drvdata(pdev);
+	struct resource *res_emma;
+	int irq_emma;
+	int err = 0;
+
+	res_emma = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	irq_emma = platform_get_irq(pdev, 1);
+	if (!res_emma || !irq_emma) {
+		dev_err(pcdev->dev, "no EMMA resources\n");
+		err = -ENODEV;
+		goto out;
+	}
+
+	pcdev->base_emma = devm_ioremap_resource(pcdev->dev, res_emma);
+	if (IS_ERR(pcdev->base_emma)) {
+		err = PTR_ERR(pcdev->base_emma);
+		goto out;
+	}
+
+	err = devm_request_irq(pcdev->dev, irq_emma, mx27_camera_emma_irq, 0,
+			       MX2_CAM_DRV_NAME, pcdev);
+	if (err) {
+		dev_err(pcdev->dev, "Camera EMMA interrupt register failed\n");
+		goto out;
+	}
+
+	pcdev->clk_emma_ipg = devm_clk_get(pcdev->dev, "emma-ipg");
+	if (IS_ERR(pcdev->clk_emma_ipg)) {
+		err = PTR_ERR(pcdev->clk_emma_ipg);
+		goto out;
+	}
+
+	clk_prepare_enable(pcdev->clk_emma_ipg);
+
+	pcdev->clk_emma_ahb = devm_clk_get(pcdev->dev, "emma-ahb");
+	if (IS_ERR(pcdev->clk_emma_ahb)) {
+		err = PTR_ERR(pcdev->clk_emma_ahb);
+		goto exit_clk_emma_ipg;
+	}
+
+	clk_prepare_enable(pcdev->clk_emma_ahb);
+
+	err = mx27_camera_emma_prp_reset(pcdev);
+	if (err)
+		goto exit_clk_emma_ahb;
+
+	return err;
+
+exit_clk_emma_ahb:
+	clk_disable_unprepare(pcdev->clk_emma_ahb);
+exit_clk_emma_ipg:
+	clk_disable_unprepare(pcdev->clk_emma_ipg);
+out:
+	return err;
+}
+
+static int mx2_camera_probe(struct platform_device *pdev)
+{
+	struct mx2_camera_dev *pcdev;
+	struct resource *res_csi;
+	int irq_csi;
+	int err = 0;
+
+	dev_dbg(&pdev->dev, "initialising\n");
+
+	res_csi = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	irq_csi = platform_get_irq(pdev, 0);
+	if (res_csi == NULL || irq_csi < 0) {
+		dev_err(&pdev->dev, "Missing platform resources data\n");
+		err = -ENODEV;
+		goto exit;
+	}
+
+	pcdev = devm_kzalloc(&pdev->dev, sizeof(*pcdev), GFP_KERNEL);
+	if (!pcdev) {
+		dev_err(&pdev->dev, "Could not allocate pcdev\n");
+		err = -ENOMEM;
+		goto exit;
+	}
+
+	pcdev->clk_csi_ahb = devm_clk_get(&pdev->dev, "ahb");
+	if (IS_ERR(pcdev->clk_csi_ahb)) {
+		dev_err(&pdev->dev, "Could not get csi ahb clock\n");
+		err = PTR_ERR(pcdev->clk_csi_ahb);
+		goto exit;
+	}
+
+	pcdev->clk_csi_per = devm_clk_get(&pdev->dev, "per");
+	if (IS_ERR(pcdev->clk_csi_per)) {
+		dev_err(&pdev->dev, "Could not get csi per clock\n");
+		err = PTR_ERR(pcdev->clk_csi_per);
+		goto exit;
+	}
+
+	pcdev->pdata = pdev->dev.platform_data;
+	if (pcdev->pdata) {
+		long rate;
+
+		pcdev->platform_flags = pcdev->pdata->flags;
+
+		rate = clk_round_rate(pcdev->clk_csi_per,
+						pcdev->pdata->clk * 2);
+		if (rate <= 0) {
+			err = -ENODEV;
+			goto exit;
+		}
+		err = clk_set_rate(pcdev->clk_csi_per, rate);
+		if (err < 0)
+			goto exit;
+	}
+
+	INIT_LIST_HEAD(&pcdev->capture);
+	INIT_LIST_HEAD(&pcdev->active_bufs);
+	INIT_LIST_HEAD(&pcdev->discard);
+	spin_lock_init(&pcdev->lock);
+
+	pcdev->base_csi = devm_ioremap_resource(&pdev->dev, res_csi);
+	if (IS_ERR(pcdev->base_csi)) {
+		err = PTR_ERR(pcdev->base_csi);
+		goto exit;
+	}
+
+	pcdev->dev = &pdev->dev;
+	platform_set_drvdata(pdev, pcdev);
+
+	err = mx27_camera_emma_init(pdev);
+	if (err)
+		goto exit;
+
+	/*
+	 * We're done with drvdata here.  Clear the pointer so that
+	 * v4l2 core can start using drvdata on its purpose.
+	 */
+	platform_set_drvdata(pdev, NULL);
+
+	pcdev->soc_host.drv_name	= MX2_CAM_DRV_NAME,
+	pcdev->soc_host.ops		= &mx2_soc_camera_host_ops,
+	pcdev->soc_host.priv		= pcdev;
+	pcdev->soc_host.v4l2_dev.dev	= &pdev->dev;
+	pcdev->soc_host.nr		= pdev->id;
+
+	pcdev->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
+	if (IS_ERR(pcdev->alloc_ctx)) {
+		err = PTR_ERR(pcdev->alloc_ctx);
+		goto eallocctx;
+	}
+	err = soc_camera_host_register(&pcdev->soc_host);
+	if (err)
+		goto exit_free_emma;
+
+	dev_info(&pdev->dev, "MX2 Camera (CSI) driver probed, clock frequency: %ld\n",
+			clk_get_rate(pcdev->clk_csi_per));
+
+	return 0;
+
+exit_free_emma:
+	vb2_dma_contig_cleanup_ctx(pcdev->alloc_ctx);
+eallocctx:
+	clk_disable_unprepare(pcdev->clk_emma_ipg);
+	clk_disable_unprepare(pcdev->clk_emma_ahb);
+exit:
+	return err;
+}
+
+static int mx2_camera_remove(struct platform_device *pdev)
+{
+	struct soc_camera_host *soc_host = to_soc_camera_host(&pdev->dev);
+	struct mx2_camera_dev *pcdev = container_of(soc_host,
+			struct mx2_camera_dev, soc_host);
+
+	soc_camera_host_unregister(&pcdev->soc_host);
+
+	vb2_dma_contig_cleanup_ctx(pcdev->alloc_ctx);
+
+	clk_disable_unprepare(pcdev->clk_emma_ipg);
+	clk_disable_unprepare(pcdev->clk_emma_ahb);
+
+	dev_info(&pdev->dev, "MX2 Camera driver unloaded\n");
+
+	return 0;
+}
+
+static struct platform_driver mx2_camera_driver = {
+	.driver		= {
+		.name	= MX2_CAM_DRV_NAME,
+	},
+	.id_table	= mx2_camera_devtype,
+	.remove		= mx2_camera_remove,
+};
+
+module_platform_driver_probe(mx2_camera_driver, mx2_camera_probe);
+
+MODULE_DESCRIPTION("i.MX27 SoC Camera Host driver");
+MODULE_AUTHOR("Sascha Hauer <sha@pengutronix.de>");
+MODULE_LICENSE("GPL");
+MODULE_VERSION(MX2_CAM_VERSION);
diff --git a/drivers/media/platform/soc_camera/mx3_camera.c b/drivers/media/platform/soc_camera/mx3_camera.c
new file mode 100644
index 0000000..49c3a25
--- /dev/null
+++ b/drivers/media/platform/soc_camera/mx3_camera.c
@@ -0,0 +1,1288 @@
+/*
+ * V4L2 Driver for i.MX3x camera host
+ *
+ * Copyright (C) 2008
+ * Guennadi Liakhovetski, DENX Software Engineering, <lg@denx.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/videodev2.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/vmalloc.h>
+#include <linux/interrupt.h>
+#include <linux/sched.h>
+#include <linux/dma/ipu-dma.h>
+
+#include <media/v4l2-common.h>
+#include <media/v4l2-dev.h>
+#include <media/videobuf2-dma-contig.h>
+#include <media/soc_camera.h>
+#include <media/soc_mediabus.h>
+
+#include <linux/platform_data/camera-mx3.h>
+#include <linux/platform_data/dma-imx.h>
+
+#define MX3_CAM_DRV_NAME "mx3-camera"
+
+/* CMOS Sensor Interface Registers */
+#define CSI_REG_START		0x60
+
+#define CSI_SENS_CONF		(0x60 - CSI_REG_START)
+#define CSI_SENS_FRM_SIZE	(0x64 - CSI_REG_START)
+#define CSI_ACT_FRM_SIZE	(0x68 - CSI_REG_START)
+#define CSI_OUT_FRM_CTRL	(0x6C - CSI_REG_START)
+#define CSI_TST_CTRL		(0x70 - CSI_REG_START)
+#define CSI_CCIR_CODE_1		(0x74 - CSI_REG_START)
+#define CSI_CCIR_CODE_2		(0x78 - CSI_REG_START)
+#define CSI_CCIR_CODE_3		(0x7C - CSI_REG_START)
+#define CSI_FLASH_STROBE_1	(0x80 - CSI_REG_START)
+#define CSI_FLASH_STROBE_2	(0x84 - CSI_REG_START)
+
+#define CSI_SENS_CONF_VSYNC_POL_SHIFT		0
+#define CSI_SENS_CONF_HSYNC_POL_SHIFT		1
+#define CSI_SENS_CONF_DATA_POL_SHIFT		2
+#define CSI_SENS_CONF_PIX_CLK_POL_SHIFT		3
+#define CSI_SENS_CONF_SENS_PRTCL_SHIFT		4
+#define CSI_SENS_CONF_SENS_CLKSRC_SHIFT		7
+#define CSI_SENS_CONF_DATA_FMT_SHIFT		8
+#define CSI_SENS_CONF_DATA_WIDTH_SHIFT		10
+#define CSI_SENS_CONF_EXT_VSYNC_SHIFT		15
+#define CSI_SENS_CONF_DIVRATIO_SHIFT		16
+
+#define CSI_SENS_CONF_DATA_FMT_RGB_YUV444	(0UL << CSI_SENS_CONF_DATA_FMT_SHIFT)
+#define CSI_SENS_CONF_DATA_FMT_YUV422		(2UL << CSI_SENS_CONF_DATA_FMT_SHIFT)
+#define CSI_SENS_CONF_DATA_FMT_BAYER		(3UL << CSI_SENS_CONF_DATA_FMT_SHIFT)
+
+#define MAX_VIDEO_MEM 16
+
+struct mx3_camera_buffer {
+	/* common v4l buffer stuff -- must be first */
+	struct vb2_v4l2_buffer vb;
+	struct list_head			queue;
+
+	/* One descriptot per scatterlist (per frame) */
+	struct dma_async_tx_descriptor		*txd;
+
+	/* We have to "build" a scatterlist ourselves - one element per frame */
+	struct scatterlist			sg;
+};
+
+/**
+ * struct mx3_camera_dev - i.MX3x camera (CSI) object
+ * @dev:		camera device, to which the coherent buffer is attached
+ * @icd:		currently attached camera sensor
+ * @clk:		pointer to clock
+ * @base:		remapped register base address
+ * @pdata:		platform data
+ * @platform_flags:	platform flags
+ * @mclk:		master clock frequency in Hz
+ * @capture:		list of capture videobuffers
+ * @lock:		protects video buffer lists
+ * @active:		active video buffer
+ * @idmac_channel:	array of pointers to IPU DMAC DMA channels
+ * @soc_host:		embedded soc_host object
+ */
+struct mx3_camera_dev {
+	/*
+	 * i.MX3x is only supposed to handle one camera on its Camera Sensor
+	 * Interface. If anyone ever builds hardware to enable more than one
+	 * camera _simultaneously_, they will have to modify this driver too
+	 */
+	struct clk		*clk;
+
+	void __iomem		*base;
+
+	struct mx3_camera_pdata	*pdata;
+
+	unsigned long		platform_flags;
+	unsigned long		mclk;
+	u16			width_flags;	/* max 15 bits */
+
+	struct list_head	capture;
+	spinlock_t		lock;		/* Protects video buffer lists */
+	struct mx3_camera_buffer *active;
+	size_t			buf_total;
+	struct vb2_alloc_ctx	*alloc_ctx;
+	enum v4l2_field		field;
+	int			sequence;
+
+	/* IDMAC / dmaengine interface */
+	struct idmac_channel	*idmac_channel[1];	/* We need one channel */
+
+	struct soc_camera_host	soc_host;
+};
+
+struct dma_chan_request {
+	struct mx3_camera_dev	*mx3_cam;
+	enum ipu_channel	id;
+};
+
+static u32 csi_reg_read(struct mx3_camera_dev *mx3, off_t reg)
+{
+	return __raw_readl(mx3->base + reg);
+}
+
+static void csi_reg_write(struct mx3_camera_dev *mx3, u32 value, off_t reg)
+{
+	__raw_writel(value, mx3->base + reg);
+}
+
+static struct mx3_camera_buffer *to_mx3_vb(struct vb2_v4l2_buffer *vb)
+{
+	return container_of(vb, struct mx3_camera_buffer, vb);
+}
+
+/* Called from the IPU IDMAC ISR */
+static void mx3_cam_dma_done(void *arg)
+{
+	struct idmac_tx_desc *desc = to_tx_desc(arg);
+	struct dma_chan *chan = desc->txd.chan;
+	struct idmac_channel *ichannel = to_idmac_chan(chan);
+	struct mx3_camera_dev *mx3_cam = ichannel->client;
+
+	dev_dbg(chan->device->dev, "callback cookie %d, active DMA 0x%08x\n",
+		desc->txd.cookie, mx3_cam->active ? sg_dma_address(&mx3_cam->active->sg) : 0);
+
+	spin_lock(&mx3_cam->lock);
+	if (mx3_cam->active) {
+		struct vb2_v4l2_buffer *vb = &mx3_cam->active->vb;
+		struct mx3_camera_buffer *buf = to_mx3_vb(vb);
+
+		list_del_init(&buf->queue);
+		v4l2_get_timestamp(&vb->timestamp);
+		vb->field = mx3_cam->field;
+		vb->sequence = mx3_cam->sequence++;
+		vb2_buffer_done(&vb->vb2_buf, VB2_BUF_STATE_DONE);
+	}
+
+	if (list_empty(&mx3_cam->capture)) {
+		mx3_cam->active = NULL;
+		spin_unlock(&mx3_cam->lock);
+
+		/*
+		 * stop capture - without further buffers IPU_CHA_BUF0_RDY will
+		 * not get updated
+		 */
+		return;
+	}
+
+	mx3_cam->active = list_entry(mx3_cam->capture.next,
+				     struct mx3_camera_buffer, queue);
+	spin_unlock(&mx3_cam->lock);
+}
+
+/*
+ * Videobuf operations
+ */
+
+/*
+ * Calculate the __buffer__ (not data) size and number of buffers.
+ */
+static int mx3_videobuf_setup(struct vb2_queue *vq,
+			const void *parg,
+			unsigned int *count, unsigned int *num_planes,
+			unsigned int sizes[], void *alloc_ctxs[])
+{
+	const struct v4l2_format *fmt = parg;
+	struct soc_camera_device *icd = soc_camera_from_vb2q(vq);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct mx3_camera_dev *mx3_cam = ici->priv;
+
+	if (!mx3_cam->idmac_channel[0])
+		return -EINVAL;
+
+	if (fmt) {
+		const struct soc_camera_format_xlate *xlate = soc_camera_xlate_by_fourcc(icd,
+								fmt->fmt.pix.pixelformat);
+		unsigned int bytes_per_line;
+		int ret;
+
+		if (!xlate)
+			return -EINVAL;
+
+		ret = soc_mbus_bytes_per_line(fmt->fmt.pix.width,
+					      xlate->host_fmt);
+		if (ret < 0)
+			return ret;
+
+		bytes_per_line = max_t(u32, fmt->fmt.pix.bytesperline, ret);
+
+		ret = soc_mbus_image_size(xlate->host_fmt, bytes_per_line,
+					  fmt->fmt.pix.height);
+		if (ret < 0)
+			return ret;
+
+		sizes[0] = max_t(u32, fmt->fmt.pix.sizeimage, ret);
+	} else {
+		/* Called from VIDIOC_REQBUFS or in compatibility mode */
+		sizes[0] = icd->sizeimage;
+	}
+
+	alloc_ctxs[0] = mx3_cam->alloc_ctx;
+
+	if (!vq->num_buffers)
+		mx3_cam->sequence = 0;
+
+	if (!*count)
+		*count = 2;
+
+	/* If *num_planes != 0, we have already verified *count. */
+	if (!*num_planes &&
+	    sizes[0] * *count + mx3_cam->buf_total > MAX_VIDEO_MEM * 1024 * 1024)
+		*count = (MAX_VIDEO_MEM * 1024 * 1024 - mx3_cam->buf_total) /
+			sizes[0];
+
+	*num_planes = 1;
+
+	return 0;
+}
+
+static enum pixel_fmt fourcc_to_ipu_pix(__u32 fourcc)
+{
+	/* Add more formats as need arises and test possibilities appear... */
+	switch (fourcc) {
+	case V4L2_PIX_FMT_RGB24:
+		return IPU_PIX_FMT_RGB24;
+	case V4L2_PIX_FMT_UYVY:
+	case V4L2_PIX_FMT_RGB565:
+	default:
+		return IPU_PIX_FMT_GENERIC;
+	}
+}
+
+static void mx3_videobuf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct mx3_camera_dev *mx3_cam = ici->priv;
+	struct mx3_camera_buffer *buf = to_mx3_vb(vbuf);
+	struct scatterlist *sg = &buf->sg;
+	struct dma_async_tx_descriptor *txd;
+	struct idmac_channel *ichan = mx3_cam->idmac_channel[0];
+	struct idmac_video_param *video = &ichan->params.video;
+	const struct soc_mbus_pixelfmt *host_fmt = icd->current_fmt->host_fmt;
+	dma_cookie_t cookie;
+	size_t new_size;
+
+	new_size = icd->sizeimage;
+
+	if (vb2_plane_size(vb, 0) < new_size) {
+		dev_err(icd->parent, "Buffer #%d too small (%lu < %zu)\n",
+			vbuf->vb2_buf.index, vb2_plane_size(vb, 0), new_size);
+		goto error;
+	}
+
+	if (!buf->txd) {
+		sg_dma_address(sg)	= vb2_dma_contig_plane_dma_addr(vb, 0);
+		sg_dma_len(sg)		= new_size;
+
+		txd = dmaengine_prep_slave_sg(
+			&ichan->dma_chan, sg, 1, DMA_DEV_TO_MEM,
+			DMA_PREP_INTERRUPT);
+		if (!txd)
+			goto error;
+
+		txd->callback_param	= txd;
+		txd->callback		= mx3_cam_dma_done;
+
+		buf->txd		= txd;
+	} else {
+		txd = buf->txd;
+	}
+
+	vb2_set_plane_payload(vb, 0, new_size);
+
+	/* This is the configuration of one sg-element */
+	video->out_pixel_fmt = fourcc_to_ipu_pix(host_fmt->fourcc);
+
+	if (video->out_pixel_fmt == IPU_PIX_FMT_GENERIC) {
+		/*
+		 * If the IPU DMA channel is configured to transfer generic
+		 * 8-bit data, we have to set up the geometry parameters
+		 * correctly, according to the current pixel format. The DMA
+		 * horizontal parameters in this case are expressed in bytes,
+		 * not in pixels.
+		 */
+		video->out_width	= icd->bytesperline;
+		video->out_height	= icd->user_height;
+		video->out_stride	= icd->bytesperline;
+	} else {
+		/*
+		 * For IPU known formats the pixel unit will be managed
+		 * successfully by the IPU code
+		 */
+		video->out_width	= icd->user_width;
+		video->out_height	= icd->user_height;
+		video->out_stride	= icd->user_width;
+	}
+
+#ifdef DEBUG
+	/* helps to see what DMA actually has written */
+	if (vb2_plane_vaddr(vb, 0))
+		memset(vb2_plane_vaddr(vb, 0), 0xaa, vb2_get_plane_payload(vb, 0));
+#endif
+
+	spin_lock_irq(&mx3_cam->lock);
+	list_add_tail(&buf->queue, &mx3_cam->capture);
+
+	if (!mx3_cam->active)
+		mx3_cam->active = buf;
+
+	spin_unlock_irq(&mx3_cam->lock);
+
+	cookie = txd->tx_submit(txd);
+	dev_dbg(icd->parent, "Submitted cookie %d DMA 0x%08x\n",
+		cookie, sg_dma_address(&buf->sg));
+
+	if (cookie >= 0)
+		return;
+
+	spin_lock_irq(&mx3_cam->lock);
+
+	/* Submit error */
+	list_del_init(&buf->queue);
+
+	if (mx3_cam->active == buf)
+		mx3_cam->active = NULL;
+
+	spin_unlock_irq(&mx3_cam->lock);
+error:
+	vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
+}
+
+static void mx3_videobuf_release(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct mx3_camera_dev *mx3_cam = ici->priv;
+	struct mx3_camera_buffer *buf = to_mx3_vb(vbuf);
+	struct dma_async_tx_descriptor *txd = buf->txd;
+	unsigned long flags;
+
+	dev_dbg(icd->parent,
+		"Release%s DMA 0x%08x, queue %sempty\n",
+		mx3_cam->active == buf ? " active" : "", sg_dma_address(&buf->sg),
+		list_empty(&buf->queue) ? "" : "not ");
+
+	spin_lock_irqsave(&mx3_cam->lock, flags);
+
+	if (mx3_cam->active == buf)
+		mx3_cam->active = NULL;
+
+	/* Doesn't hurt also if the list is empty */
+	list_del_init(&buf->queue);
+
+	if (txd) {
+		buf->txd = NULL;
+		if (mx3_cam->idmac_channel[0])
+			async_tx_ack(txd);
+	}
+
+	spin_unlock_irqrestore(&mx3_cam->lock, flags);
+
+	mx3_cam->buf_total -= vb2_plane_size(vb, 0);
+}
+
+static int mx3_videobuf_init(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct mx3_camera_dev *mx3_cam = ici->priv;
+	struct mx3_camera_buffer *buf = to_mx3_vb(vbuf);
+
+	if (!buf->txd) {
+		/* This is for locking debugging only */
+		INIT_LIST_HEAD(&buf->queue);
+		sg_init_table(&buf->sg, 1);
+
+		mx3_cam->buf_total += vb2_plane_size(vb, 0);
+	}
+
+	return 0;
+}
+
+static void mx3_stop_streaming(struct vb2_queue *q)
+{
+	struct soc_camera_device *icd = soc_camera_from_vb2q(q);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct mx3_camera_dev *mx3_cam = ici->priv;
+	struct idmac_channel *ichan = mx3_cam->idmac_channel[0];
+	struct mx3_camera_buffer *buf, *tmp;
+	unsigned long flags;
+
+	if (ichan)
+		dmaengine_pause(&ichan->dma_chan);
+
+	spin_lock_irqsave(&mx3_cam->lock, flags);
+
+	mx3_cam->active = NULL;
+
+	list_for_each_entry_safe(buf, tmp, &mx3_cam->capture, queue) {
+		list_del_init(&buf->queue);
+		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+	}
+
+	spin_unlock_irqrestore(&mx3_cam->lock, flags);
+}
+
+static struct vb2_ops mx3_videobuf_ops = {
+	.queue_setup	= mx3_videobuf_setup,
+	.buf_queue	= mx3_videobuf_queue,
+	.buf_cleanup	= mx3_videobuf_release,
+	.buf_init	= mx3_videobuf_init,
+	.wait_prepare	= vb2_ops_wait_prepare,
+	.wait_finish	= vb2_ops_wait_finish,
+	.stop_streaming	= mx3_stop_streaming,
+};
+
+static int mx3_camera_init_videobuf(struct vb2_queue *q,
+				     struct soc_camera_device *icd)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+
+	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	q->io_modes = VB2_MMAP | VB2_USERPTR;
+	q->drv_priv = icd;
+	q->ops = &mx3_videobuf_ops;
+	q->mem_ops = &vb2_dma_contig_memops;
+	q->buf_struct_size = sizeof(struct mx3_camera_buffer);
+	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	q->lock = &ici->host_lock;
+
+	return vb2_queue_init(q);
+}
+
+/* First part of ipu_csi_init_interface() */
+static void mx3_camera_activate(struct mx3_camera_dev *mx3_cam)
+{
+	u32 conf;
+	long rate;
+
+	/* Set default size: ipu_csi_set_window_size() */
+	csi_reg_write(mx3_cam, (640 - 1) | ((480 - 1) << 16), CSI_ACT_FRM_SIZE);
+	/* ...and position to 0:0: ipu_csi_set_window_pos() */
+	conf = csi_reg_read(mx3_cam, CSI_OUT_FRM_CTRL) & 0xffff0000;
+	csi_reg_write(mx3_cam, conf, CSI_OUT_FRM_CTRL);
+
+	/* We use only gated clock synchronisation mode so far */
+	conf = 0 << CSI_SENS_CONF_SENS_PRTCL_SHIFT;
+
+	/* Set generic data, platform-biggest bus-width */
+	conf |= CSI_SENS_CONF_DATA_FMT_BAYER;
+
+	if (mx3_cam->platform_flags & MX3_CAMERA_DATAWIDTH_15)
+		conf |= 3 << CSI_SENS_CONF_DATA_WIDTH_SHIFT;
+	else if (mx3_cam->platform_flags & MX3_CAMERA_DATAWIDTH_10)
+		conf |= 2 << CSI_SENS_CONF_DATA_WIDTH_SHIFT;
+	else if (mx3_cam->platform_flags & MX3_CAMERA_DATAWIDTH_8)
+		conf |= 1 << CSI_SENS_CONF_DATA_WIDTH_SHIFT;
+	else/* if (mx3_cam->platform_flags & MX3_CAMERA_DATAWIDTH_4)*/
+		conf |= 0 << CSI_SENS_CONF_DATA_WIDTH_SHIFT;
+
+	if (mx3_cam->platform_flags & MX3_CAMERA_CLK_SRC)
+		conf |= 1 << CSI_SENS_CONF_SENS_CLKSRC_SHIFT;
+	if (mx3_cam->platform_flags & MX3_CAMERA_EXT_VSYNC)
+		conf |= 1 << CSI_SENS_CONF_EXT_VSYNC_SHIFT;
+	if (mx3_cam->platform_flags & MX3_CAMERA_DP)
+		conf |= 1 << CSI_SENS_CONF_DATA_POL_SHIFT;
+	if (mx3_cam->platform_flags & MX3_CAMERA_PCP)
+		conf |= 1 << CSI_SENS_CONF_PIX_CLK_POL_SHIFT;
+	if (mx3_cam->platform_flags & MX3_CAMERA_HSP)
+		conf |= 1 << CSI_SENS_CONF_HSYNC_POL_SHIFT;
+	if (mx3_cam->platform_flags & MX3_CAMERA_VSP)
+		conf |= 1 << CSI_SENS_CONF_VSYNC_POL_SHIFT;
+
+	/* ipu_csi_init_interface() */
+	csi_reg_write(mx3_cam, conf, CSI_SENS_CONF);
+
+	clk_prepare_enable(mx3_cam->clk);
+	rate = clk_round_rate(mx3_cam->clk, mx3_cam->mclk);
+	dev_dbg(mx3_cam->soc_host.v4l2_dev.dev, "Set SENS_CONF to %x, rate %ld\n", conf, rate);
+	if (rate)
+		clk_set_rate(mx3_cam->clk, rate);
+}
+
+static int mx3_camera_add_device(struct soc_camera_device *icd)
+{
+	dev_info(icd->parent, "MX3 Camera driver attached to camera %d\n",
+		 icd->devnum);
+
+	return 0;
+}
+
+static void mx3_camera_remove_device(struct soc_camera_device *icd)
+{
+	dev_info(icd->parent, "MX3 Camera driver detached from camera %d\n",
+		 icd->devnum);
+}
+
+/* Called with .host_lock held */
+static int mx3_camera_clock_start(struct soc_camera_host *ici)
+{
+	struct mx3_camera_dev *mx3_cam = ici->priv;
+
+	mx3_camera_activate(mx3_cam);
+
+	mx3_cam->buf_total = 0;
+
+	return 0;
+}
+
+/* Called with .host_lock held */
+static void mx3_camera_clock_stop(struct soc_camera_host *ici)
+{
+	struct mx3_camera_dev *mx3_cam = ici->priv;
+	struct idmac_channel **ichan = &mx3_cam->idmac_channel[0];
+
+	if (*ichan) {
+		dma_release_channel(&(*ichan)->dma_chan);
+		*ichan = NULL;
+	}
+
+	clk_disable_unprepare(mx3_cam->clk);
+}
+
+static int test_platform_param(struct mx3_camera_dev *mx3_cam,
+			       unsigned char buswidth, unsigned long *flags)
+{
+	/*
+	 * If requested data width is supported by the platform, use it or any
+	 * possible lower value - i.MX31 is smart enough to shift bits
+	 */
+	if (buswidth > fls(mx3_cam->width_flags))
+		return -EINVAL;
+
+	/*
+	 * Platform specified synchronization and pixel clock polarities are
+	 * only a recommendation and are only used during probing. MX3x
+	 * camera interface only works in master mode, i.e., uses HSYNC and
+	 * VSYNC signals from the sensor
+	 */
+	*flags = V4L2_MBUS_MASTER |
+		V4L2_MBUS_HSYNC_ACTIVE_HIGH |
+		V4L2_MBUS_HSYNC_ACTIVE_LOW |
+		V4L2_MBUS_VSYNC_ACTIVE_HIGH |
+		V4L2_MBUS_VSYNC_ACTIVE_LOW |
+		V4L2_MBUS_PCLK_SAMPLE_RISING |
+		V4L2_MBUS_PCLK_SAMPLE_FALLING |
+		V4L2_MBUS_DATA_ACTIVE_HIGH |
+		V4L2_MBUS_DATA_ACTIVE_LOW;
+
+	return 0;
+}
+
+static int mx3_camera_try_bus_param(struct soc_camera_device *icd,
+				    const unsigned int depth)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct mx3_camera_dev *mx3_cam = ici->priv;
+	struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
+	unsigned long bus_flags, common_flags;
+	int ret = test_platform_param(mx3_cam, depth, &bus_flags);
+
+	dev_dbg(icd->parent, "request bus width %d bit: %d\n", depth, ret);
+
+	if (ret < 0)
+		return ret;
+
+	ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
+	if (!ret) {
+		common_flags = soc_mbus_config_compatible(&cfg,
+							  bus_flags);
+		if (!common_flags) {
+			dev_warn(icd->parent,
+				 "Flags incompatible: camera 0x%x, host 0x%lx\n",
+				 cfg.flags, bus_flags);
+			return -EINVAL;
+		}
+	} else if (ret != -ENOIOCTLCMD) {
+		return ret;
+	}
+
+	return 0;
+}
+
+static bool chan_filter(struct dma_chan *chan, void *arg)
+{
+	struct dma_chan_request *rq = arg;
+	struct mx3_camera_pdata *pdata;
+
+	if (!imx_dma_is_ipu(chan))
+		return false;
+
+	if (!rq)
+		return false;
+
+	pdata = rq->mx3_cam->soc_host.v4l2_dev.dev->platform_data;
+
+	return rq->id == chan->chan_id &&
+		pdata->dma_dev == chan->device->dev;
+}
+
+static const struct soc_mbus_pixelfmt mx3_camera_formats[] = {
+	{
+		.fourcc			= V4L2_PIX_FMT_SBGGR8,
+		.name			= "Bayer BGGR (sRGB) 8 bit",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_NONE,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	}, {
+		.fourcc			= V4L2_PIX_FMT_GREY,
+		.name			= "Monochrome 8 bit",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_NONE,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+};
+
+/* This will be corrected as we get more formats */
+static bool mx3_camera_packing_supported(const struct soc_mbus_pixelfmt *fmt)
+{
+	return	fmt->packing == SOC_MBUS_PACKING_NONE ||
+		(fmt->bits_per_sample == 8 &&
+		 fmt->packing == SOC_MBUS_PACKING_2X8_PADHI) ||
+		(fmt->bits_per_sample > 8 &&
+		 fmt->packing == SOC_MBUS_PACKING_EXTEND16);
+}
+
+static int mx3_camera_get_formats(struct soc_camera_device *icd, unsigned int idx,
+				  struct soc_camera_format_xlate *xlate)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct device *dev = icd->parent;
+	int formats = 0, ret;
+	struct v4l2_subdev_mbus_code_enum code = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.index = idx,
+	};
+	const struct soc_mbus_pixelfmt *fmt;
+
+	ret = v4l2_subdev_call(sd, pad, enum_mbus_code, NULL, &code);
+	if (ret < 0)
+		/* No more formats */
+		return 0;
+
+	fmt = soc_mbus_get_fmtdesc(code.code);
+	if (!fmt) {
+		dev_warn(icd->parent,
+			 "Unsupported format code #%u: 0x%x\n", idx, code.code);
+		return 0;
+	}
+
+	/* This also checks support for the requested bits-per-sample */
+	ret = mx3_camera_try_bus_param(icd, fmt->bits_per_sample);
+	if (ret < 0)
+		return 0;
+
+	switch (code.code) {
+	case MEDIA_BUS_FMT_SBGGR10_1X10:
+		formats++;
+		if (xlate) {
+			xlate->host_fmt	= &mx3_camera_formats[0];
+			xlate->code	= code.code;
+			xlate++;
+			dev_dbg(dev, "Providing format %s using code 0x%x\n",
+				mx3_camera_formats[0].name, code.code);
+		}
+		break;
+	case MEDIA_BUS_FMT_Y10_1X10:
+		formats++;
+		if (xlate) {
+			xlate->host_fmt	= &mx3_camera_formats[1];
+			xlate->code	= code.code;
+			xlate++;
+			dev_dbg(dev, "Providing format %s using code 0x%x\n",
+				mx3_camera_formats[1].name, code.code);
+		}
+		break;
+	default:
+		if (!mx3_camera_packing_supported(fmt))
+			return 0;
+	}
+
+	/* Generic pass-through */
+	formats++;
+	if (xlate) {
+		xlate->host_fmt	= fmt;
+		xlate->code	= code.code;
+		dev_dbg(dev, "Providing format %c%c%c%c in pass-through mode\n",
+			(fmt->fourcc >> (0*8)) & 0xFF,
+			(fmt->fourcc >> (1*8)) & 0xFF,
+			(fmt->fourcc >> (2*8)) & 0xFF,
+			(fmt->fourcc >> (3*8)) & 0xFF);
+		xlate++;
+	}
+
+	return formats;
+}
+
+static void configure_geometry(struct mx3_camera_dev *mx3_cam,
+			       unsigned int width, unsigned int height,
+			       const struct soc_mbus_pixelfmt *fmt)
+{
+	u32 ctrl, width_field, height_field;
+
+	if (fourcc_to_ipu_pix(fmt->fourcc) == IPU_PIX_FMT_GENERIC) {
+		/*
+		 * As the CSI will be configured to output BAYER, here
+		 * the width parameter count the number of samples to
+		 * capture to complete the whole image width.
+		 */
+		unsigned int num, den;
+		int ret = soc_mbus_samples_per_pixel(fmt, &num, &den);
+		BUG_ON(ret < 0);
+		width = width * num / den;
+	}
+
+	/* Setup frame size - this cannot be changed on-the-fly... */
+	width_field = width - 1;
+	height_field = height - 1;
+	csi_reg_write(mx3_cam, width_field | (height_field << 16), CSI_SENS_FRM_SIZE);
+
+	csi_reg_write(mx3_cam, width_field << 16, CSI_FLASH_STROBE_1);
+	csi_reg_write(mx3_cam, (height_field << 16) | 0x22, CSI_FLASH_STROBE_2);
+
+	csi_reg_write(mx3_cam, width_field | (height_field << 16), CSI_ACT_FRM_SIZE);
+
+	/* ...and position */
+	ctrl = csi_reg_read(mx3_cam, CSI_OUT_FRM_CTRL) & 0xffff0000;
+	/* Sensor does the cropping */
+	csi_reg_write(mx3_cam, ctrl | 0 | (0 << 8), CSI_OUT_FRM_CTRL);
+}
+
+static int acquire_dma_channel(struct mx3_camera_dev *mx3_cam)
+{
+	dma_cap_mask_t mask;
+	struct dma_chan *chan;
+	struct idmac_channel **ichan = &mx3_cam->idmac_channel[0];
+	/* We have to use IDMAC_IC_7 for Bayer / generic data */
+	struct dma_chan_request rq = {.mx3_cam = mx3_cam,
+				      .id = IDMAC_IC_7};
+
+	dma_cap_zero(mask);
+	dma_cap_set(DMA_SLAVE, mask);
+	dma_cap_set(DMA_PRIVATE, mask);
+	chan = dma_request_channel(mask, chan_filter, &rq);
+	if (!chan)
+		return -EBUSY;
+
+	*ichan = to_idmac_chan(chan);
+	(*ichan)->client = mx3_cam;
+
+	return 0;
+}
+
+/*
+ * FIXME: learn to use stride != width, then we can keep stride properly aligned
+ * and support arbitrary (even) widths.
+ */
+static inline void stride_align(__u32 *width)
+{
+	if (ALIGN(*width, 8) < 4096)
+		*width = ALIGN(*width, 8);
+	else
+		*width = *width &  ~7;
+}
+
+/*
+ * As long as we don't implement host-side cropping and scaling, we can use
+ * default g_crop and cropcap from soc_camera.c
+ */
+static int mx3_camera_set_crop(struct soc_camera_device *icd,
+			       const struct v4l2_crop *a)
+{
+	struct v4l2_crop a_writable = *a;
+	struct v4l2_rect *rect = &a_writable.c;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct mx3_camera_dev *mx3_cam = ici->priv;
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_mbus_framefmt *mf = &fmt.format;
+	int ret;
+
+	soc_camera_limit_side(&rect->left, &rect->width, 0, 2, 4096);
+	soc_camera_limit_side(&rect->top, &rect->height, 0, 2, 4096);
+
+	ret = v4l2_subdev_call(sd, video, s_crop, a);
+	if (ret < 0)
+		return ret;
+
+	/* The capture device might have changed its output sizes */
+	ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &fmt);
+	if (ret < 0)
+		return ret;
+
+	if (mf->code != icd->current_fmt->code)
+		return -EINVAL;
+
+	if (mf->width & 7) {
+		/* Ouch! We can only handle 8-byte aligned width... */
+		stride_align(&mf->width);
+		ret = v4l2_subdev_call(sd, pad, set_fmt, NULL, &fmt);
+		if (ret < 0)
+			return ret;
+	}
+
+	if (mf->width != icd->user_width || mf->height != icd->user_height)
+		configure_geometry(mx3_cam, mf->width, mf->height,
+				   icd->current_fmt->host_fmt);
+
+	dev_dbg(icd->parent, "Sensor cropped %dx%d\n",
+		mf->width, mf->height);
+
+	icd->user_width		= mf->width;
+	icd->user_height	= mf->height;
+
+	return ret;
+}
+
+static int mx3_camera_set_fmt(struct soc_camera_device *icd,
+			      struct v4l2_format *f)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct mx3_camera_dev *mx3_cam = ici->priv;
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	const struct soc_camera_format_xlate *xlate;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_mbus_framefmt *mf = &format.format;
+	int ret;
+
+	xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
+	if (!xlate) {
+		dev_warn(icd->parent, "Format %x not found\n",
+			 pix->pixelformat);
+		return -EINVAL;
+	}
+
+	stride_align(&pix->width);
+	dev_dbg(icd->parent, "Set format %dx%d\n", pix->width, pix->height);
+
+	/*
+	 * Might have to perform a complete interface initialisation like in
+	 * ipu_csi_init_interface() in mxc_v4l2_s_param(). Also consider
+	 * mxc_v4l2_s_fmt()
+	 */
+
+	configure_geometry(mx3_cam, pix->width, pix->height, xlate->host_fmt);
+
+	mf->width	= pix->width;
+	mf->height	= pix->height;
+	mf->field	= pix->field;
+	mf->colorspace	= pix->colorspace;
+	mf->code	= xlate->code;
+
+	ret = v4l2_subdev_call(sd, pad, set_fmt, NULL, &format);
+	if (ret < 0)
+		return ret;
+
+	if (mf->code != xlate->code)
+		return -EINVAL;
+
+	if (!mx3_cam->idmac_channel[0]) {
+		ret = acquire_dma_channel(mx3_cam);
+		if (ret < 0)
+			return ret;
+	}
+
+	pix->width		= mf->width;
+	pix->height		= mf->height;
+	pix->field		= mf->field;
+	mx3_cam->field		= mf->field;
+	pix->colorspace		= mf->colorspace;
+	icd->current_fmt	= xlate;
+
+	dev_dbg(icd->parent, "Sensor set %dx%d\n", pix->width, pix->height);
+
+	return ret;
+}
+
+static int mx3_camera_try_fmt(struct soc_camera_device *icd,
+			      struct v4l2_format *f)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	const struct soc_camera_format_xlate *xlate;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct v4l2_subdev_pad_config pad_cfg;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+	};
+	struct v4l2_mbus_framefmt *mf = &format.format;
+	__u32 pixfmt = pix->pixelformat;
+	int ret;
+
+	xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
+	if (pixfmt && !xlate) {
+		dev_warn(icd->parent, "Format %x not found\n", pixfmt);
+		return -EINVAL;
+	}
+
+	/* limit to MX3 hardware capabilities */
+	if (pix->height > 4096)
+		pix->height = 4096;
+	if (pix->width > 4096)
+		pix->width = 4096;
+
+	/* limit to sensor capabilities */
+	mf->width	= pix->width;
+	mf->height	= pix->height;
+	mf->field	= pix->field;
+	mf->colorspace	= pix->colorspace;
+	mf->code	= xlate->code;
+
+	ret = v4l2_subdev_call(sd, pad, set_fmt, &pad_cfg, &format);
+	if (ret < 0)
+		return ret;
+
+	pix->width	= mf->width;
+	pix->height	= mf->height;
+	pix->colorspace	= mf->colorspace;
+
+	switch (mf->field) {
+	case V4L2_FIELD_ANY:
+		pix->field = V4L2_FIELD_NONE;
+		break;
+	case V4L2_FIELD_NONE:
+		break;
+	default:
+		dev_err(icd->parent, "Field type %d unsupported.\n",
+			mf->field);
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static int mx3_camera_reqbufs(struct soc_camera_device *icd,
+			      struct v4l2_requestbuffers *p)
+{
+	return 0;
+}
+
+static unsigned int mx3_camera_poll(struct file *file, poll_table *pt)
+{
+	struct soc_camera_device *icd = file->private_data;
+
+	return vb2_poll(&icd->vb2_vidq, file, pt);
+}
+
+static int mx3_camera_querycap(struct soc_camera_host *ici,
+			       struct v4l2_capability *cap)
+{
+	/* cap->name is set by the firendly caller:-> */
+	strlcpy(cap->card, "i.MX3x Camera", sizeof(cap->card));
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+
+	return 0;
+}
+
+static int mx3_camera_set_bus_param(struct soc_camera_device *icd)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct mx3_camera_dev *mx3_cam = ici->priv;
+	struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
+	u32 pixfmt = icd->current_fmt->host_fmt->fourcc;
+	unsigned long bus_flags, common_flags;
+	u32 dw, sens_conf;
+	const struct soc_mbus_pixelfmt *fmt;
+	int buswidth;
+	int ret;
+	const struct soc_camera_format_xlate *xlate;
+	struct device *dev = icd->parent;
+
+	fmt = soc_mbus_get_fmtdesc(icd->current_fmt->code);
+	if (!fmt)
+		return -EINVAL;
+
+	xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
+	if (!xlate) {
+		dev_warn(dev, "Format %x not found\n", pixfmt);
+		return -EINVAL;
+	}
+
+	buswidth = fmt->bits_per_sample;
+	ret = test_platform_param(mx3_cam, buswidth, &bus_flags);
+
+	dev_dbg(dev, "requested bus width %d bit: %d\n", buswidth, ret);
+
+	if (ret < 0)
+		return ret;
+
+	ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
+	if (!ret) {
+		common_flags = soc_mbus_config_compatible(&cfg,
+							  bus_flags);
+		if (!common_flags) {
+			dev_warn(icd->parent,
+				 "Flags incompatible: camera 0x%x, host 0x%lx\n",
+				 cfg.flags, bus_flags);
+			return -EINVAL;
+		}
+	} else if (ret != -ENOIOCTLCMD) {
+		return ret;
+	} else {
+		common_flags = bus_flags;
+	}
+
+	dev_dbg(dev, "Flags cam: 0x%x host: 0x%lx common: 0x%lx\n",
+		cfg.flags, bus_flags, common_flags);
+
+	/* Make choices, based on platform preferences */
+	if ((common_flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH) &&
+	    (common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)) {
+		if (mx3_cam->platform_flags & MX3_CAMERA_HSP)
+			common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_HIGH;
+		else
+			common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_LOW;
+	}
+
+	if ((common_flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH) &&
+	    (common_flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)) {
+		if (mx3_cam->platform_flags & MX3_CAMERA_VSP)
+			common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_HIGH;
+		else
+			common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_LOW;
+	}
+
+	if ((common_flags & V4L2_MBUS_DATA_ACTIVE_HIGH) &&
+	    (common_flags & V4L2_MBUS_DATA_ACTIVE_LOW)) {
+		if (mx3_cam->platform_flags & MX3_CAMERA_DP)
+			common_flags &= ~V4L2_MBUS_DATA_ACTIVE_HIGH;
+		else
+			common_flags &= ~V4L2_MBUS_DATA_ACTIVE_LOW;
+	}
+
+	if ((common_flags & V4L2_MBUS_PCLK_SAMPLE_RISING) &&
+	    (common_flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)) {
+		if (mx3_cam->platform_flags & MX3_CAMERA_PCP)
+			common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_RISING;
+		else
+			common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_FALLING;
+	}
+
+	cfg.flags = common_flags;
+	ret = v4l2_subdev_call(sd, video, s_mbus_config, &cfg);
+	if (ret < 0 && ret != -ENOIOCTLCMD) {
+		dev_dbg(dev, "camera s_mbus_config(0x%lx) returned %d\n",
+			common_flags, ret);
+		return ret;
+	}
+
+	/*
+	 * So far only gated clock mode is supported. Add a line
+	 *	(3 << CSI_SENS_CONF_SENS_PRTCL_SHIFT) |
+	 * below and select the required mode when supporting other
+	 * synchronisation protocols.
+	 */
+	sens_conf = csi_reg_read(mx3_cam, CSI_SENS_CONF) &
+		~((1 << CSI_SENS_CONF_VSYNC_POL_SHIFT) |
+		  (1 << CSI_SENS_CONF_HSYNC_POL_SHIFT) |
+		  (1 << CSI_SENS_CONF_DATA_POL_SHIFT) |
+		  (1 << CSI_SENS_CONF_PIX_CLK_POL_SHIFT) |
+		  (3 << CSI_SENS_CONF_DATA_FMT_SHIFT) |
+		  (3 << CSI_SENS_CONF_DATA_WIDTH_SHIFT));
+
+	/* TODO: Support RGB and YUV formats */
+
+	/* This has been set in mx3_camera_activate(), but we clear it above */
+	sens_conf |= CSI_SENS_CONF_DATA_FMT_BAYER;
+
+	if (common_flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
+		sens_conf |= 1 << CSI_SENS_CONF_PIX_CLK_POL_SHIFT;
+	if (common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)
+		sens_conf |= 1 << CSI_SENS_CONF_HSYNC_POL_SHIFT;
+	if (common_flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
+		sens_conf |= 1 << CSI_SENS_CONF_VSYNC_POL_SHIFT;
+	if (common_flags & V4L2_MBUS_DATA_ACTIVE_LOW)
+		sens_conf |= 1 << CSI_SENS_CONF_DATA_POL_SHIFT;
+
+	/* Just do what we're asked to do */
+	switch (xlate->host_fmt->bits_per_sample) {
+	case 4:
+		dw = 0 << CSI_SENS_CONF_DATA_WIDTH_SHIFT;
+		break;
+	case 8:
+		dw = 1 << CSI_SENS_CONF_DATA_WIDTH_SHIFT;
+		break;
+	case 10:
+		dw = 2 << CSI_SENS_CONF_DATA_WIDTH_SHIFT;
+		break;
+	default:
+		/*
+		 * Actually it can only be 15 now, default is just to silence
+		 * compiler warnings
+		 */
+	case 15:
+		dw = 3 << CSI_SENS_CONF_DATA_WIDTH_SHIFT;
+	}
+
+	csi_reg_write(mx3_cam, sens_conf | dw, CSI_SENS_CONF);
+
+	dev_dbg(dev, "Set SENS_CONF to %x\n", sens_conf | dw);
+
+	return 0;
+}
+
+static struct soc_camera_host_ops mx3_soc_camera_host_ops = {
+	.owner		= THIS_MODULE,
+	.add		= mx3_camera_add_device,
+	.remove		= mx3_camera_remove_device,
+	.clock_start	= mx3_camera_clock_start,
+	.clock_stop	= mx3_camera_clock_stop,
+	.set_crop	= mx3_camera_set_crop,
+	.set_fmt	= mx3_camera_set_fmt,
+	.try_fmt	= mx3_camera_try_fmt,
+	.get_formats	= mx3_camera_get_formats,
+	.init_videobuf2	= mx3_camera_init_videobuf,
+	.reqbufs	= mx3_camera_reqbufs,
+	.poll		= mx3_camera_poll,
+	.querycap	= mx3_camera_querycap,
+	.set_bus_param	= mx3_camera_set_bus_param,
+};
+
+static int mx3_camera_probe(struct platform_device *pdev)
+{
+	struct mx3_camera_pdata	*pdata = pdev->dev.platform_data;
+	struct mx3_camera_dev *mx3_cam;
+	struct resource *res;
+	void __iomem *base;
+	int err = 0;
+	struct soc_camera_host *soc_host;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
+
+	if (!pdata)
+		return -EINVAL;
+
+	mx3_cam = devm_kzalloc(&pdev->dev, sizeof(*mx3_cam), GFP_KERNEL);
+	if (!mx3_cam) {
+		dev_err(&pdev->dev, "Could not allocate mx3 camera object\n");
+		return -ENOMEM;
+	}
+
+	mx3_cam->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(mx3_cam->clk))
+		return PTR_ERR(mx3_cam->clk);
+
+	mx3_cam->pdata = pdata;
+	mx3_cam->platform_flags = pdata->flags;
+	if (!(mx3_cam->platform_flags & MX3_CAMERA_DATAWIDTH_MASK)) {
+		/*
+		 * Platform hasn't set available data widths. This is bad.
+		 * Warn and use a default.
+		 */
+		dev_warn(&pdev->dev, "WARNING! Platform hasn't set available "
+			 "data widths, using default 8 bit\n");
+		mx3_cam->platform_flags |= MX3_CAMERA_DATAWIDTH_8;
+	}
+	if (mx3_cam->platform_flags & MX3_CAMERA_DATAWIDTH_4)
+		mx3_cam->width_flags = 1 << 3;
+	if (mx3_cam->platform_flags & MX3_CAMERA_DATAWIDTH_8)
+		mx3_cam->width_flags |= 1 << 7;
+	if (mx3_cam->platform_flags & MX3_CAMERA_DATAWIDTH_10)
+		mx3_cam->width_flags |= 1 << 9;
+	if (mx3_cam->platform_flags & MX3_CAMERA_DATAWIDTH_15)
+		mx3_cam->width_flags |= 1 << 14;
+
+	mx3_cam->mclk = pdata->mclk_10khz * 10000;
+	if (!mx3_cam->mclk) {
+		dev_warn(&pdev->dev,
+			 "mclk_10khz == 0! Please, fix your platform data. "
+			 "Using default 20MHz\n");
+		mx3_cam->mclk = 20000000;
+	}
+
+	/* list of video-buffers */
+	INIT_LIST_HEAD(&mx3_cam->capture);
+	spin_lock_init(&mx3_cam->lock);
+
+	mx3_cam->base	= base;
+
+	soc_host		= &mx3_cam->soc_host;
+	soc_host->drv_name	= MX3_CAM_DRV_NAME;
+	soc_host->ops		= &mx3_soc_camera_host_ops;
+	soc_host->priv		= mx3_cam;
+	soc_host->v4l2_dev.dev	= &pdev->dev;
+	soc_host->nr		= pdev->id;
+
+	mx3_cam->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
+	if (IS_ERR(mx3_cam->alloc_ctx))
+		return PTR_ERR(mx3_cam->alloc_ctx);
+
+	if (pdata->asd_sizes) {
+		soc_host->asd = pdata->asd;
+		soc_host->asd_sizes = pdata->asd_sizes;
+	}
+
+	err = soc_camera_host_register(soc_host);
+	if (err)
+		goto ecamhostreg;
+
+	/* IDMAC interface */
+	dmaengine_get();
+
+	return 0;
+
+ecamhostreg:
+	vb2_dma_contig_cleanup_ctx(mx3_cam->alloc_ctx);
+	return err;
+}
+
+static int mx3_camera_remove(struct platform_device *pdev)
+{
+	struct soc_camera_host *soc_host = to_soc_camera_host(&pdev->dev);
+	struct mx3_camera_dev *mx3_cam = container_of(soc_host,
+					struct mx3_camera_dev, soc_host);
+
+	soc_camera_host_unregister(soc_host);
+
+	/*
+	 * The channel has either not been allocated,
+	 * or should have been released
+	 */
+	if (WARN_ON(mx3_cam->idmac_channel[0]))
+		dma_release_channel(&mx3_cam->idmac_channel[0]->dma_chan);
+
+	vb2_dma_contig_cleanup_ctx(mx3_cam->alloc_ctx);
+
+	dmaengine_put();
+
+	return 0;
+}
+
+static struct platform_driver mx3_camera_driver = {
+	.driver		= {
+		.name	= MX3_CAM_DRV_NAME,
+	},
+	.probe		= mx3_camera_probe,
+	.remove		= mx3_camera_remove,
+};
+
+module_platform_driver(mx3_camera_driver);
+
+MODULE_DESCRIPTION("i.MX3x SoC Camera Host driver");
+MODULE_AUTHOR("Guennadi Liakhovetski <lg@denx.de>");
+MODULE_LICENSE("GPL v2");
+MODULE_VERSION("0.2.3");
+MODULE_ALIAS("platform:" MX3_CAM_DRV_NAME);
diff --git a/drivers/media/platform/soc_camera/omap1_camera.c b/drivers/media/platform/soc_camera/omap1_camera.c
new file mode 100644
index 0000000..ba8dcd1
--- /dev/null
+++ b/drivers/media/platform/soc_camera/omap1_camera.c
@@ -0,0 +1,1738 @@
+/*
+ * V4L2 SoC Camera driver for OMAP1 Camera Interface
+ *
+ * Copyright (C) 2010, Janusz Krzysztofik <jkrzyszt@tis.icnet.pl>
+ *
+ * Based on V4L2 Driver for i.MXL/i.MXL camera (CSI) host
+ * Copyright (C) 2008, Paulius Zaleckas <paulius.zaleckas@teltonika.lt>
+ * Copyright (C) 2009, Darius Augulis <augulis.darius@gmail.com>
+ *
+ * Based on PXA SoC camera driver
+ * Copyright (C) 2006, Sascha Hauer, Pengutronix
+ * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
+ *
+ * Hardware specific bits initialy based on former work by Matt Callow
+ * drivers/media/platform/omap/omap1510cam.c
+ * Copyright (C) 2006 Matt Callow
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+
+#include <linux/clk.h>
+#include <linux/dma-mapping.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#include <media/omap1_camera.h>
+#include <media/soc_camera.h>
+#include <media/soc_mediabus.h>
+#include <media/videobuf-dma-contig.h>
+#include <media/videobuf-dma-sg.h>
+
+#include <linux/omap-dma.h>
+
+
+#define DRIVER_NAME		"omap1-camera"
+#define DRIVER_VERSION		"0.0.2"
+
+#define OMAP_DMA_CAMERA_IF_RX		20
+
+/*
+ * ---------------------------------------------------------------------------
+ *  OMAP1 Camera Interface registers
+ * ---------------------------------------------------------------------------
+ */
+
+#define REG_CTRLCLOCK		0x00
+#define REG_IT_STATUS		0x04
+#define REG_MODE		0x08
+#define REG_STATUS		0x0C
+#define REG_CAMDATA		0x10
+#define REG_GPIO		0x14
+#define REG_PEAK_COUNTER	0x18
+
+/* CTRLCLOCK bit shifts */
+#define LCLK_EN			BIT(7)
+#define DPLL_EN			BIT(6)
+#define MCLK_EN			BIT(5)
+#define CAMEXCLK_EN		BIT(4)
+#define POLCLK			BIT(3)
+#define FOSCMOD_SHIFT		0
+#define FOSCMOD_MASK		(0x7 << FOSCMOD_SHIFT)
+#define FOSCMOD_12MHz		0x0
+#define FOSCMOD_6MHz		0x2
+#define FOSCMOD_9_6MHz		0x4
+#define FOSCMOD_24MHz		0x5
+#define FOSCMOD_8MHz		0x6
+
+/* IT_STATUS bit shifts */
+#define DATA_TRANSFER		BIT(5)
+#define FIFO_FULL		BIT(4)
+#define H_DOWN			BIT(3)
+#define H_UP			BIT(2)
+#define V_DOWN			BIT(1)
+#define V_UP			BIT(0)
+
+/* MODE bit shifts */
+#define RAZ_FIFO		BIT(18)
+#define EN_FIFO_FULL		BIT(17)
+#define EN_NIRQ			BIT(16)
+#define THRESHOLD_SHIFT		9
+#define THRESHOLD_MASK		(0x7f << THRESHOLD_SHIFT)
+#define DMA			BIT(8)
+#define EN_H_DOWN		BIT(7)
+#define EN_H_UP			BIT(6)
+#define EN_V_DOWN		BIT(5)
+#define EN_V_UP			BIT(4)
+#define ORDERCAMD		BIT(3)
+
+#define IRQ_MASK		(EN_V_UP | EN_V_DOWN | EN_H_UP | EN_H_DOWN | \
+				 EN_NIRQ | EN_FIFO_FULL)
+
+/* STATUS bit shifts */
+#define HSTATUS			BIT(1)
+#define VSTATUS			BIT(0)
+
+/* GPIO bit shifts */
+#define CAM_RST			BIT(0)
+
+/* end of OMAP1 Camera Interface registers */
+
+
+#define SOCAM_BUS_FLAGS	(V4L2_MBUS_MASTER | \
+			V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_HIGH | \
+			V4L2_MBUS_PCLK_SAMPLE_RISING | V4L2_MBUS_PCLK_SAMPLE_FALLING | \
+			V4L2_MBUS_DATA_ACTIVE_HIGH)
+
+
+#define FIFO_SIZE		((THRESHOLD_MASK >> THRESHOLD_SHIFT) + 1)
+#define FIFO_SHIFT		__fls(FIFO_SIZE)
+
+#define DMA_BURST_SHIFT		(1 + OMAP_DMA_DATA_BURST_4)
+#define DMA_BURST_SIZE		(1 << DMA_BURST_SHIFT)
+
+#define DMA_ELEMENT_SHIFT	OMAP_DMA_DATA_TYPE_S32
+#define DMA_ELEMENT_SIZE	(1 << DMA_ELEMENT_SHIFT)
+
+#define DMA_FRAME_SHIFT_CONTIG	(FIFO_SHIFT - 1)
+#define DMA_FRAME_SHIFT_SG	DMA_BURST_SHIFT
+
+#define DMA_FRAME_SHIFT(x)	((x) == OMAP1_CAM_DMA_CONTIG ? \
+						DMA_FRAME_SHIFT_CONTIG : \
+						DMA_FRAME_SHIFT_SG)
+#define DMA_FRAME_SIZE(x)	(1 << DMA_FRAME_SHIFT(x))
+#define DMA_SYNC		OMAP_DMA_SYNC_FRAME
+#define THRESHOLD_LEVEL		DMA_FRAME_SIZE
+
+
+#define MAX_VIDEO_MEM		4	/* arbitrary video memory limit in MB */
+
+
+/*
+ * Structures
+ */
+
+/* buffer for one video frame */
+struct omap1_cam_buf {
+	struct videobuf_buffer		vb;
+	u32	code;
+	int				inwork;
+	struct scatterlist		*sgbuf;
+	int				sgcount;
+	int				bytes_left;
+	enum videobuf_state		result;
+};
+
+struct omap1_cam_dev {
+	struct soc_camera_host		soc_host;
+	struct clk			*clk;
+
+	unsigned int			irq;
+	void __iomem			*base;
+
+	int				dma_ch;
+
+	struct omap1_cam_platform_data	*pdata;
+	struct resource			*res;
+	unsigned long			pflags;
+	unsigned long			camexclk;
+
+	struct list_head		capture;
+
+	/* lock used to protect videobuf */
+	spinlock_t			lock;
+
+	/* Pointers to DMA buffers */
+	struct omap1_cam_buf		*active;
+	struct omap1_cam_buf		*ready;
+
+	enum omap1_cam_vb_mode		vb_mode;
+	int				(*mmap_mapper)(struct videobuf_queue *q,
+						struct videobuf_buffer *buf,
+						struct vm_area_struct *vma);
+
+	u32				reg_cache[0];
+};
+
+
+static void cam_write(struct omap1_cam_dev *pcdev, u16 reg, u32 val)
+{
+	pcdev->reg_cache[reg / sizeof(u32)] = val;
+	__raw_writel(val, pcdev->base + reg);
+}
+
+static u32 cam_read(struct omap1_cam_dev *pcdev, u16 reg, bool from_cache)
+{
+	return !from_cache ? __raw_readl(pcdev->base + reg) :
+			pcdev->reg_cache[reg / sizeof(u32)];
+}
+
+#define CAM_READ(pcdev, reg) \
+		cam_read(pcdev, REG_##reg, false)
+#define CAM_WRITE(pcdev, reg, val) \
+		cam_write(pcdev, REG_##reg, val)
+#define CAM_READ_CACHE(pcdev, reg) \
+		cam_read(pcdev, REG_##reg, true)
+
+/*
+ *  Videobuf operations
+ */
+static int omap1_videobuf_setup(struct videobuf_queue *vq, unsigned int *count,
+		unsigned int *size)
+{
+	struct soc_camera_device *icd = vq->priv_data;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct omap1_cam_dev *pcdev = ici->priv;
+
+	*size = icd->sizeimage;
+
+	if (!*count || *count < OMAP1_CAMERA_MIN_BUF_COUNT(pcdev->vb_mode))
+		*count = OMAP1_CAMERA_MIN_BUF_COUNT(pcdev->vb_mode);
+
+	if (*size * *count > MAX_VIDEO_MEM * 1024 * 1024)
+		*count = (MAX_VIDEO_MEM * 1024 * 1024) / *size;
+
+	dev_dbg(icd->parent,
+			"%s: count=%d, size=%d\n", __func__, *count, *size);
+
+	return 0;
+}
+
+static void free_buffer(struct videobuf_queue *vq, struct omap1_cam_buf *buf,
+		enum omap1_cam_vb_mode vb_mode)
+{
+	struct videobuf_buffer *vb = &buf->vb;
+
+	BUG_ON(in_interrupt());
+
+	videobuf_waiton(vq, vb, 0, 0);
+
+	if (vb_mode == OMAP1_CAM_DMA_CONTIG) {
+		videobuf_dma_contig_free(vq, vb);
+	} else {
+		struct soc_camera_device *icd = vq->priv_data;
+		struct device *dev = icd->parent;
+		struct videobuf_dmabuf *dma = videobuf_to_dma(vb);
+
+		videobuf_dma_unmap(dev, dma);
+		videobuf_dma_free(dma);
+	}
+
+	vb->state = VIDEOBUF_NEEDS_INIT;
+}
+
+static int omap1_videobuf_prepare(struct videobuf_queue *vq,
+		struct videobuf_buffer *vb, enum v4l2_field field)
+{
+	struct soc_camera_device *icd = vq->priv_data;
+	struct omap1_cam_buf *buf = container_of(vb, struct omap1_cam_buf, vb);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct omap1_cam_dev *pcdev = ici->priv;
+	int ret;
+
+	WARN_ON(!list_empty(&vb->queue));
+
+	BUG_ON(NULL == icd->current_fmt);
+
+	buf->inwork = 1;
+
+	if (buf->code != icd->current_fmt->code || vb->field != field ||
+			vb->width  != icd->user_width ||
+			vb->height != icd->user_height) {
+		buf->code  = icd->current_fmt->code;
+		vb->width  = icd->user_width;
+		vb->height = icd->user_height;
+		vb->field  = field;
+		vb->state  = VIDEOBUF_NEEDS_INIT;
+	}
+
+	vb->size = icd->sizeimage;
+
+	if (vb->baddr && vb->bsize < vb->size) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (vb->state == VIDEOBUF_NEEDS_INIT) {
+		ret = videobuf_iolock(vq, vb, NULL);
+		if (ret)
+			goto fail;
+
+		vb->state = VIDEOBUF_PREPARED;
+	}
+	buf->inwork = 0;
+
+	return 0;
+fail:
+	free_buffer(vq, buf, pcdev->vb_mode);
+out:
+	buf->inwork = 0;
+	return ret;
+}
+
+static void set_dma_dest_params(int dma_ch, struct omap1_cam_buf *buf,
+		enum omap1_cam_vb_mode vb_mode)
+{
+	dma_addr_t dma_addr;
+	unsigned int block_size;
+
+	if (vb_mode == OMAP1_CAM_DMA_CONTIG) {
+		dma_addr = videobuf_to_dma_contig(&buf->vb);
+		block_size = buf->vb.size;
+	} else {
+		if (WARN_ON(!buf->sgbuf)) {
+			buf->result = VIDEOBUF_ERROR;
+			return;
+		}
+		dma_addr = sg_dma_address(buf->sgbuf);
+		if (WARN_ON(!dma_addr)) {
+			buf->sgbuf = NULL;
+			buf->result = VIDEOBUF_ERROR;
+			return;
+		}
+		block_size = sg_dma_len(buf->sgbuf);
+		if (WARN_ON(!block_size)) {
+			buf->sgbuf = NULL;
+			buf->result = VIDEOBUF_ERROR;
+			return;
+		}
+		if (unlikely(buf->bytes_left < block_size))
+			block_size = buf->bytes_left;
+		if (WARN_ON(dma_addr & (DMA_FRAME_SIZE(vb_mode) *
+				DMA_ELEMENT_SIZE - 1))) {
+			dma_addr = ALIGN(dma_addr, DMA_FRAME_SIZE(vb_mode) *
+					DMA_ELEMENT_SIZE);
+			block_size &= ~(DMA_FRAME_SIZE(vb_mode) *
+					DMA_ELEMENT_SIZE - 1);
+		}
+		buf->bytes_left -= block_size;
+		buf->sgcount++;
+	}
+
+	omap_set_dma_dest_params(dma_ch,
+		OMAP_DMA_PORT_EMIFF, OMAP_DMA_AMODE_POST_INC, dma_addr, 0, 0);
+	omap_set_dma_transfer_params(dma_ch,
+		OMAP_DMA_DATA_TYPE_S32, DMA_FRAME_SIZE(vb_mode),
+		block_size >> (DMA_FRAME_SHIFT(vb_mode) + DMA_ELEMENT_SHIFT),
+		DMA_SYNC, 0, 0);
+}
+
+static struct omap1_cam_buf *prepare_next_vb(struct omap1_cam_dev *pcdev)
+{
+	struct omap1_cam_buf *buf;
+
+	/*
+	 * If there is already a buffer pointed out by the pcdev->ready,
+	 * (re)use it, otherwise try to fetch and configure a new one.
+	 */
+	buf = pcdev->ready;
+	if (!buf) {
+		if (list_empty(&pcdev->capture))
+			return buf;
+		buf = list_entry(pcdev->capture.next,
+				struct omap1_cam_buf, vb.queue);
+		buf->vb.state = VIDEOBUF_ACTIVE;
+		pcdev->ready = buf;
+		list_del_init(&buf->vb.queue);
+	}
+
+	if (pcdev->vb_mode == OMAP1_CAM_DMA_CONTIG) {
+		/*
+		 * In CONTIG mode, we can safely enter next buffer parameters
+		 * into the DMA programming register set after the DMA
+		 * has already been activated on the previous buffer
+		 */
+		set_dma_dest_params(pcdev->dma_ch, buf, pcdev->vb_mode);
+	} else {
+		/*
+		 * In SG mode, the above is not safe since there are probably
+		 * a bunch of sgbufs from previous sglist still pending.
+		 * Instead, mark the sglist fresh for the upcoming
+		 * try_next_sgbuf().
+		 */
+		buf->sgbuf = NULL;
+	}
+
+	return buf;
+}
+
+static struct scatterlist *try_next_sgbuf(int dma_ch, struct omap1_cam_buf *buf)
+{
+	struct scatterlist *sgbuf;
+
+	if (likely(buf->sgbuf)) {
+		/* current sglist is active */
+		if (unlikely(!buf->bytes_left)) {
+			/* indicate sglist complete */
+			sgbuf = NULL;
+		} else {
+			/* process next sgbuf */
+			sgbuf = sg_next(buf->sgbuf);
+			if (WARN_ON(!sgbuf)) {
+				buf->result = VIDEOBUF_ERROR;
+			} else if (WARN_ON(!sg_dma_len(sgbuf))) {
+				sgbuf = NULL;
+				buf->result = VIDEOBUF_ERROR;
+			}
+		}
+		buf->sgbuf = sgbuf;
+	} else {
+		/* sglist is fresh, initialize it before using */
+		struct videobuf_dmabuf *dma = videobuf_to_dma(&buf->vb);
+
+		sgbuf = dma->sglist;
+		if (!(WARN_ON(!sgbuf))) {
+			buf->sgbuf = sgbuf;
+			buf->sgcount = 0;
+			buf->bytes_left = buf->vb.size;
+			buf->result = VIDEOBUF_DONE;
+		}
+	}
+	if (sgbuf)
+		/*
+		 * Put our next sgbuf parameters (address, size)
+		 * into the DMA programming register set.
+		 */
+		set_dma_dest_params(dma_ch, buf, OMAP1_CAM_DMA_SG);
+
+	return sgbuf;
+}
+
+static void start_capture(struct omap1_cam_dev *pcdev)
+{
+	struct omap1_cam_buf *buf = pcdev->active;
+	u32 ctrlclock = CAM_READ_CACHE(pcdev, CTRLCLOCK);
+	u32 mode = CAM_READ_CACHE(pcdev, MODE) & ~EN_V_DOWN;
+
+	if (WARN_ON(!buf))
+		return;
+
+	/*
+	 * Enable start of frame interrupt, which we will use for activating
+	 * our end of frame watchdog when capture actually starts.
+	 */
+	mode |= EN_V_UP;
+
+	if (unlikely(ctrlclock & LCLK_EN))
+		/* stop pixel clock before FIFO reset */
+		CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock & ~LCLK_EN);
+	/* reset FIFO */
+	CAM_WRITE(pcdev, MODE, mode | RAZ_FIFO);
+
+	omap_start_dma(pcdev->dma_ch);
+
+	if (pcdev->vb_mode == OMAP1_CAM_DMA_SG) {
+		/*
+		 * In SG mode, it's a good moment for fetching next sgbuf
+		 * from the current sglist and, if available, already putting
+		 * its parameters into the DMA programming register set.
+		 */
+		try_next_sgbuf(pcdev->dma_ch, buf);
+	}
+
+	/* (re)enable pixel clock */
+	CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock | LCLK_EN);
+	/* release FIFO reset */
+	CAM_WRITE(pcdev, MODE, mode);
+}
+
+static void suspend_capture(struct omap1_cam_dev *pcdev)
+{
+	u32 ctrlclock = CAM_READ_CACHE(pcdev, CTRLCLOCK);
+
+	CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock & ~LCLK_EN);
+	omap_stop_dma(pcdev->dma_ch);
+}
+
+static void disable_capture(struct omap1_cam_dev *pcdev)
+{
+	u32 mode = CAM_READ_CACHE(pcdev, MODE);
+
+	CAM_WRITE(pcdev, MODE, mode & ~(IRQ_MASK | DMA));
+}
+
+static void omap1_videobuf_queue(struct videobuf_queue *vq,
+						struct videobuf_buffer *vb)
+{
+	struct soc_camera_device *icd = vq->priv_data;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct omap1_cam_dev *pcdev = ici->priv;
+	struct omap1_cam_buf *buf;
+	u32 mode;
+
+	list_add_tail(&vb->queue, &pcdev->capture);
+	vb->state = VIDEOBUF_QUEUED;
+
+	if (pcdev->active) {
+		/*
+		 * Capture in progress, so don't touch pcdev->ready even if
+		 * empty. Since the transfer of the DMA programming register set
+		 * content to the DMA working register set is done automatically
+		 * by the DMA hardware, this can pretty well happen while we
+		 * are keeping the lock here. Leave fetching it from the queue
+		 * to be done when a next DMA interrupt occures instead.
+		 */
+		return;
+	}
+
+	WARN_ON(pcdev->ready);
+
+	buf = prepare_next_vb(pcdev);
+	if (WARN_ON(!buf))
+		return;
+
+	pcdev->active = buf;
+	pcdev->ready = NULL;
+
+	dev_dbg(icd->parent,
+		"%s: capture not active, setup FIFO, start DMA\n", __func__);
+	mode = CAM_READ_CACHE(pcdev, MODE) & ~THRESHOLD_MASK;
+	mode |= THRESHOLD_LEVEL(pcdev->vb_mode) << THRESHOLD_SHIFT;
+	CAM_WRITE(pcdev, MODE, mode | EN_FIFO_FULL | DMA);
+
+	if (pcdev->vb_mode == OMAP1_CAM_DMA_SG) {
+		/*
+		 * In SG mode, the above prepare_next_vb() didn't actually
+		 * put anything into the DMA programming register set,
+		 * so we have to do it now, before activating DMA.
+		 */
+		try_next_sgbuf(pcdev->dma_ch, buf);
+	}
+
+	start_capture(pcdev);
+}
+
+static void omap1_videobuf_release(struct videobuf_queue *vq,
+				 struct videobuf_buffer *vb)
+{
+	struct omap1_cam_buf *buf =
+			container_of(vb, struct omap1_cam_buf, vb);
+	struct soc_camera_device *icd = vq->priv_data;
+	struct device *dev = icd->parent;
+	struct soc_camera_host *ici = to_soc_camera_host(dev);
+	struct omap1_cam_dev *pcdev = ici->priv;
+
+	switch (vb->state) {
+	case VIDEOBUF_DONE:
+		dev_dbg(dev, "%s (done)\n", __func__);
+		break;
+	case VIDEOBUF_ACTIVE:
+		dev_dbg(dev, "%s (active)\n", __func__);
+		break;
+	case VIDEOBUF_QUEUED:
+		dev_dbg(dev, "%s (queued)\n", __func__);
+		break;
+	case VIDEOBUF_PREPARED:
+		dev_dbg(dev, "%s (prepared)\n", __func__);
+		break;
+	default:
+		dev_dbg(dev, "%s (unknown %d)\n", __func__, vb->state);
+		break;
+	}
+
+	free_buffer(vq, buf, pcdev->vb_mode);
+}
+
+static void videobuf_done(struct omap1_cam_dev *pcdev,
+		enum videobuf_state result)
+{
+	struct omap1_cam_buf *buf = pcdev->active;
+	struct videobuf_buffer *vb;
+	struct device *dev = pcdev->soc_host.icd->parent;
+
+	if (WARN_ON(!buf)) {
+		suspend_capture(pcdev);
+		disable_capture(pcdev);
+		return;
+	}
+
+	if (result == VIDEOBUF_ERROR)
+		suspend_capture(pcdev);
+
+	vb = &buf->vb;
+	if (waitqueue_active(&vb->done)) {
+		if (!pcdev->ready && result != VIDEOBUF_ERROR) {
+			/*
+			 * No next buffer has been entered into the DMA
+			 * programming register set on time (could be done only
+			 * while the previous DMA interurpt was processed, not
+			 * later), so the last DMA block, be it a whole buffer
+			 * if in CONTIG or its last sgbuf if in SG mode, is
+			 * about to be reused by the just autoreinitialized DMA
+			 * engine, and overwritten with next frame data. Best we
+			 * can do is stopping the capture as soon as possible,
+			 * hopefully before the next frame start.
+			 */
+			suspend_capture(pcdev);
+		}
+		vb->state = result;
+		v4l2_get_timestamp(&vb->ts);
+		if (result != VIDEOBUF_ERROR)
+			vb->field_count++;
+		wake_up(&vb->done);
+
+		/* shift in next buffer */
+		buf = pcdev->ready;
+		pcdev->active = buf;
+		pcdev->ready = NULL;
+
+		if (!buf) {
+			/*
+			 * No next buffer was ready on time (see above), so
+			 * indicate error condition to force capture restart or
+			 * stop, depending on next buffer already queued or not.
+			 */
+			result = VIDEOBUF_ERROR;
+			prepare_next_vb(pcdev);
+
+			buf = pcdev->ready;
+			pcdev->active = buf;
+			pcdev->ready = NULL;
+		}
+	} else if (pcdev->ready) {
+		/*
+		 * In both CONTIG and SG mode, the DMA engine has possibly
+		 * been already autoreinitialized with the preprogrammed
+		 * pcdev->ready buffer.  We can either accept this fact
+		 * and just swap the buffers, or provoke an error condition
+		 * and restart capture.  The former seems less intrusive.
+		 */
+		dev_dbg(dev, "%s: nobody waiting on videobuf, swap with next\n",
+				__func__);
+		pcdev->active = pcdev->ready;
+
+		if (pcdev->vb_mode == OMAP1_CAM_DMA_SG) {
+			/*
+			 * In SG mode, we have to make sure that the buffer we
+			 * are putting back into the pcdev->ready is marked
+			 * fresh.
+			 */
+			buf->sgbuf = NULL;
+		}
+		pcdev->ready = buf;
+
+		buf = pcdev->active;
+	} else {
+		/*
+		 * No next buffer has been entered into
+		 * the DMA programming register set on time.
+		 */
+		if (pcdev->vb_mode == OMAP1_CAM_DMA_CONTIG) {
+			/*
+			 * In CONTIG mode, the DMA engine has already been
+			 * reinitialized with the current buffer. Best we can do
+			 * is not touching it.
+			 */
+			dev_dbg(dev,
+				"%s: nobody waiting on videobuf, reuse it\n",
+				__func__);
+		} else {
+			/*
+			 * In SG mode, the DMA engine has just been
+			 * autoreinitialized with the last sgbuf from the
+			 * current list. Restart capture in order to transfer
+			 * next frame start into the first sgbuf, not the last
+			 * one.
+			 */
+			if (result != VIDEOBUF_ERROR) {
+				suspend_capture(pcdev);
+				result = VIDEOBUF_ERROR;
+			}
+		}
+	}
+
+	if (!buf) {
+		dev_dbg(dev, "%s: no more videobufs, stop capture\n", __func__);
+		disable_capture(pcdev);
+		return;
+	}
+
+	if (pcdev->vb_mode == OMAP1_CAM_DMA_CONTIG) {
+		/*
+		 * In CONTIG mode, the current buffer parameters had already
+		 * been entered into the DMA programming register set while the
+		 * buffer was fetched with prepare_next_vb(), they may have also
+		 * been transferred into the runtime set and already active if
+		 * the DMA still running.
+		 */
+	} else {
+		/* In SG mode, extra steps are required */
+		if (result == VIDEOBUF_ERROR)
+			/* make sure we (re)use sglist from start on error */
+			buf->sgbuf = NULL;
+
+		/*
+		 * In any case, enter the next sgbuf parameters into the DMA
+		 * programming register set.  They will be used either during
+		 * nearest DMA autoreinitialization or, in case of an error,
+		 * on DMA startup below.
+		 */
+		try_next_sgbuf(pcdev->dma_ch, buf);
+	}
+
+	if (result == VIDEOBUF_ERROR) {
+		dev_dbg(dev, "%s: videobuf error; reset FIFO, restart DMA\n",
+				__func__);
+		start_capture(pcdev);
+		/*
+		 * In SG mode, the above also resulted in the next sgbuf
+		 * parameters being entered into the DMA programming register
+		 * set, making them ready for next DMA autoreinitialization.
+		 */
+	}
+
+	/*
+	 * Finally, try fetching next buffer.
+	 * In CONTIG mode, it will also enter it into the DMA programming
+	 * register set, making it ready for next DMA autoreinitialization.
+	 */
+	prepare_next_vb(pcdev);
+}
+
+static void dma_isr(int channel, unsigned short status, void *data)
+{
+	struct omap1_cam_dev *pcdev = data;
+	struct omap1_cam_buf *buf = pcdev->active;
+	unsigned long flags;
+
+	spin_lock_irqsave(&pcdev->lock, flags);
+
+	if (WARN_ON(!buf)) {
+		suspend_capture(pcdev);
+		disable_capture(pcdev);
+		goto out;
+	}
+
+	if (pcdev->vb_mode == OMAP1_CAM_DMA_CONTIG) {
+		/*
+		 * In CONTIG mode, assume we have just managed to collect the
+		 * whole frame, hopefully before our end of frame watchdog is
+		 * triggered. Then, all we have to do is disabling the watchdog
+		 * for this frame, and calling videobuf_done() with success
+		 * indicated.
+		 */
+		CAM_WRITE(pcdev, MODE,
+				CAM_READ_CACHE(pcdev, MODE) & ~EN_V_DOWN);
+		videobuf_done(pcdev, VIDEOBUF_DONE);
+	} else {
+		/*
+		 * In SG mode, we have to process every sgbuf from the current
+		 * sglist, one after another.
+		 */
+		if (buf->sgbuf) {
+			/*
+			 * Current sglist not completed yet, try fetching next
+			 * sgbuf, hopefully putting it into the DMA programming
+			 * register set, making it ready for next DMA
+			 * autoreinitialization.
+			 */
+			try_next_sgbuf(pcdev->dma_ch, buf);
+			if (buf->sgbuf)
+				goto out;
+
+			/*
+			 * No more sgbufs left in the current sglist. This
+			 * doesn't mean that the whole videobuffer is already
+			 * complete, but only that the last sgbuf from the
+			 * current sglist is about to be filled. It will be
+			 * ready on next DMA interrupt, signalled with the
+			 * buf->sgbuf set back to NULL.
+			 */
+			if (buf->result != VIDEOBUF_ERROR) {
+				/*
+				 * Video frame collected without errors so far,
+				 * we can prepare for collecting a next one
+				 * as soon as DMA gets autoreinitialized
+				 * after the current (last) sgbuf is completed.
+				 */
+				buf = prepare_next_vb(pcdev);
+				if (!buf)
+					goto out;
+
+				try_next_sgbuf(pcdev->dma_ch, buf);
+				goto out;
+			}
+		}
+		/* end of videobuf */
+		videobuf_done(pcdev, buf->result);
+	}
+
+out:
+	spin_unlock_irqrestore(&pcdev->lock, flags);
+}
+
+static irqreturn_t cam_isr(int irq, void *data)
+{
+	struct omap1_cam_dev *pcdev = data;
+	struct device *dev = pcdev->soc_host.icd->parent;
+	struct omap1_cam_buf *buf = pcdev->active;
+	u32 it_status;
+	unsigned long flags;
+
+	it_status = CAM_READ(pcdev, IT_STATUS);
+	if (!it_status)
+		return IRQ_NONE;
+
+	spin_lock_irqsave(&pcdev->lock, flags);
+
+	if (WARN_ON(!buf)) {
+		dev_warn(dev, "%s: unhandled camera interrupt, status == %#x\n",
+			 __func__, it_status);
+		suspend_capture(pcdev);
+		disable_capture(pcdev);
+		goto out;
+	}
+
+	if (unlikely(it_status & FIFO_FULL)) {
+		dev_warn(dev, "%s: FIFO overflow\n", __func__);
+
+	} else if (it_status & V_DOWN) {
+		/* end of video frame watchdog */
+		if (pcdev->vb_mode == OMAP1_CAM_DMA_CONTIG) {
+			/*
+			 * In CONTIG mode, the watchdog is disabled with
+			 * successful DMA end of block interrupt, and reenabled
+			 * on next frame start. If we get here, there is nothing
+			 * to check, we must be out of sync.
+			 */
+		} else {
+			if (buf->sgcount == 2) {
+				/*
+				 * If exactly 2 sgbufs from the next sglist have
+				 * been programmed into the DMA engine (the
+				 * first one already transferred into the DMA
+				 * runtime register set, the second one still
+				 * in the programming set), then we are in sync.
+				 */
+				goto out;
+			}
+		}
+		dev_notice(dev, "%s: unexpected end of video frame\n",
+				__func__);
+
+	} else if (it_status & V_UP) {
+		u32 mode;
+
+		if (pcdev->vb_mode == OMAP1_CAM_DMA_CONTIG) {
+			/*
+			 * In CONTIG mode, we need this interrupt every frame
+			 * in oredr to reenable our end of frame watchdog.
+			 */
+			mode = CAM_READ_CACHE(pcdev, MODE);
+		} else {
+			/*
+			 * In SG mode, the below enabled end of frame watchdog
+			 * is kept on permanently, so we can turn this one shot
+			 * setup off.
+			 */
+			mode = CAM_READ_CACHE(pcdev, MODE) & ~EN_V_UP;
+		}
+
+		if (!(mode & EN_V_DOWN)) {
+			/* (re)enable end of frame watchdog interrupt */
+			mode |= EN_V_DOWN;
+		}
+		CAM_WRITE(pcdev, MODE, mode);
+		goto out;
+
+	} else {
+		dev_warn(dev, "%s: unhandled camera interrupt, status == %#x\n",
+				__func__, it_status);
+		goto out;
+	}
+
+	videobuf_done(pcdev, VIDEOBUF_ERROR);
+out:
+	spin_unlock_irqrestore(&pcdev->lock, flags);
+	return IRQ_HANDLED;
+}
+
+static struct videobuf_queue_ops omap1_videobuf_ops = {
+	.buf_setup	= omap1_videobuf_setup,
+	.buf_prepare	= omap1_videobuf_prepare,
+	.buf_queue	= omap1_videobuf_queue,
+	.buf_release	= omap1_videobuf_release,
+};
+
+
+/*
+ * SOC Camera host operations
+ */
+
+static void sensor_reset(struct omap1_cam_dev *pcdev, bool reset)
+{
+	/* apply/release camera sensor reset if requested by platform data */
+	if (pcdev->pflags & OMAP1_CAMERA_RST_HIGH)
+		CAM_WRITE(pcdev, GPIO, reset);
+	else if (pcdev->pflags & OMAP1_CAMERA_RST_LOW)
+		CAM_WRITE(pcdev, GPIO, !reset);
+}
+
+static int omap1_cam_add_device(struct soc_camera_device *icd)
+{
+	dev_dbg(icd->parent, "OMAP1 Camera driver attached to camera %d\n",
+			icd->devnum);
+
+	return 0;
+}
+
+static void omap1_cam_remove_device(struct soc_camera_device *icd)
+{
+	dev_dbg(icd->parent,
+		"OMAP1 Camera driver detached from camera %d\n", icd->devnum);
+}
+
+/*
+ * The following two functions absolutely depend on the fact, that
+ * there can be only one camera on OMAP1 camera sensor interface
+ */
+static int omap1_cam_clock_start(struct soc_camera_host *ici)
+{
+	struct omap1_cam_dev *pcdev = ici->priv;
+	u32 ctrlclock;
+
+	clk_enable(pcdev->clk);
+
+	/* setup sensor clock */
+	ctrlclock = CAM_READ(pcdev, CTRLCLOCK);
+	ctrlclock &= ~(CAMEXCLK_EN | MCLK_EN | DPLL_EN);
+	CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock);
+
+	ctrlclock &= ~FOSCMOD_MASK;
+	switch (pcdev->camexclk) {
+	case 6000000:
+		ctrlclock |= CAMEXCLK_EN | FOSCMOD_6MHz;
+		break;
+	case 8000000:
+		ctrlclock |= CAMEXCLK_EN | FOSCMOD_8MHz | DPLL_EN;
+		break;
+	case 9600000:
+		ctrlclock |= CAMEXCLK_EN | FOSCMOD_9_6MHz | DPLL_EN;
+		break;
+	case 12000000:
+		ctrlclock |= CAMEXCLK_EN | FOSCMOD_12MHz;
+		break;
+	case 24000000:
+		ctrlclock |= CAMEXCLK_EN | FOSCMOD_24MHz | DPLL_EN;
+	default:
+		break;
+	}
+	CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock & ~DPLL_EN);
+
+	/* enable internal clock */
+	ctrlclock |= MCLK_EN;
+	CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock);
+
+	sensor_reset(pcdev, false);
+
+	return 0;
+}
+
+static void omap1_cam_clock_stop(struct soc_camera_host *ici)
+{
+	struct omap1_cam_dev *pcdev = ici->priv;
+	u32 ctrlclock;
+
+	suspend_capture(pcdev);
+	disable_capture(pcdev);
+
+	sensor_reset(pcdev, true);
+
+	/* disable and release system clocks */
+	ctrlclock = CAM_READ_CACHE(pcdev, CTRLCLOCK);
+	ctrlclock &= ~(MCLK_EN | DPLL_EN | CAMEXCLK_EN);
+	CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock);
+
+	ctrlclock = (ctrlclock & ~FOSCMOD_MASK) | FOSCMOD_12MHz;
+	CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock);
+	CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock | MCLK_EN);
+
+	CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock & ~MCLK_EN);
+
+	clk_disable(pcdev->clk);
+}
+
+/* Duplicate standard formats based on host capability of byte swapping */
+static const struct soc_mbus_lookup omap1_cam_formats[] = {
+{
+	.code = MEDIA_BUS_FMT_UYVY8_2X8,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_YUYV,
+		.name			= "YUYV",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
+		.order			= SOC_MBUS_ORDER_BE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_VYUY8_2X8,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_YVYU,
+		.name			= "YVYU",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
+		.order			= SOC_MBUS_ORDER_BE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_YUYV8_2X8,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_UYVY,
+		.name			= "UYVY",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
+		.order			= SOC_MBUS_ORDER_BE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_YVYU8_2X8,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_VYUY,
+		.name			= "VYUY",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
+		.order			= SOC_MBUS_ORDER_BE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_RGB555,
+		.name			= "RGB555",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
+		.order			= SOC_MBUS_ORDER_BE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_RGB555X,
+		.name			= "RGB555X",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
+		.order			= SOC_MBUS_ORDER_BE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_RGB565_2X8_BE,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_RGB565,
+		.name			= "RGB565",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
+		.order			= SOC_MBUS_ORDER_BE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_RGB565_2X8_LE,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_RGB565X,
+		.name			= "RGB565X",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
+		.order			= SOC_MBUS_ORDER_BE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+},
+};
+
+static int omap1_cam_get_formats(struct soc_camera_device *icd,
+		unsigned int idx, struct soc_camera_format_xlate *xlate)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct device *dev = icd->parent;
+	int formats = 0, ret;
+	struct v4l2_subdev_mbus_code_enum code = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.index = idx,
+	};
+	const struct soc_mbus_pixelfmt *fmt;
+
+	ret = v4l2_subdev_call(sd, pad, enum_mbus_code, NULL, &code);
+	if (ret < 0)
+		/* No more formats */
+		return 0;
+
+	fmt = soc_mbus_get_fmtdesc(code.code);
+	if (!fmt) {
+		dev_warn(dev, "%s: unsupported format code #%d: %d\n", __func__,
+				idx, code.code);
+		return 0;
+	}
+
+	/* Check support for the requested bits-per-sample */
+	if (fmt->bits_per_sample != 8)
+		return 0;
+
+	switch (code.code) {
+	case MEDIA_BUS_FMT_YUYV8_2X8:
+	case MEDIA_BUS_FMT_YVYU8_2X8:
+	case MEDIA_BUS_FMT_UYVY8_2X8:
+	case MEDIA_BUS_FMT_VYUY8_2X8:
+	case MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE:
+	case MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE:
+	case MEDIA_BUS_FMT_RGB565_2X8_BE:
+	case MEDIA_BUS_FMT_RGB565_2X8_LE:
+		formats++;
+		if (xlate) {
+			xlate->host_fmt	= soc_mbus_find_fmtdesc(code.code,
+						omap1_cam_formats,
+						ARRAY_SIZE(omap1_cam_formats));
+			xlate->code	= code.code;
+			xlate++;
+			dev_dbg(dev,
+				"%s: providing format %s as byte swapped code #%d\n",
+				__func__, xlate->host_fmt->name, code.code);
+		}
+	default:
+		if (xlate)
+			dev_dbg(dev,
+				"%s: providing format %s in pass-through mode\n",
+				__func__, fmt->name);
+	}
+	formats++;
+	if (xlate) {
+		xlate->host_fmt	= fmt;
+		xlate->code	= code.code;
+		xlate++;
+	}
+
+	return formats;
+}
+
+static bool is_dma_aligned(s32 bytes_per_line, unsigned int height,
+		enum omap1_cam_vb_mode vb_mode)
+{
+	int size = bytes_per_line * height;
+
+	return IS_ALIGNED(bytes_per_line, DMA_ELEMENT_SIZE) &&
+		IS_ALIGNED(size, DMA_FRAME_SIZE(vb_mode) * DMA_ELEMENT_SIZE);
+}
+
+static int dma_align(int *width, int *height,
+		const struct soc_mbus_pixelfmt *fmt,
+		enum omap1_cam_vb_mode vb_mode, bool enlarge)
+{
+	s32 bytes_per_line = soc_mbus_bytes_per_line(*width, fmt);
+
+	if (bytes_per_line < 0)
+		return bytes_per_line;
+
+	if (!is_dma_aligned(bytes_per_line, *height, vb_mode)) {
+		unsigned int pxalign = __fls(bytes_per_line / *width);
+		unsigned int salign  = DMA_FRAME_SHIFT(vb_mode) +
+				DMA_ELEMENT_SHIFT - pxalign;
+		unsigned int incr    = enlarge << salign;
+
+		v4l_bound_align_image(width, 1, *width + incr, 0,
+				height, 1, *height + incr, 0, salign);
+		return 0;
+	}
+	return 1;
+}
+
+#define subdev_call_with_sense(pcdev, dev, icd, sd, op, function, args...)		     \
+({										     \
+	struct soc_camera_sense sense = {					     \
+		.master_clock		= pcdev->camexclk,			     \
+		.pixel_clock_max	= 0,					     \
+	};									     \
+	int __ret;								     \
+										     \
+	if (pcdev->pdata)							     \
+		sense.pixel_clock_max = pcdev->pdata->lclk_khz_max * 1000;	     \
+	icd->sense = &sense;							     \
+	__ret = v4l2_subdev_call(sd, op, function, ##args);			     \
+	icd->sense = NULL;							     \
+										     \
+	if (sense.flags & SOCAM_SENSE_PCLK_CHANGED) {				     \
+		if (sense.pixel_clock > sense.pixel_clock_max) {		     \
+			dev_err(dev,						     \
+				"%s: pixel clock %lu set by the camera too high!\n", \
+				__func__, sense.pixel_clock);			     \
+			__ret = -EINVAL;					     \
+		}								     \
+	}									     \
+	__ret;									     \
+})
+
+static int set_format(struct omap1_cam_dev *pcdev, struct device *dev,
+		struct soc_camera_device *icd, struct v4l2_subdev *sd,
+		struct v4l2_subdev_format *format,
+		const struct soc_camera_format_xlate *xlate)
+{
+	s32 bytes_per_line;
+	struct v4l2_mbus_framefmt *mf = &format->format;
+	int ret = subdev_call_with_sense(pcdev, dev, icd, sd, pad, set_fmt, NULL, format);
+
+	if (ret < 0) {
+		dev_err(dev, "%s: set_fmt failed\n", __func__);
+		return ret;
+	}
+
+	if (mf->code != xlate->code) {
+		dev_err(dev, "%s: unexpected pixel code change\n", __func__);
+		return -EINVAL;
+	}
+
+	bytes_per_line = soc_mbus_bytes_per_line(mf->width, xlate->host_fmt);
+	if (bytes_per_line < 0) {
+		dev_err(dev, "%s: soc_mbus_bytes_per_line() failed\n",
+				__func__);
+		return bytes_per_line;
+	}
+
+	if (!is_dma_aligned(bytes_per_line, mf->height, pcdev->vb_mode)) {
+		dev_err(dev, "%s: resulting geometry %ux%u not DMA aligned\n",
+				__func__, mf->width, mf->height);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int omap1_cam_set_crop(struct soc_camera_device *icd,
+			       const struct v4l2_crop *crop)
+{
+	const struct v4l2_rect *rect = &crop->c;
+	const struct soc_camera_format_xlate *xlate = icd->current_fmt;
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct device *dev = icd->parent;
+	struct soc_camera_host *ici = to_soc_camera_host(dev);
+	struct omap1_cam_dev *pcdev = ici->priv;
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_mbus_framefmt *mf = &fmt.format;
+	int ret;
+
+	ret = subdev_call_with_sense(pcdev, dev, icd, sd, video, s_crop, crop);
+	if (ret < 0) {
+		dev_warn(dev, "%s: failed to crop to %ux%u@%u:%u\n", __func__,
+			 rect->width, rect->height, rect->left, rect->top);
+		return ret;
+	}
+
+	ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &fmt);
+	if (ret < 0) {
+		dev_warn(dev, "%s: failed to fetch current format\n", __func__);
+		return ret;
+	}
+
+	ret = dma_align(&mf->width, &mf->height, xlate->host_fmt, pcdev->vb_mode,
+			false);
+	if (ret < 0) {
+		dev_err(dev, "%s: failed to align %ux%u %s with DMA\n",
+				__func__, mf->width, mf->height,
+				xlate->host_fmt->name);
+		return ret;
+	}
+
+	if (!ret) {
+		/* sensor returned geometry not DMA aligned, trying to fix */
+		ret = set_format(pcdev, dev, icd, sd, &fmt, xlate);
+		if (ret < 0) {
+			dev_err(dev, "%s: failed to set format\n", __func__);
+			return ret;
+		}
+	}
+
+	icd->user_width	 = mf->width;
+	icd->user_height = mf->height;
+
+	return 0;
+}
+
+static int omap1_cam_set_fmt(struct soc_camera_device *icd,
+			      struct v4l2_format *f)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	const struct soc_camera_format_xlate *xlate;
+	struct device *dev = icd->parent;
+	struct soc_camera_host *ici = to_soc_camera_host(dev);
+	struct omap1_cam_dev *pcdev = ici->priv;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_mbus_framefmt *mf = &format.format;
+	int ret;
+
+	xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
+	if (!xlate) {
+		dev_warn(dev, "%s: format %#x not found\n", __func__,
+				pix->pixelformat);
+		return -EINVAL;
+	}
+
+	mf->width	= pix->width;
+	mf->height	= pix->height;
+	mf->field	= pix->field;
+	mf->colorspace	= pix->colorspace;
+	mf->code	= xlate->code;
+
+	ret = dma_align(&mf->width, &mf->height, xlate->host_fmt, pcdev->vb_mode,
+			true);
+	if (ret < 0) {
+		dev_err(dev, "%s: failed to align %ux%u %s with DMA\n",
+				__func__, pix->width, pix->height,
+				xlate->host_fmt->name);
+		return ret;
+	}
+
+	ret = set_format(pcdev, dev, icd, sd, &format, xlate);
+	if (ret < 0) {
+		dev_err(dev, "%s: failed to set format\n", __func__);
+		return ret;
+	}
+
+	pix->width	 = mf->width;
+	pix->height	 = mf->height;
+	pix->field	 = mf->field;
+	pix->colorspace  = mf->colorspace;
+	icd->current_fmt = xlate;
+
+	return 0;
+}
+
+static int omap1_cam_try_fmt(struct soc_camera_device *icd,
+			      struct v4l2_format *f)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	const struct soc_camera_format_xlate *xlate;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct v4l2_subdev_pad_config pad_cfg;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+	};
+	struct v4l2_mbus_framefmt *mf = &format.format;
+	int ret;
+	/* TODO: limit to mx1 hardware capabilities */
+
+	xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
+	if (!xlate) {
+		dev_warn(icd->parent, "Format %#x not found\n",
+			 pix->pixelformat);
+		return -EINVAL;
+	}
+
+	mf->width	= pix->width;
+	mf->height	= pix->height;
+	mf->field	= pix->field;
+	mf->colorspace	= pix->colorspace;
+	mf->code	= xlate->code;
+
+	/* limit to sensor capabilities */
+	ret = v4l2_subdev_call(sd, pad, set_fmt, &pad_cfg, &format);
+	if (ret < 0)
+		return ret;
+
+	pix->width	= mf->width;
+	pix->height	= mf->height;
+	pix->field	= mf->field;
+	pix->colorspace	= mf->colorspace;
+
+	return 0;
+}
+
+static bool sg_mode;
+
+/*
+ * Local mmap_mapper wrapper,
+ * used for detecting videobuf-dma-contig buffer allocation failures
+ * and switching to videobuf-dma-sg automatically for future attempts.
+ */
+static int omap1_cam_mmap_mapper(struct videobuf_queue *q,
+				  struct videobuf_buffer *buf,
+				  struct vm_area_struct *vma)
+{
+	struct soc_camera_device *icd = q->priv_data;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct omap1_cam_dev *pcdev = ici->priv;
+	int ret;
+
+	ret = pcdev->mmap_mapper(q, buf, vma);
+
+	if (ret == -ENOMEM)
+		sg_mode = true;
+
+	return ret;
+}
+
+static void omap1_cam_init_videobuf(struct videobuf_queue *q,
+				     struct soc_camera_device *icd)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct omap1_cam_dev *pcdev = ici->priv;
+
+	if (!sg_mode)
+		videobuf_queue_dma_contig_init(q, &omap1_videobuf_ops,
+				icd->parent, &pcdev->lock,
+				V4L2_BUF_TYPE_VIDEO_CAPTURE, V4L2_FIELD_NONE,
+				sizeof(struct omap1_cam_buf), icd, &ici->host_lock);
+	else
+		videobuf_queue_sg_init(q, &omap1_videobuf_ops,
+				icd->parent, &pcdev->lock,
+				V4L2_BUF_TYPE_VIDEO_CAPTURE, V4L2_FIELD_NONE,
+				sizeof(struct omap1_cam_buf), icd, &ici->host_lock);
+
+	/* use videobuf mode (auto)selected with the module parameter */
+	pcdev->vb_mode = sg_mode ? OMAP1_CAM_DMA_SG : OMAP1_CAM_DMA_CONTIG;
+
+	/*
+	 * Ensure we substitute the videobuf-dma-contig version of the
+	 * mmap_mapper() callback with our own wrapper, used for switching
+	 * automatically to videobuf-dma-sg on buffer allocation failure.
+	 */
+	if (!sg_mode && q->int_ops->mmap_mapper != omap1_cam_mmap_mapper) {
+		pcdev->mmap_mapper = q->int_ops->mmap_mapper;
+		q->int_ops->mmap_mapper = omap1_cam_mmap_mapper;
+	}
+}
+
+static int omap1_cam_reqbufs(struct soc_camera_device *icd,
+			      struct v4l2_requestbuffers *p)
+{
+	int i;
+
+	/*
+	 * This is for locking debugging only. I removed spinlocks and now I
+	 * check whether .prepare is ever called on a linked buffer, or whether
+	 * a dma IRQ can occur for an in-work or unlinked buffer. Until now
+	 * it hadn't triggered
+	 */
+	for (i = 0; i < p->count; i++) {
+		struct omap1_cam_buf *buf = container_of(icd->vb_vidq.bufs[i],
+						      struct omap1_cam_buf, vb);
+		buf->inwork = 0;
+		INIT_LIST_HEAD(&buf->vb.queue);
+	}
+
+	return 0;
+}
+
+static int omap1_cam_querycap(struct soc_camera_host *ici,
+			       struct v4l2_capability *cap)
+{
+	/* cap->name is set by the friendly caller:-> */
+	strlcpy(cap->card, "OMAP1 Camera", sizeof(cap->card));
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+
+	return 0;
+}
+
+static int omap1_cam_set_bus_param(struct soc_camera_device *icd)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct device *dev = icd->parent;
+	struct soc_camera_host *ici = to_soc_camera_host(dev);
+	struct omap1_cam_dev *pcdev = ici->priv;
+	u32 pixfmt = icd->current_fmt->host_fmt->fourcc;
+	const struct soc_camera_format_xlate *xlate;
+	const struct soc_mbus_pixelfmt *fmt;
+	struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
+	unsigned long common_flags;
+	u32 ctrlclock, mode;
+	int ret;
+
+	ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
+	if (!ret) {
+		common_flags = soc_mbus_config_compatible(&cfg, SOCAM_BUS_FLAGS);
+		if (!common_flags) {
+			dev_warn(dev,
+				 "Flags incompatible: camera 0x%x, host 0x%x\n",
+				 cfg.flags, SOCAM_BUS_FLAGS);
+			return -EINVAL;
+		}
+	} else if (ret != -ENOIOCTLCMD) {
+		return ret;
+	} else {
+		common_flags = SOCAM_BUS_FLAGS;
+	}
+
+	/* Make choices, possibly based on platform configuration */
+	if ((common_flags & V4L2_MBUS_PCLK_SAMPLE_RISING) &&
+			(common_flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)) {
+		if (!pcdev->pdata ||
+				pcdev->pdata->flags & OMAP1_CAMERA_LCLK_RISING)
+			common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_FALLING;
+		else
+			common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_RISING;
+	}
+
+	cfg.flags = common_flags;
+	ret = v4l2_subdev_call(sd, video, s_mbus_config, &cfg);
+	if (ret < 0 && ret != -ENOIOCTLCMD) {
+		dev_dbg(dev, "camera s_mbus_config(0x%lx) returned %d\n",
+			common_flags, ret);
+		return ret;
+	}
+
+	ctrlclock = CAM_READ_CACHE(pcdev, CTRLCLOCK);
+	if (ctrlclock & LCLK_EN)
+		CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock & ~LCLK_EN);
+
+	if (common_flags & V4L2_MBUS_PCLK_SAMPLE_RISING) {
+		dev_dbg(dev, "CTRLCLOCK_REG |= POLCLK\n");
+		ctrlclock |= POLCLK;
+	} else {
+		dev_dbg(dev, "CTRLCLOCK_REG &= ~POLCLK\n");
+		ctrlclock &= ~POLCLK;
+	}
+	CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock & ~LCLK_EN);
+
+	if (ctrlclock & LCLK_EN)
+		CAM_WRITE(pcdev, CTRLCLOCK, ctrlclock);
+
+	/* select bus endianness */
+	xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
+	fmt = xlate->host_fmt;
+
+	mode = CAM_READ(pcdev, MODE) & ~(RAZ_FIFO | IRQ_MASK | DMA);
+	if (fmt->order == SOC_MBUS_ORDER_LE) {
+		dev_dbg(dev, "MODE_REG &= ~ORDERCAMD\n");
+		CAM_WRITE(pcdev, MODE, mode & ~ORDERCAMD);
+	} else {
+		dev_dbg(dev, "MODE_REG |= ORDERCAMD\n");
+		CAM_WRITE(pcdev, MODE, mode | ORDERCAMD);
+	}
+
+	return 0;
+}
+
+static unsigned int omap1_cam_poll(struct file *file, poll_table *pt)
+{
+	struct soc_camera_device *icd = file->private_data;
+	struct omap1_cam_buf *buf;
+
+	buf = list_entry(icd->vb_vidq.stream.next, struct omap1_cam_buf,
+			 vb.stream);
+
+	poll_wait(file, &buf->vb.done, pt);
+
+	if (buf->vb.state == VIDEOBUF_DONE ||
+	    buf->vb.state == VIDEOBUF_ERROR)
+		return POLLIN | POLLRDNORM;
+
+	return 0;
+}
+
+static struct soc_camera_host_ops omap1_host_ops = {
+	.owner		= THIS_MODULE,
+	.add		= omap1_cam_add_device,
+	.remove		= omap1_cam_remove_device,
+	.clock_start	= omap1_cam_clock_start,
+	.clock_stop	= omap1_cam_clock_stop,
+	.get_formats	= omap1_cam_get_formats,
+	.set_crop	= omap1_cam_set_crop,
+	.set_fmt	= omap1_cam_set_fmt,
+	.try_fmt	= omap1_cam_try_fmt,
+	.init_videobuf	= omap1_cam_init_videobuf,
+	.reqbufs	= omap1_cam_reqbufs,
+	.querycap	= omap1_cam_querycap,
+	.set_bus_param	= omap1_cam_set_bus_param,
+	.poll		= omap1_cam_poll,
+};
+
+static int omap1_cam_probe(struct platform_device *pdev)
+{
+	struct omap1_cam_dev *pcdev;
+	struct resource *res;
+	struct clk *clk;
+	void __iomem *base;
+	unsigned int irq;
+	int err = 0;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	irq = platform_get_irq(pdev, 0);
+	if (!res || (int)irq <= 0) {
+		err = -ENODEV;
+		goto exit;
+	}
+
+	clk = clk_get(&pdev->dev, "armper_ck");
+	if (IS_ERR(clk)) {
+		err = PTR_ERR(clk);
+		goto exit;
+	}
+
+	pcdev = kzalloc(sizeof(*pcdev) + resource_size(res), GFP_KERNEL);
+	if (!pcdev) {
+		dev_err(&pdev->dev, "Could not allocate pcdev\n");
+		err = -ENOMEM;
+		goto exit_put_clk;
+	}
+
+	pcdev->res = res;
+	pcdev->clk = clk;
+
+	pcdev->pdata = pdev->dev.platform_data;
+	if (pcdev->pdata) {
+		pcdev->pflags = pcdev->pdata->flags;
+		pcdev->camexclk = pcdev->pdata->camexclk_khz * 1000;
+	}
+
+	switch (pcdev->camexclk) {
+	case 6000000:
+	case 8000000:
+	case 9600000:
+	case 12000000:
+	case 24000000:
+		break;
+	default:
+		/* pcdev->camexclk != 0 => pcdev->pdata != NULL */
+		dev_warn(&pdev->dev,
+				"Incorrect sensor clock frequency %ld kHz, "
+				"should be one of 0, 6, 8, 9.6, 12 or 24 MHz, "
+				"please correct your platform data\n",
+				pcdev->pdata->camexclk_khz);
+		pcdev->camexclk = 0;
+	case 0:
+		dev_info(&pdev->dev, "Not providing sensor clock\n");
+	}
+
+	INIT_LIST_HEAD(&pcdev->capture);
+	spin_lock_init(&pcdev->lock);
+
+	/*
+	 * Request the region.
+	 */
+	if (!request_mem_region(res->start, resource_size(res), DRIVER_NAME)) {
+		err = -EBUSY;
+		goto exit_kfree;
+	}
+
+	base = ioremap(res->start, resource_size(res));
+	if (!base) {
+		err = -ENOMEM;
+		goto exit_release;
+	}
+	pcdev->irq = irq;
+	pcdev->base = base;
+
+	sensor_reset(pcdev, true);
+
+	err = omap_request_dma(OMAP_DMA_CAMERA_IF_RX, DRIVER_NAME,
+			dma_isr, (void *)pcdev, &pcdev->dma_ch);
+	if (err < 0) {
+		dev_err(&pdev->dev, "Can't request DMA for OMAP1 Camera\n");
+		err = -EBUSY;
+		goto exit_iounmap;
+	}
+	dev_dbg(&pdev->dev, "got DMA channel %d\n", pcdev->dma_ch);
+
+	/* preconfigure DMA */
+	omap_set_dma_src_params(pcdev->dma_ch, OMAP_DMA_PORT_TIPB,
+			OMAP_DMA_AMODE_CONSTANT, res->start + REG_CAMDATA,
+			0, 0);
+	omap_set_dma_dest_burst_mode(pcdev->dma_ch, OMAP_DMA_DATA_BURST_4);
+	/* setup DMA autoinitialization */
+	omap_dma_link_lch(pcdev->dma_ch, pcdev->dma_ch);
+
+	err = request_irq(pcdev->irq, cam_isr, 0, DRIVER_NAME, pcdev);
+	if (err) {
+		dev_err(&pdev->dev, "Camera interrupt register failed\n");
+		goto exit_free_dma;
+	}
+
+	pcdev->soc_host.drv_name	= DRIVER_NAME;
+	pcdev->soc_host.ops		= &omap1_host_ops;
+	pcdev->soc_host.priv		= pcdev;
+	pcdev->soc_host.v4l2_dev.dev	= &pdev->dev;
+	pcdev->soc_host.nr		= pdev->id;
+
+	err = soc_camera_host_register(&pcdev->soc_host);
+	if (err)
+		goto exit_free_irq;
+
+	dev_info(&pdev->dev, "OMAP1 Camera Interface driver loaded\n");
+
+	return 0;
+
+exit_free_irq:
+	free_irq(pcdev->irq, pcdev);
+exit_free_dma:
+	omap_free_dma(pcdev->dma_ch);
+exit_iounmap:
+	iounmap(base);
+exit_release:
+	release_mem_region(res->start, resource_size(res));
+exit_kfree:
+	kfree(pcdev);
+exit_put_clk:
+	clk_put(clk);
+exit:
+	return err;
+}
+
+static int omap1_cam_remove(struct platform_device *pdev)
+{
+	struct soc_camera_host *soc_host = to_soc_camera_host(&pdev->dev);
+	struct omap1_cam_dev *pcdev = container_of(soc_host,
+					struct omap1_cam_dev, soc_host);
+	struct resource *res;
+
+	free_irq(pcdev->irq, pcdev);
+
+	omap_free_dma(pcdev->dma_ch);
+
+	soc_camera_host_unregister(soc_host);
+
+	iounmap(pcdev->base);
+
+	res = pcdev->res;
+	release_mem_region(res->start, resource_size(res));
+
+	clk_put(pcdev->clk);
+
+	kfree(pcdev);
+
+	dev_info(&pdev->dev, "OMAP1 Camera Interface driver unloaded\n");
+
+	return 0;
+}
+
+static struct platform_driver omap1_cam_driver = {
+	.driver		= {
+		.name	= DRIVER_NAME,
+	},
+	.probe		= omap1_cam_probe,
+	.remove		= omap1_cam_remove,
+};
+
+module_platform_driver(omap1_cam_driver);
+
+module_param(sg_mode, bool, 0644);
+MODULE_PARM_DESC(sg_mode, "videobuf mode, 0: dma-contig (default), 1: dma-sg");
+
+MODULE_DESCRIPTION("OMAP1 Camera Interface driver");
+MODULE_AUTHOR("Janusz Krzysztofik <jkrzyszt@tis.icnet.pl>");
+MODULE_LICENSE("GPL v2");
+MODULE_VERSION(DRIVER_VERSION);
+MODULE_ALIAS("platform:" DRIVER_NAME);
diff --git a/drivers/media/platform/soc_camera/pxa_camera.c b/drivers/media/platform/soc_camera/pxa_camera.c
new file mode 100644
index 0000000..fcb942d
--- /dev/null
+++ b/drivers/media/platform/soc_camera/pxa_camera.c
@@ -0,0 +1,1908 @@
+/*
+ * V4L2 Driver for PXA camera host
+ *
+ * Copyright (C) 2006, Sascha Hauer, Pengutronix
+ * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/io.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/err.h>
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/moduleparam.h>
+#include <linux/time.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+
+#include <media/v4l2-common.h>
+#include <media/v4l2-dev.h>
+#include <media/videobuf-dma-sg.h>
+#include <media/soc_camera.h>
+#include <media/soc_mediabus.h>
+#include <media/v4l2-of.h>
+
+#include <linux/videodev2.h>
+
+#include <mach/dma.h>
+#include <linux/platform_data/camera-pxa.h>
+
+#define PXA_CAM_VERSION "0.0.6"
+#define PXA_CAM_DRV_NAME "pxa27x-camera"
+
+/* Camera Interface */
+#define CICR0		0x0000
+#define CICR1		0x0004
+#define CICR2		0x0008
+#define CICR3		0x000C
+#define CICR4		0x0010
+#define CISR		0x0014
+#define CIFR		0x0018
+#define CITOR		0x001C
+#define CIBR0		0x0028
+#define CIBR1		0x0030
+#define CIBR2		0x0038
+
+#define CICR0_DMAEN	(1 << 31)	/* DMA request enable */
+#define CICR0_PAR_EN	(1 << 30)	/* Parity enable */
+#define CICR0_SL_CAP_EN	(1 << 29)	/* Capture enable for slave mode */
+#define CICR0_ENB	(1 << 28)	/* Camera interface enable */
+#define CICR0_DIS	(1 << 27)	/* Camera interface disable */
+#define CICR0_SIM	(0x7 << 24)	/* Sensor interface mode mask */
+#define CICR0_TOM	(1 << 9)	/* Time-out mask */
+#define CICR0_RDAVM	(1 << 8)	/* Receive-data-available mask */
+#define CICR0_FEM	(1 << 7)	/* FIFO-empty mask */
+#define CICR0_EOLM	(1 << 6)	/* End-of-line mask */
+#define CICR0_PERRM	(1 << 5)	/* Parity-error mask */
+#define CICR0_QDM	(1 << 4)	/* Quick-disable mask */
+#define CICR0_CDM	(1 << 3)	/* Disable-done mask */
+#define CICR0_SOFM	(1 << 2)	/* Start-of-frame mask */
+#define CICR0_EOFM	(1 << 1)	/* End-of-frame mask */
+#define CICR0_FOM	(1 << 0)	/* FIFO-overrun mask */
+
+#define CICR1_TBIT	(1 << 31)	/* Transparency bit */
+#define CICR1_RGBT_CONV	(0x3 << 29)	/* RGBT conversion mask */
+#define CICR1_PPL	(0x7ff << 15)	/* Pixels per line mask */
+#define CICR1_RGB_CONV	(0x7 << 12)	/* RGB conversion mask */
+#define CICR1_RGB_F	(1 << 11)	/* RGB format */
+#define CICR1_YCBCR_F	(1 << 10)	/* YCbCr format */
+#define CICR1_RGB_BPP	(0x7 << 7)	/* RGB bis per pixel mask */
+#define CICR1_RAW_BPP	(0x3 << 5)	/* Raw bis per pixel mask */
+#define CICR1_COLOR_SP	(0x3 << 3)	/* Color space mask */
+#define CICR1_DW	(0x7 << 0)	/* Data width mask */
+
+#define CICR2_BLW	(0xff << 24)	/* Beginning-of-line pixel clock
+					   wait count mask */
+#define CICR2_ELW	(0xff << 16)	/* End-of-line pixel clock
+					   wait count mask */
+#define CICR2_HSW	(0x3f << 10)	/* Horizontal sync pulse width mask */
+#define CICR2_BFPW	(0x3f << 3)	/* Beginning-of-frame pixel clock
+					   wait count mask */
+#define CICR2_FSW	(0x7 << 0)	/* Frame stabilization
+					   wait count mask */
+
+#define CICR3_BFW	(0xff << 24)	/* Beginning-of-frame line clock
+					   wait count mask */
+#define CICR3_EFW	(0xff << 16)	/* End-of-frame line clock
+					   wait count mask */
+#define CICR3_VSW	(0x3f << 10)	/* Vertical sync pulse width mask */
+#define CICR3_BFPW	(0x3f << 3)	/* Beginning-of-frame pixel clock
+					   wait count mask */
+#define CICR3_LPF	(0x7ff << 0)	/* Lines per frame mask */
+
+#define CICR4_MCLK_DLY	(0x3 << 24)	/* MCLK Data Capture Delay mask */
+#define CICR4_PCLK_EN	(1 << 23)	/* Pixel clock enable */
+#define CICR4_PCP	(1 << 22)	/* Pixel clock polarity */
+#define CICR4_HSP	(1 << 21)	/* Horizontal sync polarity */
+#define CICR4_VSP	(1 << 20)	/* Vertical sync polarity */
+#define CICR4_MCLK_EN	(1 << 19)	/* MCLK enable */
+#define CICR4_FR_RATE	(0x7 << 8)	/* Frame rate mask */
+#define CICR4_DIV	(0xff << 0)	/* Clock divisor mask */
+
+#define CISR_FTO	(1 << 15)	/* FIFO time-out */
+#define CISR_RDAV_2	(1 << 14)	/* Channel 2 receive data available */
+#define CISR_RDAV_1	(1 << 13)	/* Channel 1 receive data available */
+#define CISR_RDAV_0	(1 << 12)	/* Channel 0 receive data available */
+#define CISR_FEMPTY_2	(1 << 11)	/* Channel 2 FIFO empty */
+#define CISR_FEMPTY_1	(1 << 10)	/* Channel 1 FIFO empty */
+#define CISR_FEMPTY_0	(1 << 9)	/* Channel 0 FIFO empty */
+#define CISR_EOL	(1 << 8)	/* End of line */
+#define CISR_PAR_ERR	(1 << 7)	/* Parity error */
+#define CISR_CQD	(1 << 6)	/* Camera interface quick disable */
+#define CISR_CDD	(1 << 5)	/* Camera interface disable done */
+#define CISR_SOF	(1 << 4)	/* Start of frame */
+#define CISR_EOF	(1 << 3)	/* End of frame */
+#define CISR_IFO_2	(1 << 2)	/* FIFO overrun for Channel 2 */
+#define CISR_IFO_1	(1 << 1)	/* FIFO overrun for Channel 1 */
+#define CISR_IFO_0	(1 << 0)	/* FIFO overrun for Channel 0 */
+
+#define CIFR_FLVL2	(0x7f << 23)	/* FIFO 2 level mask */
+#define CIFR_FLVL1	(0x7f << 16)	/* FIFO 1 level mask */
+#define CIFR_FLVL0	(0xff << 8)	/* FIFO 0 level mask */
+#define CIFR_THL_0	(0x3 << 4)	/* Threshold Level for Channel 0 FIFO */
+#define CIFR_RESET_F	(1 << 3)	/* Reset input FIFOs */
+#define CIFR_FEN2	(1 << 2)	/* FIFO enable for channel 2 */
+#define CIFR_FEN1	(1 << 1)	/* FIFO enable for channel 1 */
+#define CIFR_FEN0	(1 << 0)	/* FIFO enable for channel 0 */
+
+#define CICR0_SIM_MP	(0 << 24)
+#define CICR0_SIM_SP	(1 << 24)
+#define CICR0_SIM_MS	(2 << 24)
+#define CICR0_SIM_EP	(3 << 24)
+#define CICR0_SIM_ES	(4 << 24)
+
+#define CICR1_DW_VAL(x)   ((x) & CICR1_DW)	    /* Data bus width */
+#define CICR1_PPL_VAL(x)  (((x) << 15) & CICR1_PPL) /* Pixels per line */
+#define CICR1_COLOR_SP_VAL(x)	(((x) << 3) & CICR1_COLOR_SP)	/* color space */
+#define CICR1_RGB_BPP_VAL(x)	(((x) << 7) & CICR1_RGB_BPP)	/* bpp for rgb */
+#define CICR1_RGBT_CONV_VAL(x)	(((x) << 29) & CICR1_RGBT_CONV)	/* rgbt conv */
+
+#define CICR2_BLW_VAL(x)  (((x) << 24) & CICR2_BLW) /* Beginning-of-line pixel clock wait count */
+#define CICR2_ELW_VAL(x)  (((x) << 16) & CICR2_ELW) /* End-of-line pixel clock wait count */
+#define CICR2_HSW_VAL(x)  (((x) << 10) & CICR2_HSW) /* Horizontal sync pulse width */
+#define CICR2_BFPW_VAL(x) (((x) << 3) & CICR2_BFPW) /* Beginning-of-frame pixel clock wait count */
+#define CICR2_FSW_VAL(x)  (((x) << 0) & CICR2_FSW)  /* Frame stabilization wait count */
+
+#define CICR3_BFW_VAL(x)  (((x) << 24) & CICR3_BFW) /* Beginning-of-frame line clock wait count  */
+#define CICR3_EFW_VAL(x)  (((x) << 16) & CICR3_EFW) /* End-of-frame line clock wait count */
+#define CICR3_VSW_VAL(x)  (((x) << 11) & CICR3_VSW) /* Vertical sync pulse width */
+#define CICR3_LPF_VAL(x)  (((x) << 0) & CICR3_LPF)  /* Lines per frame */
+
+#define CICR0_IRQ_MASK (CICR0_TOM | CICR0_RDAVM | CICR0_FEM | CICR0_EOLM | \
+			CICR0_PERRM | CICR0_QDM | CICR0_CDM | CICR0_SOFM | \
+			CICR0_EOFM | CICR0_FOM)
+
+/*
+ * Structures
+ */
+enum pxa_camera_active_dma {
+	DMA_Y = 0x1,
+	DMA_U = 0x2,
+	DMA_V = 0x4,
+};
+
+/* descriptor needed for the PXA DMA engine */
+struct pxa_cam_dma {
+	dma_addr_t		sg_dma;
+	struct pxa_dma_desc	*sg_cpu;
+	size_t			sg_size;
+	int			sglen;
+};
+
+/* buffer for one video frame */
+struct pxa_buffer {
+	/* common v4l buffer stuff -- must be first */
+	struct videobuf_buffer		vb;
+	u32	code;
+	/* our descriptor lists for Y, U and V channels */
+	struct pxa_cam_dma		dmas[3];
+	int				inwork;
+	enum pxa_camera_active_dma	active_dma;
+};
+
+struct pxa_camera_dev {
+	struct soc_camera_host	soc_host;
+	/*
+	 * PXA27x is only supposed to handle one camera on its Quick Capture
+	 * interface. If anyone ever builds hardware to enable more than
+	 * one camera, they will have to modify this driver too
+	 */
+	struct clk		*clk;
+
+	unsigned int		irq;
+	void __iomem		*base;
+
+	int			channels;
+	unsigned int		dma_chans[3];
+
+	struct pxacamera_platform_data *pdata;
+	struct resource		*res;
+	unsigned long		platform_flags;
+	unsigned long		ciclk;
+	unsigned long		mclk;
+	u32			mclk_divisor;
+	u16			width_flags;	/* max 10 bits */
+
+	struct list_head	capture;
+
+	spinlock_t		lock;
+
+	struct pxa_buffer	*active;
+	struct pxa_dma_desc	*sg_tail[3];
+
+	u32			save_cicr[5];
+};
+
+struct pxa_cam {
+	unsigned long flags;
+};
+
+static const char *pxa_cam_driver_description = "PXA_Camera";
+
+static unsigned int vid_limit = 16;	/* Video memory limit, in Mb */
+
+/*
+ *  Videobuf operations
+ */
+static int pxa_videobuf_setup(struct videobuf_queue *vq, unsigned int *count,
+			      unsigned int *size)
+{
+	struct soc_camera_device *icd = vq->priv_data;
+
+	dev_dbg(icd->parent, "count=%d, size=%d\n", *count, *size);
+
+	*size = icd->sizeimage;
+
+	if (0 == *count)
+		*count = 32;
+	if (*size * *count > vid_limit * 1024 * 1024)
+		*count = (vid_limit * 1024 * 1024) / *size;
+
+	return 0;
+}
+
+static void free_buffer(struct videobuf_queue *vq, struct pxa_buffer *buf)
+{
+	struct soc_camera_device *icd = vq->priv_data;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct videobuf_dmabuf *dma = videobuf_to_dma(&buf->vb);
+	int i;
+
+	BUG_ON(in_interrupt());
+
+	dev_dbg(icd->parent, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
+		&buf->vb, buf->vb.baddr, buf->vb.bsize);
+
+	/*
+	 * This waits until this buffer is out of danger, i.e., until it is no
+	 * longer in STATE_QUEUED or STATE_ACTIVE
+	 */
+	videobuf_waiton(vq, &buf->vb, 0, 0);
+	videobuf_dma_unmap(vq->dev, dma);
+	videobuf_dma_free(dma);
+
+	for (i = 0; i < ARRAY_SIZE(buf->dmas); i++) {
+		if (buf->dmas[i].sg_cpu)
+			dma_free_coherent(ici->v4l2_dev.dev,
+					  buf->dmas[i].sg_size,
+					  buf->dmas[i].sg_cpu,
+					  buf->dmas[i].sg_dma);
+		buf->dmas[i].sg_cpu = NULL;
+	}
+
+	buf->vb.state = VIDEOBUF_NEEDS_INIT;
+}
+
+static int calculate_dma_sglen(struct scatterlist *sglist, int sglen,
+			       int sg_first_ofs, int size)
+{
+	int i, offset, dma_len, xfer_len;
+	struct scatterlist *sg;
+
+	offset = sg_first_ofs;
+	for_each_sg(sglist, sg, sglen, i) {
+		dma_len = sg_dma_len(sg);
+
+		/* PXA27x Developer's Manual 27.4.4.1: round up to 8 bytes */
+		xfer_len = roundup(min(dma_len - offset, size), 8);
+
+		size = max(0, size - xfer_len);
+		offset = 0;
+		if (size == 0)
+			break;
+	}
+
+	BUG_ON(size != 0);
+	return i + 1;
+}
+
+/**
+ * pxa_init_dma_channel - init dma descriptors
+ * @pcdev: pxa camera device
+ * @buf: pxa buffer to find pxa dma channel
+ * @dma: dma video buffer
+ * @channel: dma channel (0 => 'Y', 1 => 'U', 2 => 'V')
+ * @cibr: camera Receive Buffer Register
+ * @size: bytes to transfer
+ * @sg_first: first element of sg_list
+ * @sg_first_ofs: offset in first element of sg_list
+ *
+ * Prepares the pxa dma descriptors to transfer one camera channel.
+ * Beware sg_first and sg_first_ofs are both input and output parameters.
+ *
+ * Returns 0 or -ENOMEM if no coherent memory is available
+ */
+static int pxa_init_dma_channel(struct pxa_camera_dev *pcdev,
+				struct pxa_buffer *buf,
+				struct videobuf_dmabuf *dma, int channel,
+				int cibr, int size,
+				struct scatterlist **sg_first, int *sg_first_ofs)
+{
+	struct pxa_cam_dma *pxa_dma = &buf->dmas[channel];
+	struct device *dev = pcdev->soc_host.v4l2_dev.dev;
+	struct scatterlist *sg;
+	int i, offset, sglen;
+	int dma_len = 0, xfer_len = 0;
+
+	if (pxa_dma->sg_cpu)
+		dma_free_coherent(dev, pxa_dma->sg_size,
+				  pxa_dma->sg_cpu, pxa_dma->sg_dma);
+
+	sglen = calculate_dma_sglen(*sg_first, dma->sglen,
+				    *sg_first_ofs, size);
+
+	pxa_dma->sg_size = (sglen + 1) * sizeof(struct pxa_dma_desc);
+	pxa_dma->sg_cpu = dma_alloc_coherent(dev, pxa_dma->sg_size,
+					     &pxa_dma->sg_dma, GFP_KERNEL);
+	if (!pxa_dma->sg_cpu)
+		return -ENOMEM;
+
+	pxa_dma->sglen = sglen;
+	offset = *sg_first_ofs;
+
+	dev_dbg(dev, "DMA: sg_first=%p, sglen=%d, ofs=%d, dma.desc=%x\n",
+		*sg_first, sglen, *sg_first_ofs, pxa_dma->sg_dma);
+
+
+	for_each_sg(*sg_first, sg, sglen, i) {
+		dma_len = sg_dma_len(sg);
+
+		/* PXA27x Developer's Manual 27.4.4.1: round up to 8 bytes */
+		xfer_len = roundup(min(dma_len - offset, size), 8);
+
+		size = max(0, size - xfer_len);
+
+		pxa_dma->sg_cpu[i].dsadr = pcdev->res->start + cibr;
+		pxa_dma->sg_cpu[i].dtadr = sg_dma_address(sg) + offset;
+		pxa_dma->sg_cpu[i].dcmd =
+			DCMD_FLOWSRC | DCMD_BURST8 | DCMD_INCTRGADDR | xfer_len;
+#ifdef DEBUG
+		if (!i)
+			pxa_dma->sg_cpu[i].dcmd |= DCMD_STARTIRQEN;
+#endif
+		pxa_dma->sg_cpu[i].ddadr =
+			pxa_dma->sg_dma + (i + 1) * sizeof(struct pxa_dma_desc);
+
+		dev_vdbg(dev, "DMA: desc.%08x->@phys=0x%08x, len=%d\n",
+			 pxa_dma->sg_dma + i * sizeof(struct pxa_dma_desc),
+			 sg_dma_address(sg) + offset, xfer_len);
+		offset = 0;
+
+		if (size == 0)
+			break;
+	}
+
+	pxa_dma->sg_cpu[sglen].ddadr = DDADR_STOP;
+	pxa_dma->sg_cpu[sglen].dcmd  = DCMD_FLOWSRC | DCMD_BURST8 | DCMD_ENDIRQEN;
+
+	/*
+	 * Handle 1 special case :
+	 *  - in 3 planes (YUV422P format), we might finish with xfer_len equal
+	 *    to dma_len (end on PAGE boundary). In this case, the sg element
+	 *    for next plane should be the next after the last used to store the
+	 *    last scatter gather RAM page
+	 */
+	if (xfer_len >= dma_len) {
+		*sg_first_ofs = xfer_len - dma_len;
+		*sg_first = sg_next(sg);
+	} else {
+		*sg_first_ofs = xfer_len;
+		*sg_first = sg;
+	}
+
+	return 0;
+}
+
+static void pxa_videobuf_set_actdma(struct pxa_camera_dev *pcdev,
+				    struct pxa_buffer *buf)
+{
+	buf->active_dma = DMA_Y;
+	if (pcdev->channels == 3)
+		buf->active_dma |= DMA_U | DMA_V;
+}
+
+/*
+ * Please check the DMA prepared buffer structure in :
+ *   Documentation/video4linux/pxa_camera.txt
+ * Please check also in pxa_camera_check_link_miss() to understand why DMA chain
+ * modification while DMA chain is running will work anyway.
+ */
+static int pxa_videobuf_prepare(struct videobuf_queue *vq,
+		struct videobuf_buffer *vb, enum v4l2_field field)
+{
+	struct soc_camera_device *icd = vq->priv_data;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct pxa_camera_dev *pcdev = ici->priv;
+	struct device *dev = pcdev->soc_host.v4l2_dev.dev;
+	struct pxa_buffer *buf = container_of(vb, struct pxa_buffer, vb);
+	int ret;
+	int size_y, size_u = 0, size_v = 0;
+
+	dev_dbg(dev, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
+		vb, vb->baddr, vb->bsize);
+
+	/* Added list head initialization on alloc */
+	WARN_ON(!list_empty(&vb->queue));
+
+#ifdef DEBUG
+	/*
+	 * This can be useful if you want to see if we actually fill
+	 * the buffer with something
+	 */
+	memset((void *)vb->baddr, 0xaa, vb->bsize);
+#endif
+
+	BUG_ON(NULL == icd->current_fmt);
+
+	/*
+	 * I think, in buf_prepare you only have to protect global data,
+	 * the actual buffer is yours
+	 */
+	buf->inwork = 1;
+
+	if (buf->code	!= icd->current_fmt->code ||
+	    vb->width	!= icd->user_width ||
+	    vb->height	!= icd->user_height ||
+	    vb->field	!= field) {
+		buf->code	= icd->current_fmt->code;
+		vb->width	= icd->user_width;
+		vb->height	= icd->user_height;
+		vb->field	= field;
+		vb->state	= VIDEOBUF_NEEDS_INIT;
+	}
+
+	vb->size = icd->sizeimage;
+	if (0 != vb->baddr && vb->bsize < vb->size) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (vb->state == VIDEOBUF_NEEDS_INIT) {
+		int size = vb->size;
+		int next_ofs = 0;
+		struct videobuf_dmabuf *dma = videobuf_to_dma(vb);
+		struct scatterlist *sg;
+
+		ret = videobuf_iolock(vq, vb, NULL);
+		if (ret)
+			goto fail;
+
+		if (pcdev->channels == 3) {
+			size_y = size / 2;
+			size_u = size_v = size / 4;
+		} else {
+			size_y = size;
+		}
+
+		sg = dma->sglist;
+
+		/* init DMA for Y channel */
+		ret = pxa_init_dma_channel(pcdev, buf, dma, 0, CIBR0, size_y,
+					   &sg, &next_ofs);
+		if (ret) {
+			dev_err(dev, "DMA initialization for Y/RGB failed\n");
+			goto fail;
+		}
+
+		/* init DMA for U channel */
+		if (size_u)
+			ret = pxa_init_dma_channel(pcdev, buf, dma, 1, CIBR1,
+						   size_u, &sg, &next_ofs);
+		if (ret) {
+			dev_err(dev, "DMA initialization for U failed\n");
+			goto fail_u;
+		}
+
+		/* init DMA for V channel */
+		if (size_v)
+			ret = pxa_init_dma_channel(pcdev, buf, dma, 2, CIBR2,
+						   size_v, &sg, &next_ofs);
+		if (ret) {
+			dev_err(dev, "DMA initialization for V failed\n");
+			goto fail_v;
+		}
+
+		vb->state = VIDEOBUF_PREPARED;
+	}
+
+	buf->inwork = 0;
+	pxa_videobuf_set_actdma(pcdev, buf);
+
+	return 0;
+
+fail_v:
+	dma_free_coherent(dev, buf->dmas[1].sg_size,
+			  buf->dmas[1].sg_cpu, buf->dmas[1].sg_dma);
+fail_u:
+	dma_free_coherent(dev, buf->dmas[0].sg_size,
+			  buf->dmas[0].sg_cpu, buf->dmas[0].sg_dma);
+fail:
+	free_buffer(vq, buf);
+out:
+	buf->inwork = 0;
+	return ret;
+}
+
+/**
+ * pxa_dma_start_channels - start DMA channel for active buffer
+ * @pcdev: pxa camera device
+ *
+ * Initialize DMA channels to the beginning of the active video buffer, and
+ * start these channels.
+ */
+static void pxa_dma_start_channels(struct pxa_camera_dev *pcdev)
+{
+	int i;
+	struct pxa_buffer *active;
+
+	active = pcdev->active;
+
+	for (i = 0; i < pcdev->channels; i++) {
+		dev_dbg(pcdev->soc_host.v4l2_dev.dev,
+			"%s (channel=%d) ddadr=%08x\n", __func__,
+			i, active->dmas[i].sg_dma);
+		DDADR(pcdev->dma_chans[i]) = active->dmas[i].sg_dma;
+		DCSR(pcdev->dma_chans[i]) = DCSR_RUN;
+	}
+}
+
+static void pxa_dma_stop_channels(struct pxa_camera_dev *pcdev)
+{
+	int i;
+
+	for (i = 0; i < pcdev->channels; i++) {
+		dev_dbg(pcdev->soc_host.v4l2_dev.dev,
+			"%s (channel=%d)\n", __func__, i);
+		DCSR(pcdev->dma_chans[i]) = 0;
+	}
+}
+
+static void pxa_dma_add_tail_buf(struct pxa_camera_dev *pcdev,
+				 struct pxa_buffer *buf)
+{
+	int i;
+	struct pxa_dma_desc *buf_last_desc;
+
+	for (i = 0; i < pcdev->channels; i++) {
+		buf_last_desc = buf->dmas[i].sg_cpu + buf->dmas[i].sglen;
+		buf_last_desc->ddadr = DDADR_STOP;
+
+		if (pcdev->sg_tail[i])
+			/* Link the new buffer to the old tail */
+			pcdev->sg_tail[i]->ddadr = buf->dmas[i].sg_dma;
+
+		/* Update the channel tail */
+		pcdev->sg_tail[i] = buf_last_desc;
+	}
+}
+
+/**
+ * pxa_camera_start_capture - start video capturing
+ * @pcdev: camera device
+ *
+ * Launch capturing. DMA channels should not be active yet. They should get
+ * activated at the end of frame interrupt, to capture only whole frames, and
+ * never begin the capture of a partial frame.
+ */
+static void pxa_camera_start_capture(struct pxa_camera_dev *pcdev)
+{
+	unsigned long cicr0;
+
+	dev_dbg(pcdev->soc_host.v4l2_dev.dev, "%s\n", __func__);
+	/* Enable End-Of-Frame Interrupt */
+	cicr0 = __raw_readl(pcdev->base + CICR0) | CICR0_ENB;
+	cicr0 &= ~CICR0_EOFM;
+	__raw_writel(cicr0, pcdev->base + CICR0);
+}
+
+static void pxa_camera_stop_capture(struct pxa_camera_dev *pcdev)
+{
+	unsigned long cicr0;
+
+	pxa_dma_stop_channels(pcdev);
+
+	cicr0 = __raw_readl(pcdev->base + CICR0) & ~CICR0_ENB;
+	__raw_writel(cicr0, pcdev->base + CICR0);
+
+	pcdev->active = NULL;
+	dev_dbg(pcdev->soc_host.v4l2_dev.dev, "%s\n", __func__);
+}
+
+/* Called under spinlock_irqsave(&pcdev->lock, ...) */
+static void pxa_videobuf_queue(struct videobuf_queue *vq,
+			       struct videobuf_buffer *vb)
+{
+	struct soc_camera_device *icd = vq->priv_data;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct pxa_camera_dev *pcdev = ici->priv;
+	struct pxa_buffer *buf = container_of(vb, struct pxa_buffer, vb);
+
+	dev_dbg(icd->parent, "%s (vb=0x%p) 0x%08lx %d active=%p\n",
+		__func__, vb, vb->baddr, vb->bsize, pcdev->active);
+
+	list_add_tail(&vb->queue, &pcdev->capture);
+
+	vb->state = VIDEOBUF_ACTIVE;
+	pxa_dma_add_tail_buf(pcdev, buf);
+
+	if (!pcdev->active)
+		pxa_camera_start_capture(pcdev);
+}
+
+static void pxa_videobuf_release(struct videobuf_queue *vq,
+				 struct videobuf_buffer *vb)
+{
+	struct pxa_buffer *buf = container_of(vb, struct pxa_buffer, vb);
+#ifdef DEBUG
+	struct soc_camera_device *icd = vq->priv_data;
+	struct device *dev = icd->parent;
+
+	dev_dbg(dev, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
+		vb, vb->baddr, vb->bsize);
+
+	switch (vb->state) {
+	case VIDEOBUF_ACTIVE:
+		dev_dbg(dev, "%s (active)\n", __func__);
+		break;
+	case VIDEOBUF_QUEUED:
+		dev_dbg(dev, "%s (queued)\n", __func__);
+		break;
+	case VIDEOBUF_PREPARED:
+		dev_dbg(dev, "%s (prepared)\n", __func__);
+		break;
+	default:
+		dev_dbg(dev, "%s (unknown)\n", __func__);
+		break;
+	}
+#endif
+
+	free_buffer(vq, buf);
+}
+
+static void pxa_camera_wakeup(struct pxa_camera_dev *pcdev,
+			      struct videobuf_buffer *vb,
+			      struct pxa_buffer *buf)
+{
+	int i;
+
+	/* _init is used to debug races, see comment in pxa_camera_reqbufs() */
+	list_del_init(&vb->queue);
+	vb->state = VIDEOBUF_DONE;
+	v4l2_get_timestamp(&vb->ts);
+	vb->field_count++;
+	wake_up(&vb->done);
+	dev_dbg(pcdev->soc_host.v4l2_dev.dev, "%s dequeud buffer (vb=0x%p)\n",
+		__func__, vb);
+
+	if (list_empty(&pcdev->capture)) {
+		pxa_camera_stop_capture(pcdev);
+		for (i = 0; i < pcdev->channels; i++)
+			pcdev->sg_tail[i] = NULL;
+		return;
+	}
+
+	pcdev->active = list_entry(pcdev->capture.next,
+				   struct pxa_buffer, vb.queue);
+}
+
+/**
+ * pxa_camera_check_link_miss - check missed DMA linking
+ * @pcdev: camera device
+ *
+ * The DMA chaining is done with DMA running. This means a tiny temporal window
+ * remains, where a buffer is queued on the chain, while the chain is already
+ * stopped. This means the tailed buffer would never be transferred by DMA.
+ * This function restarts the capture for this corner case, where :
+ *  - DADR() == DADDR_STOP
+ *  - a videobuffer is queued on the pcdev->capture list
+ *
+ * Please check the "DMA hot chaining timeslice issue" in
+ *   Documentation/video4linux/pxa_camera.txt
+ *
+ * Context: should only be called within the dma irq handler
+ */
+static void pxa_camera_check_link_miss(struct pxa_camera_dev *pcdev)
+{
+	int i, is_dma_stopped = 1;
+
+	for (i = 0; i < pcdev->channels; i++)
+		if (DDADR(pcdev->dma_chans[i]) != DDADR_STOP)
+			is_dma_stopped = 0;
+	dev_dbg(pcdev->soc_host.v4l2_dev.dev,
+		"%s : top queued buffer=%p, dma_stopped=%d\n",
+		__func__, pcdev->active, is_dma_stopped);
+	if (pcdev->active && is_dma_stopped)
+		pxa_camera_start_capture(pcdev);
+}
+
+static void pxa_camera_dma_irq(int channel, struct pxa_camera_dev *pcdev,
+			       enum pxa_camera_active_dma act_dma)
+{
+	struct device *dev = pcdev->soc_host.v4l2_dev.dev;
+	struct pxa_buffer *buf;
+	unsigned long flags;
+	u32 status, camera_status, overrun;
+	struct videobuf_buffer *vb;
+
+	spin_lock_irqsave(&pcdev->lock, flags);
+
+	status = DCSR(channel);
+	DCSR(channel) = status;
+
+	camera_status = __raw_readl(pcdev->base + CISR);
+	overrun = CISR_IFO_0;
+	if (pcdev->channels == 3)
+		overrun |= CISR_IFO_1 | CISR_IFO_2;
+
+	if (status & DCSR_BUSERR) {
+		dev_err(dev, "DMA Bus Error IRQ!\n");
+		goto out;
+	}
+
+	if (!(status & (DCSR_ENDINTR | DCSR_STARTINTR))) {
+		dev_err(dev, "Unknown DMA IRQ source, status: 0x%08x\n",
+			status);
+		goto out;
+	}
+
+	/*
+	 * pcdev->active should not be NULL in DMA irq handler.
+	 *
+	 * But there is one corner case : if capture was stopped due to an
+	 * overrun of channel 1, and at that same channel 2 was completed.
+	 *
+	 * When handling the overrun in DMA irq for channel 1, we'll stop the
+	 * capture and restart it (and thus set pcdev->active to NULL). But the
+	 * DMA irq handler will already be pending for channel 2. So on entering
+	 * the DMA irq handler for channel 2 there will be no active buffer, yet
+	 * that is normal.
+	 */
+	if (!pcdev->active)
+		goto out;
+
+	vb = &pcdev->active->vb;
+	buf = container_of(vb, struct pxa_buffer, vb);
+	WARN_ON(buf->inwork || list_empty(&vb->queue));
+
+	dev_dbg(dev, "%s channel=%d %s%s(vb=0x%p) dma.desc=%x\n",
+		__func__, channel, status & DCSR_STARTINTR ? "SOF " : "",
+		status & DCSR_ENDINTR ? "EOF " : "", vb, DDADR(channel));
+
+	if (status & DCSR_ENDINTR) {
+		/*
+		 * It's normal if the last frame creates an overrun, as there
+		 * are no more DMA descriptors to fetch from QCI fifos
+		 */
+		if (camera_status & overrun &&
+		    !list_is_last(pcdev->capture.next, &pcdev->capture)) {
+			dev_dbg(dev, "FIFO overrun! CISR: %x\n",
+				camera_status);
+			pxa_camera_stop_capture(pcdev);
+			pxa_camera_start_capture(pcdev);
+			goto out;
+		}
+		buf->active_dma &= ~act_dma;
+		if (!buf->active_dma) {
+			pxa_camera_wakeup(pcdev, vb, buf);
+			pxa_camera_check_link_miss(pcdev);
+		}
+	}
+
+out:
+	spin_unlock_irqrestore(&pcdev->lock, flags);
+}
+
+static void pxa_camera_dma_irq_y(int channel, void *data)
+{
+	struct pxa_camera_dev *pcdev = data;
+	pxa_camera_dma_irq(channel, pcdev, DMA_Y);
+}
+
+static void pxa_camera_dma_irq_u(int channel, void *data)
+{
+	struct pxa_camera_dev *pcdev = data;
+	pxa_camera_dma_irq(channel, pcdev, DMA_U);
+}
+
+static void pxa_camera_dma_irq_v(int channel, void *data)
+{
+	struct pxa_camera_dev *pcdev = data;
+	pxa_camera_dma_irq(channel, pcdev, DMA_V);
+}
+
+static struct videobuf_queue_ops pxa_videobuf_ops = {
+	.buf_setup      = pxa_videobuf_setup,
+	.buf_prepare    = pxa_videobuf_prepare,
+	.buf_queue      = pxa_videobuf_queue,
+	.buf_release    = pxa_videobuf_release,
+};
+
+static void pxa_camera_init_videobuf(struct videobuf_queue *q,
+			      struct soc_camera_device *icd)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct pxa_camera_dev *pcdev = ici->priv;
+
+	/*
+	 * We must pass NULL as dev pointer, then all pci_* dma operations
+	 * transform to normal dma_* ones.
+	 */
+	videobuf_queue_sg_init(q, &pxa_videobuf_ops, NULL, &pcdev->lock,
+				V4L2_BUF_TYPE_VIDEO_CAPTURE, V4L2_FIELD_NONE,
+				sizeof(struct pxa_buffer), icd, &ici->host_lock);
+}
+
+static u32 mclk_get_divisor(struct platform_device *pdev,
+			    struct pxa_camera_dev *pcdev)
+{
+	unsigned long mclk = pcdev->mclk;
+	struct device *dev = &pdev->dev;
+	u32 div;
+	unsigned long lcdclk;
+
+	lcdclk = clk_get_rate(pcdev->clk);
+	pcdev->ciclk = lcdclk;
+
+	/* mclk <= ciclk / 4 (27.4.2) */
+	if (mclk > lcdclk / 4) {
+		mclk = lcdclk / 4;
+		dev_warn(dev, "Limiting master clock to %lu\n", mclk);
+	}
+
+	/* We verify mclk != 0, so if anyone breaks it, here comes their Oops */
+	div = (lcdclk + 2 * mclk - 1) / (2 * mclk) - 1;
+
+	/* If we're not supplying MCLK, leave it at 0 */
+	if (pcdev->platform_flags & PXA_CAMERA_MCLK_EN)
+		pcdev->mclk = lcdclk / (2 * (div + 1));
+
+	dev_dbg(dev, "LCD clock %luHz, target freq %luHz, divisor %u\n",
+		lcdclk, mclk, div);
+
+	return div;
+}
+
+static void recalculate_fifo_timeout(struct pxa_camera_dev *pcdev,
+				     unsigned long pclk)
+{
+	/* We want a timeout > 1 pixel time, not ">=" */
+	u32 ciclk_per_pixel = pcdev->ciclk / pclk + 1;
+
+	__raw_writel(ciclk_per_pixel, pcdev->base + CITOR);
+}
+
+static void pxa_camera_activate(struct pxa_camera_dev *pcdev)
+{
+	u32 cicr4 = 0;
+
+	/* disable all interrupts */
+	__raw_writel(0x3ff, pcdev->base + CICR0);
+
+	if (pcdev->platform_flags & PXA_CAMERA_PCLK_EN)
+		cicr4 |= CICR4_PCLK_EN;
+	if (pcdev->platform_flags & PXA_CAMERA_MCLK_EN)
+		cicr4 |= CICR4_MCLK_EN;
+	if (pcdev->platform_flags & PXA_CAMERA_PCP)
+		cicr4 |= CICR4_PCP;
+	if (pcdev->platform_flags & PXA_CAMERA_HSP)
+		cicr4 |= CICR4_HSP;
+	if (pcdev->platform_flags & PXA_CAMERA_VSP)
+		cicr4 |= CICR4_VSP;
+
+	__raw_writel(pcdev->mclk_divisor | cicr4, pcdev->base + CICR4);
+
+	if (pcdev->platform_flags & PXA_CAMERA_MCLK_EN)
+		/* Initialise the timeout under the assumption pclk = mclk */
+		recalculate_fifo_timeout(pcdev, pcdev->mclk);
+	else
+		/* "Safe default" - 13MHz */
+		recalculate_fifo_timeout(pcdev, 13000000);
+
+	clk_prepare_enable(pcdev->clk);
+}
+
+static void pxa_camera_deactivate(struct pxa_camera_dev *pcdev)
+{
+	clk_disable_unprepare(pcdev->clk);
+}
+
+static irqreturn_t pxa_camera_irq(int irq, void *data)
+{
+	struct pxa_camera_dev *pcdev = data;
+	unsigned long status, cifr, cicr0;
+	struct pxa_buffer *buf;
+	struct videobuf_buffer *vb;
+
+	status = __raw_readl(pcdev->base + CISR);
+	dev_dbg(pcdev->soc_host.v4l2_dev.dev,
+		"Camera interrupt status 0x%lx\n", status);
+
+	if (!status)
+		return IRQ_NONE;
+
+	__raw_writel(status, pcdev->base + CISR);
+
+	if (status & CISR_EOF) {
+		/* Reset the FIFOs */
+		cifr = __raw_readl(pcdev->base + CIFR) | CIFR_RESET_F;
+		__raw_writel(cifr, pcdev->base + CIFR);
+
+		pcdev->active = list_first_entry(&pcdev->capture,
+					   struct pxa_buffer, vb.queue);
+		vb = &pcdev->active->vb;
+		buf = container_of(vb, struct pxa_buffer, vb);
+		pxa_videobuf_set_actdma(pcdev, buf);
+
+		pxa_dma_start_channels(pcdev);
+
+		cicr0 = __raw_readl(pcdev->base + CICR0) | CICR0_EOFM;
+		__raw_writel(cicr0, pcdev->base + CICR0);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int pxa_camera_add_device(struct soc_camera_device *icd)
+{
+	dev_info(icd->parent, "PXA Camera driver attached to camera %d\n",
+		 icd->devnum);
+
+	return 0;
+}
+
+static void pxa_camera_remove_device(struct soc_camera_device *icd)
+{
+	dev_info(icd->parent, "PXA Camera driver detached from camera %d\n",
+		 icd->devnum);
+}
+
+/*
+ * The following two functions absolutely depend on the fact, that
+ * there can be only one camera on PXA quick capture interface
+ * Called with .host_lock held
+ */
+static int pxa_camera_clock_start(struct soc_camera_host *ici)
+{
+	struct pxa_camera_dev *pcdev = ici->priv;
+
+	pxa_camera_activate(pcdev);
+
+	return 0;
+}
+
+/* Called with .host_lock held */
+static void pxa_camera_clock_stop(struct soc_camera_host *ici)
+{
+	struct pxa_camera_dev *pcdev = ici->priv;
+
+	/* disable capture, disable interrupts */
+	__raw_writel(0x3ff, pcdev->base + CICR0);
+
+	/* Stop DMA engine */
+	DCSR(pcdev->dma_chans[0]) = 0;
+	DCSR(pcdev->dma_chans[1]) = 0;
+	DCSR(pcdev->dma_chans[2]) = 0;
+
+	pxa_camera_deactivate(pcdev);
+}
+
+static int test_platform_param(struct pxa_camera_dev *pcdev,
+			       unsigned char buswidth, unsigned long *flags)
+{
+	/*
+	 * Platform specified synchronization and pixel clock polarities are
+	 * only a recommendation and are only used during probing. The PXA270
+	 * quick capture interface supports both.
+	 */
+	*flags = (pcdev->platform_flags & PXA_CAMERA_MASTER ?
+		  V4L2_MBUS_MASTER : V4L2_MBUS_SLAVE) |
+		V4L2_MBUS_HSYNC_ACTIVE_HIGH |
+		V4L2_MBUS_HSYNC_ACTIVE_LOW |
+		V4L2_MBUS_VSYNC_ACTIVE_HIGH |
+		V4L2_MBUS_VSYNC_ACTIVE_LOW |
+		V4L2_MBUS_DATA_ACTIVE_HIGH |
+		V4L2_MBUS_PCLK_SAMPLE_RISING |
+		V4L2_MBUS_PCLK_SAMPLE_FALLING;
+
+	/* If requested data width is supported by the platform, use it */
+	if ((1 << (buswidth - 1)) & pcdev->width_flags)
+		return 0;
+
+	return -EINVAL;
+}
+
+static void pxa_camera_setup_cicr(struct soc_camera_device *icd,
+				  unsigned long flags, __u32 pixfmt)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct pxa_camera_dev *pcdev = ici->priv;
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	unsigned long dw, bpp;
+	u32 cicr0, cicr1, cicr2, cicr3, cicr4 = 0, y_skip_top;
+	int ret = v4l2_subdev_call(sd, sensor, g_skip_top_lines, &y_skip_top);
+
+	if (ret < 0)
+		y_skip_top = 0;
+
+	/*
+	 * Datawidth is now guaranteed to be equal to one of the three values.
+	 * We fix bit-per-pixel equal to data-width...
+	 */
+	switch (icd->current_fmt->host_fmt->bits_per_sample) {
+	case 10:
+		dw = 4;
+		bpp = 0x40;
+		break;
+	case 9:
+		dw = 3;
+		bpp = 0x20;
+		break;
+	default:
+		/*
+		 * Actually it can only be 8 now,
+		 * default is just to silence compiler warnings
+		 */
+	case 8:
+		dw = 2;
+		bpp = 0;
+	}
+
+	if (pcdev->platform_flags & PXA_CAMERA_PCLK_EN)
+		cicr4 |= CICR4_PCLK_EN;
+	if (pcdev->platform_flags & PXA_CAMERA_MCLK_EN)
+		cicr4 |= CICR4_MCLK_EN;
+	if (flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
+		cicr4 |= CICR4_PCP;
+	if (flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)
+		cicr4 |= CICR4_HSP;
+	if (flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
+		cicr4 |= CICR4_VSP;
+
+	cicr0 = __raw_readl(pcdev->base + CICR0);
+	if (cicr0 & CICR0_ENB)
+		__raw_writel(cicr0 & ~CICR0_ENB, pcdev->base + CICR0);
+
+	cicr1 = CICR1_PPL_VAL(icd->user_width - 1) | bpp | dw;
+
+	switch (pixfmt) {
+	case V4L2_PIX_FMT_YUV422P:
+		pcdev->channels = 3;
+		cicr1 |= CICR1_YCBCR_F;
+		/*
+		 * Normally, pxa bus wants as input UYVY format. We allow all
+		 * reorderings of the YUV422 format, as no processing is done,
+		 * and the YUV stream is just passed through without any
+		 * transformation. Note that UYVY is the only format that
+		 * should be used if pxa framebuffer Overlay2 is used.
+		 */
+	case V4L2_PIX_FMT_UYVY:
+	case V4L2_PIX_FMT_VYUY:
+	case V4L2_PIX_FMT_YUYV:
+	case V4L2_PIX_FMT_YVYU:
+		cicr1 |= CICR1_COLOR_SP_VAL(2);
+		break;
+	case V4L2_PIX_FMT_RGB555:
+		cicr1 |= CICR1_RGB_BPP_VAL(1) | CICR1_RGBT_CONV_VAL(2) |
+			CICR1_TBIT | CICR1_COLOR_SP_VAL(1);
+		break;
+	case V4L2_PIX_FMT_RGB565:
+		cicr1 |= CICR1_COLOR_SP_VAL(1) | CICR1_RGB_BPP_VAL(2);
+		break;
+	}
+
+	cicr2 = 0;
+	cicr3 = CICR3_LPF_VAL(icd->user_height - 1) |
+		CICR3_BFW_VAL(min((u32)255, y_skip_top));
+	cicr4 |= pcdev->mclk_divisor;
+
+	__raw_writel(cicr1, pcdev->base + CICR1);
+	__raw_writel(cicr2, pcdev->base + CICR2);
+	__raw_writel(cicr3, pcdev->base + CICR3);
+	__raw_writel(cicr4, pcdev->base + CICR4);
+
+	/* CIF interrupts are not used, only DMA */
+	cicr0 = (cicr0 & CICR0_ENB) | (pcdev->platform_flags & PXA_CAMERA_MASTER ?
+		CICR0_SIM_MP : (CICR0_SL_CAP_EN | CICR0_SIM_SP));
+	cicr0 |= CICR0_DMAEN | CICR0_IRQ_MASK;
+	__raw_writel(cicr0, pcdev->base + CICR0);
+}
+
+static int pxa_camera_set_bus_param(struct soc_camera_device *icd)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct pxa_camera_dev *pcdev = ici->priv;
+	struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
+	u32 pixfmt = icd->current_fmt->host_fmt->fourcc;
+	unsigned long bus_flags, common_flags;
+	int ret;
+	struct pxa_cam *cam = icd->host_priv;
+
+	ret = test_platform_param(pcdev, icd->current_fmt->host_fmt->bits_per_sample,
+				  &bus_flags);
+	if (ret < 0)
+		return ret;
+
+	ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
+	if (!ret) {
+		common_flags = soc_mbus_config_compatible(&cfg,
+							  bus_flags);
+		if (!common_flags) {
+			dev_warn(icd->parent,
+				 "Flags incompatible: camera 0x%x, host 0x%lx\n",
+				 cfg.flags, bus_flags);
+			return -EINVAL;
+		}
+	} else if (ret != -ENOIOCTLCMD) {
+		return ret;
+	} else {
+		common_flags = bus_flags;
+	}
+
+	pcdev->channels = 1;
+
+	/* Make choises, based on platform preferences */
+	if ((common_flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH) &&
+	    (common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)) {
+		if (pcdev->platform_flags & PXA_CAMERA_HSP)
+			common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_HIGH;
+		else
+			common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_LOW;
+	}
+
+	if ((common_flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH) &&
+	    (common_flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)) {
+		if (pcdev->platform_flags & PXA_CAMERA_VSP)
+			common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_HIGH;
+		else
+			common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_LOW;
+	}
+
+	if ((common_flags & V4L2_MBUS_PCLK_SAMPLE_RISING) &&
+	    (common_flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)) {
+		if (pcdev->platform_flags & PXA_CAMERA_PCP)
+			common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_RISING;
+		else
+			common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_FALLING;
+	}
+
+	cfg.flags = common_flags;
+	ret = v4l2_subdev_call(sd, video, s_mbus_config, &cfg);
+	if (ret < 0 && ret != -ENOIOCTLCMD) {
+		dev_dbg(icd->parent, "camera s_mbus_config(0x%lx) returned %d\n",
+			common_flags, ret);
+		return ret;
+	}
+
+	cam->flags = common_flags;
+
+	pxa_camera_setup_cicr(icd, common_flags, pixfmt);
+
+	return 0;
+}
+
+static int pxa_camera_try_bus_param(struct soc_camera_device *icd,
+				    unsigned char buswidth)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct pxa_camera_dev *pcdev = ici->priv;
+	struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
+	unsigned long bus_flags, common_flags;
+	int ret = test_platform_param(pcdev, buswidth, &bus_flags);
+
+	if (ret < 0)
+		return ret;
+
+	ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
+	if (!ret) {
+		common_flags = soc_mbus_config_compatible(&cfg,
+							  bus_flags);
+		if (!common_flags) {
+			dev_warn(icd->parent,
+				 "Flags incompatible: camera 0x%x, host 0x%lx\n",
+				 cfg.flags, bus_flags);
+			return -EINVAL;
+		}
+	} else if (ret == -ENOIOCTLCMD) {
+		ret = 0;
+	}
+
+	return ret;
+}
+
+static const struct soc_mbus_pixelfmt pxa_camera_formats[] = {
+	{
+		.fourcc			= V4L2_PIX_FMT_YUV422P,
+		.name			= "Planar YUV422 16 bit",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PLANAR_2Y_U_V,
+	},
+};
+
+/* This will be corrected as we get more formats */
+static bool pxa_camera_packing_supported(const struct soc_mbus_pixelfmt *fmt)
+{
+	return	fmt->packing == SOC_MBUS_PACKING_NONE ||
+		(fmt->bits_per_sample == 8 &&
+		 fmt->packing == SOC_MBUS_PACKING_2X8_PADHI) ||
+		(fmt->bits_per_sample > 8 &&
+		 fmt->packing == SOC_MBUS_PACKING_EXTEND16);
+}
+
+static int pxa_camera_get_formats(struct soc_camera_device *icd, unsigned int idx,
+				  struct soc_camera_format_xlate *xlate)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct device *dev = icd->parent;
+	int formats = 0, ret;
+	struct pxa_cam *cam;
+	struct v4l2_subdev_mbus_code_enum code = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.index = idx,
+	};
+	const struct soc_mbus_pixelfmt *fmt;
+
+	ret = v4l2_subdev_call(sd, pad, enum_mbus_code, NULL, &code);
+	if (ret < 0)
+		/* No more formats */
+		return 0;
+
+	fmt = soc_mbus_get_fmtdesc(code.code);
+	if (!fmt) {
+		dev_err(dev, "Invalid format code #%u: %d\n", idx, code.code);
+		return 0;
+	}
+
+	/* This also checks support for the requested bits-per-sample */
+	ret = pxa_camera_try_bus_param(icd, fmt->bits_per_sample);
+	if (ret < 0)
+		return 0;
+
+	if (!icd->host_priv) {
+		cam = kzalloc(sizeof(*cam), GFP_KERNEL);
+		if (!cam)
+			return -ENOMEM;
+
+		icd->host_priv = cam;
+	} else {
+		cam = icd->host_priv;
+	}
+
+	switch (code.code) {
+	case MEDIA_BUS_FMT_UYVY8_2X8:
+		formats++;
+		if (xlate) {
+			xlate->host_fmt	= &pxa_camera_formats[0];
+			xlate->code	= code.code;
+			xlate++;
+			dev_dbg(dev, "Providing format %s using code %d\n",
+				pxa_camera_formats[0].name, code.code);
+		}
+	case MEDIA_BUS_FMT_VYUY8_2X8:
+	case MEDIA_BUS_FMT_YUYV8_2X8:
+	case MEDIA_BUS_FMT_YVYU8_2X8:
+	case MEDIA_BUS_FMT_RGB565_2X8_LE:
+	case MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE:
+		if (xlate)
+			dev_dbg(dev, "Providing format %s packed\n",
+				fmt->name);
+		break;
+	default:
+		if (!pxa_camera_packing_supported(fmt))
+			return 0;
+		if (xlate)
+			dev_dbg(dev,
+				"Providing format %s in pass-through mode\n",
+				fmt->name);
+	}
+
+	/* Generic pass-through */
+	formats++;
+	if (xlate) {
+		xlate->host_fmt	= fmt;
+		xlate->code	= code.code;
+		xlate++;
+	}
+
+	return formats;
+}
+
+static void pxa_camera_put_formats(struct soc_camera_device *icd)
+{
+	kfree(icd->host_priv);
+	icd->host_priv = NULL;
+}
+
+static int pxa_camera_check_frame(u32 width, u32 height)
+{
+	/* limit to pxa hardware capabilities */
+	return height < 32 || height > 2048 || width < 48 || width > 2048 ||
+		(width & 0x01);
+}
+
+static int pxa_camera_set_crop(struct soc_camera_device *icd,
+			       const struct v4l2_crop *a)
+{
+	const struct v4l2_rect *rect = &a->c;
+	struct device *dev = icd->parent;
+	struct soc_camera_host *ici = to_soc_camera_host(dev);
+	struct pxa_camera_dev *pcdev = ici->priv;
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct soc_camera_sense sense = {
+		.master_clock = pcdev->mclk,
+		.pixel_clock_max = pcdev->ciclk / 4,
+	};
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_mbus_framefmt *mf = &fmt.format;
+	struct pxa_cam *cam = icd->host_priv;
+	u32 fourcc = icd->current_fmt->host_fmt->fourcc;
+	int ret;
+
+	/* If PCLK is used to latch data from the sensor, check sense */
+	if (pcdev->platform_flags & PXA_CAMERA_PCLK_EN)
+		icd->sense = &sense;
+
+	ret = v4l2_subdev_call(sd, video, s_crop, a);
+
+	icd->sense = NULL;
+
+	if (ret < 0) {
+		dev_warn(dev, "Failed to crop to %ux%u@%u:%u\n",
+			 rect->width, rect->height, rect->left, rect->top);
+		return ret;
+	}
+
+	ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &fmt);
+	if (ret < 0)
+		return ret;
+
+	if (pxa_camera_check_frame(mf->width, mf->height)) {
+		/*
+		 * Camera cropping produced a frame beyond our capabilities.
+		 * FIXME: just extract a subframe, that we can process.
+		 */
+		v4l_bound_align_image(&mf->width, 48, 2048, 1,
+			&mf->height, 32, 2048, 0,
+			fourcc == V4L2_PIX_FMT_YUV422P ? 4 : 0);
+		ret = v4l2_subdev_call(sd, pad, set_fmt, NULL, &fmt);
+		if (ret < 0)
+			return ret;
+
+		if (pxa_camera_check_frame(mf->width, mf->height)) {
+			dev_warn(icd->parent,
+				 "Inconsistent state. Use S_FMT to repair\n");
+			return -EINVAL;
+		}
+	}
+
+	if (sense.flags & SOCAM_SENSE_PCLK_CHANGED) {
+		if (sense.pixel_clock > sense.pixel_clock_max) {
+			dev_err(dev,
+				"pixel clock %lu set by the camera too high!",
+				sense.pixel_clock);
+			return -EIO;
+		}
+		recalculate_fifo_timeout(pcdev, sense.pixel_clock);
+	}
+
+	icd->user_width		= mf->width;
+	icd->user_height	= mf->height;
+
+	pxa_camera_setup_cicr(icd, cam->flags, fourcc);
+
+	return ret;
+}
+
+static int pxa_camera_set_fmt(struct soc_camera_device *icd,
+			      struct v4l2_format *f)
+{
+	struct device *dev = icd->parent;
+	struct soc_camera_host *ici = to_soc_camera_host(dev);
+	struct pxa_camera_dev *pcdev = ici->priv;
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	const struct soc_camera_format_xlate *xlate = NULL;
+	struct soc_camera_sense sense = {
+		.master_clock = pcdev->mclk,
+		.pixel_clock_max = pcdev->ciclk / 4,
+	};
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_mbus_framefmt *mf = &format.format;
+	int ret;
+
+	xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
+	if (!xlate) {
+		dev_warn(dev, "Format %x not found\n", pix->pixelformat);
+		return -EINVAL;
+	}
+
+	/* If PCLK is used to latch data from the sensor, check sense */
+	if (pcdev->platform_flags & PXA_CAMERA_PCLK_EN)
+		/* The caller holds a mutex. */
+		icd->sense = &sense;
+
+	mf->width	= pix->width;
+	mf->height	= pix->height;
+	mf->field	= pix->field;
+	mf->colorspace	= pix->colorspace;
+	mf->code	= xlate->code;
+
+	ret = v4l2_subdev_call(sd, pad, set_fmt, NULL, &format);
+
+	if (mf->code != xlate->code)
+		return -EINVAL;
+
+	icd->sense = NULL;
+
+	if (ret < 0) {
+		dev_warn(dev, "Failed to configure for format %x\n",
+			 pix->pixelformat);
+	} else if (pxa_camera_check_frame(mf->width, mf->height)) {
+		dev_warn(dev,
+			 "Camera driver produced an unsupported frame %dx%d\n",
+			 mf->width, mf->height);
+		ret = -EINVAL;
+	} else if (sense.flags & SOCAM_SENSE_PCLK_CHANGED) {
+		if (sense.pixel_clock > sense.pixel_clock_max) {
+			dev_err(dev,
+				"pixel clock %lu set by the camera too high!",
+				sense.pixel_clock);
+			return -EIO;
+		}
+		recalculate_fifo_timeout(pcdev, sense.pixel_clock);
+	}
+
+	if (ret < 0)
+		return ret;
+
+	pix->width		= mf->width;
+	pix->height		= mf->height;
+	pix->field		= mf->field;
+	pix->colorspace		= mf->colorspace;
+	icd->current_fmt	= xlate;
+
+	return ret;
+}
+
+static int pxa_camera_try_fmt(struct soc_camera_device *icd,
+			      struct v4l2_format *f)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	const struct soc_camera_format_xlate *xlate;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct v4l2_subdev_pad_config pad_cfg;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+	};
+	struct v4l2_mbus_framefmt *mf = &format.format;
+	__u32 pixfmt = pix->pixelformat;
+	int ret;
+
+	xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
+	if (!xlate) {
+		dev_warn(icd->parent, "Format %x not found\n", pixfmt);
+		return -EINVAL;
+	}
+
+	/*
+	 * Limit to pxa hardware capabilities.  YUV422P planar format requires
+	 * images size to be a multiple of 16 bytes.  If not, zeros will be
+	 * inserted between Y and U planes, and U and V planes, which violates
+	 * the YUV422P standard.
+	 */
+	v4l_bound_align_image(&pix->width, 48, 2048, 1,
+			      &pix->height, 32, 2048, 0,
+			      pixfmt == V4L2_PIX_FMT_YUV422P ? 4 : 0);
+
+	/* limit to sensor capabilities */
+	mf->width	= pix->width;
+	mf->height	= pix->height;
+	/* Only progressive video supported so far */
+	mf->field	= V4L2_FIELD_NONE;
+	mf->colorspace	= pix->colorspace;
+	mf->code	= xlate->code;
+
+	ret = v4l2_subdev_call(sd, pad, set_fmt, &pad_cfg, &format);
+	if (ret < 0)
+		return ret;
+
+	pix->width	= mf->width;
+	pix->height	= mf->height;
+	pix->colorspace	= mf->colorspace;
+
+	switch (mf->field) {
+	case V4L2_FIELD_ANY:
+	case V4L2_FIELD_NONE:
+		pix->field	= V4L2_FIELD_NONE;
+		break;
+	default:
+		/* TODO: support interlaced at least in pass-through mode */
+		dev_err(icd->parent, "Field type %d unsupported.\n",
+			mf->field);
+		return -EINVAL;
+	}
+
+	return ret;
+}
+
+static int pxa_camera_reqbufs(struct soc_camera_device *icd,
+			      struct v4l2_requestbuffers *p)
+{
+	int i;
+
+	/*
+	 * This is for locking debugging only. I removed spinlocks and now I
+	 * check whether .prepare is ever called on a linked buffer, or whether
+	 * a dma IRQ can occur for an in-work or unlinked buffer. Until now
+	 * it hadn't triggered
+	 */
+	for (i = 0; i < p->count; i++) {
+		struct pxa_buffer *buf = container_of(icd->vb_vidq.bufs[i],
+						      struct pxa_buffer, vb);
+		buf->inwork = 0;
+		INIT_LIST_HEAD(&buf->vb.queue);
+	}
+
+	return 0;
+}
+
+static unsigned int pxa_camera_poll(struct file *file, poll_table *pt)
+{
+	struct soc_camera_device *icd = file->private_data;
+	struct pxa_buffer *buf;
+
+	buf = list_entry(icd->vb_vidq.stream.next, struct pxa_buffer,
+			 vb.stream);
+
+	poll_wait(file, &buf->vb.done, pt);
+
+	if (buf->vb.state == VIDEOBUF_DONE ||
+	    buf->vb.state == VIDEOBUF_ERROR)
+		return POLLIN|POLLRDNORM;
+
+	return 0;
+}
+
+static int pxa_camera_querycap(struct soc_camera_host *ici,
+			       struct v4l2_capability *cap)
+{
+	/* cap->name is set by the firendly caller:-> */
+	strlcpy(cap->card, pxa_cam_driver_description, sizeof(cap->card));
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+
+	return 0;
+}
+
+static int pxa_camera_suspend(struct device *dev)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(dev);
+	struct pxa_camera_dev *pcdev = ici->priv;
+	int i = 0, ret = 0;
+
+	pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR0);
+	pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR1);
+	pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR2);
+	pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR3);
+	pcdev->save_cicr[i++] = __raw_readl(pcdev->base + CICR4);
+
+	if (pcdev->soc_host.icd) {
+		struct v4l2_subdev *sd = soc_camera_to_subdev(pcdev->soc_host.icd);
+		ret = v4l2_subdev_call(sd, core, s_power, 0);
+		if (ret == -ENOIOCTLCMD)
+			ret = 0;
+	}
+
+	return ret;
+}
+
+static int pxa_camera_resume(struct device *dev)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(dev);
+	struct pxa_camera_dev *pcdev = ici->priv;
+	int i = 0, ret = 0;
+
+	DRCMR(68) = pcdev->dma_chans[0] | DRCMR_MAPVLD;
+	DRCMR(69) = pcdev->dma_chans[1] | DRCMR_MAPVLD;
+	DRCMR(70) = pcdev->dma_chans[2] | DRCMR_MAPVLD;
+
+	__raw_writel(pcdev->save_cicr[i++] & ~CICR0_ENB, pcdev->base + CICR0);
+	__raw_writel(pcdev->save_cicr[i++], pcdev->base + CICR1);
+	__raw_writel(pcdev->save_cicr[i++], pcdev->base + CICR2);
+	__raw_writel(pcdev->save_cicr[i++], pcdev->base + CICR3);
+	__raw_writel(pcdev->save_cicr[i++], pcdev->base + CICR4);
+
+	if (pcdev->soc_host.icd) {
+		struct v4l2_subdev *sd = soc_camera_to_subdev(pcdev->soc_host.icd);
+		ret = v4l2_subdev_call(sd, core, s_power, 1);
+		if (ret == -ENOIOCTLCMD)
+			ret = 0;
+	}
+
+	/* Restart frame capture if active buffer exists */
+	if (!ret && pcdev->active)
+		pxa_camera_start_capture(pcdev);
+
+	return ret;
+}
+
+static struct soc_camera_host_ops pxa_soc_camera_host_ops = {
+	.owner		= THIS_MODULE,
+	.add		= pxa_camera_add_device,
+	.remove		= pxa_camera_remove_device,
+	.clock_start	= pxa_camera_clock_start,
+	.clock_stop	= pxa_camera_clock_stop,
+	.set_crop	= pxa_camera_set_crop,
+	.get_formats	= pxa_camera_get_formats,
+	.put_formats	= pxa_camera_put_formats,
+	.set_fmt	= pxa_camera_set_fmt,
+	.try_fmt	= pxa_camera_try_fmt,
+	.init_videobuf	= pxa_camera_init_videobuf,
+	.reqbufs	= pxa_camera_reqbufs,
+	.poll		= pxa_camera_poll,
+	.querycap	= pxa_camera_querycap,
+	.set_bus_param	= pxa_camera_set_bus_param,
+};
+
+static int pxa_camera_pdata_from_dt(struct device *dev,
+				    struct pxa_camera_dev *pcdev)
+{
+	u32 mclk_rate;
+	struct device_node *np = dev->of_node;
+	struct v4l2_of_endpoint ep;
+	int err = of_property_read_u32(np, "clock-frequency",
+				       &mclk_rate);
+	if (!err) {
+		pcdev->platform_flags |= PXA_CAMERA_MCLK_EN;
+		pcdev->mclk = mclk_rate;
+	}
+
+	np = of_graph_get_next_endpoint(np, NULL);
+	if (!np) {
+		dev_err(dev, "could not find endpoint\n");
+		return -EINVAL;
+	}
+
+	err = v4l2_of_parse_endpoint(np, &ep);
+	if (err) {
+		dev_err(dev, "could not parse endpoint\n");
+		goto out;
+	}
+
+	switch (ep.bus.parallel.bus_width) {
+	case 4:
+		pcdev->platform_flags |= PXA_CAMERA_DATAWIDTH_4;
+		break;
+	case 5:
+		pcdev->platform_flags |= PXA_CAMERA_DATAWIDTH_5;
+		break;
+	case 8:
+		pcdev->platform_flags |= PXA_CAMERA_DATAWIDTH_8;
+		break;
+	case 9:
+		pcdev->platform_flags |= PXA_CAMERA_DATAWIDTH_9;
+		break;
+	case 10:
+		pcdev->platform_flags |= PXA_CAMERA_DATAWIDTH_10;
+		break;
+	default:
+		break;
+	}
+
+	if (ep.bus.parallel.flags & V4L2_MBUS_MASTER)
+		pcdev->platform_flags |= PXA_CAMERA_MASTER;
+	if (ep.bus.parallel.flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
+		pcdev->platform_flags |= PXA_CAMERA_HSP;
+	if (ep.bus.parallel.flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH)
+		pcdev->platform_flags |= PXA_CAMERA_VSP;
+	if (ep.bus.parallel.flags & V4L2_MBUS_PCLK_SAMPLE_RISING)
+		pcdev->platform_flags |= PXA_CAMERA_PCLK_EN | PXA_CAMERA_PCP;
+	if (ep.bus.parallel.flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
+		pcdev->platform_flags |= PXA_CAMERA_PCLK_EN;
+
+out:
+	of_node_put(np);
+
+	return err;
+}
+
+static int pxa_camera_probe(struct platform_device *pdev)
+{
+	struct pxa_camera_dev *pcdev;
+	struct resource *res;
+	void __iomem *base;
+	int irq;
+	int err = 0;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	irq = platform_get_irq(pdev, 0);
+	if (!res || irq < 0)
+		return -ENODEV;
+
+	pcdev = devm_kzalloc(&pdev->dev, sizeof(*pcdev), GFP_KERNEL);
+	if (!pcdev) {
+		dev_err(&pdev->dev, "Could not allocate pcdev\n");
+		return -ENOMEM;
+	}
+
+	pcdev->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(pcdev->clk))
+		return PTR_ERR(pcdev->clk);
+
+	pcdev->res = res;
+
+	pcdev->pdata = pdev->dev.platform_data;
+	if (&pdev->dev.of_node && !pcdev->pdata) {
+		err = pxa_camera_pdata_from_dt(&pdev->dev, pcdev);
+	} else {
+		pcdev->platform_flags = pcdev->pdata->flags;
+		pcdev->mclk = pcdev->pdata->mclk_10khz * 10000;
+	}
+	if (err < 0)
+		return err;
+
+	if (!(pcdev->platform_flags & (PXA_CAMERA_DATAWIDTH_8 |
+			PXA_CAMERA_DATAWIDTH_9 | PXA_CAMERA_DATAWIDTH_10))) {
+		/*
+		 * Platform hasn't set available data widths. This is bad.
+		 * Warn and use a default.
+		 */
+		dev_warn(&pdev->dev, "WARNING! Platform hasn't set available "
+			 "data widths, using default 10 bit\n");
+		pcdev->platform_flags |= PXA_CAMERA_DATAWIDTH_10;
+	}
+	if (pcdev->platform_flags & PXA_CAMERA_DATAWIDTH_8)
+		pcdev->width_flags = 1 << 7;
+	if (pcdev->platform_flags & PXA_CAMERA_DATAWIDTH_9)
+		pcdev->width_flags |= 1 << 8;
+	if (pcdev->platform_flags & PXA_CAMERA_DATAWIDTH_10)
+		pcdev->width_flags |= 1 << 9;
+	if (!pcdev->mclk) {
+		dev_warn(&pdev->dev,
+			 "mclk == 0! Please, fix your platform data. "
+			 "Using default 20MHz\n");
+		pcdev->mclk = 20000000;
+	}
+
+	pcdev->mclk_divisor = mclk_get_divisor(pdev, pcdev);
+
+	INIT_LIST_HEAD(&pcdev->capture);
+	spin_lock_init(&pcdev->lock);
+
+	/*
+	 * Request the regions.
+	 */
+	base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
+
+	pcdev->irq = irq;
+	pcdev->base = base;
+
+	/* request dma */
+	err = pxa_request_dma("CI_Y", DMA_PRIO_HIGH,
+			      pxa_camera_dma_irq_y, pcdev);
+	if (err < 0) {
+		dev_err(&pdev->dev, "Can't request DMA for Y\n");
+		return err;
+	}
+	pcdev->dma_chans[0] = err;
+	dev_dbg(&pdev->dev, "got DMA channel %d\n", pcdev->dma_chans[0]);
+
+	err = pxa_request_dma("CI_U", DMA_PRIO_HIGH,
+			      pxa_camera_dma_irq_u, pcdev);
+	if (err < 0) {
+		dev_err(&pdev->dev, "Can't request DMA for U\n");
+		goto exit_free_dma_y;
+	}
+	pcdev->dma_chans[1] = err;
+	dev_dbg(&pdev->dev, "got DMA channel (U) %d\n", pcdev->dma_chans[1]);
+
+	err = pxa_request_dma("CI_V", DMA_PRIO_HIGH,
+			      pxa_camera_dma_irq_v, pcdev);
+	if (err < 0) {
+		dev_err(&pdev->dev, "Can't request DMA for V\n");
+		goto exit_free_dma_u;
+	}
+	pcdev->dma_chans[2] = err;
+	dev_dbg(&pdev->dev, "got DMA channel (V) %d\n", pcdev->dma_chans[2]);
+
+	DRCMR(68) = pcdev->dma_chans[0] | DRCMR_MAPVLD;
+	DRCMR(69) = pcdev->dma_chans[1] | DRCMR_MAPVLD;
+	DRCMR(70) = pcdev->dma_chans[2] | DRCMR_MAPVLD;
+
+	/* request irq */
+	err = devm_request_irq(&pdev->dev, pcdev->irq, pxa_camera_irq, 0,
+			       PXA_CAM_DRV_NAME, pcdev);
+	if (err) {
+		dev_err(&pdev->dev, "Camera interrupt register failed\n");
+		goto exit_free_dma;
+	}
+
+	pcdev->soc_host.drv_name	= PXA_CAM_DRV_NAME;
+	pcdev->soc_host.ops		= &pxa_soc_camera_host_ops;
+	pcdev->soc_host.priv		= pcdev;
+	pcdev->soc_host.v4l2_dev.dev	= &pdev->dev;
+	pcdev->soc_host.nr		= pdev->id;
+
+	err = soc_camera_host_register(&pcdev->soc_host);
+	if (err)
+		goto exit_free_dma;
+
+	return 0;
+
+exit_free_dma:
+	pxa_free_dma(pcdev->dma_chans[2]);
+exit_free_dma_u:
+	pxa_free_dma(pcdev->dma_chans[1]);
+exit_free_dma_y:
+	pxa_free_dma(pcdev->dma_chans[0]);
+	return err;
+}
+
+static int pxa_camera_remove(struct platform_device *pdev)
+{
+	struct soc_camera_host *soc_host = to_soc_camera_host(&pdev->dev);
+	struct pxa_camera_dev *pcdev = container_of(soc_host,
+					struct pxa_camera_dev, soc_host);
+
+	pxa_free_dma(pcdev->dma_chans[0]);
+	pxa_free_dma(pcdev->dma_chans[1]);
+	pxa_free_dma(pcdev->dma_chans[2]);
+
+	soc_camera_host_unregister(soc_host);
+
+	dev_info(&pdev->dev, "PXA Camera driver unloaded\n");
+
+	return 0;
+}
+
+static const struct dev_pm_ops pxa_camera_pm = {
+	.suspend	= pxa_camera_suspend,
+	.resume		= pxa_camera_resume,
+};
+
+static const struct of_device_id pxa_camera_of_match[] = {
+	{ .compatible = "marvell,pxa270-qci", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, pxa_camera_of_match);
+
+static struct platform_driver pxa_camera_driver = {
+	.driver		= {
+		.name	= PXA_CAM_DRV_NAME,
+		.pm	= &pxa_camera_pm,
+		.of_match_table = of_match_ptr(pxa_camera_of_match),
+	},
+	.probe		= pxa_camera_probe,
+	.remove		= pxa_camera_remove,
+};
+
+module_platform_driver(pxa_camera_driver);
+
+MODULE_DESCRIPTION("PXA27x SoC Camera Host driver");
+MODULE_AUTHOR("Guennadi Liakhovetski <kernel@pengutronix.de>");
+MODULE_LICENSE("GPL");
+MODULE_VERSION(PXA_CAM_VERSION);
+MODULE_ALIAS("platform:" PXA_CAM_DRV_NAME);
diff --git a/drivers/media/platform/soc_camera/rcar_vin.c b/drivers/media/platform/soc_camera/rcar_vin.c
new file mode 100644
index 0000000..efe57b2
--- /dev/null
+++ b/drivers/media/platform/soc_camera/rcar_vin.c
@@ -0,0 +1,2002 @@
+/*
+ * SoC-camera host driver for Renesas R-Car VIN unit
+ *
+ * Copyright (C) 2011-2013 Renesas Solutions Corp.
+ * Copyright (C) 2013 Cogent Embedded, Inc., <source@cogentembedded.com>
+ *
+ * Based on V4L2 Driver for SuperH Mobile CEU interface "sh_mobile_ceu_camera.c"
+ *
+ * Copyright (C) 2008 Magnus Damm
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ */
+
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/platform_data/camera-rcar.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+
+#include <media/soc_camera.h>
+#include <media/soc_mediabus.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-mediabus.h>
+#include <media/v4l2-of.h>
+#include <media/v4l2-subdev.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "soc_scale_crop.h"
+
+#define DRV_NAME "rcar_vin"
+
+/* Register offsets for R-Car VIN */
+#define VNMC_REG	0x00	/* Video n Main Control Register */
+#define VNMS_REG	0x04	/* Video n Module Status Register */
+#define VNFC_REG	0x08	/* Video n Frame Capture Register */
+#define VNSLPRC_REG	0x0C	/* Video n Start Line Pre-Clip Register */
+#define VNELPRC_REG	0x10	/* Video n End Line Pre-Clip Register */
+#define VNSPPRC_REG	0x14	/* Video n Start Pixel Pre-Clip Register */
+#define VNEPPRC_REG	0x18	/* Video n End Pixel Pre-Clip Register */
+#define VNSLPOC_REG	0x1C	/* Video n Start Line Post-Clip Register */
+#define VNELPOC_REG	0x20	/* Video n End Line Post-Clip Register */
+#define VNSPPOC_REG	0x24	/* Video n Start Pixel Post-Clip Register */
+#define VNEPPOC_REG	0x28	/* Video n End Pixel Post-Clip Register */
+#define VNIS_REG	0x2C	/* Video n Image Stride Register */
+#define VNMB_REG(m)	(0x30 + ((m) << 2)) /* Video n Memory Base m Register */
+#define VNIE_REG	0x40	/* Video n Interrupt Enable Register */
+#define VNINTS_REG	0x44	/* Video n Interrupt Status Register */
+#define VNSI_REG	0x48	/* Video n Scanline Interrupt Register */
+#define VNMTC_REG	0x4C	/* Video n Memory Transfer Control Register */
+#define VNYS_REG	0x50	/* Video n Y Scale Register */
+#define VNXS_REG	0x54	/* Video n X Scale Register */
+#define VNDMR_REG	0x58	/* Video n Data Mode Register */
+#define VNDMR2_REG	0x5C	/* Video n Data Mode Register 2 */
+#define VNUVAOF_REG	0x60	/* Video n UV Address Offset Register */
+#define VNC1A_REG	0x80	/* Video n Coefficient Set C1A Register */
+#define VNC1B_REG	0x84	/* Video n Coefficient Set C1B Register */
+#define VNC1C_REG	0x88	/* Video n Coefficient Set C1C Register */
+#define VNC2A_REG	0x90	/* Video n Coefficient Set C2A Register */
+#define VNC2B_REG	0x94	/* Video n Coefficient Set C2B Register */
+#define VNC2C_REG	0x98	/* Video n Coefficient Set C2C Register */
+#define VNC3A_REG	0xA0	/* Video n Coefficient Set C3A Register */
+#define VNC3B_REG	0xA4	/* Video n Coefficient Set C3B Register */
+#define VNC3C_REG	0xA8	/* Video n Coefficient Set C3C Register */
+#define VNC4A_REG	0xB0	/* Video n Coefficient Set C4A Register */
+#define VNC4B_REG	0xB4	/* Video n Coefficient Set C4B Register */
+#define VNC4C_REG	0xB8	/* Video n Coefficient Set C4C Register */
+#define VNC5A_REG	0xC0	/* Video n Coefficient Set C5A Register */
+#define VNC5B_REG	0xC4	/* Video n Coefficient Set C5B Register */
+#define VNC5C_REG	0xC8	/* Video n Coefficient Set C5C Register */
+#define VNC6A_REG	0xD0	/* Video n Coefficient Set C6A Register */
+#define VNC6B_REG	0xD4	/* Video n Coefficient Set C6B Register */
+#define VNC6C_REG	0xD8	/* Video n Coefficient Set C6C Register */
+#define VNC7A_REG	0xE0	/* Video n Coefficient Set C7A Register */
+#define VNC7B_REG	0xE4	/* Video n Coefficient Set C7B Register */
+#define VNC7C_REG	0xE8	/* Video n Coefficient Set C7C Register */
+#define VNC8A_REG	0xF0	/* Video n Coefficient Set C8A Register */
+#define VNC8B_REG	0xF4	/* Video n Coefficient Set C8B Register */
+#define VNC8C_REG	0xF8	/* Video n Coefficient Set C8C Register */
+
+/* Register bit fields for R-Car VIN */
+/* Video n Main Control Register bits */
+#define VNMC_FOC		(1 << 21)
+#define VNMC_YCAL		(1 << 19)
+#define VNMC_INF_YUV8_BT656	(0 << 16)
+#define VNMC_INF_YUV8_BT601	(1 << 16)
+#define VNMC_INF_YUV10_BT656	(2 << 16)
+#define VNMC_INF_YUV10_BT601	(3 << 16)
+#define VNMC_INF_YUV16		(5 << 16)
+#define VNMC_INF_RGB888		(6 << 16)
+#define VNMC_VUP		(1 << 10)
+#define VNMC_IM_ODD		(0 << 3)
+#define VNMC_IM_ODD_EVEN	(1 << 3)
+#define VNMC_IM_EVEN		(2 << 3)
+#define VNMC_IM_FULL		(3 << 3)
+#define VNMC_BPS		(1 << 1)
+#define VNMC_ME			(1 << 0)
+
+/* Video n Module Status Register bits */
+#define VNMS_FBS_MASK		(3 << 3)
+#define VNMS_FBS_SHIFT		3
+#define VNMS_AV			(1 << 1)
+#define VNMS_CA			(1 << 0)
+
+/* Video n Frame Capture Register bits */
+#define VNFC_C_FRAME		(1 << 1)
+#define VNFC_S_FRAME		(1 << 0)
+
+/* Video n Interrupt Enable Register bits */
+#define VNIE_FIE		(1 << 4)
+#define VNIE_EFE		(1 << 1)
+
+/* Video n Data Mode Register bits */
+#define VNDMR_EXRGB		(1 << 8)
+#define VNDMR_BPSM		(1 << 4)
+#define VNDMR_DTMD_YCSEP	(1 << 1)
+#define VNDMR_DTMD_ARGB1555	(1 << 0)
+
+/* Video n Data Mode Register 2 bits */
+#define VNDMR2_VPS		(1 << 30)
+#define VNDMR2_HPS		(1 << 29)
+#define VNDMR2_FTEV		(1 << 17)
+#define VNDMR2_VLV(n)		((n & 0xf) << 12)
+
+#define VIN_MAX_WIDTH		2048
+#define VIN_MAX_HEIGHT		2048
+
+#define TIMEOUT_MS		100
+
+enum chip_id {
+	RCAR_GEN2,
+	RCAR_H1,
+	RCAR_M1,
+	RCAR_E1,
+};
+
+struct vin_coeff {
+	unsigned short xs_value;
+	u32 coeff_set[24];
+};
+
+static const struct vin_coeff vin_coeff_set[] = {
+	{ 0x0000, {
+		0x00000000,		0x00000000,		0x00000000,
+		0x00000000,		0x00000000,		0x00000000,
+		0x00000000,		0x00000000,		0x00000000,
+		0x00000000,		0x00000000,		0x00000000,
+		0x00000000,		0x00000000,		0x00000000,
+		0x00000000,		0x00000000,		0x00000000,
+		0x00000000,		0x00000000,		0x00000000,
+		0x00000000,		0x00000000,		0x00000000 },
+	},
+	{ 0x1000, {
+		0x000fa400,		0x000fa400,		0x09625902,
+		0x000003f8,		0x00000403,		0x3de0d9f0,
+		0x001fffed,		0x00000804,		0x3cc1f9c3,
+		0x001003de,		0x00000c01,		0x3cb34d7f,
+		0x002003d2,		0x00000c00,		0x3d24a92d,
+		0x00200bca,		0x00000bff,		0x3df600d2,
+		0x002013cc,		0x000007ff,		0x3ed70c7e,
+		0x00100fde,		0x00000000,		0x3f87c036 },
+	},
+	{ 0x1200, {
+		0x002ffff1,		0x002ffff1,		0x02a0a9c8,
+		0x002003e7,		0x001ffffa,		0x000185bc,
+		0x002007dc,		0x000003ff,		0x3e52859c,
+		0x00200bd4,		0x00000002,		0x3d53996b,
+		0x00100fd0,		0x00000403,		0x3d04ad2d,
+		0x00000bd5,		0x00000403,		0x3d35ace7,
+		0x3ff003e4,		0x00000801,		0x3dc674a1,
+		0x3fffe800,		0x00000800,		0x3e76f461 },
+	},
+	{ 0x1400, {
+		0x00100be3,		0x00100be3,		0x04d1359a,
+		0x00000fdb,		0x002003ed,		0x0211fd93,
+		0x00000fd6,		0x002003f4,		0x0002d97b,
+		0x000007d6,		0x002ffffb,		0x3e93b956,
+		0x3ff003da,		0x001003ff,		0x3db49926,
+		0x3fffefe9,		0x00100001,		0x3d655cee,
+		0x3fffd400,		0x00000003,		0x3d65f4b6,
+		0x000fb421,		0x00000402,		0x3dc6547e },
+	},
+	{ 0x1600, {
+		0x00000bdd,		0x00000bdd,		0x06519578,
+		0x3ff007da,		0x00000be3,		0x03c24973,
+		0x3ff003d9,		0x00000be9,		0x01b30d5f,
+		0x3ffff7df,		0x001003f1,		0x0003c542,
+		0x000fdfec,		0x001003f7,		0x3ec4711d,
+		0x000fc400,		0x002ffffd,		0x3df504f1,
+		0x001fa81a,		0x002ffc00,		0x3d957cc2,
+		0x002f8c3c,		0x00100000,		0x3db5c891 },
+	},
+	{ 0x1800, {
+		0x3ff003dc,		0x3ff003dc,		0x0791e558,
+		0x000ff7dd,		0x3ff007de,		0x05328554,
+		0x000fe7e3,		0x3ff00be2,		0x03232546,
+		0x000fd7ee,		0x000007e9,		0x0143bd30,
+		0x001fb800,		0x000007ee,		0x00044511,
+		0x002fa015,		0x000007f4,		0x3ef4bcee,
+		0x002f8832,		0x001003f9,		0x3e4514c7,
+		0x001f7853,		0x001003fd,		0x3de54c9f },
+	},
+	{ 0x1a00, {
+		0x000fefe0,		0x000fefe0,		0x08721d3c,
+		0x001fdbe7,		0x000ffbde,		0x0652a139,
+		0x001fcbf0,		0x000003df,		0x0463292e,
+		0x002fb3ff,		0x3ff007e3,		0x0293a91d,
+		0x002f9c12,		0x3ff00be7,		0x01241905,
+		0x001f8c29,		0x000007ed,		0x3fe470eb,
+		0x000f7c46,		0x000007f2,		0x3f04b8ca,
+		0x3fef7865,		0x000007f6,		0x3e74e4a8 },
+	},
+	{ 0x1c00, {
+		0x001fd3e9,		0x001fd3e9,		0x08f23d26,
+		0x002fbff3,		0x001fe3e4,		0x0712ad23,
+		0x002fa800,		0x000ff3e0,		0x05631d1b,
+		0x001f9810,		0x000ffbe1,		0x03b3890d,
+		0x000f8c23,		0x000003e3,		0x0233e8fa,
+		0x3fef843b,		0x000003e7,		0x00f430e4,
+		0x3fbf8456,		0x3ff00bea,		0x00046cc8,
+		0x3f8f8c72,		0x3ff00bef,		0x3f3490ac },
+	},
+	{ 0x1e00, {
+		0x001fbbf4,		0x001fbbf4,		0x09425112,
+		0x001fa800,		0x002fc7ed,		0x0792b110,
+		0x000f980e,		0x001fdbe6,		0x0613110a,
+		0x3fff8c20,		0x001fe7e3,		0x04a368fd,
+		0x3fcf8c33,		0x000ff7e2,		0x0343b8ed,
+		0x3f9f8c4a,		0x000fffe3,		0x0203f8da,
+		0x3f5f9c61,		0x000003e6,		0x00e428c5,
+		0x3f1fb07b,		0x000003eb,		0x3fe440af },
+	},
+	{ 0x2000, {
+		0x000fa400,		0x000fa400,		0x09625902,
+		0x3fff980c,		0x001fb7f5,		0x0812b0ff,
+		0x3fdf901c,		0x001fc7ed,		0x06b2fcfa,
+		0x3faf902d,		0x001fd3e8,		0x055348f1,
+		0x3f7f983f,		0x001fe3e5,		0x04038ce3,
+		0x3f3fa454,		0x001fefe3,		0x02e3c8d1,
+		0x3f0fb86a,		0x001ff7e4,		0x01c3e8c0,
+		0x3ecfd880,		0x000fffe6,		0x00c404ac },
+	},
+	{ 0x2200, {
+		0x3fdf9c0b,		0x3fdf9c0b,		0x09725cf4,
+		0x3fbf9818,		0x3fffa400,		0x0842a8f1,
+		0x3f8f9827,		0x000fb3f7,		0x0702f0ec,
+		0x3f5fa037,		0x000fc3ef,		0x05d330e4,
+		0x3f2fac49,		0x001fcfea,		0x04a364d9,
+		0x3effc05c,		0x001fdbe7,		0x038394ca,
+		0x3ecfdc6f,		0x001fe7e6,		0x0273b0bb,
+		0x3ea00083,		0x001fefe6,		0x0183c0a9 },
+	},
+	{ 0x2400, {
+		0x3f9fa014,		0x3f9fa014,		0x098260e6,
+		0x3f7f9c23,		0x3fcf9c0a,		0x08629ce5,
+		0x3f4fa431,		0x3fefa400,		0x0742d8e1,
+		0x3f1fb440,		0x3fffb3f8,		0x062310d9,
+		0x3eefc850,		0x000fbbf2,		0x050340d0,
+		0x3ecfe062,		0x000fcbec,		0x041364c2,
+		0x3ea00073,		0x001fd3ea,		0x03037cb5,
+		0x3e902086,		0x001fdfe8,		0x022388a5 },
+	},
+	{ 0x2600, {
+		0x3f5fa81e,		0x3f5fa81e,		0x096258da,
+		0x3f3fac2b,		0x3f8fa412,		0x088290d8,
+		0x3f0fbc38,		0x3fafa408,		0x0772c8d5,
+		0x3eefcc47,		0x3fcfa800,		0x0672f4ce,
+		0x3ecfe456,		0x3fefaffa,		0x05531cc6,
+		0x3eb00066,		0x3fffbbf3,		0x047334bb,
+		0x3ea01c77,		0x000fc7ee,		0x039348ae,
+		0x3ea04486,		0x000fd3eb,		0x02b350a1 },
+	},
+	{ 0x2800, {
+		0x3f2fb426,		0x3f2fb426,		0x094250ce,
+		0x3f0fc032,		0x3f4fac1b,		0x086284cd,
+		0x3eefd040,		0x3f7fa811,		0x0782acc9,
+		0x3ecfe84c,		0x3f9fa807,		0x06a2d8c4,
+		0x3eb0005b,		0x3fbfac00,		0x05b2f4bc,
+		0x3eb0186a,		0x3fdfb3fa,		0x04c308b4,
+		0x3eb04077,		0x3fefbbf4,		0x03f31ca8,
+		0x3ec06884,		0x000fbff2,		0x03031c9e },
+	},
+	{ 0x2a00, {
+		0x3f0fc42d,		0x3f0fc42d,		0x090240c4,
+		0x3eefd439,		0x3f2fb822,		0x08526cc2,
+		0x3edfe845,		0x3f4fb018,		0x078294bf,
+		0x3ec00051,		0x3f6fac0f,		0x06b2b4bb,
+		0x3ec0185f,		0x3f8fac07,		0x05e2ccb4,
+		0x3ec0386b,		0x3fafac00,		0x0502e8ac,
+		0x3ed05c77,		0x3fcfb3fb,		0x0432f0a3,
+		0x3ef08482,		0x3fdfbbf6,		0x0372f898 },
+	},
+	{ 0x2c00, {
+		0x3eefdc31,		0x3eefdc31,		0x08e238b8,
+		0x3edfec3d,		0x3f0fc828,		0x082258b9,
+		0x3ed00049,		0x3f1fc01e,		0x077278b6,
+		0x3ed01455,		0x3f3fb815,		0x06c294b2,
+		0x3ed03460,		0x3f5fb40d,		0x0602acac,
+		0x3ef0506c,		0x3f7fb006,		0x0542c0a4,
+		0x3f107476,		0x3f9fb400,		0x0472c89d,
+		0x3f309c80,		0x3fbfb7fc,		0x03b2cc94 },
+	},
+	{ 0x2e00, {
+		0x3eefec37,		0x3eefec37,		0x088220b0,
+		0x3ee00041,		0x3effdc2d,		0x07f244ae,
+		0x3ee0144c,		0x3f0fd023,		0x07625cad,
+		0x3ef02c57,		0x3f1fc81a,		0x06c274a9,
+		0x3f004861,		0x3f3fbc13,		0x060288a6,
+		0x3f20686b,		0x3f5fb80c,		0x05529c9e,
+		0x3f408c74,		0x3f6fb805,		0x04b2ac96,
+		0x3f80ac7e,		0x3f8fb800,		0x0402ac8e },
+	},
+	{ 0x3000, {
+		0x3ef0003a,		0x3ef0003a,		0x084210a6,
+		0x3ef01045,		0x3effec32,		0x07b228a7,
+		0x3f00284e,		0x3f0fdc29,		0x073244a4,
+		0x3f104058,		0x3f0fd420,		0x06a258a2,
+		0x3f305c62,		0x3f2fc818,		0x0612689d,
+		0x3f508069,		0x3f3fc011,		0x05728496,
+		0x3f80a072,		0x3f4fc00a,		0x04d28c90,
+		0x3fc0c07b,		0x3f6fbc04,		0x04429088 },
+	},
+	{ 0x3200, {
+		0x3f00103e,		0x3f00103e,		0x07f1fc9e,
+		0x3f102447,		0x3f000035,		0x0782149d,
+		0x3f203c4f,		0x3f0ff02c,		0x07122c9c,
+		0x3f405458,		0x3f0fe424,		0x06924099,
+		0x3f607061,		0x3f1fd41d,		0x06024c97,
+		0x3f909068,		0x3f2fcc16,		0x05726490,
+		0x3fc0b070,		0x3f3fc80f,		0x04f26c8a,
+		0x0000d077,		0x3f4fc409,		0x04627484 },
+	},
+	{ 0x3400, {
+		0x3f202040,		0x3f202040,		0x07a1e898,
+		0x3f303449,		0x3f100c38,		0x0741fc98,
+		0x3f504c50,		0x3f10002f,		0x06e21495,
+		0x3f706459,		0x3f1ff028,		0x06722492,
+		0x3fa08060,		0x3f1fe421,		0x05f2348f,
+		0x3fd09c67,		0x3f1fdc19,		0x05824c89,
+		0x0000bc6e,		0x3f2fd014,		0x04f25086,
+		0x0040dc74,		0x3f3fcc0d,		0x04825c7f },
+	},
+	{ 0x3600, {
+		0x3f403042,		0x3f403042,		0x0761d890,
+		0x3f504848,		0x3f301c3b,		0x0701f090,
+		0x3f805c50,		0x3f200c33,		0x06a2008f,
+		0x3fa07458,		0x3f10002b,		0x06520c8d,
+		0x3fd0905e,		0x3f1ff424,		0x05e22089,
+		0x0000ac65,		0x3f1fe81d,		0x05823483,
+		0x0030cc6a,		0x3f2fdc18,		0x04f23c81,
+		0x0080e871,		0x3f2fd412,		0x0482407c },
+	},
+	{ 0x3800, {
+		0x3f604043,		0x3f604043,		0x0721c88a,
+		0x3f80544a,		0x3f502c3c,		0x06d1d88a,
+		0x3fb06851,		0x3f301c35,		0x0681e889,
+		0x3fd08456,		0x3f30082f,		0x0611fc88,
+		0x00009c5d,		0x3f200027,		0x05d20884,
+		0x0030b863,		0x3f2ff421,		0x05621880,
+		0x0070d468,		0x3f2fe81b,		0x0502247c,
+		0x00c0ec6f,		0x3f2fe015,		0x04a22877 },
+	},
+	{ 0x3a00, {
+		0x3f904c44,		0x3f904c44,		0x06e1b884,
+		0x3fb0604a,		0x3f70383e,		0x0691c885,
+		0x3fe07451,		0x3f502c36,		0x0661d483,
+		0x00009055,		0x3f401831,		0x0601ec81,
+		0x0030a85b,		0x3f300c2a,		0x05b1f480,
+		0x0070c061,		0x3f300024,		0x0562047a,
+		0x00b0d867,		0x3f3ff41e,		0x05020c77,
+		0x00f0f46b,		0x3f2fec19,		0x04a21474 },
+	},
+	{ 0x3c00, {
+		0x3fb05c43,		0x3fb05c43,		0x06c1b07e,
+		0x3fe06c4b,		0x3f902c3f,		0x0681c081,
+		0x0000844f,		0x3f703838,		0x0631cc7d,
+		0x00309855,		0x3f602433,		0x05d1d47e,
+		0x0060b459,		0x3f50142e,		0x0581e47b,
+		0x00a0c85f,		0x3f400828,		0x0531f078,
+		0x00e0e064,		0x3f300021,		0x0501fc73,
+		0x00b0fc6a,		0x3f3ff41d,		0x04a20873 },
+	},
+	{ 0x3e00, {
+		0x3fe06444,		0x3fe06444,		0x0681a07a,
+		0x00007849,		0x3fc0503f,		0x0641b07a,
+		0x0020904d,		0x3fa0403a,		0x05f1c07a,
+		0x0060a453,		0x3f803034,		0x05c1c878,
+		0x0090b858,		0x3f70202f,		0x0571d477,
+		0x00d0d05d,		0x3f501829,		0x0531e073,
+		0x0110e462,		0x3f500825,		0x04e1e471,
+		0x01510065,		0x3f40001f,		0x04a1f06d },
+	},
+	{ 0x4000, {
+		0x00007044,		0x00007044,		0x06519476,
+		0x00208448,		0x3fe05c3f,		0x0621a476,
+		0x0050984d,		0x3fc04c3a,		0x05e1b075,
+		0x0080ac52,		0x3fa03c35,		0x05a1b875,
+		0x00c0c056,		0x3f803030,		0x0561c473,
+		0x0100d45b,		0x3f70202b,		0x0521d46f,
+		0x0140e860,		0x3f601427,		0x04d1d46e,
+		0x01810064,		0x3f500822,		0x0491dc6b },
+	},
+	{ 0x5000, {
+		0x0110a442,		0x0110a442,		0x0551545e,
+		0x0140b045,		0x00e0983f,		0x0531585f,
+		0x0160c047,		0x00c08c3c,		0x0511645e,
+		0x0190cc4a,		0x00908039,		0x04f1685f,
+		0x01c0dc4c,		0x00707436,		0x04d1705e,
+		0x0200e850,		0x00506833,		0x04b1785b,
+		0x0230f453,		0x00305c30,		0x0491805a,
+		0x02710056,		0x0010542d,		0x04718059 },
+	},
+	{ 0x6000, {
+		0x01c0bc40,		0x01c0bc40,		0x04c13052,
+		0x01e0c841,		0x01a0b43d,		0x04c13851,
+		0x0210cc44,		0x0180a83c,		0x04a13453,
+		0x0230d845,		0x0160a03a,		0x04913c52,
+		0x0260e047,		0x01409838,		0x04714052,
+		0x0280ec49,		0x01208c37,		0x04514c50,
+		0x02b0f44b,		0x01008435,		0x04414c50,
+		0x02d1004c,		0x00e07c33,		0x0431544f },
+	},
+	{ 0x7000, {
+		0x0230c83e,		0x0230c83e,		0x04711c4c,
+		0x0250d03f,		0x0210c43c,		0x0471204b,
+		0x0270d840,		0x0200b83c,		0x0451244b,
+		0x0290dc42,		0x01e0b43a,		0x0441244c,
+		0x02b0e443,		0x01c0b038,		0x0441284b,
+		0x02d0ec44,		0x01b0a438,		0x0421304a,
+		0x02f0f445,		0x0190a036,		0x04213449,
+		0x0310f847,		0x01709c34,		0x04213848 },
+	},
+	{ 0x8000, {
+		0x0280d03d,		0x0280d03d,		0x04310c48,
+		0x02a0d43e,		0x0270c83c,		0x04311047,
+		0x02b0dc3e,		0x0250c83a,		0x04311447,
+		0x02d0e040,		0x0240c03a,		0x04211446,
+		0x02e0e840,		0x0220bc39,		0x04111847,
+		0x0300e842,		0x0210b438,		0x04012445,
+		0x0310f043,		0x0200b037,		0x04012045,
+		0x0330f444,		0x01e0ac36,		0x03f12445 },
+	},
+	{ 0xefff, {
+		0x0340dc3a,		0x0340dc3a,		0x03b0ec40,
+		0x0340e03a,		0x0330e039,		0x03c0f03e,
+		0x0350e03b,		0x0330dc39,		0x03c0ec3e,
+		0x0350e43a,		0x0320dc38,		0x03c0f43e,
+		0x0360e43b,		0x0320d839,		0x03b0f03e,
+		0x0360e83b,		0x0310d838,		0x03c0fc3b,
+		0x0370e83b,		0x0310d439,		0x03a0f83d,
+		0x0370e83c,		0x0300d438,		0x03b0fc3c },
+	}
+};
+
+enum rcar_vin_state {
+	STOPPED = 0,
+	RUNNING,
+	STOPPING,
+};
+
+struct rcar_vin_priv {
+	void __iomem			*base;
+	spinlock_t			lock;
+	int				sequence;
+	/* State of the VIN module in capturing mode */
+	enum rcar_vin_state		state;
+	struct soc_camera_host		ici;
+	struct list_head		capture;
+#define MAX_BUFFER_NUM			3
+	struct vb2_v4l2_buffer		*queue_buf[MAX_BUFFER_NUM];
+	struct vb2_alloc_ctx		*alloc_ctx;
+	enum v4l2_field			field;
+	unsigned int			pdata_flags;
+	unsigned int			vb_count;
+	unsigned int			nr_hw_slots;
+	bool				request_to_stop;
+	struct completion		capture_stop;
+	enum chip_id			chip;
+};
+
+#define is_continuous_transfer(priv)	(priv->vb_count > MAX_BUFFER_NUM)
+
+struct rcar_vin_buffer {
+	struct vb2_v4l2_buffer vb;
+	struct list_head		list;
+};
+
+#define to_buf_list(vb2_buffer)	(&container_of(vb2_buffer, \
+						       struct rcar_vin_buffer, \
+						       vb)->list)
+
+struct rcar_vin_cam {
+	/* VIN offsets within the camera output, before the VIN scaler */
+	unsigned int			vin_left;
+	unsigned int			vin_top;
+	/* Client output, as seen by the VIN */
+	unsigned int			width;
+	unsigned int			height;
+	/* User window from S_FMT */
+	unsigned int out_width;
+	unsigned int out_height;
+	/*
+	 * User window from S_CROP / G_CROP, produced by client cropping and
+	 * scaling, VIN scaling and VIN cropping, mapped back onto the client
+	 * input window
+	 */
+	struct v4l2_rect		subrect;
+	/* Camera cropping rectangle */
+	struct v4l2_rect		rect;
+	const struct soc_mbus_pixelfmt	*extra_fmt;
+};
+
+/*
+ * .queue_setup() is called to check whether the driver can accept the requested
+ * number of buffers and to fill in plane sizes for the current frame format if
+ * required
+ */
+static int rcar_vin_videobuf_setup(struct vb2_queue *vq,
+				   const void *parg,
+				   unsigned int *count,
+				   unsigned int *num_planes,
+				   unsigned int sizes[], void *alloc_ctxs[])
+{
+	const struct v4l2_format *fmt = parg;
+	struct soc_camera_device *icd = soc_camera_from_vb2q(vq);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct rcar_vin_priv *priv = ici->priv;
+
+	if (fmt) {
+		const struct soc_camera_format_xlate *xlate;
+		unsigned int bytes_per_line;
+		int ret;
+
+		if (fmt->fmt.pix.sizeimage < icd->sizeimage)
+			return -EINVAL;
+
+		xlate = soc_camera_xlate_by_fourcc(icd,
+						   fmt->fmt.pix.pixelformat);
+		if (!xlate)
+			return -EINVAL;
+		ret = soc_mbus_bytes_per_line(fmt->fmt.pix.width,
+					      xlate->host_fmt);
+		if (ret < 0)
+			return ret;
+
+		bytes_per_line = max_t(u32, fmt->fmt.pix.bytesperline, ret);
+
+		ret = soc_mbus_image_size(xlate->host_fmt, bytes_per_line,
+					  fmt->fmt.pix.height);
+		if (ret < 0)
+			return ret;
+
+		sizes[0] = max_t(u32, fmt->fmt.pix.sizeimage, ret);
+	} else {
+		/* Called from VIDIOC_REQBUFS or in compatibility mode */
+		sizes[0] = icd->sizeimage;
+	}
+
+	alloc_ctxs[0] = priv->alloc_ctx;
+
+	if (!vq->num_buffers)
+		priv->sequence = 0;
+
+	if (!*count)
+		*count = 2;
+	priv->vb_count = *count;
+
+	*num_planes = 1;
+
+	/* Number of hardware slots */
+	if (is_continuous_transfer(priv))
+		priv->nr_hw_slots = MAX_BUFFER_NUM;
+	else
+		priv->nr_hw_slots = 1;
+
+	dev_dbg(icd->parent, "count=%d, size=%u\n", *count, sizes[0]);
+
+	return 0;
+}
+
+static int rcar_vin_setup(struct rcar_vin_priv *priv)
+{
+	struct soc_camera_device *icd = priv->ici.icd;
+	struct rcar_vin_cam *cam = icd->host_priv;
+	u32 vnmc, dmr, interrupts;
+	bool progressive = false, output_is_yuv = false, input_is_yuv = false;
+
+	switch (priv->field) {
+	case V4L2_FIELD_TOP:
+		vnmc = VNMC_IM_ODD;
+		break;
+	case V4L2_FIELD_BOTTOM:
+		vnmc = VNMC_IM_EVEN;
+		break;
+	case V4L2_FIELD_INTERLACED:
+	case V4L2_FIELD_INTERLACED_TB:
+		vnmc = VNMC_IM_FULL;
+		break;
+	case V4L2_FIELD_INTERLACED_BT:
+		vnmc = VNMC_IM_FULL | VNMC_FOC;
+		break;
+	case V4L2_FIELD_NONE:
+		if (is_continuous_transfer(priv)) {
+			vnmc = VNMC_IM_ODD_EVEN;
+			progressive = true;
+		} else {
+			vnmc = VNMC_IM_ODD;
+		}
+		break;
+	default:
+		vnmc = VNMC_IM_ODD;
+		break;
+	}
+
+	/* input interface */
+	switch (icd->current_fmt->code) {
+	case MEDIA_BUS_FMT_YUYV8_1X16:
+		/* BT.601/BT.1358 16bit YCbCr422 */
+		vnmc |= VNMC_INF_YUV16;
+		input_is_yuv = true;
+		break;
+	case MEDIA_BUS_FMT_YUYV8_2X8:
+		/* BT.656 8bit YCbCr422 or BT.601 8bit YCbCr422 */
+		vnmc |= priv->pdata_flags & RCAR_VIN_BT656 ?
+			VNMC_INF_YUV8_BT656 : VNMC_INF_YUV8_BT601;
+		input_is_yuv = true;
+		break;
+	case MEDIA_BUS_FMT_RGB888_1X24:
+		vnmc |= VNMC_INF_RGB888;
+		break;
+	case MEDIA_BUS_FMT_YUYV10_2X10:
+		/* BT.656 10bit YCbCr422 or BT.601 10bit YCbCr422 */
+		vnmc |= priv->pdata_flags & RCAR_VIN_BT656 ?
+			VNMC_INF_YUV10_BT656 : VNMC_INF_YUV10_BT601;
+		input_is_yuv = true;
+		break;
+	default:
+		break;
+	}
+
+	/* output format */
+	switch (icd->current_fmt->host_fmt->fourcc) {
+	case V4L2_PIX_FMT_NV16:
+		iowrite32(ALIGN(cam->width * cam->height, 0x80),
+			  priv->base + VNUVAOF_REG);
+		dmr = VNDMR_DTMD_YCSEP;
+		output_is_yuv = true;
+		break;
+	case V4L2_PIX_FMT_YUYV:
+		dmr = VNDMR_BPSM;
+		output_is_yuv = true;
+		break;
+	case V4L2_PIX_FMT_UYVY:
+		dmr = 0;
+		output_is_yuv = true;
+		break;
+	case V4L2_PIX_FMT_RGB555X:
+		dmr = VNDMR_DTMD_ARGB1555;
+		break;
+	case V4L2_PIX_FMT_RGB565:
+		dmr = 0;
+		break;
+	case V4L2_PIX_FMT_RGB32:
+		if (priv->chip == RCAR_GEN2 || priv->chip == RCAR_H1 ||
+		    priv->chip == RCAR_E1) {
+			dmr = VNDMR_EXRGB;
+			break;
+		}
+	default:
+		dev_warn(icd->parent, "Invalid fourcc format (0x%x)\n",
+			 icd->current_fmt->host_fmt->fourcc);
+		return -EINVAL;
+	}
+
+	/* Always update on field change */
+	vnmc |= VNMC_VUP;
+
+	/* If input and output use the same colorspace, use bypass mode */
+	if (input_is_yuv == output_is_yuv)
+		vnmc |= VNMC_BPS;
+
+	/* progressive or interlaced mode */
+	interrupts = progressive ? VNIE_FIE : VNIE_EFE;
+
+	/* ack interrupts */
+	iowrite32(interrupts, priv->base + VNINTS_REG);
+	/* enable interrupts */
+	iowrite32(interrupts, priv->base + VNIE_REG);
+	/* start capturing */
+	iowrite32(dmr, priv->base + VNDMR_REG);
+	iowrite32(vnmc | VNMC_ME, priv->base + VNMC_REG);
+
+	return 0;
+}
+
+static void rcar_vin_capture(struct rcar_vin_priv *priv)
+{
+	if (is_continuous_transfer(priv))
+		/* Continuous Frame Capture Mode */
+		iowrite32(VNFC_C_FRAME, priv->base + VNFC_REG);
+	else
+		/* Single Frame Capture Mode */
+		iowrite32(VNFC_S_FRAME, priv->base + VNFC_REG);
+}
+
+static void rcar_vin_request_capture_stop(struct rcar_vin_priv *priv)
+{
+	priv->state = STOPPING;
+
+	/* set continuous & single transfer off */
+	iowrite32(0, priv->base + VNFC_REG);
+	/* disable capture (release DMA buffer), reset */
+	iowrite32(ioread32(priv->base + VNMC_REG) & ~VNMC_ME,
+		  priv->base + VNMC_REG);
+
+	/* update the status if stopped already */
+	if (!(ioread32(priv->base + VNMS_REG) & VNMS_CA))
+		priv->state = STOPPED;
+}
+
+static int rcar_vin_get_free_hw_slot(struct rcar_vin_priv *priv)
+{
+	int slot;
+
+	for (slot = 0; slot < priv->nr_hw_slots; slot++)
+		if (priv->queue_buf[slot] == NULL)
+			return slot;
+
+	return -1;
+}
+
+static int rcar_vin_hw_ready(struct rcar_vin_priv *priv)
+{
+	/* Ensure all HW slots are filled */
+	return rcar_vin_get_free_hw_slot(priv) < 0 ? 1 : 0;
+}
+
+/* Moves a buffer from the queue to the HW slots */
+static int rcar_vin_fill_hw_slot(struct rcar_vin_priv *priv)
+{
+	struct vb2_v4l2_buffer *vbuf;
+	dma_addr_t phys_addr_top;
+	int slot;
+
+	if (list_empty(&priv->capture))
+		return 0;
+
+	/* Find a free HW slot */
+	slot = rcar_vin_get_free_hw_slot(priv);
+	if (slot < 0)
+		return 0;
+
+	vbuf = &list_entry(priv->capture.next,
+			struct rcar_vin_buffer, list)->vb;
+	list_del_init(to_buf_list(vbuf));
+	priv->queue_buf[slot] = vbuf;
+	phys_addr_top = vb2_dma_contig_plane_dma_addr(&vbuf->vb2_buf, 0);
+	iowrite32(phys_addr_top, priv->base + VNMB_REG(slot));
+
+	return 1;
+}
+
+static void rcar_vin_videobuf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct rcar_vin_priv *priv = ici->priv;
+	unsigned long size;
+
+	size = icd->sizeimage;
+
+	if (vb2_plane_size(vb, 0) < size) {
+		dev_err(icd->parent, "Buffer #%d too small (%lu < %lu)\n",
+			vb->index, vb2_plane_size(vb, 0), size);
+		goto error;
+	}
+
+	vb2_set_plane_payload(vb, 0, size);
+
+	dev_dbg(icd->parent, "%s (vb=0x%p) 0x%p %lu\n", __func__,
+		vb, vb2_plane_vaddr(vb, 0), vb2_get_plane_payload(vb, 0));
+
+	spin_lock_irq(&priv->lock);
+
+	list_add_tail(to_buf_list(vbuf), &priv->capture);
+	rcar_vin_fill_hw_slot(priv);
+
+	/* If we weren't running, and have enough buffers, start capturing! */
+	if (priv->state != RUNNING && rcar_vin_hw_ready(priv)) {
+		if (rcar_vin_setup(priv)) {
+			/* Submit error */
+			list_del_init(to_buf_list(vbuf));
+			spin_unlock_irq(&priv->lock);
+			goto error;
+		}
+		priv->request_to_stop = false;
+		init_completion(&priv->capture_stop);
+		priv->state = RUNNING;
+		rcar_vin_capture(priv);
+	}
+
+	spin_unlock_irq(&priv->lock);
+
+	return;
+
+error:
+	vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
+}
+
+/*
+ * Wait for capture to stop and all in-flight buffers to be finished with by
+ * the video hardware. This must be called under &priv->lock
+ *
+ */
+static void rcar_vin_wait_stop_streaming(struct rcar_vin_priv *priv)
+{
+	while (priv->state != STOPPED) {
+		/* issue stop if running */
+		if (priv->state == RUNNING)
+			rcar_vin_request_capture_stop(priv);
+
+		/* wait until capturing has been stopped */
+		if (priv->state == STOPPING) {
+			priv->request_to_stop = true;
+			spin_unlock_irq(&priv->lock);
+			if (!wait_for_completion_timeout(
+					&priv->capture_stop,
+					msecs_to_jiffies(TIMEOUT_MS)))
+				priv->state = STOPPED;
+			spin_lock_irq(&priv->lock);
+		}
+	}
+}
+
+static void rcar_vin_stop_streaming(struct vb2_queue *vq)
+{
+	struct soc_camera_device *icd = soc_camera_from_vb2q(vq);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct rcar_vin_priv *priv = ici->priv;
+	struct list_head *buf_head, *tmp;
+	int i;
+
+	spin_lock_irq(&priv->lock);
+	rcar_vin_wait_stop_streaming(priv);
+
+	for (i = 0; i < MAX_BUFFER_NUM; i++) {
+		if (priv->queue_buf[i]) {
+			vb2_buffer_done(&priv->queue_buf[i]->vb2_buf,
+					VB2_BUF_STATE_ERROR);
+			priv->queue_buf[i] = NULL;
+		}
+	}
+
+	list_for_each_safe(buf_head, tmp, &priv->capture) {
+		vb2_buffer_done(&list_entry(buf_head,
+				struct rcar_vin_buffer, list)->vb.vb2_buf,
+				VB2_BUF_STATE_ERROR);
+		list_del_init(buf_head);
+	}
+	spin_unlock_irq(&priv->lock);
+}
+
+static struct vb2_ops rcar_vin_vb2_ops = {
+	.queue_setup	= rcar_vin_videobuf_setup,
+	.buf_queue	= rcar_vin_videobuf_queue,
+	.stop_streaming	= rcar_vin_stop_streaming,
+	.wait_prepare	= vb2_ops_wait_prepare,
+	.wait_finish	= vb2_ops_wait_finish,
+};
+
+static irqreturn_t rcar_vin_irq(int irq, void *data)
+{
+	struct rcar_vin_priv *priv = data;
+	u32 int_status;
+	bool can_run = false, hw_stopped;
+	int slot;
+	unsigned int handled = 0;
+
+	spin_lock(&priv->lock);
+
+	int_status = ioread32(priv->base + VNINTS_REG);
+	if (!int_status)
+		goto done;
+	/* ack interrupts */
+	iowrite32(int_status, priv->base + VNINTS_REG);
+	handled = 1;
+
+	/* nothing to do if capture status is 'STOPPED' */
+	if (priv->state == STOPPED)
+		goto done;
+
+	hw_stopped = !(ioread32(priv->base + VNMS_REG) & VNMS_CA);
+
+	if (!priv->request_to_stop) {
+		if (is_continuous_transfer(priv))
+			slot = (ioread32(priv->base + VNMS_REG) &
+				VNMS_FBS_MASK) >> VNMS_FBS_SHIFT;
+		else
+			slot = 0;
+
+		priv->queue_buf[slot]->field = priv->field;
+		priv->queue_buf[slot]->sequence = priv->sequence++;
+		v4l2_get_timestamp(&priv->queue_buf[slot]->timestamp);
+		vb2_buffer_done(&priv->queue_buf[slot]->vb2_buf,
+				VB2_BUF_STATE_DONE);
+		priv->queue_buf[slot] = NULL;
+
+		if (priv->state != STOPPING)
+			can_run = rcar_vin_fill_hw_slot(priv);
+
+		if (hw_stopped || !can_run) {
+			priv->state = STOPPED;
+		} else if (is_continuous_transfer(priv) &&
+			   list_empty(&priv->capture) &&
+			   priv->state == RUNNING) {
+			/*
+			 * The continuous capturing requires an explicit stop
+			 * operation when there is no buffer to be set into
+			 * the VnMBm registers.
+			 */
+			rcar_vin_request_capture_stop(priv);
+		} else {
+			rcar_vin_capture(priv);
+		}
+
+	} else if (hw_stopped) {
+		priv->state = STOPPED;
+		priv->request_to_stop = false;
+		complete(&priv->capture_stop);
+	}
+
+done:
+	spin_unlock(&priv->lock);
+
+	return IRQ_RETVAL(handled);
+}
+
+static int rcar_vin_add_device(struct soc_camera_device *icd)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct rcar_vin_priv *priv = ici->priv;
+	int i;
+
+	for (i = 0; i < MAX_BUFFER_NUM; i++)
+		priv->queue_buf[i] = NULL;
+
+	pm_runtime_get_sync(ici->v4l2_dev.dev);
+
+	dev_dbg(icd->parent, "R-Car VIN driver attached to camera %d\n",
+		icd->devnum);
+
+	return 0;
+}
+
+static void rcar_vin_remove_device(struct soc_camera_device *icd)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct rcar_vin_priv *priv = ici->priv;
+	struct vb2_v4l2_buffer *vbuf;
+	int i;
+
+	/* disable capture, disable interrupts */
+	iowrite32(ioread32(priv->base + VNMC_REG) & ~VNMC_ME,
+		  priv->base + VNMC_REG);
+	iowrite32(0, priv->base + VNIE_REG);
+
+	priv->state = STOPPED;
+	priv->request_to_stop = false;
+
+	/* make sure active buffer is cancelled */
+	spin_lock_irq(&priv->lock);
+	for (i = 0; i < MAX_BUFFER_NUM; i++) {
+		vbuf = priv->queue_buf[i];
+		if (vbuf) {
+			list_del_init(to_buf_list(vbuf));
+			vb2_buffer_done(&vbuf->vb2_buf, VB2_BUF_STATE_ERROR);
+		}
+	}
+	spin_unlock_irq(&priv->lock);
+
+	pm_runtime_put(ici->v4l2_dev.dev);
+
+	dev_dbg(icd->parent, "R-Car VIN driver detached from camera %d\n",
+		icd->devnum);
+}
+
+static void set_coeff(struct rcar_vin_priv *priv, unsigned short xs)
+{
+	int i;
+	const struct vin_coeff *p_prev_set = NULL;
+	const struct vin_coeff *p_set = NULL;
+
+	/* Look for suitable coefficient values */
+	for (i = 0; i < ARRAY_SIZE(vin_coeff_set); i++) {
+		p_prev_set = p_set;
+		p_set = &vin_coeff_set[i];
+
+		if (xs < p_set->xs_value)
+			break;
+	}
+
+	/* Use previous value if its XS value is closer */
+	if (p_prev_set && p_set &&
+	    xs - p_prev_set->xs_value < p_set->xs_value - xs)
+		p_set = p_prev_set;
+
+	/* Set coefficient registers */
+	iowrite32(p_set->coeff_set[0], priv->base + VNC1A_REG);
+	iowrite32(p_set->coeff_set[1], priv->base + VNC1B_REG);
+	iowrite32(p_set->coeff_set[2], priv->base + VNC1C_REG);
+
+	iowrite32(p_set->coeff_set[3], priv->base + VNC2A_REG);
+	iowrite32(p_set->coeff_set[4], priv->base + VNC2B_REG);
+	iowrite32(p_set->coeff_set[5], priv->base + VNC2C_REG);
+
+	iowrite32(p_set->coeff_set[6], priv->base + VNC3A_REG);
+	iowrite32(p_set->coeff_set[7], priv->base + VNC3B_REG);
+	iowrite32(p_set->coeff_set[8], priv->base + VNC3C_REG);
+
+	iowrite32(p_set->coeff_set[9], priv->base + VNC4A_REG);
+	iowrite32(p_set->coeff_set[10], priv->base + VNC4B_REG);
+	iowrite32(p_set->coeff_set[11], priv->base + VNC4C_REG);
+
+	iowrite32(p_set->coeff_set[12], priv->base + VNC5A_REG);
+	iowrite32(p_set->coeff_set[13], priv->base + VNC5B_REG);
+	iowrite32(p_set->coeff_set[14], priv->base + VNC5C_REG);
+
+	iowrite32(p_set->coeff_set[15], priv->base + VNC6A_REG);
+	iowrite32(p_set->coeff_set[16], priv->base + VNC6B_REG);
+	iowrite32(p_set->coeff_set[17], priv->base + VNC6C_REG);
+
+	iowrite32(p_set->coeff_set[18], priv->base + VNC7A_REG);
+	iowrite32(p_set->coeff_set[19], priv->base + VNC7B_REG);
+	iowrite32(p_set->coeff_set[20], priv->base + VNC7C_REG);
+
+	iowrite32(p_set->coeff_set[21], priv->base + VNC8A_REG);
+	iowrite32(p_set->coeff_set[22], priv->base + VNC8B_REG);
+	iowrite32(p_set->coeff_set[23], priv->base + VNC8C_REG);
+}
+
+/* rect is guaranteed to not exceed the scaled camera rectangle */
+static int rcar_vin_set_rect(struct soc_camera_device *icd)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct rcar_vin_cam *cam = icd->host_priv;
+	struct rcar_vin_priv *priv = ici->priv;
+	unsigned int left_offset, top_offset;
+	unsigned char dsize = 0;
+	struct v4l2_rect *cam_subrect = &cam->subrect;
+	u32 value;
+
+	dev_dbg(icd->parent, "Crop %ux%u@%u:%u\n",
+		icd->user_width, icd->user_height, cam->vin_left, cam->vin_top);
+
+	left_offset = cam->vin_left;
+	top_offset = cam->vin_top;
+
+	if (icd->current_fmt->host_fmt->fourcc == V4L2_PIX_FMT_RGB32 &&
+	    priv->chip == RCAR_E1)
+		dsize = 1;
+
+	dev_dbg(icd->parent, "Cam %ux%u@%u:%u\n",
+		cam->width, cam->height, cam->vin_left, cam->vin_top);
+	dev_dbg(icd->parent, "Cam subrect %ux%u@%u:%u\n",
+		cam_subrect->width, cam_subrect->height,
+		cam_subrect->left, cam_subrect->top);
+
+	/* Set Start/End Pixel/Line Pre-Clip */
+	iowrite32(left_offset << dsize, priv->base + VNSPPRC_REG);
+	iowrite32((left_offset + cam_subrect->width - 1) << dsize,
+		  priv->base + VNEPPRC_REG);
+	switch (priv->field) {
+	case V4L2_FIELD_INTERLACED:
+	case V4L2_FIELD_INTERLACED_TB:
+	case V4L2_FIELD_INTERLACED_BT:
+		iowrite32(top_offset / 2, priv->base + VNSLPRC_REG);
+		iowrite32((top_offset + cam_subrect->height) / 2 - 1,
+			  priv->base + VNELPRC_REG);
+		break;
+	default:
+		iowrite32(top_offset, priv->base + VNSLPRC_REG);
+		iowrite32(top_offset + cam_subrect->height - 1,
+			  priv->base + VNELPRC_REG);
+		break;
+	}
+
+	/* Set scaling coefficient */
+	value = 0;
+	if (cam_subrect->height != cam->out_height)
+		value = (4096 * cam_subrect->height) / cam->out_height;
+	dev_dbg(icd->parent, "YS Value: %x\n", value);
+	iowrite32(value, priv->base + VNYS_REG);
+
+	value = 0;
+	if (cam_subrect->width != cam->out_width)
+		value = (4096 * cam_subrect->width) / cam->out_width;
+
+	/* Horizontal upscaling is up to double size */
+	if (0 < value && value < 2048)
+		value = 2048;
+
+	dev_dbg(icd->parent, "XS Value: %x\n", value);
+	iowrite32(value, priv->base + VNXS_REG);
+
+	/* Horizontal upscaling is carried out by scaling down from double size */
+	if (value < 4096)
+		value *= 2;
+
+	set_coeff(priv, value);
+
+	/* Set Start/End Pixel/Line Post-Clip */
+	iowrite32(0, priv->base + VNSPPOC_REG);
+	iowrite32(0, priv->base + VNSLPOC_REG);
+	iowrite32((cam->out_width - 1) << dsize, priv->base + VNEPPOC_REG);
+	switch (priv->field) {
+	case V4L2_FIELD_INTERLACED:
+	case V4L2_FIELD_INTERLACED_TB:
+	case V4L2_FIELD_INTERLACED_BT:
+		iowrite32(cam->out_height / 2 - 1,
+			  priv->base + VNELPOC_REG);
+		break;
+	default:
+		iowrite32(cam->out_height - 1, priv->base + VNELPOC_REG);
+		break;
+	}
+
+	iowrite32(ALIGN(cam->out_width, 0x10), priv->base + VNIS_REG);
+
+	return 0;
+}
+
+static void capture_stop_preserve(struct rcar_vin_priv *priv, u32 *vnmc)
+{
+	*vnmc = ioread32(priv->base + VNMC_REG);
+	/* module disable */
+	iowrite32(*vnmc & ~VNMC_ME, priv->base + VNMC_REG);
+}
+
+static void capture_restore(struct rcar_vin_priv *priv, u32 vnmc)
+{
+	unsigned long timeout = jiffies + 10 * HZ;
+
+	/*
+	 * Wait until the end of the current frame. It can take a long time,
+	 * but if it has been aborted by a MRST1 reset, it should exit sooner.
+	 */
+	while ((ioread32(priv->base + VNMS_REG) & VNMS_AV) &&
+		time_before(jiffies, timeout))
+		msleep(1);
+
+	if (time_after(jiffies, timeout)) {
+		dev_err(priv->ici.v4l2_dev.dev,
+			"Timeout waiting for frame end! Interface problem?\n");
+		return;
+	}
+
+	iowrite32(vnmc, priv->base + VNMC_REG);
+}
+
+#define VIN_MBUS_FLAGS	(V4L2_MBUS_MASTER |		\
+			 V4L2_MBUS_PCLK_SAMPLE_RISING |	\
+			 V4L2_MBUS_HSYNC_ACTIVE_HIGH |	\
+			 V4L2_MBUS_HSYNC_ACTIVE_LOW |	\
+			 V4L2_MBUS_VSYNC_ACTIVE_HIGH |	\
+			 V4L2_MBUS_VSYNC_ACTIVE_LOW |	\
+			 V4L2_MBUS_DATA_ACTIVE_HIGH)
+
+static int rcar_vin_set_bus_param(struct soc_camera_device *icd)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct rcar_vin_priv *priv = ici->priv;
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct v4l2_mbus_config cfg;
+	unsigned long common_flags;
+	u32 vnmc;
+	u32 val;
+	int ret;
+
+	capture_stop_preserve(priv, &vnmc);
+
+	ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
+	if (!ret) {
+		common_flags = soc_mbus_config_compatible(&cfg, VIN_MBUS_FLAGS);
+		if (!common_flags) {
+			dev_warn(icd->parent,
+				 "MBUS flags incompatible: camera 0x%x, host 0x%x\n",
+				 cfg.flags, VIN_MBUS_FLAGS);
+			return -EINVAL;
+		}
+	} else if (ret != -ENOIOCTLCMD) {
+		return ret;
+	} else {
+		common_flags = VIN_MBUS_FLAGS;
+	}
+
+	/* Make choises, based on platform preferences */
+	if ((common_flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH) &&
+	    (common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)) {
+		if (priv->pdata_flags & RCAR_VIN_HSYNC_ACTIVE_LOW)
+			common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_HIGH;
+		else
+			common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_LOW;
+	}
+
+	if ((common_flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH) &&
+	    (common_flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)) {
+		if (priv->pdata_flags & RCAR_VIN_VSYNC_ACTIVE_LOW)
+			common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_HIGH;
+		else
+			common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_LOW;
+	}
+
+	cfg.flags = common_flags;
+	ret = v4l2_subdev_call(sd, video, s_mbus_config, &cfg);
+	if (ret < 0 && ret != -ENOIOCTLCMD)
+		return ret;
+
+	val = VNDMR2_FTEV | VNDMR2_VLV(1);
+	if (!(common_flags & V4L2_MBUS_VSYNC_ACTIVE_LOW))
+		val |= VNDMR2_VPS;
+	if (!(common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW))
+		val |= VNDMR2_HPS;
+	iowrite32(val, priv->base + VNDMR2_REG);
+
+	ret = rcar_vin_set_rect(icd);
+	if (ret < 0)
+		return ret;
+
+	capture_restore(priv, vnmc);
+
+	return 0;
+}
+
+static int rcar_vin_try_bus_param(struct soc_camera_device *icd,
+				  unsigned char buswidth)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct v4l2_mbus_config cfg;
+	int ret;
+
+	ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
+	if (ret == -ENOIOCTLCMD)
+		return 0;
+	else if (ret)
+		return ret;
+
+	if (buswidth > 24)
+		return -EINVAL;
+
+	/* check is there common mbus flags */
+	ret = soc_mbus_config_compatible(&cfg, VIN_MBUS_FLAGS);
+	if (ret)
+		return 0;
+
+	dev_warn(icd->parent,
+		"MBUS flags incompatible: camera 0x%x, host 0x%x\n",
+		 cfg.flags, VIN_MBUS_FLAGS);
+
+	return -EINVAL;
+}
+
+static bool rcar_vin_packing_supported(const struct soc_mbus_pixelfmt *fmt)
+{
+	return	fmt->packing == SOC_MBUS_PACKING_NONE ||
+		(fmt->bits_per_sample > 8 &&
+		 fmt->packing == SOC_MBUS_PACKING_EXTEND16);
+}
+
+static const struct soc_mbus_pixelfmt rcar_vin_formats[] = {
+	{
+		.fourcc			= V4L2_PIX_FMT_NV16,
+		.name			= "NV16",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PLANAR_Y_C,
+	},
+	{
+		.fourcc			= V4L2_PIX_FMT_YUYV,
+		.name			= "YUYV",
+		.bits_per_sample	= 16,
+		.packing		= SOC_MBUS_PACKING_NONE,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+	{
+		.fourcc			= V4L2_PIX_FMT_UYVY,
+		.name			= "UYVY",
+		.bits_per_sample	= 16,
+		.packing		= SOC_MBUS_PACKING_NONE,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+	{
+		.fourcc			= V4L2_PIX_FMT_RGB565,
+		.name			= "RGB565",
+		.bits_per_sample	= 16,
+		.packing		= SOC_MBUS_PACKING_NONE,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+	{
+		.fourcc			= V4L2_PIX_FMT_RGB555X,
+		.name			= "ARGB1555",
+		.bits_per_sample	= 16,
+		.packing		= SOC_MBUS_PACKING_NONE,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+	{
+		.fourcc			= V4L2_PIX_FMT_RGB32,
+		.name			= "RGB888",
+		.bits_per_sample	= 32,
+		.packing		= SOC_MBUS_PACKING_NONE,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+};
+
+static int rcar_vin_get_formats(struct soc_camera_device *icd, unsigned int idx,
+				struct soc_camera_format_xlate *xlate)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct device *dev = icd->parent;
+	int ret, k, n;
+	int formats = 0;
+	struct rcar_vin_cam *cam;
+	struct v4l2_subdev_mbus_code_enum code = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.index = idx,
+	};
+	const struct soc_mbus_pixelfmt *fmt;
+
+	ret = v4l2_subdev_call(sd, pad, enum_mbus_code, NULL, &code);
+	if (ret < 0)
+		return 0;
+
+	fmt = soc_mbus_get_fmtdesc(code.code);
+	if (!fmt) {
+		dev_warn(dev, "unsupported format code #%u: %d\n", idx, code.code);
+		return 0;
+	}
+
+	ret = rcar_vin_try_bus_param(icd, fmt->bits_per_sample);
+	if (ret < 0)
+		return 0;
+
+	if (!icd->host_priv) {
+		struct v4l2_subdev_format fmt = {
+			.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		};
+		struct v4l2_mbus_framefmt *mf = &fmt.format;
+		struct v4l2_rect rect;
+		struct device *dev = icd->parent;
+		int shift;
+
+		ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &fmt);
+		if (ret < 0)
+			return ret;
+
+		/* Cache current client geometry */
+		ret = soc_camera_client_g_rect(sd, &rect);
+		if (ret == -ENOIOCTLCMD) {
+			/* Sensor driver doesn't support cropping */
+			rect.left = 0;
+			rect.top = 0;
+			rect.width = mf->width;
+			rect.height = mf->height;
+		} else if (ret < 0) {
+			return ret;
+		}
+
+		/*
+		 * If sensor proposes too large format then try smaller ones:
+		 * 1280x960, 640x480, 320x240
+		 */
+		for (shift = 0; shift < 3; shift++) {
+			if (mf->width <= VIN_MAX_WIDTH &&
+			    mf->height <= VIN_MAX_HEIGHT)
+				break;
+
+			mf->width = 1280 >> shift;
+			mf->height = 960 >> shift;
+			ret = v4l2_device_call_until_err(sd->v4l2_dev,
+							 soc_camera_grp_id(icd),
+							 pad, set_fmt, NULL,
+							 &fmt);
+			if (ret < 0)
+				return ret;
+		}
+
+		if (shift == 3) {
+			dev_err(dev,
+				"Failed to configure the client below %ux%u\n",
+				mf->width, mf->height);
+			return -EIO;
+		}
+
+		dev_dbg(dev, "camera fmt %ux%u\n", mf->width, mf->height);
+
+		cam = kzalloc(sizeof(*cam), GFP_KERNEL);
+		if (!cam)
+			return -ENOMEM;
+		/*
+		 * We are called with current camera crop,
+		 * initialise subrect with it
+		 */
+		cam->rect = rect;
+		cam->subrect = rect;
+		cam->width = mf->width;
+		cam->height = mf->height;
+		cam->out_width	= mf->width;
+		cam->out_height	= mf->height;
+
+		icd->host_priv = cam;
+	} else {
+		cam = icd->host_priv;
+	}
+
+	/* Beginning of a pass */
+	if (!idx)
+		cam->extra_fmt = NULL;
+
+	switch (code.code) {
+	case MEDIA_BUS_FMT_YUYV8_1X16:
+	case MEDIA_BUS_FMT_YUYV8_2X8:
+	case MEDIA_BUS_FMT_YUYV10_2X10:
+	case MEDIA_BUS_FMT_RGB888_1X24:
+		if (cam->extra_fmt)
+			break;
+
+		/* Add all our formats that can be generated by VIN */
+		cam->extra_fmt = rcar_vin_formats;
+
+		n = ARRAY_SIZE(rcar_vin_formats);
+		formats += n;
+		for (k = 0; xlate && k < n; k++, xlate++) {
+			xlate->host_fmt = &rcar_vin_formats[k];
+			xlate->code = code.code;
+			dev_dbg(dev, "Providing format %s using code %d\n",
+				rcar_vin_formats[k].name, code.code);
+		}
+		break;
+	default:
+		if (!rcar_vin_packing_supported(fmt))
+			return 0;
+
+		dev_dbg(dev, "Providing format %s in pass-through mode\n",
+			fmt->name);
+		break;
+	}
+
+	/* Generic pass-through */
+	formats++;
+	if (xlate) {
+		xlate->host_fmt = fmt;
+		xlate->code = code.code;
+		xlate++;
+	}
+
+	return formats;
+}
+
+static void rcar_vin_put_formats(struct soc_camera_device *icd)
+{
+	kfree(icd->host_priv);
+	icd->host_priv = NULL;
+}
+
+static int rcar_vin_set_crop(struct soc_camera_device *icd,
+			     const struct v4l2_crop *a)
+{
+	struct v4l2_crop a_writable = *a;
+	const struct v4l2_rect *rect = &a_writable.c;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct rcar_vin_priv *priv = ici->priv;
+	struct v4l2_crop cam_crop;
+	struct rcar_vin_cam *cam = icd->host_priv;
+	struct v4l2_rect *cam_rect = &cam_crop.c;
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct device *dev = icd->parent;
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_mbus_framefmt *mf = &fmt.format;
+	u32 vnmc;
+	int ret, i;
+
+	dev_dbg(dev, "S_CROP(%ux%u@%u:%u)\n", rect->width, rect->height,
+		rect->left, rect->top);
+
+	/* During camera cropping its output window can change too, stop VIN */
+	capture_stop_preserve(priv, &vnmc);
+	dev_dbg(dev, "VNMC_REG 0x%x\n", vnmc);
+
+	/* Apply iterative camera S_CROP for new input window. */
+	ret = soc_camera_client_s_crop(sd, &a_writable, &cam_crop,
+				       &cam->rect, &cam->subrect);
+	if (ret < 0)
+		return ret;
+
+	dev_dbg(dev, "camera cropped to %ux%u@%u:%u\n",
+		cam_rect->width, cam_rect->height,
+		cam_rect->left, cam_rect->top);
+
+	/* On success cam_crop contains current camera crop */
+
+	/* Retrieve camera output window */
+	ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &fmt);
+	if (ret < 0)
+		return ret;
+
+	if (mf->width > VIN_MAX_WIDTH || mf->height > VIN_MAX_HEIGHT)
+		return -EINVAL;
+
+	/* Cache camera output window */
+	cam->width = mf->width;
+	cam->height = mf->height;
+
+	icd->user_width  = cam->width;
+	icd->user_height = cam->height;
+
+	cam->vin_left = rect->left & ~1;
+	cam->vin_top = rect->top & ~1;
+
+	/* Use VIN cropping to crop to the new window. */
+	ret = rcar_vin_set_rect(icd);
+	if (ret < 0)
+		return ret;
+
+	cam->subrect = *rect;
+
+	dev_dbg(dev, "VIN cropped to %ux%u@%u:%u\n",
+		icd->user_width, icd->user_height,
+		cam->vin_left, cam->vin_top);
+
+	/* Restore capture */
+	for (i = 0; i < MAX_BUFFER_NUM; i++) {
+		if (priv->queue_buf[i] && priv->state == STOPPED) {
+			vnmc |= VNMC_ME;
+			break;
+		}
+	}
+	capture_restore(priv, vnmc);
+
+	/* Even if only camera cropping succeeded */
+	return ret;
+}
+
+static int rcar_vin_get_crop(struct soc_camera_device *icd,
+			     struct v4l2_crop *a)
+{
+	struct rcar_vin_cam *cam = icd->host_priv;
+
+	a->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	a->c = cam->subrect;
+
+	return 0;
+}
+
+/* Similar to set_crop multistage iterative algorithm */
+static int rcar_vin_set_fmt(struct soc_camera_device *icd,
+			    struct v4l2_format *f)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct rcar_vin_priv *priv = ici->priv;
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct rcar_vin_cam *cam = icd->host_priv;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct v4l2_mbus_framefmt mf;
+	struct device *dev = icd->parent;
+	__u32 pixfmt = pix->pixelformat;
+	const struct soc_camera_format_xlate *xlate;
+	unsigned int vin_sub_width = 0, vin_sub_height = 0;
+	int ret;
+	bool can_scale;
+	enum v4l2_field field;
+	v4l2_std_id std;
+
+	dev_dbg(dev, "S_FMT(pix=0x%x, %ux%u)\n",
+		pixfmt, pix->width, pix->height);
+
+	switch (pix->field) {
+	default:
+		pix->field = V4L2_FIELD_NONE;
+		/* fall-through */
+	case V4L2_FIELD_NONE:
+	case V4L2_FIELD_TOP:
+	case V4L2_FIELD_BOTTOM:
+	case V4L2_FIELD_INTERLACED_TB:
+	case V4L2_FIELD_INTERLACED_BT:
+		field = pix->field;
+		break;
+	case V4L2_FIELD_INTERLACED:
+		/* Query for standard if not explicitly mentioned _TB/_BT */
+		ret = v4l2_subdev_call(sd, video, querystd, &std);
+		if (ret == -ENOIOCTLCMD) {
+			field = V4L2_FIELD_NONE;
+		} else if (ret < 0) {
+			return ret;
+		} else {
+			field = std & V4L2_STD_625_50 ?
+				V4L2_FIELD_INTERLACED_TB :
+				V4L2_FIELD_INTERLACED_BT;
+		}
+		break;
+	}
+
+	xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
+	if (!xlate) {
+		dev_warn(dev, "Format %x not found\n", pixfmt);
+		return -EINVAL;
+	}
+	/* Calculate client output geometry */
+	soc_camera_calc_client_output(icd, &cam->rect, &cam->subrect, pix, &mf,
+				      12);
+	mf.field = pix->field;
+	mf.colorspace = pix->colorspace;
+	mf.code	 = xlate->code;
+
+	switch (pixfmt) {
+	case V4L2_PIX_FMT_RGB32:
+		can_scale = priv->chip != RCAR_E1;
+		break;
+	case V4L2_PIX_FMT_UYVY:
+	case V4L2_PIX_FMT_YUYV:
+	case V4L2_PIX_FMT_RGB565:
+	case V4L2_PIX_FMT_RGB555X:
+		can_scale = true;
+		break;
+	default:
+		can_scale = false;
+		break;
+	}
+
+	dev_dbg(dev, "request camera output %ux%u\n", mf.width, mf.height);
+
+	ret = soc_camera_client_scale(icd, &cam->rect, &cam->subrect,
+				      &mf, &vin_sub_width, &vin_sub_height,
+				      can_scale, 12);
+
+	/* Done with the camera. Now see if we can improve the result */
+	dev_dbg(dev, "Camera %d fmt %ux%u, requested %ux%u\n",
+		ret, mf.width, mf.height, pix->width, pix->height);
+
+	if (ret == -ENOIOCTLCMD)
+		dev_dbg(dev, "Sensor doesn't support scaling\n");
+	else if (ret < 0)
+		return ret;
+
+	if (mf.code != xlate->code)
+		return -EINVAL;
+
+	/* Prepare VIN crop */
+	cam->width = mf.width;
+	cam->height = mf.height;
+
+	/* Use VIN scaling to scale to the requested user window. */
+
+	/* We cannot scale up */
+	if (pix->width > vin_sub_width)
+		vin_sub_width = pix->width;
+
+	if (pix->height > vin_sub_height)
+		vin_sub_height = pix->height;
+
+	pix->colorspace = mf.colorspace;
+
+	if (!can_scale) {
+		pix->width = vin_sub_width;
+		pix->height = vin_sub_height;
+	}
+
+	/*
+	 * We have calculated CFLCR, the actual configuration will be performed
+	 * in rcar_vin_set_bus_param()
+	 */
+
+	dev_dbg(dev, "W: %u : %u, H: %u : %u\n",
+		vin_sub_width, pix->width, vin_sub_height, pix->height);
+
+	cam->out_width = pix->width;
+	cam->out_height = pix->height;
+
+	icd->current_fmt = xlate;
+
+	priv->field = field;
+
+	return 0;
+}
+
+static int rcar_vin_try_fmt(struct soc_camera_device *icd,
+			    struct v4l2_format *f)
+{
+	const struct soc_camera_format_xlate *xlate;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct v4l2_subdev_pad_config pad_cfg;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+	};
+	struct v4l2_mbus_framefmt *mf = &format.format;
+	__u32 pixfmt = pix->pixelformat;
+	int width, height;
+	int ret;
+
+	xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
+	if (!xlate) {
+		xlate = icd->current_fmt;
+		dev_dbg(icd->parent, "Format %x not found, keeping %x\n",
+			pixfmt, xlate->host_fmt->fourcc);
+		pixfmt = xlate->host_fmt->fourcc;
+		pix->pixelformat = pixfmt;
+		pix->colorspace = icd->colorspace;
+	}
+
+	/* FIXME: calculate using depth and bus width */
+	v4l_bound_align_image(&pix->width, 2, VIN_MAX_WIDTH, 1,
+			      &pix->height, 4, VIN_MAX_HEIGHT, 2, 0);
+
+	width = pix->width;
+	height = pix->height;
+
+	/* let soc-camera calculate these values */
+	pix->bytesperline = 0;
+	pix->sizeimage = 0;
+
+	/* limit to sensor capabilities */
+	mf->width = pix->width;
+	mf->height = pix->height;
+	mf->field = pix->field;
+	mf->code = xlate->code;
+	mf->colorspace = pix->colorspace;
+
+	ret = v4l2_device_call_until_err(sd->v4l2_dev, soc_camera_grp_id(icd),
+					 pad, set_fmt, &pad_cfg, &format);
+	if (ret < 0)
+		return ret;
+
+	/* Adjust only if VIN cannot scale */
+	if (pix->width > mf->width * 2)
+		pix->width = mf->width * 2;
+	if (pix->height > mf->height * 3)
+		pix->height = mf->height * 3;
+
+	pix->field = mf->field;
+	pix->colorspace = mf->colorspace;
+
+	if (pixfmt == V4L2_PIX_FMT_NV16) {
+		/* FIXME: check against rect_max after converting soc-camera */
+		/* We can scale precisely, need a bigger image from camera */
+		if (pix->width < width || pix->height < height) {
+			/*
+			 * We presume, the sensor behaves sanely, i.e. if
+			 * requested a bigger rectangle, it will not return a
+			 * smaller one.
+			 */
+			mf->width = VIN_MAX_WIDTH;
+			mf->height = VIN_MAX_HEIGHT;
+			ret = v4l2_device_call_until_err(sd->v4l2_dev,
+							 soc_camera_grp_id(icd),
+							 pad, set_fmt, &pad_cfg,
+							 &format);
+			if (ret < 0) {
+				dev_err(icd->parent,
+					"client try_fmt() = %d\n", ret);
+				return ret;
+			}
+		}
+		/* We will scale exactly */
+		if (mf->width > width)
+			pix->width = width;
+		if (mf->height > height)
+			pix->height = height;
+	}
+
+	return ret;
+}
+
+static unsigned int rcar_vin_poll(struct file *file, poll_table *pt)
+{
+	struct soc_camera_device *icd = file->private_data;
+
+	return vb2_poll(&icd->vb2_vidq, file, pt);
+}
+
+static int rcar_vin_querycap(struct soc_camera_host *ici,
+			     struct v4l2_capability *cap)
+{
+	strlcpy(cap->card, "R_Car_VIN", sizeof(cap->card));
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s%d", DRV_NAME, ici->nr);
+
+	return 0;
+}
+
+static int rcar_vin_init_videobuf2(struct vb2_queue *vq,
+				   struct soc_camera_device *icd)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+
+	vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	vq->io_modes = VB2_MMAP | VB2_USERPTR;
+	vq->drv_priv = icd;
+	vq->ops = &rcar_vin_vb2_ops;
+	vq->mem_ops = &vb2_dma_contig_memops;
+	vq->buf_struct_size = sizeof(struct rcar_vin_buffer);
+	vq->timestamp_flags  = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	vq->lock = &ici->host_lock;
+
+	return vb2_queue_init(vq);
+}
+
+static struct soc_camera_host_ops rcar_vin_host_ops = {
+	.owner		= THIS_MODULE,
+	.add		= rcar_vin_add_device,
+	.remove		= rcar_vin_remove_device,
+	.get_formats	= rcar_vin_get_formats,
+	.put_formats	= rcar_vin_put_formats,
+	.get_crop	= rcar_vin_get_crop,
+	.set_crop	= rcar_vin_set_crop,
+	.try_fmt	= rcar_vin_try_fmt,
+	.set_fmt	= rcar_vin_set_fmt,
+	.poll		= rcar_vin_poll,
+	.querycap	= rcar_vin_querycap,
+	.set_bus_param	= rcar_vin_set_bus_param,
+	.init_videobuf2	= rcar_vin_init_videobuf2,
+};
+
+#ifdef CONFIG_OF
+static const struct of_device_id rcar_vin_of_table[] = {
+	{ .compatible = "renesas,vin-r8a7794", .data = (void *)RCAR_GEN2 },
+	{ .compatible = "renesas,vin-r8a7793", .data = (void *)RCAR_GEN2 },
+	{ .compatible = "renesas,vin-r8a7791", .data = (void *)RCAR_GEN2 },
+	{ .compatible = "renesas,vin-r8a7790", .data = (void *)RCAR_GEN2 },
+	{ .compatible = "renesas,vin-r8a7779", .data = (void *)RCAR_H1 },
+	{ .compatible = "renesas,vin-r8a7778", .data = (void *)RCAR_M1 },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, rcar_vin_of_table);
+#endif
+
+static struct platform_device_id rcar_vin_id_table[] = {
+	{ "r8a7779-vin",  RCAR_H1 },
+	{ "r8a7778-vin",  RCAR_M1 },
+	{ "uPD35004-vin", RCAR_E1 },
+	{},
+};
+MODULE_DEVICE_TABLE(platform, rcar_vin_id_table);
+
+static int rcar_vin_probe(struct platform_device *pdev)
+{
+	const struct of_device_id *match = NULL;
+	struct rcar_vin_priv *priv;
+	struct resource *mem;
+	struct rcar_vin_platform_data *pdata;
+	unsigned int pdata_flags;
+	int irq, ret;
+
+	if (pdev->dev.of_node) {
+		struct v4l2_of_endpoint ep;
+		struct device_node *np;
+
+		match = of_match_device(of_match_ptr(rcar_vin_of_table),
+					&pdev->dev);
+
+		np = of_graph_get_next_endpoint(pdev->dev.of_node, NULL);
+		if (!np) {
+			dev_err(&pdev->dev, "could not find endpoint\n");
+			return -EINVAL;
+		}
+
+		ret = v4l2_of_parse_endpoint(np, &ep);
+		if (ret) {
+			dev_err(&pdev->dev, "could not parse endpoint\n");
+			return ret;
+		}
+
+		if (ep.bus_type == V4L2_MBUS_BT656)
+			pdata_flags = RCAR_VIN_BT656;
+		else {
+			pdata_flags = 0;
+			if (ep.bus.parallel.flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)
+				pdata_flags |= RCAR_VIN_HSYNC_ACTIVE_LOW;
+			if (ep.bus.parallel.flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
+				pdata_flags |= RCAR_VIN_VSYNC_ACTIVE_LOW;
+		}
+
+		of_node_put(np);
+
+		dev_dbg(&pdev->dev, "pdata_flags = %08x\n", pdata_flags);
+	} else {
+		pdata = pdev->dev.platform_data;
+		if (!pdata || !pdata->flags) {
+			dev_err(&pdev->dev, "platform data not set\n");
+			return -EINVAL;
+		}
+		pdata_flags = pdata->flags;
+	}
+
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (mem == NULL)
+		return -EINVAL;
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq <= 0)
+		return -EINVAL;
+
+	priv = devm_kzalloc(&pdev->dev, sizeof(struct rcar_vin_priv),
+			    GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->base = devm_ioremap_resource(&pdev->dev, mem);
+	if (IS_ERR(priv->base))
+		return PTR_ERR(priv->base);
+
+	ret = devm_request_irq(&pdev->dev, irq, rcar_vin_irq, IRQF_SHARED,
+			       dev_name(&pdev->dev), priv);
+	if (ret)
+		return ret;
+
+	priv->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
+	if (IS_ERR(priv->alloc_ctx))
+		return PTR_ERR(priv->alloc_ctx);
+
+	priv->ici.priv = priv;
+	priv->ici.v4l2_dev.dev = &pdev->dev;
+	priv->ici.drv_name = dev_name(&pdev->dev);
+	priv->ici.ops = &rcar_vin_host_ops;
+
+	priv->pdata_flags = pdata_flags;
+	if (!match) {
+		priv->ici.nr = pdev->id;
+		priv->chip = pdev->id_entry->driver_data;
+	} else {
+		priv->ici.nr = of_alias_get_id(pdev->dev.of_node, "vin");
+		priv->chip = (enum chip_id)match->data;
+	}
+
+	spin_lock_init(&priv->lock);
+	INIT_LIST_HEAD(&priv->capture);
+
+	priv->state = STOPPED;
+
+	pm_suspend_ignore_children(&pdev->dev, true);
+	pm_runtime_enable(&pdev->dev);
+
+	ret = soc_camera_host_register(&priv->ici);
+	if (ret)
+		goto cleanup;
+
+	return 0;
+
+cleanup:
+	pm_runtime_disable(&pdev->dev);
+	vb2_dma_contig_cleanup_ctx(priv->alloc_ctx);
+
+	return ret;
+}
+
+static int rcar_vin_remove(struct platform_device *pdev)
+{
+	struct soc_camera_host *soc_host = to_soc_camera_host(&pdev->dev);
+	struct rcar_vin_priv *priv = container_of(soc_host,
+						  struct rcar_vin_priv, ici);
+
+	soc_camera_host_unregister(soc_host);
+	pm_runtime_disable(&pdev->dev);
+	vb2_dma_contig_cleanup_ctx(priv->alloc_ctx);
+
+	return 0;
+}
+
+static struct platform_driver rcar_vin_driver = {
+	.probe		= rcar_vin_probe,
+	.remove		= rcar_vin_remove,
+	.driver		= {
+		.name		= DRV_NAME,
+		.of_match_table	= of_match_ptr(rcar_vin_of_table),
+	},
+	.id_table	= rcar_vin_id_table,
+};
+
+module_platform_driver(rcar_vin_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:rcar_vin");
+MODULE_DESCRIPTION("Renesas R-Car VIN camera host driver");
diff --git a/drivers/media/platform/soc_camera/sh_mobile_ceu_camera.c b/drivers/media/platform/soc_camera/sh_mobile_ceu_camera.c
new file mode 100644
index 0000000..67a669d
--- /dev/null
+++ b/drivers/media/platform/soc_camera/sh_mobile_ceu_camera.c
@@ -0,0 +1,2070 @@
+/*
+ * V4L2 Driver for SuperH Mobile CEU interface
+ *
+ * Copyright (C) 2008 Magnus Damm
+ *
+ * Based on V4L2 Driver for PXA camera host - "pxa_camera.c",
+ *
+ * Copyright (C) 2006, Sascha Hauer, Pengutronix
+ * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/io.h>
+#include <linux/completion.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/err.h>
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/moduleparam.h>
+#include <linux/of.h>
+#include <linux/time.h>
+#include <linux/slab.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/videodev2.h>
+#include <linux/pm_runtime.h>
+#include <linux/sched.h>
+
+#include <media/v4l2-async.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-dev.h>
+#include <media/soc_camera.h>
+#include <media/sh_mobile_ceu.h>
+#include <media/sh_mobile_csi2.h>
+#include <media/videobuf2-dma-contig.h>
+#include <media/v4l2-mediabus.h>
+#include <media/soc_mediabus.h>
+
+#include "soc_scale_crop.h"
+
+/* register offsets for sh7722 / sh7723 */
+
+#define CAPSR  0x00 /* Capture start register */
+#define CAPCR  0x04 /* Capture control register */
+#define CAMCR  0x08 /* Capture interface control register */
+#define CMCYR  0x0c /* Capture interface cycle  register */
+#define CAMOR  0x10 /* Capture interface offset register */
+#define CAPWR  0x14 /* Capture interface width register */
+#define CAIFR  0x18 /* Capture interface input format register */
+#define CSTCR  0x20 /* Camera strobe control register (<= sh7722) */
+#define CSECR  0x24 /* Camera strobe emission count register (<= sh7722) */
+#define CRCNTR 0x28 /* CEU register control register */
+#define CRCMPR 0x2c /* CEU register forcible control register */
+#define CFLCR  0x30 /* Capture filter control register */
+#define CFSZR  0x34 /* Capture filter size clip register */
+#define CDWDR  0x38 /* Capture destination width register */
+#define CDAYR  0x3c /* Capture data address Y register */
+#define CDACR  0x40 /* Capture data address C register */
+#define CDBYR  0x44 /* Capture data bottom-field address Y register */
+#define CDBCR  0x48 /* Capture data bottom-field address C register */
+#define CBDSR  0x4c /* Capture bundle destination size register */
+#define CFWCR  0x5c /* Firewall operation control register */
+#define CLFCR  0x60 /* Capture low-pass filter control register */
+#define CDOCR  0x64 /* Capture data output control register */
+#define CDDCR  0x68 /* Capture data complexity level register */
+#define CDDAR  0x6c /* Capture data complexity level address register */
+#define CEIER  0x70 /* Capture event interrupt enable register */
+#define CETCR  0x74 /* Capture event flag clear register */
+#define CSTSR  0x7c /* Capture status register */
+#define CSRTR  0x80 /* Capture software reset register */
+#define CDSSR  0x84 /* Capture data size register */
+#define CDAYR2 0x90 /* Capture data address Y register 2 */
+#define CDACR2 0x94 /* Capture data address C register 2 */
+#define CDBYR2 0x98 /* Capture data bottom-field address Y register 2 */
+#define CDBCR2 0x9c /* Capture data bottom-field address C register 2 */
+
+#undef DEBUG_GEOMETRY
+#ifdef DEBUG_GEOMETRY
+#define dev_geo	dev_info
+#else
+#define dev_geo	dev_dbg
+#endif
+
+/* per video frame buffer */
+struct sh_mobile_ceu_buffer {
+	struct vb2_v4l2_buffer vb; /* v4l buffer must be first */
+	struct list_head queue;
+};
+
+struct sh_mobile_ceu_dev {
+	struct soc_camera_host ici;
+	/* Asynchronous CSI2 linking */
+	struct v4l2_async_subdev *csi2_asd;
+	struct v4l2_subdev *csi2_sd;
+	/* Synchronous probing compatibility */
+	struct platform_device *csi2_pdev;
+
+	unsigned int irq;
+	void __iomem *base;
+	size_t video_limit;
+	size_t buf_total;
+
+	spinlock_t lock;		/* Protects video buffer lists */
+	struct list_head capture;
+	struct vb2_v4l2_buffer *active;
+	struct vb2_alloc_ctx *alloc_ctx;
+
+	struct sh_mobile_ceu_info *pdata;
+	struct completion complete;
+
+	u32 cflcr;
+
+	/* static max sizes either from platform data or default */
+	int max_width;
+	int max_height;
+
+	enum v4l2_field field;
+	int sequence;
+	unsigned long flags;
+
+	unsigned int image_mode:1;
+	unsigned int is_16bit:1;
+	unsigned int frozen:1;
+};
+
+struct sh_mobile_ceu_cam {
+	/* CEU offsets within the camera output, before the CEU scaler */
+	unsigned int ceu_left;
+	unsigned int ceu_top;
+	/* Client output, as seen by the CEU */
+	unsigned int width;
+	unsigned int height;
+	/*
+	 * User window from S_CROP / G_CROP, produced by client cropping and
+	 * scaling, CEU scaling and CEU cropping, mapped back onto the client
+	 * input window
+	 */
+	struct v4l2_rect subrect;
+	/* Camera cropping rectangle */
+	struct v4l2_rect rect;
+	const struct soc_mbus_pixelfmt *extra_fmt;
+	u32 code;
+};
+
+static struct sh_mobile_ceu_buffer *to_ceu_vb(struct vb2_v4l2_buffer *vbuf)
+{
+	return container_of(vbuf, struct sh_mobile_ceu_buffer, vb);
+}
+
+static void ceu_write(struct sh_mobile_ceu_dev *priv,
+		      unsigned long reg_offs, u32 data)
+{
+	iowrite32(data, priv->base + reg_offs);
+}
+
+static u32 ceu_read(struct sh_mobile_ceu_dev *priv, unsigned long reg_offs)
+{
+	return ioread32(priv->base + reg_offs);
+}
+
+static int sh_mobile_ceu_soft_reset(struct sh_mobile_ceu_dev *pcdev)
+{
+	int i, success = 0;
+
+	ceu_write(pcdev, CAPSR, 1 << 16); /* reset */
+
+	/* wait CSTSR.CPTON bit */
+	for (i = 0; i < 1000; i++) {
+		if (!(ceu_read(pcdev, CSTSR) & 1)) {
+			success++;
+			break;
+		}
+		udelay(1);
+	}
+
+	/* wait CAPSR.CPKIL bit */
+	for (i = 0; i < 1000; i++) {
+		if (!(ceu_read(pcdev, CAPSR) & (1 << 16))) {
+			success++;
+			break;
+		}
+		udelay(1);
+	}
+
+	if (2 != success) {
+		dev_warn(pcdev->ici.v4l2_dev.dev, "soft reset time out\n");
+		return -EIO;
+	}
+
+	return 0;
+}
+
+/*
+ *  Videobuf operations
+ */
+
+/*
+ * .queue_setup() is called to check, whether the driver can accept the
+ *		  requested number of buffers and to fill in plane sizes
+ *		  for the current frame format if required
+ */
+static int sh_mobile_ceu_videobuf_setup(struct vb2_queue *vq,
+			const void *parg,
+			unsigned int *count, unsigned int *num_planes,
+			unsigned int sizes[], void *alloc_ctxs[])
+{
+	const struct v4l2_format *fmt = parg;
+	struct soc_camera_device *icd = container_of(vq,
+			struct soc_camera_device, vb2_vidq);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct sh_mobile_ceu_dev *pcdev = ici->priv;
+
+	if (fmt) {
+		const struct soc_camera_format_xlate *xlate = soc_camera_xlate_by_fourcc(icd,
+								fmt->fmt.pix.pixelformat);
+		unsigned int bytes_per_line;
+		int ret;
+
+		if (!xlate)
+			return -EINVAL;
+
+		ret = soc_mbus_bytes_per_line(fmt->fmt.pix.width,
+					      xlate->host_fmt);
+		if (ret < 0)
+			return ret;
+
+		bytes_per_line = max_t(u32, fmt->fmt.pix.bytesperline, ret);
+
+		ret = soc_mbus_image_size(xlate->host_fmt, bytes_per_line,
+					  fmt->fmt.pix.height);
+		if (ret < 0)
+			return ret;
+
+		sizes[0] = max_t(u32, fmt->fmt.pix.sizeimage, ret);
+	} else {
+		/* Called from VIDIOC_REQBUFS or in compatibility mode */
+		sizes[0] = icd->sizeimage;
+	}
+
+	alloc_ctxs[0] = pcdev->alloc_ctx;
+
+	if (!vq->num_buffers)
+		pcdev->sequence = 0;
+
+	if (!*count)
+		*count = 2;
+
+	/* If *num_planes != 0, we have already verified *count. */
+	if (pcdev->video_limit && !*num_planes) {
+		size_t size = PAGE_ALIGN(sizes[0]) * *count;
+
+		if (size + pcdev->buf_total > pcdev->video_limit)
+			*count = (pcdev->video_limit - pcdev->buf_total) /
+				PAGE_ALIGN(sizes[0]);
+	}
+
+	*num_planes = 1;
+
+	dev_dbg(icd->parent, "count=%d, size=%u\n", *count, sizes[0]);
+
+	return 0;
+}
+
+#define CEU_CETCR_MAGIC 0x0317f313 /* acknowledge magical interrupt sources */
+#define CEU_CETCR_IGRW (1 << 4) /* prohibited register access interrupt bit */
+#define CEU_CEIER_CPEIE (1 << 0) /* one-frame capture end interrupt */
+#define CEU_CEIER_VBP   (1 << 20) /* vbp error */
+#define CEU_CAPCR_CTNCP (1 << 16) /* continuous capture mode (if set) */
+#define CEU_CEIER_MASK (CEU_CEIER_CPEIE | CEU_CEIER_VBP)
+
+
+/*
+ * return value doesn't reflex the success/failure to queue the new buffer,
+ * but rather the status of the previous buffer.
+ */
+static int sh_mobile_ceu_capture(struct sh_mobile_ceu_dev *pcdev)
+{
+	struct soc_camera_device *icd = pcdev->ici.icd;
+	dma_addr_t phys_addr_top, phys_addr_bottom;
+	unsigned long top1, top2;
+	unsigned long bottom1, bottom2;
+	u32 status;
+	bool planar;
+	int ret = 0;
+
+	/*
+	 * The hardware is _very_ picky about this sequence. Especially
+	 * the CEU_CETCR_MAGIC value. It seems like we need to acknowledge
+	 * several not-so-well documented interrupt sources in CETCR.
+	 */
+	ceu_write(pcdev, CEIER, ceu_read(pcdev, CEIER) & ~CEU_CEIER_MASK);
+	status = ceu_read(pcdev, CETCR);
+	ceu_write(pcdev, CETCR, ~status & CEU_CETCR_MAGIC);
+	if (!pcdev->frozen)
+		ceu_write(pcdev, CEIER, ceu_read(pcdev, CEIER) | CEU_CEIER_MASK);
+	ceu_write(pcdev, CAPCR, ceu_read(pcdev, CAPCR) & ~CEU_CAPCR_CTNCP);
+	ceu_write(pcdev, CETCR, CEU_CETCR_MAGIC ^ CEU_CETCR_IGRW);
+
+	/*
+	 * When a VBP interrupt occurs, a capture end interrupt does not occur
+	 * and the image of that frame is not captured correctly. So, soft reset
+	 * is needed here.
+	 */
+	if (status & CEU_CEIER_VBP) {
+		sh_mobile_ceu_soft_reset(pcdev);
+		ret = -EIO;
+	}
+
+	if (pcdev->frozen) {
+		complete(&pcdev->complete);
+		return ret;
+	}
+
+	if (!pcdev->active)
+		return ret;
+
+	if (V4L2_FIELD_INTERLACED_BT == pcdev->field) {
+		top1	= CDBYR;
+		top2	= CDBCR;
+		bottom1	= CDAYR;
+		bottom2	= CDACR;
+	} else {
+		top1	= CDAYR;
+		top2	= CDACR;
+		bottom1	= CDBYR;
+		bottom2	= CDBCR;
+	}
+
+	phys_addr_top =
+		vb2_dma_contig_plane_dma_addr(&pcdev->active->vb2_buf, 0);
+
+	switch (icd->current_fmt->host_fmt->fourcc) {
+	case V4L2_PIX_FMT_NV12:
+	case V4L2_PIX_FMT_NV21:
+	case V4L2_PIX_FMT_NV16:
+	case V4L2_PIX_FMT_NV61:
+		planar = true;
+		break;
+	default:
+		planar = false;
+	}
+
+	ceu_write(pcdev, top1, phys_addr_top);
+	if (V4L2_FIELD_NONE != pcdev->field) {
+		phys_addr_bottom = phys_addr_top + icd->bytesperline;
+		ceu_write(pcdev, bottom1, phys_addr_bottom);
+	}
+
+	if (planar) {
+		phys_addr_top += icd->bytesperline * icd->user_height;
+		ceu_write(pcdev, top2, phys_addr_top);
+		if (V4L2_FIELD_NONE != pcdev->field) {
+			phys_addr_bottom = phys_addr_top + icd->bytesperline;
+			ceu_write(pcdev, bottom2, phys_addr_bottom);
+		}
+	}
+
+	ceu_write(pcdev, CAPSR, 0x1); /* start capture */
+
+	return ret;
+}
+
+static int sh_mobile_ceu_videobuf_prepare(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct sh_mobile_ceu_buffer *buf = to_ceu_vb(vbuf);
+
+	/* Added list head initialization on alloc */
+	WARN(!list_empty(&buf->queue), "Buffer %p on queue!\n", vb);
+
+	return 0;
+}
+
+static void sh_mobile_ceu_videobuf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct soc_camera_device *icd = container_of(vb->vb2_queue,
+			struct soc_camera_device, vb2_vidq);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct sh_mobile_ceu_dev *pcdev = ici->priv;
+	struct sh_mobile_ceu_buffer *buf = to_ceu_vb(vbuf);
+	unsigned long size;
+
+	size = icd->sizeimage;
+
+	if (vb2_plane_size(vb, 0) < size) {
+		dev_err(icd->parent, "Buffer #%d too small (%lu < %lu)\n",
+			vb->index, vb2_plane_size(vb, 0), size);
+		goto error;
+	}
+
+	vb2_set_plane_payload(vb, 0, size);
+
+	dev_dbg(icd->parent, "%s (vb=0x%p) 0x%p %lu\n", __func__,
+		vb, vb2_plane_vaddr(vb, 0), vb2_get_plane_payload(vb, 0));
+
+#ifdef DEBUG
+	/*
+	 * This can be useful if you want to see if we actually fill
+	 * the buffer with something
+	 */
+	if (vb2_plane_vaddr(vb, 0))
+		memset(vb2_plane_vaddr(vb, 0), 0xaa, vb2_get_plane_payload(vb, 0));
+#endif
+
+	spin_lock_irq(&pcdev->lock);
+	list_add_tail(&buf->queue, &pcdev->capture);
+
+	if (!pcdev->active) {
+		/*
+		 * Because there were no active buffer at this moment,
+		 * we are not interested in the return value of
+		 * sh_mobile_ceu_capture here.
+		 */
+		pcdev->active = vbuf;
+		sh_mobile_ceu_capture(pcdev);
+	}
+	spin_unlock_irq(&pcdev->lock);
+
+	return;
+
+error:
+	vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
+}
+
+static void sh_mobile_ceu_videobuf_release(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct soc_camera_device *icd = container_of(vb->vb2_queue,
+			struct soc_camera_device, vb2_vidq);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct sh_mobile_ceu_buffer *buf = to_ceu_vb(vbuf);
+	struct sh_mobile_ceu_dev *pcdev = ici->priv;
+
+	spin_lock_irq(&pcdev->lock);
+
+	if (pcdev->active == vbuf) {
+		/* disable capture (release DMA buffer), reset */
+		ceu_write(pcdev, CAPSR, 1 << 16);
+		pcdev->active = NULL;
+	}
+
+	/*
+	 * Doesn't hurt also if the list is empty, but it hurts, if queuing the
+	 * buffer failed, and .buf_init() hasn't been called
+	 */
+	if (buf->queue.next)
+		list_del_init(&buf->queue);
+
+	pcdev->buf_total -= PAGE_ALIGN(vb2_plane_size(vb, 0));
+	dev_dbg(icd->parent, "%s() %zu bytes buffers\n", __func__,
+		pcdev->buf_total);
+
+	spin_unlock_irq(&pcdev->lock);
+}
+
+static int sh_mobile_ceu_videobuf_init(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct soc_camera_device *icd = container_of(vb->vb2_queue,
+			struct soc_camera_device, vb2_vidq);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct sh_mobile_ceu_dev *pcdev = ici->priv;
+
+	pcdev->buf_total += PAGE_ALIGN(vb2_plane_size(vb, 0));
+	dev_dbg(icd->parent, "%s() %zu bytes buffers\n", __func__,
+		pcdev->buf_total);
+
+	/* This is for locking debugging only */
+	INIT_LIST_HEAD(&to_ceu_vb(vbuf)->queue);
+	return 0;
+}
+
+static void sh_mobile_ceu_stop_streaming(struct vb2_queue *q)
+{
+	struct soc_camera_device *icd = container_of(q, struct soc_camera_device, vb2_vidq);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct sh_mobile_ceu_dev *pcdev = ici->priv;
+	struct list_head *buf_head, *tmp;
+
+	spin_lock_irq(&pcdev->lock);
+
+	pcdev->active = NULL;
+
+	list_for_each_safe(buf_head, tmp, &pcdev->capture)
+		list_del_init(buf_head);
+
+	spin_unlock_irq(&pcdev->lock);
+
+	sh_mobile_ceu_soft_reset(pcdev);
+}
+
+static struct vb2_ops sh_mobile_ceu_videobuf_ops = {
+	.queue_setup	= sh_mobile_ceu_videobuf_setup,
+	.buf_prepare	= sh_mobile_ceu_videobuf_prepare,
+	.buf_queue	= sh_mobile_ceu_videobuf_queue,
+	.buf_cleanup	= sh_mobile_ceu_videobuf_release,
+	.buf_init	= sh_mobile_ceu_videobuf_init,
+	.wait_prepare	= vb2_ops_wait_prepare,
+	.wait_finish	= vb2_ops_wait_finish,
+	.stop_streaming	= sh_mobile_ceu_stop_streaming,
+};
+
+static irqreturn_t sh_mobile_ceu_irq(int irq, void *data)
+{
+	struct sh_mobile_ceu_dev *pcdev = data;
+	struct vb2_v4l2_buffer *vbuf;
+	int ret;
+
+	spin_lock(&pcdev->lock);
+
+	vbuf = pcdev->active;
+	if (!vbuf)
+		/* Stale interrupt from a released buffer */
+		goto out;
+
+	list_del_init(&to_ceu_vb(vbuf)->queue);
+
+	if (!list_empty(&pcdev->capture))
+		pcdev->active = &list_entry(pcdev->capture.next,
+					    struct sh_mobile_ceu_buffer, queue)->vb;
+	else
+		pcdev->active = NULL;
+
+	ret = sh_mobile_ceu_capture(pcdev);
+	v4l2_get_timestamp(&vbuf->timestamp);
+	if (!ret) {
+		vbuf->field = pcdev->field;
+		vbuf->sequence = pcdev->sequence++;
+	}
+	vb2_buffer_done(&vbuf->vb2_buf,
+			ret < 0 ? VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+
+out:
+	spin_unlock(&pcdev->lock);
+
+	return IRQ_HANDLED;
+}
+
+static struct v4l2_subdev *find_csi2(struct sh_mobile_ceu_dev *pcdev)
+{
+	struct v4l2_subdev *sd;
+
+	if (pcdev->csi2_sd)
+		return pcdev->csi2_sd;
+
+	if (pcdev->csi2_asd) {
+		char name[] = "sh-mobile-csi2";
+		v4l2_device_for_each_subdev(sd, &pcdev->ici.v4l2_dev)
+			if (!strncmp(name, sd->name, sizeof(name) - 1)) {
+				pcdev->csi2_sd = sd;
+				return sd;
+			}
+	}
+
+	return NULL;
+}
+
+static struct v4l2_subdev *csi2_subdev(struct sh_mobile_ceu_dev *pcdev,
+				       struct soc_camera_device *icd)
+{
+	struct v4l2_subdev *sd = pcdev->csi2_sd;
+
+	return sd && sd->grp_id == soc_camera_grp_id(icd) ? sd : NULL;
+}
+
+static int sh_mobile_ceu_add_device(struct soc_camera_device *icd)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct sh_mobile_ceu_dev *pcdev = ici->priv;
+	struct v4l2_subdev *csi2_sd = find_csi2(pcdev);
+	int ret;
+
+	if (csi2_sd) {
+		csi2_sd->grp_id = soc_camera_grp_id(icd);
+		v4l2_set_subdev_hostdata(csi2_sd, icd);
+	}
+
+	ret = v4l2_subdev_call(csi2_sd, core, s_power, 1);
+	if (ret < 0 && ret != -ENOIOCTLCMD && ret != -ENODEV)
+		return ret;
+
+	/*
+	 * -ENODEV is special: either csi2_sd == NULL or the CSI-2 driver
+	 * has not found this soc-camera device among its clients
+	 */
+	if (csi2_sd && ret == -ENODEV)
+		csi2_sd->grp_id = 0;
+
+	dev_info(icd->parent,
+		 "SuperH Mobile CEU%s driver attached to camera %d\n",
+		 csi2_sd && csi2_sd->grp_id ? "/CSI-2" : "", icd->devnum);
+
+	return 0;
+}
+
+static void sh_mobile_ceu_remove_device(struct soc_camera_device *icd)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct sh_mobile_ceu_dev *pcdev = ici->priv;
+	struct v4l2_subdev *csi2_sd = find_csi2(pcdev);
+
+	dev_info(icd->parent,
+		 "SuperH Mobile CEU driver detached from camera %d\n",
+		 icd->devnum);
+
+	v4l2_subdev_call(csi2_sd, core, s_power, 0);
+}
+
+/* Called with .host_lock held */
+static int sh_mobile_ceu_clock_start(struct soc_camera_host *ici)
+{
+	struct sh_mobile_ceu_dev *pcdev = ici->priv;
+
+	pm_runtime_get_sync(ici->v4l2_dev.dev);
+
+	pcdev->buf_total = 0;
+
+	sh_mobile_ceu_soft_reset(pcdev);
+
+	return 0;
+}
+
+/* Called with .host_lock held */
+static void sh_mobile_ceu_clock_stop(struct soc_camera_host *ici)
+{
+	struct sh_mobile_ceu_dev *pcdev = ici->priv;
+
+	/* disable capture, disable interrupts */
+	ceu_write(pcdev, CEIER, 0);
+	sh_mobile_ceu_soft_reset(pcdev);
+
+	/* make sure active buffer is canceled */
+	spin_lock_irq(&pcdev->lock);
+	if (pcdev->active) {
+		list_del_init(&to_ceu_vb(pcdev->active)->queue);
+		vb2_buffer_done(&pcdev->active->vb2_buf, VB2_BUF_STATE_ERROR);
+		pcdev->active = NULL;
+	}
+	spin_unlock_irq(&pcdev->lock);
+
+	pm_runtime_put(ici->v4l2_dev.dev);
+}
+
+/*
+ * See chapter 29.4.12 "Capture Filter Control Register (CFLCR)"
+ * in SH7722 Hardware Manual
+ */
+static unsigned int size_dst(unsigned int src, unsigned int scale)
+{
+	unsigned int mant_pre = scale >> 12;
+	if (!src || !scale)
+		return src;
+	return ((mant_pre + 2 * (src - 1)) / (2 * mant_pre) - 1) *
+		mant_pre * 4096 / scale + 1;
+}
+
+static u16 calc_scale(unsigned int src, unsigned int *dst)
+{
+	u16 scale;
+
+	if (src == *dst)
+		return 0;
+
+	scale = (src * 4096 / *dst) & ~7;
+
+	while (scale > 4096 && size_dst(src, scale) < *dst)
+		scale -= 8;
+
+	*dst = size_dst(src, scale);
+
+	return scale;
+}
+
+/* rect is guaranteed to not exceed the scaled camera rectangle */
+static void sh_mobile_ceu_set_rect(struct soc_camera_device *icd)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct sh_mobile_ceu_cam *cam = icd->host_priv;
+	struct sh_mobile_ceu_dev *pcdev = ici->priv;
+	unsigned int height, width, cdwdr_width, in_width, in_height;
+	unsigned int left_offset, top_offset;
+	u32 camor;
+
+	dev_geo(icd->parent, "Crop %ux%u@%u:%u\n",
+		icd->user_width, icd->user_height, cam->ceu_left, cam->ceu_top);
+
+	left_offset	= cam->ceu_left;
+	top_offset	= cam->ceu_top;
+
+	WARN_ON(icd->user_width & 3 || icd->user_height & 3);
+
+	width = icd->user_width;
+
+	if (pcdev->image_mode) {
+		in_width = cam->width;
+		if (!pcdev->is_16bit) {
+			in_width *= 2;
+			left_offset *= 2;
+		}
+	} else {
+		unsigned int w_factor;
+
+		switch (icd->current_fmt->host_fmt->packing) {
+		case SOC_MBUS_PACKING_2X8_PADHI:
+			w_factor = 2;
+			break;
+		default:
+			w_factor = 1;
+		}
+
+		in_width = cam->width * w_factor;
+		left_offset *= w_factor;
+	}
+
+	cdwdr_width = icd->bytesperline;
+
+	height = icd->user_height;
+	in_height = cam->height;
+	if (V4L2_FIELD_NONE != pcdev->field) {
+		height = (height / 2) & ~3;
+		in_height /= 2;
+		top_offset /= 2;
+		cdwdr_width *= 2;
+	}
+
+	/* CSI2 special configuration */
+	if (csi2_subdev(pcdev, icd)) {
+		in_width = ((in_width - 2) * 2);
+		left_offset *= 2;
+	}
+
+	/* Set CAMOR, CAPWR, CFSZR, take care of CDWDR */
+	camor = left_offset | (top_offset << 16);
+
+	dev_geo(icd->parent,
+		"CAMOR 0x%x, CAPWR 0x%x, CFSZR 0x%x, CDWDR 0x%x\n", camor,
+		(in_height << 16) | in_width, (height << 16) | width,
+		cdwdr_width);
+
+	ceu_write(pcdev, CAMOR, camor);
+	ceu_write(pcdev, CAPWR, (in_height << 16) | in_width);
+	/* CFSZR clipping is applied _after_ the scaling filter (CFLCR) */
+	ceu_write(pcdev, CFSZR, (height << 16) | width);
+	ceu_write(pcdev, CDWDR, cdwdr_width);
+}
+
+static u32 capture_save_reset(struct sh_mobile_ceu_dev *pcdev)
+{
+	u32 capsr = ceu_read(pcdev, CAPSR);
+	ceu_write(pcdev, CAPSR, 1 << 16); /* reset, stop capture */
+	return capsr;
+}
+
+static void capture_restore(struct sh_mobile_ceu_dev *pcdev, u32 capsr)
+{
+	unsigned long timeout = jiffies + 10 * HZ;
+
+	/*
+	 * Wait until the end of the current frame. It can take a long time,
+	 * but if it has been aborted by a CAPSR reset, it shoule exit sooner.
+	 */
+	while ((ceu_read(pcdev, CSTSR) & 1) && time_before(jiffies, timeout))
+		msleep(1);
+
+	if (time_after(jiffies, timeout)) {
+		dev_err(pcdev->ici.v4l2_dev.dev,
+			"Timeout waiting for frame end! Interface problem?\n");
+		return;
+	}
+
+	/* Wait until reset clears, this shall not hang... */
+	while (ceu_read(pcdev, CAPSR) & (1 << 16))
+		udelay(10);
+
+	/* Anything to restore? */
+	if (capsr & ~(1 << 16))
+		ceu_write(pcdev, CAPSR, capsr);
+}
+
+/* Find the bus subdevice driver, e.g., CSI2 */
+static struct v4l2_subdev *find_bus_subdev(struct sh_mobile_ceu_dev *pcdev,
+					   struct soc_camera_device *icd)
+{
+	return csi2_subdev(pcdev, icd) ? : soc_camera_to_subdev(icd);
+}
+
+#define CEU_BUS_FLAGS (V4L2_MBUS_MASTER |	\
+		V4L2_MBUS_PCLK_SAMPLE_RISING |	\
+		V4L2_MBUS_HSYNC_ACTIVE_HIGH |	\
+		V4L2_MBUS_HSYNC_ACTIVE_LOW |	\
+		V4L2_MBUS_VSYNC_ACTIVE_HIGH |	\
+		V4L2_MBUS_VSYNC_ACTIVE_LOW |	\
+		V4L2_MBUS_DATA_ACTIVE_HIGH)
+
+/* Capture is not running, no interrupts, no locking needed */
+static int sh_mobile_ceu_set_bus_param(struct soc_camera_device *icd)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct sh_mobile_ceu_dev *pcdev = ici->priv;
+	struct v4l2_subdev *sd = find_bus_subdev(pcdev, icd);
+	struct sh_mobile_ceu_cam *cam = icd->host_priv;
+	struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
+	unsigned long value, common_flags = CEU_BUS_FLAGS;
+	u32 capsr = capture_save_reset(pcdev);
+	unsigned int yuv_lineskip;
+	int ret;
+
+	/*
+	 * If the client doesn't implement g_mbus_config, we just use our
+	 * platform data
+	 */
+	ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
+	if (!ret) {
+		common_flags = soc_mbus_config_compatible(&cfg,
+							  common_flags);
+		if (!common_flags)
+			return -EINVAL;
+	} else if (ret != -ENOIOCTLCMD) {
+		return ret;
+	}
+
+	/* Make choises, based on platform preferences */
+	if ((common_flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH) &&
+	    (common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)) {
+		if (pcdev->flags & SH_CEU_FLAG_HSYNC_LOW)
+			common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_HIGH;
+		else
+			common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_LOW;
+	}
+
+	if ((common_flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH) &&
+	    (common_flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)) {
+		if (pcdev->flags & SH_CEU_FLAG_VSYNC_LOW)
+			common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_HIGH;
+		else
+			common_flags &= ~V4L2_MBUS_VSYNC_ACTIVE_LOW;
+	}
+
+	cfg.flags = common_flags;
+	ret = v4l2_subdev_call(sd, video, s_mbus_config, &cfg);
+	if (ret < 0 && ret != -ENOIOCTLCMD)
+		return ret;
+
+	if (icd->current_fmt->host_fmt->bits_per_sample > 8)
+		pcdev->is_16bit = 1;
+	else
+		pcdev->is_16bit = 0;
+
+	ceu_write(pcdev, CRCNTR, 0);
+	ceu_write(pcdev, CRCMPR, 0);
+
+	value = 0x00000010; /* data fetch by default */
+	yuv_lineskip = 0x10;
+
+	switch (icd->current_fmt->host_fmt->fourcc) {
+	case V4L2_PIX_FMT_NV12:
+	case V4L2_PIX_FMT_NV21:
+		/* convert 4:2:2 -> 4:2:0 */
+		yuv_lineskip = 0; /* skip for NV12/21, no skip for NV16/61 */
+		/* fall-through */
+	case V4L2_PIX_FMT_NV16:
+	case V4L2_PIX_FMT_NV61:
+		switch (cam->code) {
+		case MEDIA_BUS_FMT_UYVY8_2X8:
+			value = 0x00000000; /* Cb0, Y0, Cr0, Y1 */
+			break;
+		case MEDIA_BUS_FMT_VYUY8_2X8:
+			value = 0x00000100; /* Cr0, Y0, Cb0, Y1 */
+			break;
+		case MEDIA_BUS_FMT_YUYV8_2X8:
+			value = 0x00000200; /* Y0, Cb0, Y1, Cr0 */
+			break;
+		case MEDIA_BUS_FMT_YVYU8_2X8:
+			value = 0x00000300; /* Y0, Cr0, Y1, Cb0 */
+			break;
+		default:
+			BUG();
+		}
+	}
+
+	if (icd->current_fmt->host_fmt->fourcc == V4L2_PIX_FMT_NV21 ||
+	    icd->current_fmt->host_fmt->fourcc == V4L2_PIX_FMT_NV61)
+		value ^= 0x00000100; /* swap U, V to change from NV1x->NVx1 */
+
+	value |= common_flags & V4L2_MBUS_VSYNC_ACTIVE_LOW ? 1 << 1 : 0;
+	value |= common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW ? 1 << 0 : 0;
+
+	if (csi2_subdev(pcdev, icd)) /* CSI2 mode */
+		value |= 3 << 12;
+	else if (pcdev->is_16bit)
+		value |= 1 << 12;
+	else if (pcdev->flags & SH_CEU_FLAG_LOWER_8BIT)
+		value |= 2 << 12;
+
+	ceu_write(pcdev, CAMCR, value);
+
+	ceu_write(pcdev, CAPCR, 0x00300000);
+
+	switch (pcdev->field) {
+	case V4L2_FIELD_INTERLACED_TB:
+		value = 0x101;
+		break;
+	case V4L2_FIELD_INTERLACED_BT:
+		value = 0x102;
+		break;
+	default:
+		value = 0;
+		break;
+	}
+	ceu_write(pcdev, CAIFR, value);
+
+	sh_mobile_ceu_set_rect(icd);
+	mdelay(1);
+
+	dev_geo(icd->parent, "CFLCR 0x%x\n", pcdev->cflcr);
+	ceu_write(pcdev, CFLCR, pcdev->cflcr);
+
+	/*
+	 * A few words about byte order (observed in Big Endian mode)
+	 *
+	 * In data fetch mode bytes are received in chunks of 8 bytes.
+	 * D0, D1, D2, D3, D4, D5, D6, D7 (D0 received first)
+	 *
+	 * The data is however by default written to memory in reverse order:
+	 * D7, D6, D5, D4, D3, D2, D1, D0 (D7 written to lowest byte)
+	 *
+	 * The lowest three bits of CDOCR allows us to do swapping,
+	 * using 7 we swap the data bytes to match the incoming order:
+	 * D0, D1, D2, D3, D4, D5, D6, D7
+	 */
+	value = 0x00000007 | yuv_lineskip;
+
+	ceu_write(pcdev, CDOCR, value);
+	ceu_write(pcdev, CFWCR, 0); /* keep "datafetch firewall" disabled */
+
+	capture_restore(pcdev, capsr);
+
+	/* not in bundle mode: skip CBDSR, CDAYR2, CDACR2, CDBYR2, CDBCR2 */
+	return 0;
+}
+
+static int sh_mobile_ceu_try_bus_param(struct soc_camera_device *icd,
+				       unsigned char buswidth)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct sh_mobile_ceu_dev *pcdev = ici->priv;
+	struct v4l2_subdev *sd = find_bus_subdev(pcdev, icd);
+	unsigned long common_flags = CEU_BUS_FLAGS;
+	struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
+	int ret;
+
+	ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
+	if (!ret)
+		common_flags = soc_mbus_config_compatible(&cfg,
+							  common_flags);
+	else if (ret != -ENOIOCTLCMD)
+		return ret;
+
+	if (!common_flags || buswidth > 16)
+		return -EINVAL;
+
+	return 0;
+}
+
+static const struct soc_mbus_pixelfmt sh_mobile_ceu_formats[] = {
+	{
+		.fourcc			= V4L2_PIX_FMT_NV12,
+		.name			= "NV12",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_1_5X8,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PLANAR_2Y_C,
+	}, {
+		.fourcc			= V4L2_PIX_FMT_NV21,
+		.name			= "NV21",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_1_5X8,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PLANAR_2Y_C,
+	}, {
+		.fourcc			= V4L2_PIX_FMT_NV16,
+		.name			= "NV16",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PLANAR_Y_C,
+	}, {
+		.fourcc			= V4L2_PIX_FMT_NV61,
+		.name			= "NV61",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PLANAR_Y_C,
+	},
+};
+
+/* This will be corrected as we get more formats */
+static bool sh_mobile_ceu_packing_supported(const struct soc_mbus_pixelfmt *fmt)
+{
+	return	fmt->packing == SOC_MBUS_PACKING_NONE ||
+		(fmt->bits_per_sample == 8 &&
+		 fmt->packing == SOC_MBUS_PACKING_1_5X8) ||
+		(fmt->bits_per_sample == 8 &&
+		 fmt->packing == SOC_MBUS_PACKING_2X8_PADHI) ||
+		(fmt->bits_per_sample > 8 &&
+		 fmt->packing == SOC_MBUS_PACKING_EXTEND16);
+}
+
+static struct soc_camera_device *ctrl_to_icd(struct v4l2_ctrl *ctrl)
+{
+	return container_of(ctrl->handler, struct soc_camera_device,
+							ctrl_handler);
+}
+
+static int sh_mobile_ceu_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct soc_camera_device *icd = ctrl_to_icd(ctrl);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct sh_mobile_ceu_dev *pcdev = ici->priv;
+
+	switch (ctrl->id) {
+	case V4L2_CID_SHARPNESS:
+		switch (icd->current_fmt->host_fmt->fourcc) {
+		case V4L2_PIX_FMT_NV12:
+		case V4L2_PIX_FMT_NV21:
+		case V4L2_PIX_FMT_NV16:
+		case V4L2_PIX_FMT_NV61:
+			ceu_write(pcdev, CLFCR, !ctrl->val);
+			return 0;
+		}
+		break;
+	}
+
+	return -EINVAL;
+}
+
+static const struct v4l2_ctrl_ops sh_mobile_ceu_ctrl_ops = {
+	.s_ctrl = sh_mobile_ceu_s_ctrl,
+};
+
+static int sh_mobile_ceu_get_formats(struct soc_camera_device *icd, unsigned int idx,
+				     struct soc_camera_format_xlate *xlate)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct device *dev = icd->parent;
+	struct soc_camera_host *ici = to_soc_camera_host(dev);
+	struct sh_mobile_ceu_dev *pcdev = ici->priv;
+	int ret, k, n;
+	int formats = 0;
+	struct sh_mobile_ceu_cam *cam;
+	struct v4l2_subdev_mbus_code_enum code = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.index = idx,
+	};
+	const struct soc_mbus_pixelfmt *fmt;
+
+	ret = v4l2_subdev_call(sd, pad, enum_mbus_code, NULL, &code);
+	if (ret < 0)
+		/* No more formats */
+		return 0;
+
+	fmt = soc_mbus_get_fmtdesc(code.code);
+	if (!fmt) {
+		dev_warn(dev, "unsupported format code #%u: %d\n", idx, code.code);
+		return 0;
+	}
+
+	if (!csi2_subdev(pcdev, icd)) {
+		/* Are there any restrictions in the CSI-2 case? */
+		ret = sh_mobile_ceu_try_bus_param(icd, fmt->bits_per_sample);
+		if (ret < 0)
+			return 0;
+	}
+
+	if (!icd->host_priv) {
+		struct v4l2_subdev_format fmt = {
+			.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		};
+		struct v4l2_mbus_framefmt *mf = &fmt.format;
+		struct v4l2_rect rect;
+		int shift = 0;
+
+		/* Add our control */
+		v4l2_ctrl_new_std(&icd->ctrl_handler, &sh_mobile_ceu_ctrl_ops,
+				  V4L2_CID_SHARPNESS, 0, 1, 1, 1);
+		if (icd->ctrl_handler.error)
+			return icd->ctrl_handler.error;
+
+		/* FIXME: subwindow is lost between close / open */
+
+		/* Cache current client geometry */
+		ret = soc_camera_client_g_rect(sd, &rect);
+		if (ret < 0)
+			return ret;
+
+		/* First time */
+		ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &fmt);
+		if (ret < 0)
+			return ret;
+
+		/*
+		 * All currently existing CEU implementations support 2560x1920
+		 * or larger frames. If the sensor is proposing too big a frame,
+		 * don't bother with possibly supportred by the CEU larger
+		 * sizes, just try VGA multiples. If needed, this can be
+		 * adjusted in the future.
+		 */
+		while ((mf->width > pcdev->max_width ||
+			mf->height > pcdev->max_height) && shift < 4) {
+			/* Try 2560x1920, 1280x960, 640x480, 320x240 */
+			mf->width	= 2560 >> shift;
+			mf->height	= 1920 >> shift;
+			ret = v4l2_device_call_until_err(sd->v4l2_dev,
+					soc_camera_grp_id(icd), pad,
+					set_fmt, NULL, &fmt);
+			if (ret < 0)
+				return ret;
+			shift++;
+		}
+
+		if (shift == 4) {
+			dev_err(dev, "Failed to configure the client below %ux%x\n",
+				mf->width, mf->height);
+			return -EIO;
+		}
+
+		dev_geo(dev, "camera fmt %ux%u\n", mf->width, mf->height);
+
+		cam = kzalloc(sizeof(*cam), GFP_KERNEL);
+		if (!cam)
+			return -ENOMEM;
+
+		/* We are called with current camera crop, initialise subrect with it */
+		cam->rect	= rect;
+		cam->subrect	= rect;
+
+		cam->width	= mf->width;
+		cam->height	= mf->height;
+
+		icd->host_priv = cam;
+	} else {
+		cam = icd->host_priv;
+	}
+
+	/* Beginning of a pass */
+	if (!idx)
+		cam->extra_fmt = NULL;
+
+	switch (code.code) {
+	case MEDIA_BUS_FMT_UYVY8_2X8:
+	case MEDIA_BUS_FMT_VYUY8_2X8:
+	case MEDIA_BUS_FMT_YUYV8_2X8:
+	case MEDIA_BUS_FMT_YVYU8_2X8:
+		if (cam->extra_fmt)
+			break;
+
+		/*
+		 * Our case is simple so far: for any of the above four camera
+		 * formats we add all our four synthesized NV* formats, so,
+		 * just marking the device with a single flag suffices. If
+		 * the format generation rules are more complex, you would have
+		 * to actually hang your already added / counted formats onto
+		 * the host_priv pointer and check whether the format you're
+		 * going to add now is already there.
+		 */
+		cam->extra_fmt = sh_mobile_ceu_formats;
+
+		n = ARRAY_SIZE(sh_mobile_ceu_formats);
+		formats += n;
+		for (k = 0; xlate && k < n; k++) {
+			xlate->host_fmt	= &sh_mobile_ceu_formats[k];
+			xlate->code	= code.code;
+			xlate++;
+			dev_dbg(dev, "Providing format %s using code %d\n",
+				sh_mobile_ceu_formats[k].name, code.code);
+		}
+		break;
+	default:
+		if (!sh_mobile_ceu_packing_supported(fmt))
+			return 0;
+	}
+
+	/* Generic pass-through */
+	formats++;
+	if (xlate) {
+		xlate->host_fmt	= fmt;
+		xlate->code	= code.code;
+		xlate++;
+		dev_dbg(dev, "Providing format %s in pass-through mode\n",
+			fmt->name);
+	}
+
+	return formats;
+}
+
+static void sh_mobile_ceu_put_formats(struct soc_camera_device *icd)
+{
+	kfree(icd->host_priv);
+	icd->host_priv = NULL;
+}
+
+#define scale_down(size, scale) soc_camera_shift_scale(size, 12, scale)
+#define calc_generic_scale(in, out) soc_camera_calc_scale(in, 12, out)
+
+/*
+ * CEU can scale and crop, but we don't want to waste bandwidth and kill the
+ * framerate by always requesting the maximum image from the client. See
+ * Documentation/video4linux/sh_mobile_ceu_camera.txt for a description of
+ * scaling and cropping algorithms and for the meaning of referenced here steps.
+ */
+static int sh_mobile_ceu_set_crop(struct soc_camera_device *icd,
+				  const struct v4l2_crop *a)
+{
+	struct v4l2_crop a_writable = *a;
+	const struct v4l2_rect *rect = &a_writable.c;
+	struct device *dev = icd->parent;
+	struct soc_camera_host *ici = to_soc_camera_host(dev);
+	struct sh_mobile_ceu_dev *pcdev = ici->priv;
+	struct v4l2_crop cam_crop;
+	struct sh_mobile_ceu_cam *cam = icd->host_priv;
+	struct v4l2_rect *cam_rect = &cam_crop.c;
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_mbus_framefmt *mf = &fmt.format;
+	unsigned int scale_cam_h, scale_cam_v, scale_ceu_h, scale_ceu_v,
+		out_width, out_height;
+	int interm_width, interm_height;
+	u32 capsr, cflcr;
+	int ret;
+
+	dev_geo(dev, "S_CROP(%ux%u@%u:%u)\n", rect->width, rect->height,
+		rect->left, rect->top);
+
+	/* During camera cropping its output window can change too, stop CEU */
+	capsr = capture_save_reset(pcdev);
+	dev_dbg(dev, "CAPSR 0x%x, CFLCR 0x%x\n", capsr, pcdev->cflcr);
+
+	/*
+	 * 1. - 2. Apply iterative camera S_CROP for new input window, read back
+	 * actual camera rectangle.
+	 */
+	ret = soc_camera_client_s_crop(sd, &a_writable, &cam_crop,
+				       &cam->rect, &cam->subrect);
+	if (ret < 0)
+		return ret;
+
+	dev_geo(dev, "1-2: camera cropped to %ux%u@%u:%u\n",
+		cam_rect->width, cam_rect->height,
+		cam_rect->left, cam_rect->top);
+
+	/* On success cam_crop contains current camera crop */
+
+	/* 3. Retrieve camera output window */
+	ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &fmt);
+	if (ret < 0)
+		return ret;
+
+	if (mf->width > pcdev->max_width || mf->height > pcdev->max_height)
+		return -EINVAL;
+
+	/* 4. Calculate camera scales */
+	scale_cam_h	= calc_generic_scale(cam_rect->width, mf->width);
+	scale_cam_v	= calc_generic_scale(cam_rect->height, mf->height);
+
+	/* Calculate intermediate window */
+	interm_width	= scale_down(rect->width, scale_cam_h);
+	interm_height	= scale_down(rect->height, scale_cam_v);
+
+	if (interm_width < icd->user_width) {
+		u32 new_scale_h;
+
+		new_scale_h = calc_generic_scale(rect->width, icd->user_width);
+
+		mf->width = scale_down(cam_rect->width, new_scale_h);
+	}
+
+	if (interm_height < icd->user_height) {
+		u32 new_scale_v;
+
+		new_scale_v = calc_generic_scale(rect->height, icd->user_height);
+
+		mf->height = scale_down(cam_rect->height, new_scale_v);
+	}
+
+	if (interm_width < icd->user_width || interm_height < icd->user_height) {
+		ret = v4l2_device_call_until_err(sd->v4l2_dev,
+					soc_camera_grp_id(icd), pad,
+					set_fmt, NULL, &fmt);
+		if (ret < 0)
+			return ret;
+
+		dev_geo(dev, "New camera output %ux%u\n", mf->width, mf->height);
+		scale_cam_h	= calc_generic_scale(cam_rect->width, mf->width);
+		scale_cam_v	= calc_generic_scale(cam_rect->height, mf->height);
+		interm_width	= scale_down(rect->width, scale_cam_h);
+		interm_height	= scale_down(rect->height, scale_cam_v);
+	}
+
+	/* Cache camera output window */
+	cam->width	= mf->width;
+	cam->height	= mf->height;
+
+	if (pcdev->image_mode) {
+		out_width	= min(interm_width, icd->user_width);
+		out_height	= min(interm_height, icd->user_height);
+	} else {
+		out_width	= interm_width;
+		out_height	= interm_height;
+	}
+
+	/*
+	 * 5. Calculate CEU scales from camera scales from results of (5) and
+	 *    the user window
+	 */
+	scale_ceu_h	= calc_scale(interm_width, &out_width);
+	scale_ceu_v	= calc_scale(interm_height, &out_height);
+
+	dev_geo(dev, "5: CEU scales %u:%u\n", scale_ceu_h, scale_ceu_v);
+
+	/* Apply CEU scales. */
+	cflcr = scale_ceu_h | (scale_ceu_v << 16);
+	if (cflcr != pcdev->cflcr) {
+		pcdev->cflcr = cflcr;
+		ceu_write(pcdev, CFLCR, cflcr);
+	}
+
+	icd->user_width	 = out_width & ~3;
+	icd->user_height = out_height & ~3;
+	/* Offsets are applied at the CEU scaling filter input */
+	cam->ceu_left	 = scale_down(rect->left - cam_rect->left, scale_cam_h) & ~1;
+	cam->ceu_top	 = scale_down(rect->top - cam_rect->top, scale_cam_v) & ~1;
+
+	/* 6. Use CEU cropping to crop to the new window. */
+	sh_mobile_ceu_set_rect(icd);
+
+	cam->subrect = *rect;
+
+	dev_geo(dev, "6: CEU cropped to %ux%u@%u:%u\n",
+		icd->user_width, icd->user_height,
+		cam->ceu_left, cam->ceu_top);
+
+	/* Restore capture. The CE bit can be cleared by the hardware */
+	if (pcdev->active)
+		capsr |= 1;
+	capture_restore(pcdev, capsr);
+
+	/* Even if only camera cropping succeeded */
+	return ret;
+}
+
+static int sh_mobile_ceu_get_crop(struct soc_camera_device *icd,
+				  struct v4l2_crop *a)
+{
+	struct sh_mobile_ceu_cam *cam = icd->host_priv;
+
+	a->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	a->c = cam->subrect;
+
+	return 0;
+}
+
+/* Similar to set_crop multistage iterative algorithm */
+static int sh_mobile_ceu_set_fmt(struct soc_camera_device *icd,
+				 struct v4l2_format *f)
+{
+	struct device *dev = icd->parent;
+	struct soc_camera_host *ici = to_soc_camera_host(dev);
+	struct sh_mobile_ceu_dev *pcdev = ici->priv;
+	struct sh_mobile_ceu_cam *cam = icd->host_priv;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct v4l2_mbus_framefmt mf;
+	__u32 pixfmt = pix->pixelformat;
+	const struct soc_camera_format_xlate *xlate;
+	unsigned int ceu_sub_width = pcdev->max_width,
+		ceu_sub_height = pcdev->max_height;
+	u16 scale_v, scale_h;
+	int ret;
+	bool image_mode;
+	enum v4l2_field field;
+
+	switch (pix->field) {
+	default:
+		pix->field = V4L2_FIELD_NONE;
+		/* fall-through */
+	case V4L2_FIELD_INTERLACED_TB:
+	case V4L2_FIELD_INTERLACED_BT:
+	case V4L2_FIELD_NONE:
+		field = pix->field;
+		break;
+	case V4L2_FIELD_INTERLACED:
+		field = V4L2_FIELD_INTERLACED_TB;
+		break;
+	}
+
+	xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
+	if (!xlate) {
+		dev_warn(dev, "Format %x not found\n", pixfmt);
+		return -EINVAL;
+	}
+
+	/* 1.-4. Calculate desired client output geometry */
+	soc_camera_calc_client_output(icd, &cam->rect, &cam->subrect, pix, &mf, 12);
+	mf.field	= pix->field;
+	mf.colorspace	= pix->colorspace;
+	mf.code		= xlate->code;
+
+	switch (pixfmt) {
+	case V4L2_PIX_FMT_NV12:
+	case V4L2_PIX_FMT_NV21:
+	case V4L2_PIX_FMT_NV16:
+	case V4L2_PIX_FMT_NV61:
+		image_mode = true;
+		break;
+	default:
+		image_mode = false;
+	}
+
+	dev_geo(dev, "S_FMT(pix=0x%x, fld 0x%x, code 0x%x, %ux%u)\n", pixfmt, mf.field, mf.code,
+		pix->width, pix->height);
+
+	dev_geo(dev, "4: request camera output %ux%u\n", mf.width, mf.height);
+
+	/* 5. - 9. */
+	ret = soc_camera_client_scale(icd, &cam->rect, &cam->subrect,
+				&mf, &ceu_sub_width, &ceu_sub_height,
+				image_mode && V4L2_FIELD_NONE == field, 12);
+
+	dev_geo(dev, "5-9: client scale return %d\n", ret);
+
+	/* Done with the camera. Now see if we can improve the result */
+
+	dev_geo(dev, "fmt %ux%u, requested %ux%u\n",
+		mf.width, mf.height, pix->width, pix->height);
+	if (ret < 0)
+		return ret;
+
+	if (mf.code != xlate->code)
+		return -EINVAL;
+
+	/* 9. Prepare CEU crop */
+	cam->width = mf.width;
+	cam->height = mf.height;
+
+	/* 10. Use CEU scaling to scale to the requested user window. */
+
+	/* We cannot scale up */
+	if (pix->width > ceu_sub_width)
+		ceu_sub_width = pix->width;
+
+	if (pix->height > ceu_sub_height)
+		ceu_sub_height = pix->height;
+
+	pix->colorspace = mf.colorspace;
+
+	if (image_mode) {
+		/* Scale pix->{width x height} down to width x height */
+		scale_h		= calc_scale(ceu_sub_width, &pix->width);
+		scale_v		= calc_scale(ceu_sub_height, &pix->height);
+	} else {
+		pix->width	= ceu_sub_width;
+		pix->height	= ceu_sub_height;
+		scale_h		= 0;
+		scale_v		= 0;
+	}
+
+	pcdev->cflcr = scale_h | (scale_v << 16);
+
+	/*
+	 * We have calculated CFLCR, the actual configuration will be performed
+	 * in sh_mobile_ceu_set_bus_param()
+	 */
+
+	dev_geo(dev, "10: W: %u : 0x%x = %u, H: %u : 0x%x = %u\n",
+		ceu_sub_width, scale_h, pix->width,
+		ceu_sub_height, scale_v, pix->height);
+
+	cam->code		= xlate->code;
+	icd->current_fmt	= xlate;
+
+	pcdev->field = field;
+	pcdev->image_mode = image_mode;
+
+	/* CFSZR requirement */
+	pix->width	&= ~3;
+	pix->height	&= ~3;
+
+	return 0;
+}
+
+#define CEU_CHDW_MAX	8188U	/* Maximum line stride */
+
+static int sh_mobile_ceu_try_fmt(struct soc_camera_device *icd,
+				 struct v4l2_format *f)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct sh_mobile_ceu_dev *pcdev = ici->priv;
+	const struct soc_camera_format_xlate *xlate;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct v4l2_subdev_pad_config pad_cfg;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+	};
+	struct v4l2_mbus_framefmt *mf = &format.format;
+	__u32 pixfmt = pix->pixelformat;
+	int width, height;
+	int ret;
+
+	dev_geo(icd->parent, "TRY_FMT(pix=0x%x, %ux%u)\n",
+		 pixfmt, pix->width, pix->height);
+
+	xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
+	if (!xlate) {
+		xlate = icd->current_fmt;
+		dev_dbg(icd->parent, "Format %x not found, keeping %x\n",
+			pixfmt, xlate->host_fmt->fourcc);
+		pixfmt = xlate->host_fmt->fourcc;
+		pix->pixelformat = pixfmt;
+		pix->colorspace = icd->colorspace;
+	}
+
+	/* FIXME: calculate using depth and bus width */
+
+	/* CFSZR requires height and width to be 4-pixel aligned */
+	v4l_bound_align_image(&pix->width, 2, pcdev->max_width, 2,
+			      &pix->height, 4, pcdev->max_height, 2, 0);
+
+	width = pix->width;
+	height = pix->height;
+
+	/* limit to sensor capabilities */
+	mf->width	= pix->width;
+	mf->height	= pix->height;
+	mf->field	= pix->field;
+	mf->code	= xlate->code;
+	mf->colorspace	= pix->colorspace;
+
+	ret = v4l2_device_call_until_err(sd->v4l2_dev, soc_camera_grp_id(icd),
+					 pad, set_fmt, &pad_cfg, &format);
+	if (ret < 0)
+		return ret;
+
+	pix->width	= mf->width;
+	pix->height	= mf->height;
+	pix->field	= mf->field;
+	pix->colorspace	= mf->colorspace;
+
+	switch (pixfmt) {
+	case V4L2_PIX_FMT_NV12:
+	case V4L2_PIX_FMT_NV21:
+	case V4L2_PIX_FMT_NV16:
+	case V4L2_PIX_FMT_NV61:
+		/* FIXME: check against rect_max after converting soc-camera */
+		/* We can scale precisely, need a bigger image from camera */
+		if (pix->width < width || pix->height < height) {
+			/*
+			 * We presume, the sensor behaves sanely, i.e., if
+			 * requested a bigger rectangle, it will not return a
+			 * smaller one.
+			 */
+			mf->width = pcdev->max_width;
+			mf->height = pcdev->max_height;
+			ret = v4l2_device_call_until_err(sd->v4l2_dev,
+					soc_camera_grp_id(icd), pad,
+					set_fmt, &pad_cfg, &format);
+			if (ret < 0) {
+				/* Shouldn't actually happen... */
+				dev_err(icd->parent,
+					"FIXME: client try_fmt() = %d\n", ret);
+				return ret;
+			}
+		}
+		/* We will scale exactly */
+		if (mf->width > width)
+			pix->width = width;
+		if (mf->height > height)
+			pix->height = height;
+
+		pix->bytesperline = max(pix->bytesperline, pix->width);
+		pix->bytesperline = min(pix->bytesperline, CEU_CHDW_MAX);
+		pix->bytesperline &= ~3;
+		break;
+
+	default:
+		/* Configurable stride isn't supported in pass-through mode. */
+		pix->bytesperline  = 0;
+	}
+
+	pix->width	&= ~3;
+	pix->height	&= ~3;
+	pix->sizeimage	= 0;
+
+	dev_geo(icd->parent, "%s(): return %d, fmt 0x%x, %ux%u\n",
+		__func__, ret, pix->pixelformat, pix->width, pix->height);
+
+	return ret;
+}
+
+static int sh_mobile_ceu_set_livecrop(struct soc_camera_device *icd,
+				      const struct v4l2_crop *a)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct sh_mobile_ceu_dev *pcdev = ici->priv;
+	u32 out_width = icd->user_width, out_height = icd->user_height;
+	int ret;
+
+	/* Freeze queue */
+	pcdev->frozen = 1;
+	/* Wait for frame */
+	ret = wait_for_completion_interruptible(&pcdev->complete);
+	/* Stop the client */
+	ret = v4l2_subdev_call(sd, video, s_stream, 0);
+	if (ret < 0)
+		dev_warn(icd->parent,
+			 "Client failed to stop the stream: %d\n", ret);
+	else
+		/* Do the crop, if it fails, there's nothing more we can do */
+		sh_mobile_ceu_set_crop(icd, a);
+
+	dev_geo(icd->parent, "Output after crop: %ux%u\n", icd->user_width, icd->user_height);
+
+	if (icd->user_width != out_width || icd->user_height != out_height) {
+		struct v4l2_format f = {
+			.type	= V4L2_BUF_TYPE_VIDEO_CAPTURE,
+			.fmt.pix	= {
+				.width		= out_width,
+				.height		= out_height,
+				.pixelformat	= icd->current_fmt->host_fmt->fourcc,
+				.field		= pcdev->field,
+				.colorspace	= icd->colorspace,
+			},
+		};
+		ret = sh_mobile_ceu_set_fmt(icd, &f);
+		if (!ret && (out_width != f.fmt.pix.width ||
+			     out_height != f.fmt.pix.height))
+			ret = -EINVAL;
+		if (!ret) {
+			icd->user_width		= out_width & ~3;
+			icd->user_height	= out_height & ~3;
+			ret = sh_mobile_ceu_set_bus_param(icd);
+		}
+	}
+
+	/* Thaw the queue */
+	pcdev->frozen = 0;
+	spin_lock_irq(&pcdev->lock);
+	sh_mobile_ceu_capture(pcdev);
+	spin_unlock_irq(&pcdev->lock);
+	/* Start the client */
+	ret = v4l2_subdev_call(sd, video, s_stream, 1);
+	return ret;
+}
+
+static unsigned int sh_mobile_ceu_poll(struct file *file, poll_table *pt)
+{
+	struct soc_camera_device *icd = file->private_data;
+
+	return vb2_poll(&icd->vb2_vidq, file, pt);
+}
+
+static int sh_mobile_ceu_querycap(struct soc_camera_host *ici,
+				  struct v4l2_capability *cap)
+{
+	strlcpy(cap->card, "SuperH_Mobile_CEU", sizeof(cap->card));
+	strlcpy(cap->driver, "sh_mobile_ceu", sizeof(cap->driver));
+	strlcpy(cap->bus_info, "platform:sh_mobile_ceu", sizeof(cap->bus_info));
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+
+	return 0;
+}
+
+static int sh_mobile_ceu_init_videobuf(struct vb2_queue *q,
+				       struct soc_camera_device *icd)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+
+	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	q->io_modes = VB2_MMAP | VB2_USERPTR;
+	q->drv_priv = icd;
+	q->ops = &sh_mobile_ceu_videobuf_ops;
+	q->mem_ops = &vb2_dma_contig_memops;
+	q->buf_struct_size = sizeof(struct sh_mobile_ceu_buffer);
+	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	q->lock = &ici->host_lock;
+
+	return vb2_queue_init(q);
+}
+
+static struct soc_camera_host_ops sh_mobile_ceu_host_ops = {
+	.owner		= THIS_MODULE,
+	.add		= sh_mobile_ceu_add_device,
+	.remove		= sh_mobile_ceu_remove_device,
+	.clock_start	= sh_mobile_ceu_clock_start,
+	.clock_stop	= sh_mobile_ceu_clock_stop,
+	.get_formats	= sh_mobile_ceu_get_formats,
+	.put_formats	= sh_mobile_ceu_put_formats,
+	.get_crop	= sh_mobile_ceu_get_crop,
+	.set_crop	= sh_mobile_ceu_set_crop,
+	.set_livecrop	= sh_mobile_ceu_set_livecrop,
+	.set_fmt	= sh_mobile_ceu_set_fmt,
+	.try_fmt	= sh_mobile_ceu_try_fmt,
+	.poll		= sh_mobile_ceu_poll,
+	.querycap	= sh_mobile_ceu_querycap,
+	.set_bus_param	= sh_mobile_ceu_set_bus_param,
+	.init_videobuf2	= sh_mobile_ceu_init_videobuf,
+};
+
+struct bus_wait {
+	struct notifier_block	notifier;
+	struct completion	completion;
+	struct device		*dev;
+};
+
+static int bus_notify(struct notifier_block *nb,
+		      unsigned long action, void *data)
+{
+	struct device *dev = data;
+	struct bus_wait *wait = container_of(nb, struct bus_wait, notifier);
+
+	if (wait->dev != dev)
+		return NOTIFY_DONE;
+
+	switch (action) {
+	case BUS_NOTIFY_UNBOUND_DRIVER:
+		/* Protect from module unloading */
+		wait_for_completion(&wait->completion);
+		return NOTIFY_OK;
+	}
+	return NOTIFY_DONE;
+}
+
+static int sh_mobile_ceu_probe(struct platform_device *pdev)
+{
+	struct sh_mobile_ceu_dev *pcdev;
+	struct resource *res;
+	void __iomem *base;
+	unsigned int irq;
+	int err, i;
+	struct bus_wait wait = {
+		.completion = COMPLETION_INITIALIZER_ONSTACK(wait.completion),
+		.notifier.notifier_call = bus_notify,
+	};
+	struct sh_mobile_ceu_companion *csi2;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	irq = platform_get_irq(pdev, 0);
+	if (!res || (int)irq <= 0) {
+		dev_err(&pdev->dev, "Not enough CEU platform resources.\n");
+		return -ENODEV;
+	}
+
+	pcdev = devm_kzalloc(&pdev->dev, sizeof(*pcdev), GFP_KERNEL);
+	if (!pcdev) {
+		dev_err(&pdev->dev, "Could not allocate pcdev\n");
+		return -ENOMEM;
+	}
+
+	INIT_LIST_HEAD(&pcdev->capture);
+	spin_lock_init(&pcdev->lock);
+	init_completion(&pcdev->complete);
+
+	pcdev->pdata = pdev->dev.platform_data;
+	if (!pcdev->pdata && !pdev->dev.of_node) {
+		dev_err(&pdev->dev, "CEU platform data not set.\n");
+		return -EINVAL;
+	}
+
+	/* TODO: implement per-device bus flags */
+	if (pcdev->pdata) {
+		pcdev->max_width = pcdev->pdata->max_width;
+		pcdev->max_height = pcdev->pdata->max_height;
+		pcdev->flags = pcdev->pdata->flags;
+	}
+	pcdev->field = V4L2_FIELD_NONE;
+
+	if (!pcdev->max_width) {
+		unsigned int v;
+		err = of_property_read_u32(pdev->dev.of_node, "renesas,max-width", &v);
+		if (!err)
+			pcdev->max_width = v;
+
+		if (!pcdev->max_width)
+			pcdev->max_width = 2560;
+	}
+	if (!pcdev->max_height) {
+		unsigned int v;
+		err = of_property_read_u32(pdev->dev.of_node, "renesas,max-height", &v);
+		if (!err)
+			pcdev->max_height = v;
+
+		if (!pcdev->max_height)
+			pcdev->max_height = 1920;
+	}
+
+	base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
+
+	pcdev->irq = irq;
+	pcdev->base = base;
+	pcdev->video_limit = 0; /* only enabled if second resource exists */
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	if (res) {
+		err = dma_declare_coherent_memory(&pdev->dev, res->start,
+						  res->start,
+						  resource_size(res),
+						  DMA_MEMORY_MAP |
+						  DMA_MEMORY_EXCLUSIVE);
+		if (!err) {
+			dev_err(&pdev->dev, "Unable to declare CEU memory.\n");
+			return -ENXIO;
+		}
+
+		pcdev->video_limit = resource_size(res);
+	}
+
+	/* request irq */
+	err = devm_request_irq(&pdev->dev, pcdev->irq, sh_mobile_ceu_irq,
+			       0, dev_name(&pdev->dev), pcdev);
+	if (err) {
+		dev_err(&pdev->dev, "Unable to register CEU interrupt.\n");
+		goto exit_release_mem;
+	}
+
+	pm_suspend_ignore_children(&pdev->dev, true);
+	pm_runtime_enable(&pdev->dev);
+	pm_runtime_resume(&pdev->dev);
+
+	pcdev->ici.priv = pcdev;
+	pcdev->ici.v4l2_dev.dev = &pdev->dev;
+	pcdev->ici.nr = pdev->id;
+	pcdev->ici.drv_name = dev_name(&pdev->dev);
+	pcdev->ici.ops = &sh_mobile_ceu_host_ops;
+	pcdev->ici.capabilities = SOCAM_HOST_CAP_STRIDE;
+
+	pcdev->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
+	if (IS_ERR(pcdev->alloc_ctx)) {
+		err = PTR_ERR(pcdev->alloc_ctx);
+		goto exit_free_clk;
+	}
+
+	if (pcdev->pdata && pcdev->pdata->asd_sizes) {
+		struct v4l2_async_subdev **asd;
+		char name[] = "sh-mobile-csi2";
+		int j;
+
+		/*
+		 * CSI2 interfacing: several groups can use CSI2, pick up the
+		 * first one
+		 */
+		asd = pcdev->pdata->asd;
+		for (j = 0; pcdev->pdata->asd_sizes[j]; j++) {
+			for (i = 0; i < pcdev->pdata->asd_sizes[j]; i++, asd++) {
+				dev_dbg(&pdev->dev, "%s(): subdev #%d, type %u\n",
+					__func__, i, (*asd)->match_type);
+				if ((*asd)->match_type == V4L2_ASYNC_MATCH_DEVNAME &&
+				    !strncmp(name, (*asd)->match.device_name.name,
+					     sizeof(name) - 1)) {
+					pcdev->csi2_asd = *asd;
+					break;
+				}
+			}
+			if (pcdev->csi2_asd)
+				break;
+		}
+
+		pcdev->ici.asd = pcdev->pdata->asd;
+		pcdev->ici.asd_sizes = pcdev->pdata->asd_sizes;
+	}
+
+	/* Legacy CSI2 interfacing */
+	csi2 = pcdev->pdata ? pcdev->pdata->csi2 : NULL;
+	if (csi2) {
+		/*
+		 * TODO: remove this once all users are converted to
+		 * asynchronous CSI2 probing. If it has to be kept, csi2
+		 * platform device resources have to be added, using
+		 * platform_device_add_resources()
+		 */
+		struct platform_device *csi2_pdev =
+			platform_device_alloc("sh-mobile-csi2", csi2->id);
+		struct sh_csi2_pdata *csi2_pdata = csi2->platform_data;
+
+		if (!csi2_pdev) {
+			err = -ENOMEM;
+			goto exit_free_ctx;
+		}
+
+		pcdev->csi2_pdev		= csi2_pdev;
+
+		err = platform_device_add_data(csi2_pdev, csi2_pdata,
+					       sizeof(*csi2_pdata));
+		if (err < 0)
+			goto exit_pdev_put;
+
+		csi2_pdev->resource		= csi2->resource;
+		csi2_pdev->num_resources	= csi2->num_resources;
+
+		err = platform_device_add(csi2_pdev);
+		if (err < 0)
+			goto exit_pdev_put;
+
+		wait.dev = &csi2_pdev->dev;
+
+		err = bus_register_notifier(&platform_bus_type, &wait.notifier);
+		if (err < 0)
+			goto exit_pdev_unregister;
+
+		/*
+		 * From this point the driver module will not unload, until
+		 * we complete the completion.
+		 */
+
+		if (!csi2_pdev->dev.driver) {
+			complete(&wait.completion);
+			/* Either too late, or probing failed */
+			bus_unregister_notifier(&platform_bus_type, &wait.notifier);
+			err = -ENXIO;
+			goto exit_pdev_unregister;
+		}
+
+		/*
+		 * The module is still loaded, in the worst case it is hanging
+		 * in device release on our completion. So, _now_ dereferencing
+		 * the "owner" is safe!
+		 */
+
+		err = try_module_get(csi2_pdev->dev.driver->owner);
+
+		/* Let notifier complete, if it has been locked */
+		complete(&wait.completion);
+		bus_unregister_notifier(&platform_bus_type, &wait.notifier);
+		if (!err) {
+			err = -ENODEV;
+			goto exit_pdev_unregister;
+		}
+
+		pcdev->csi2_sd = platform_get_drvdata(csi2_pdev);
+	}
+
+	err = soc_camera_host_register(&pcdev->ici);
+	if (err)
+		goto exit_csi2_unregister;
+
+	if (csi2) {
+		err = v4l2_device_register_subdev(&pcdev->ici.v4l2_dev,
+						  pcdev->csi2_sd);
+		dev_dbg(&pdev->dev, "%s(): ret(register_subdev) = %d\n",
+			__func__, err);
+		if (err < 0)
+			goto exit_host_unregister;
+		/* v4l2_device_register_subdev() took a reference too */
+		module_put(pcdev->csi2_sd->owner);
+	}
+
+	return 0;
+
+exit_host_unregister:
+	soc_camera_host_unregister(&pcdev->ici);
+exit_csi2_unregister:
+	if (csi2) {
+		module_put(pcdev->csi2_pdev->dev.driver->owner);
+exit_pdev_unregister:
+		platform_device_del(pcdev->csi2_pdev);
+exit_pdev_put:
+		pcdev->csi2_pdev->resource = NULL;
+		platform_device_put(pcdev->csi2_pdev);
+	}
+exit_free_ctx:
+	vb2_dma_contig_cleanup_ctx(pcdev->alloc_ctx);
+exit_free_clk:
+	pm_runtime_disable(&pdev->dev);
+exit_release_mem:
+	if (platform_get_resource(pdev, IORESOURCE_MEM, 1))
+		dma_release_declared_memory(&pdev->dev);
+	return err;
+}
+
+static int sh_mobile_ceu_remove(struct platform_device *pdev)
+{
+	struct soc_camera_host *soc_host = to_soc_camera_host(&pdev->dev);
+	struct sh_mobile_ceu_dev *pcdev = container_of(soc_host,
+					struct sh_mobile_ceu_dev, ici);
+	struct platform_device *csi2_pdev = pcdev->csi2_pdev;
+
+	soc_camera_host_unregister(soc_host);
+	pm_runtime_disable(&pdev->dev);
+	if (platform_get_resource(pdev, IORESOURCE_MEM, 1))
+		dma_release_declared_memory(&pdev->dev);
+	vb2_dma_contig_cleanup_ctx(pcdev->alloc_ctx);
+	if (csi2_pdev && csi2_pdev->dev.driver) {
+		struct module *csi2_drv = csi2_pdev->dev.driver->owner;
+		platform_device_del(csi2_pdev);
+		csi2_pdev->resource = NULL;
+		platform_device_put(csi2_pdev);
+		module_put(csi2_drv);
+	}
+
+	return 0;
+}
+
+static int sh_mobile_ceu_runtime_nop(struct device *dev)
+{
+	/* Runtime PM callback shared between ->runtime_suspend()
+	 * and ->runtime_resume(). Simply returns success.
+	 *
+	 * This driver re-initializes all registers after
+	 * pm_runtime_get_sync() anyway so there is no need
+	 * to save and restore registers here.
+	 */
+	return 0;
+}
+
+static const struct dev_pm_ops sh_mobile_ceu_dev_pm_ops = {
+	.runtime_suspend = sh_mobile_ceu_runtime_nop,
+	.runtime_resume = sh_mobile_ceu_runtime_nop,
+};
+
+static const struct of_device_id sh_mobile_ceu_of_match[] = {
+	{ .compatible = "renesas,sh-mobile-ceu" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, sh_mobile_ceu_of_match);
+
+static struct platform_driver sh_mobile_ceu_driver = {
+	.driver		= {
+		.name	= "sh_mobile_ceu",
+		.pm	= &sh_mobile_ceu_dev_pm_ops,
+		.of_match_table = sh_mobile_ceu_of_match,
+	},
+	.probe		= sh_mobile_ceu_probe,
+	.remove		= sh_mobile_ceu_remove,
+};
+
+static int __init sh_mobile_ceu_init(void)
+{
+	/* Whatever return code */
+	request_module("sh_mobile_csi2");
+	return platform_driver_register(&sh_mobile_ceu_driver);
+}
+
+static void __exit sh_mobile_ceu_exit(void)
+{
+	platform_driver_unregister(&sh_mobile_ceu_driver);
+}
+
+module_init(sh_mobile_ceu_init);
+module_exit(sh_mobile_ceu_exit);
+
+MODULE_DESCRIPTION("SuperH Mobile CEU driver");
+MODULE_AUTHOR("Magnus Damm");
+MODULE_LICENSE("GPL");
+MODULE_VERSION("0.1.0");
+MODULE_ALIAS("platform:sh_mobile_ceu");
diff --git a/drivers/media/platform/soc_camera/sh_mobile_csi2.c b/drivers/media/platform/soc_camera/sh_mobile_csi2.c
new file mode 100644
index 0000000..12d3626
--- /dev/null
+++ b/drivers/media/platform/soc_camera/sh_mobile_csi2.c
@@ -0,0 +1,400 @@
+/*
+ * Driver for the SH-Mobile MIPI CSI-2 unit
+ *
+ * Copyright (C) 2010, Guennadi Liakhovetski <g.liakhovetski@gmx.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/i2c.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <linux/module.h>
+
+#include <media/sh_mobile_ceu.h>
+#include <media/sh_mobile_csi2.h>
+#include <media/soc_camera.h>
+#include <media/soc_mediabus.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-mediabus.h>
+#include <media/v4l2-subdev.h>
+
+#define SH_CSI2_TREF	0x00
+#define SH_CSI2_SRST	0x04
+#define SH_CSI2_PHYCNT	0x08
+#define SH_CSI2_CHKSUM	0x0C
+#define SH_CSI2_VCDT	0x10
+
+struct sh_csi2 {
+	struct v4l2_subdev		subdev;
+	unsigned int			irq;
+	unsigned long			mipi_flags;
+	void __iomem			*base;
+	struct platform_device		*pdev;
+	struct sh_csi2_client_config	*client;
+};
+
+static void sh_csi2_hwinit(struct sh_csi2 *priv);
+
+static int sh_csi2_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
+{
+	struct sh_csi2 *priv = container_of(sd, struct sh_csi2, subdev);
+	struct sh_csi2_pdata *pdata = priv->pdev->dev.platform_data;
+	struct v4l2_mbus_framefmt *mf = &format->format;
+	u32 tmp = (priv->client->channel & 3) << 8;
+
+	if (format->pad)
+		return -EINVAL;
+
+	if (mf->width > 8188)
+		mf->width = 8188;
+	else if (mf->width & 1)
+		mf->width &= ~1;
+
+	switch (pdata->type) {
+	case SH_CSI2C:
+		switch (mf->code) {
+		case MEDIA_BUS_FMT_UYVY8_2X8:		/* YUV422 */
+		case MEDIA_BUS_FMT_YUYV8_1_5X8:		/* YUV420 */
+		case MEDIA_BUS_FMT_Y8_1X8:		/* RAW8 */
+		case MEDIA_BUS_FMT_SBGGR8_1X8:
+		case MEDIA_BUS_FMT_SGRBG8_1X8:
+			break;
+		default:
+			/* All MIPI CSI-2 devices must support one of primary formats */
+			mf->code = MEDIA_BUS_FMT_YUYV8_2X8;
+		}
+		break;
+	case SH_CSI2I:
+		switch (mf->code) {
+		case MEDIA_BUS_FMT_Y8_1X8:		/* RAW8 */
+		case MEDIA_BUS_FMT_SBGGR8_1X8:
+		case MEDIA_BUS_FMT_SGRBG8_1X8:
+		case MEDIA_BUS_FMT_SBGGR10_1X10:	/* RAW10 */
+		case MEDIA_BUS_FMT_SBGGR12_1X12:	/* RAW12 */
+			break;
+		default:
+			/* All MIPI CSI-2 devices must support one of primary formats */
+			mf->code = MEDIA_BUS_FMT_SBGGR8_1X8;
+		}
+		break;
+	}
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		cfg->try_fmt = *mf;
+		return 0;
+	}
+
+	if (mf->width > 8188 || mf->width & 1)
+		return -EINVAL;
+
+	switch (mf->code) {
+	case MEDIA_BUS_FMT_UYVY8_2X8:
+		tmp |= 0x1e;	/* YUV422 8 bit */
+		break;
+	case MEDIA_BUS_FMT_YUYV8_1_5X8:
+		tmp |= 0x18;	/* YUV420 8 bit */
+		break;
+	case MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE:
+		tmp |= 0x21;	/* RGB555 */
+		break;
+	case MEDIA_BUS_FMT_RGB565_2X8_BE:
+		tmp |= 0x22;	/* RGB565 */
+		break;
+	case MEDIA_BUS_FMT_Y8_1X8:
+	case MEDIA_BUS_FMT_SBGGR8_1X8:
+	case MEDIA_BUS_FMT_SGRBG8_1X8:
+		tmp |= 0x2a;	/* RAW8 */
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	iowrite32(tmp, priv->base + SH_CSI2_VCDT);
+
+	return 0;
+}
+
+static int sh_csi2_g_mbus_config(struct v4l2_subdev *sd,
+				 struct v4l2_mbus_config *cfg)
+{
+	struct sh_csi2 *priv = container_of(sd, struct sh_csi2, subdev);
+
+	if (!priv->mipi_flags) {
+		struct soc_camera_device *icd = v4l2_get_subdev_hostdata(sd);
+		struct v4l2_subdev *client_sd = soc_camera_to_subdev(icd);
+		struct sh_csi2_pdata *pdata = priv->pdev->dev.platform_data;
+		unsigned long common_flags, csi2_flags;
+		struct v4l2_mbus_config client_cfg = {.type = V4L2_MBUS_CSI2,};
+		int ret;
+
+		/* Check if we can support this camera */
+		csi2_flags = V4L2_MBUS_CSI2_CONTINUOUS_CLOCK |
+			V4L2_MBUS_CSI2_1_LANE;
+
+		switch (pdata->type) {
+		case SH_CSI2C:
+			if (priv->client->lanes != 1)
+				csi2_flags |= V4L2_MBUS_CSI2_2_LANE;
+			break;
+		case SH_CSI2I:
+			switch (priv->client->lanes) {
+			default:
+				csi2_flags |= V4L2_MBUS_CSI2_4_LANE;
+			case 3:
+				csi2_flags |= V4L2_MBUS_CSI2_3_LANE;
+			case 2:
+				csi2_flags |= V4L2_MBUS_CSI2_2_LANE;
+			}
+		}
+
+		ret = v4l2_subdev_call(client_sd, video, g_mbus_config, &client_cfg);
+		if (ret == -ENOIOCTLCMD)
+			common_flags = csi2_flags;
+		else if (!ret)
+			common_flags = soc_mbus_config_compatible(&client_cfg,
+								  csi2_flags);
+		else
+			common_flags = 0;
+
+		if (!common_flags)
+			return -EINVAL;
+
+		/* All good: camera MIPI configuration supported */
+		priv->mipi_flags = common_flags;
+	}
+
+	if (cfg) {
+		cfg->flags = V4L2_MBUS_PCLK_SAMPLE_RISING |
+			V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_HIGH |
+			V4L2_MBUS_MASTER | V4L2_MBUS_DATA_ACTIVE_HIGH;
+		cfg->type = V4L2_MBUS_PARALLEL;
+	}
+
+	return 0;
+}
+
+static int sh_csi2_s_mbus_config(struct v4l2_subdev *sd,
+				 const struct v4l2_mbus_config *cfg)
+{
+	struct sh_csi2 *priv = container_of(sd, struct sh_csi2, subdev);
+	struct soc_camera_device *icd = v4l2_get_subdev_hostdata(sd);
+	struct v4l2_subdev *client_sd = soc_camera_to_subdev(icd);
+	struct v4l2_mbus_config client_cfg = {.type = V4L2_MBUS_CSI2,};
+	int ret = sh_csi2_g_mbus_config(sd, NULL);
+
+	if (ret < 0)
+		return ret;
+
+	pm_runtime_get_sync(&priv->pdev->dev);
+
+	sh_csi2_hwinit(priv);
+
+	client_cfg.flags = priv->mipi_flags;
+
+	return v4l2_subdev_call(client_sd, video, s_mbus_config, &client_cfg);
+}
+
+static struct v4l2_subdev_video_ops sh_csi2_subdev_video_ops = {
+	.g_mbus_config	= sh_csi2_g_mbus_config,
+	.s_mbus_config	= sh_csi2_s_mbus_config,
+};
+
+static struct v4l2_subdev_pad_ops sh_csi2_subdev_pad_ops = {
+	.set_fmt	= sh_csi2_set_fmt,
+};
+
+static void sh_csi2_hwinit(struct sh_csi2 *priv)
+{
+	struct sh_csi2_pdata *pdata = priv->pdev->dev.platform_data;
+	__u32 tmp = 0x10; /* Enable MIPI CSI clock lane */
+
+	/* Reflect registers immediately */
+	iowrite32(0x00000001, priv->base + SH_CSI2_TREF);
+	/* reset CSI2 harware */
+	iowrite32(0x00000001, priv->base + SH_CSI2_SRST);
+	udelay(5);
+	iowrite32(0x00000000, priv->base + SH_CSI2_SRST);
+
+	switch (pdata->type) {
+	case SH_CSI2C:
+		if (priv->client->lanes == 1)
+			tmp |= 1;
+		else
+			/* Default - both lanes */
+			tmp |= 3;
+		break;
+	case SH_CSI2I:
+		if (!priv->client->lanes || priv->client->lanes > 4)
+			/* Default - all 4 lanes */
+			tmp |= 0xf;
+		else
+			tmp |= (1 << priv->client->lanes) - 1;
+	}
+
+	if (priv->client->phy == SH_CSI2_PHY_MAIN)
+		tmp |= 0x8000;
+
+	iowrite32(tmp, priv->base + SH_CSI2_PHYCNT);
+
+	tmp = 0;
+	if (pdata->flags & SH_CSI2_ECC)
+		tmp |= 2;
+	if (pdata->flags & SH_CSI2_CRC)
+		tmp |= 1;
+	iowrite32(tmp, priv->base + SH_CSI2_CHKSUM);
+}
+
+static int sh_csi2_client_connect(struct sh_csi2 *priv)
+{
+	struct device *dev = v4l2_get_subdevdata(&priv->subdev);
+	struct sh_csi2_pdata *pdata = dev->platform_data;
+	struct soc_camera_device *icd = v4l2_get_subdev_hostdata(&priv->subdev);
+	int i;
+
+	if (priv->client)
+		return -EBUSY;
+
+	for (i = 0; i < pdata->num_clients; i++)
+		if ((pdata->clients[i].pdev &&
+		     &pdata->clients[i].pdev->dev == icd->pdev) ||
+		    (icd->control &&
+		     strcmp(pdata->clients[i].name, dev_name(icd->control))))
+			break;
+
+	dev_dbg(dev, "%s(%p): found #%d\n", __func__, dev, i);
+
+	if (i == pdata->num_clients)
+		return -ENODEV;
+
+	priv->client = pdata->clients + i;
+
+	return 0;
+}
+
+static void sh_csi2_client_disconnect(struct sh_csi2 *priv)
+{
+	if (!priv->client)
+		return;
+
+	priv->client = NULL;
+
+	pm_runtime_put(v4l2_get_subdevdata(&priv->subdev));
+}
+
+static int sh_csi2_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct sh_csi2 *priv = container_of(sd, struct sh_csi2, subdev);
+
+	if (on)
+		return sh_csi2_client_connect(priv);
+
+	sh_csi2_client_disconnect(priv);
+	return 0;
+}
+
+static struct v4l2_subdev_core_ops sh_csi2_subdev_core_ops = {
+	.s_power	= sh_csi2_s_power,
+};
+
+static struct v4l2_subdev_ops sh_csi2_subdev_ops = {
+	.core	= &sh_csi2_subdev_core_ops,
+	.video	= &sh_csi2_subdev_video_ops,
+	.pad	= &sh_csi2_subdev_pad_ops,
+};
+
+static int sh_csi2_probe(struct platform_device *pdev)
+{
+	struct resource *res;
+	unsigned int irq;
+	int ret;
+	struct sh_csi2 *priv;
+	/* Platform data specify the PHY, lanes, ECC, CRC */
+	struct sh_csi2_pdata *pdata = pdev->dev.platform_data;
+
+	if (!pdata)
+		return -EINVAL;
+
+	priv = devm_kzalloc(&pdev->dev, sizeof(struct sh_csi2), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	/* Interrupt unused so far */
+	irq = platform_get_irq(pdev, 0);
+
+	if (!res || (int)irq <= 0) {
+		dev_err(&pdev->dev, "Not enough CSI2 platform resources.\n");
+		return -ENODEV;
+	}
+
+	/* TODO: Add support for CSI2I. Careful: different register layout! */
+	if (pdata->type != SH_CSI2C) {
+		dev_err(&pdev->dev, "Only CSI2C supported ATM.\n");
+		return -EINVAL;
+	}
+
+	priv->irq = irq;
+
+	priv->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(priv->base))
+		return PTR_ERR(priv->base);
+
+	priv->pdev = pdev;
+	priv->subdev.owner = THIS_MODULE;
+	priv->subdev.dev = &pdev->dev;
+	platform_set_drvdata(pdev, &priv->subdev);
+
+	v4l2_subdev_init(&priv->subdev, &sh_csi2_subdev_ops);
+	v4l2_set_subdevdata(&priv->subdev, &pdev->dev);
+
+	snprintf(priv->subdev.name, V4L2_SUBDEV_NAME_SIZE, "%s.mipi-csi",
+		 dev_name(&pdev->dev));
+
+	ret = v4l2_async_register_subdev(&priv->subdev);
+	if (ret < 0)
+		return ret;
+
+	pm_runtime_enable(&pdev->dev);
+
+	dev_dbg(&pdev->dev, "CSI2 probed.\n");
+
+	return 0;
+}
+
+static int sh_csi2_remove(struct platform_device *pdev)
+{
+	struct v4l2_subdev *subdev = platform_get_drvdata(pdev);
+	struct sh_csi2 *priv = container_of(subdev, struct sh_csi2, subdev);
+
+	v4l2_async_unregister_subdev(&priv->subdev);
+	pm_runtime_disable(&pdev->dev);
+
+	return 0;
+}
+
+static struct platform_driver __refdata sh_csi2_pdrv = {
+	.remove	= sh_csi2_remove,
+	.probe	= sh_csi2_probe,
+	.driver	= {
+		.name	= "sh-mobile-csi2",
+	},
+};
+
+module_platform_driver(sh_csi2_pdrv);
+
+MODULE_DESCRIPTION("SH-Mobile MIPI CSI-2 driver");
+MODULE_AUTHOR("Guennadi Liakhovetski <g.liakhovetski@gmx.de>");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:sh-mobile-csi2");
diff --git a/drivers/media/platform/soc_camera/soc_camera.c b/drivers/media/platform/soc_camera/soc_camera.c
new file mode 100644
index 0000000..dc98122
--- /dev/null
+++ b/drivers/media/platform/soc_camera/soc_camera.c
@@ -0,0 +1,2268 @@
+/*
+ * camera image capture (abstract) bus driver
+ *
+ * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
+ *
+ * This driver provides an interface between platform-specific camera
+ * busses and camera devices. It should be used if the camera is
+ * connected not over a "proper" bus like PCI or USB, but over a
+ * special bus, like, for example, the Quick Capture interface on PXA270
+ * SoCs. Later it should also be used for i.MX31 SoCs from Freescale.
+ * It can handle multiple cameras and / or multiple busses, which can
+ * be used, e.g., in stereo-vision applications.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/vmalloc.h>
+
+#include <media/soc_camera.h>
+#include <media/soc_mediabus.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-clk.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-of.h>
+#include <media/videobuf-core.h>
+#include <media/videobuf2-v4l2.h>
+
+/* Default to VGA resolution */
+#define DEFAULT_WIDTH	640
+#define DEFAULT_HEIGHT	480
+
+#define is_streaming(ici, icd)				\
+	(((ici)->ops->init_videobuf) ?			\
+	 (icd)->vb_vidq.streaming :			\
+	 vb2_is_streaming(&(icd)->vb2_vidq))
+
+#define MAP_MAX_NUM 32
+static DECLARE_BITMAP(device_map, MAP_MAX_NUM);
+static LIST_HEAD(hosts);
+static LIST_HEAD(devices);
+/*
+ * Protects lists and bitmaps of hosts and devices.
+ * Lock nesting: Ok to take ->host_lock under list_lock.
+ */
+static DEFINE_MUTEX(list_lock);
+
+struct soc_camera_async_client {
+	struct v4l2_async_subdev *sensor;
+	struct v4l2_async_notifier notifier;
+	struct platform_device *pdev;
+	struct list_head list;		/* needed for clean up */
+};
+
+static int soc_camera_video_start(struct soc_camera_device *icd);
+static int video_dev_create(struct soc_camera_device *icd);
+
+int soc_camera_power_on(struct device *dev, struct soc_camera_subdev_desc *ssdd,
+			struct v4l2_clk *clk)
+{
+	int ret;
+	bool clock_toggle;
+
+	if (clk && (!ssdd->unbalanced_power ||
+		    !test_and_set_bit(0, &ssdd->clock_state))) {
+		ret = v4l2_clk_enable(clk);
+		if (ret < 0) {
+			dev_err(dev, "Cannot enable clock: %d\n", ret);
+			return ret;
+		}
+		clock_toggle = true;
+	} else {
+		clock_toggle = false;
+	}
+
+	ret = regulator_bulk_enable(ssdd->sd_pdata.num_regulators,
+				    ssdd->sd_pdata.regulators);
+	if (ret < 0) {
+		dev_err(dev, "Cannot enable regulators\n");
+		goto eregenable;
+	}
+
+	if (ssdd->power) {
+		ret = ssdd->power(dev, 1);
+		if (ret < 0) {
+			dev_err(dev,
+				"Platform failed to power-on the camera.\n");
+			goto epwron;
+		}
+	}
+
+	return 0;
+
+epwron:
+	regulator_bulk_disable(ssdd->sd_pdata.num_regulators,
+			       ssdd->sd_pdata.regulators);
+eregenable:
+	if (clock_toggle)
+		v4l2_clk_disable(clk);
+
+	return ret;
+}
+EXPORT_SYMBOL(soc_camera_power_on);
+
+int soc_camera_power_off(struct device *dev, struct soc_camera_subdev_desc *ssdd,
+			 struct v4l2_clk *clk)
+{
+	int ret = 0;
+	int err;
+
+	if (ssdd->power) {
+		err = ssdd->power(dev, 0);
+		if (err < 0) {
+			dev_err(dev,
+				"Platform failed to power-off the camera.\n");
+			ret = err;
+		}
+	}
+
+	err = regulator_bulk_disable(ssdd->sd_pdata.num_regulators,
+				     ssdd->sd_pdata.regulators);
+	if (err < 0) {
+		dev_err(dev, "Cannot disable regulators\n");
+		ret = ret ? : err;
+	}
+
+	if (clk && (!ssdd->unbalanced_power || test_and_clear_bit(0, &ssdd->clock_state)))
+		v4l2_clk_disable(clk);
+
+	return ret;
+}
+EXPORT_SYMBOL(soc_camera_power_off);
+
+int soc_camera_power_init(struct device *dev, struct soc_camera_subdev_desc *ssdd)
+{
+	/* Should not have any effect in synchronous case */
+	return devm_regulator_bulk_get(dev, ssdd->sd_pdata.num_regulators,
+				       ssdd->sd_pdata.regulators);
+}
+EXPORT_SYMBOL(soc_camera_power_init);
+
+static int __soc_camera_power_on(struct soc_camera_device *icd)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	int ret;
+
+	ret = v4l2_subdev_call(sd, core, s_power, 1);
+	if (ret < 0 && ret != -ENOIOCTLCMD && ret != -ENODEV)
+		return ret;
+
+	return 0;
+}
+
+static int __soc_camera_power_off(struct soc_camera_device *icd)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	int ret;
+
+	ret = v4l2_subdev_call(sd, core, s_power, 0);
+	if (ret < 0 && ret != -ENOIOCTLCMD && ret != -ENODEV)
+		return ret;
+
+	return 0;
+}
+
+static int soc_camera_clock_start(struct soc_camera_host *ici)
+{
+	int ret;
+
+	if (!ici->ops->clock_start)
+		return 0;
+
+	mutex_lock(&ici->clk_lock);
+	ret = ici->ops->clock_start(ici);
+	mutex_unlock(&ici->clk_lock);
+
+	return ret;
+}
+
+static void soc_camera_clock_stop(struct soc_camera_host *ici)
+{
+	if (!ici->ops->clock_stop)
+		return;
+
+	mutex_lock(&ici->clk_lock);
+	ici->ops->clock_stop(ici);
+	mutex_unlock(&ici->clk_lock);
+}
+
+const struct soc_camera_format_xlate *soc_camera_xlate_by_fourcc(
+	struct soc_camera_device *icd, unsigned int fourcc)
+{
+	unsigned int i;
+
+	for (i = 0; i < icd->num_user_formats; i++)
+		if (icd->user_formats[i].host_fmt->fourcc == fourcc)
+			return icd->user_formats + i;
+	return NULL;
+}
+EXPORT_SYMBOL(soc_camera_xlate_by_fourcc);
+
+/**
+ * soc_camera_apply_board_flags() - apply platform SOCAM_SENSOR_INVERT_* flags
+ * @ssdd:	camera platform parameters
+ * @cfg:	media bus configuration
+ * @return:	resulting flags
+ */
+unsigned long soc_camera_apply_board_flags(struct soc_camera_subdev_desc *ssdd,
+					   const struct v4l2_mbus_config *cfg)
+{
+	unsigned long f, flags = cfg->flags;
+
+	/* If only one of the two polarities is supported, switch to the opposite */
+	if (ssdd->flags & SOCAM_SENSOR_INVERT_HSYNC) {
+		f = flags & (V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_HSYNC_ACTIVE_LOW);
+		if (f == V4L2_MBUS_HSYNC_ACTIVE_HIGH || f == V4L2_MBUS_HSYNC_ACTIVE_LOW)
+			flags ^= V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_HSYNC_ACTIVE_LOW;
+	}
+
+	if (ssdd->flags & SOCAM_SENSOR_INVERT_VSYNC) {
+		f = flags & (V4L2_MBUS_VSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_LOW);
+		if (f == V4L2_MBUS_VSYNC_ACTIVE_HIGH || f == V4L2_MBUS_VSYNC_ACTIVE_LOW)
+			flags ^= V4L2_MBUS_VSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_LOW;
+	}
+
+	if (ssdd->flags & SOCAM_SENSOR_INVERT_PCLK) {
+		f = flags & (V4L2_MBUS_PCLK_SAMPLE_RISING | V4L2_MBUS_PCLK_SAMPLE_FALLING);
+		if (f == V4L2_MBUS_PCLK_SAMPLE_RISING || f == V4L2_MBUS_PCLK_SAMPLE_FALLING)
+			flags ^= V4L2_MBUS_PCLK_SAMPLE_RISING | V4L2_MBUS_PCLK_SAMPLE_FALLING;
+	}
+
+	return flags;
+}
+EXPORT_SYMBOL(soc_camera_apply_board_flags);
+
+#define pixfmtstr(x) (x) & 0xff, ((x) >> 8) & 0xff, ((x) >> 16) & 0xff, \
+	((x) >> 24) & 0xff
+
+static int soc_camera_try_fmt(struct soc_camera_device *icd,
+			      struct v4l2_format *f)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	const struct soc_camera_format_xlate *xlate;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	int ret;
+
+	dev_dbg(icd->pdev, "TRY_FMT(%c%c%c%c, %ux%u)\n",
+		pixfmtstr(pix->pixelformat), pix->width, pix->height);
+
+	if (pix->pixelformat != V4L2_PIX_FMT_JPEG &&
+	    !(ici->capabilities & SOCAM_HOST_CAP_STRIDE)) {
+		pix->bytesperline = 0;
+		pix->sizeimage = 0;
+	}
+
+	ret = ici->ops->try_fmt(icd, f);
+	if (ret < 0)
+		return ret;
+
+	xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
+	if (!xlate)
+		return -EINVAL;
+
+	ret = soc_mbus_bytes_per_line(pix->width, xlate->host_fmt);
+	if (ret < 0)
+		return ret;
+
+	pix->bytesperline = max_t(u32, pix->bytesperline, ret);
+
+	ret = soc_mbus_image_size(xlate->host_fmt, pix->bytesperline,
+				  pix->height);
+	if (ret < 0)
+		return ret;
+
+	pix->sizeimage = max_t(u32, pix->sizeimage, ret);
+
+	return 0;
+}
+
+static int soc_camera_try_fmt_vid_cap(struct file *file, void *priv,
+				      struct v4l2_format *f)
+{
+	struct soc_camera_device *icd = file->private_data;
+
+	WARN_ON(priv != file->private_data);
+
+	/* Only single-plane capture is supported so far */
+	if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	/* limit format to hardware capabilities */
+	return soc_camera_try_fmt(icd, f);
+}
+
+static int soc_camera_enum_input(struct file *file, void *priv,
+				 struct v4l2_input *inp)
+{
+	struct soc_camera_device *icd = file->private_data;
+
+	if (inp->index != 0)
+		return -EINVAL;
+
+	/* default is camera */
+	inp->type = V4L2_INPUT_TYPE_CAMERA;
+	inp->std = icd->vdev->tvnorms;
+	strcpy(inp->name, "Camera");
+
+	return 0;
+}
+
+static int soc_camera_g_input(struct file *file, void *priv, unsigned int *i)
+{
+	*i = 0;
+
+	return 0;
+}
+
+static int soc_camera_s_input(struct file *file, void *priv, unsigned int i)
+{
+	if (i > 0)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int soc_camera_s_std(struct file *file, void *priv, v4l2_std_id a)
+{
+	struct soc_camera_device *icd = file->private_data;
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+
+	return v4l2_subdev_call(sd, video, s_std, a);
+}
+
+static int soc_camera_g_std(struct file *file, void *priv, v4l2_std_id *a)
+{
+	struct soc_camera_device *icd = file->private_data;
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+
+	return v4l2_subdev_call(sd, video, g_std, a);
+}
+
+static int soc_camera_enum_framesizes(struct file *file, void *fh,
+					 struct v4l2_frmsizeenum *fsize)
+{
+	struct soc_camera_device *icd = file->private_data;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+
+	return ici->ops->enum_framesizes(icd, fsize);
+}
+
+static int soc_camera_reqbufs(struct file *file, void *priv,
+			      struct v4l2_requestbuffers *p)
+{
+	int ret;
+	struct soc_camera_device *icd = file->private_data;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+
+	WARN_ON(priv != file->private_data);
+
+	if (icd->streamer && icd->streamer != file)
+		return -EBUSY;
+
+	if (ici->ops->init_videobuf) {
+		ret = videobuf_reqbufs(&icd->vb_vidq, p);
+		if (ret < 0)
+			return ret;
+
+		ret = ici->ops->reqbufs(icd, p);
+	} else {
+		ret = vb2_reqbufs(&icd->vb2_vidq, p);
+	}
+
+	if (!ret)
+		icd->streamer = p->count ? file : NULL;
+	return ret;
+}
+
+static int soc_camera_querybuf(struct file *file, void *priv,
+			       struct v4l2_buffer *p)
+{
+	struct soc_camera_device *icd = file->private_data;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+
+	WARN_ON(priv != file->private_data);
+
+	if (ici->ops->init_videobuf)
+		return videobuf_querybuf(&icd->vb_vidq, p);
+	else
+		return vb2_querybuf(&icd->vb2_vidq, p);
+}
+
+static int soc_camera_qbuf(struct file *file, void *priv,
+			   struct v4l2_buffer *p)
+{
+	struct soc_camera_device *icd = file->private_data;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+
+	WARN_ON(priv != file->private_data);
+
+	if (icd->streamer != file)
+		return -EBUSY;
+
+	if (ici->ops->init_videobuf)
+		return videobuf_qbuf(&icd->vb_vidq, p);
+	else
+		return vb2_qbuf(&icd->vb2_vidq, p);
+}
+
+static int soc_camera_dqbuf(struct file *file, void *priv,
+			    struct v4l2_buffer *p)
+{
+	struct soc_camera_device *icd = file->private_data;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+
+	WARN_ON(priv != file->private_data);
+
+	if (icd->streamer != file)
+		return -EBUSY;
+
+	if (ici->ops->init_videobuf)
+		return videobuf_dqbuf(&icd->vb_vidq, p, file->f_flags & O_NONBLOCK);
+	else
+		return vb2_dqbuf(&icd->vb2_vidq, p, file->f_flags & O_NONBLOCK);
+}
+
+static int soc_camera_create_bufs(struct file *file, void *priv,
+			    struct v4l2_create_buffers *create)
+{
+	struct soc_camera_device *icd = file->private_data;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	int ret;
+
+	/* videobuf2 only */
+	if (ici->ops->init_videobuf)
+		return -ENOTTY;
+
+	if (icd->streamer && icd->streamer != file)
+		return -EBUSY;
+
+	ret = vb2_create_bufs(&icd->vb2_vidq, create);
+	if (!ret)
+		icd->streamer = file;
+	return ret;
+}
+
+static int soc_camera_prepare_buf(struct file *file, void *priv,
+				  struct v4l2_buffer *b)
+{
+	struct soc_camera_device *icd = file->private_data;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+
+	/* videobuf2 only */
+	if (ici->ops->init_videobuf)
+		return -EINVAL;
+	else
+		return vb2_prepare_buf(&icd->vb2_vidq, b);
+}
+
+static int soc_camera_expbuf(struct file *file, void *priv,
+			     struct v4l2_exportbuffer *p)
+{
+	struct soc_camera_device *icd = file->private_data;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+
+	/* videobuf2 only */
+	if (ici->ops->init_videobuf)
+		return -ENOTTY;
+
+	if (icd->streamer && icd->streamer != file)
+		return -EBUSY;
+	return vb2_expbuf(&icd->vb2_vidq, p);
+}
+
+/* Always entered with .host_lock held */
+static int soc_camera_init_user_formats(struct soc_camera_device *icd)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	unsigned int i, fmts = 0, raw_fmts = 0;
+	int ret;
+	struct v4l2_subdev_mbus_code_enum code = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+
+	while (!v4l2_subdev_call(sd, pad, enum_mbus_code, NULL, &code)) {
+		raw_fmts++;
+		code.index++;
+	}
+
+	if (!ici->ops->get_formats)
+		/*
+		 * Fallback mode - the host will have to serve all
+		 * sensor-provided formats one-to-one to the user
+		 */
+		fmts = raw_fmts;
+	else
+		/*
+		 * First pass - only count formats this host-sensor
+		 * configuration can provide
+		 */
+		for (i = 0; i < raw_fmts; i++) {
+			ret = ici->ops->get_formats(icd, i, NULL);
+			if (ret < 0)
+				return ret;
+			fmts += ret;
+		}
+
+	if (!fmts)
+		return -ENXIO;
+
+	icd->user_formats =
+		vmalloc(fmts * sizeof(struct soc_camera_format_xlate));
+	if (!icd->user_formats)
+		return -ENOMEM;
+
+	dev_dbg(icd->pdev, "Found %d supported formats.\n", fmts);
+
+	/* Second pass - actually fill data formats */
+	fmts = 0;
+	for (i = 0; i < raw_fmts; i++)
+		if (!ici->ops->get_formats) {
+			code.index = i;
+			v4l2_subdev_call(sd, pad, enum_mbus_code, NULL, &code);
+			icd->user_formats[fmts].host_fmt =
+				soc_mbus_get_fmtdesc(code.code);
+			if (icd->user_formats[fmts].host_fmt)
+				icd->user_formats[fmts++].code = code.code;
+		} else {
+			ret = ici->ops->get_formats(icd, i,
+						    &icd->user_formats[fmts]);
+			if (ret < 0)
+				goto egfmt;
+			fmts += ret;
+		}
+
+	icd->num_user_formats = fmts;
+	icd->current_fmt = &icd->user_formats[0];
+
+	return 0;
+
+egfmt:
+	vfree(icd->user_formats);
+	return ret;
+}
+
+/* Always entered with .host_lock held */
+static void soc_camera_free_user_formats(struct soc_camera_device *icd)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+
+	if (ici->ops->put_formats)
+		ici->ops->put_formats(icd);
+	icd->current_fmt = NULL;
+	icd->num_user_formats = 0;
+	vfree(icd->user_formats);
+	icd->user_formats = NULL;
+}
+
+/* Called with .vb_lock held, or from the first open(2), see comment there */
+static int soc_camera_set_fmt(struct soc_camera_device *icd,
+			      struct v4l2_format *f)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	int ret;
+
+	dev_dbg(icd->pdev, "S_FMT(%c%c%c%c, %ux%u)\n",
+		pixfmtstr(pix->pixelformat), pix->width, pix->height);
+
+	/* We always call try_fmt() before set_fmt() or set_crop() */
+	ret = soc_camera_try_fmt(icd, f);
+	if (ret < 0)
+		return ret;
+
+	ret = ici->ops->set_fmt(icd, f);
+	if (ret < 0) {
+		return ret;
+	} else if (!icd->current_fmt ||
+		   icd->current_fmt->host_fmt->fourcc != pix->pixelformat) {
+		dev_err(icd->pdev,
+			"Host driver hasn't set up current format correctly!\n");
+		return -EINVAL;
+	}
+
+	icd->user_width		= pix->width;
+	icd->user_height	= pix->height;
+	icd->bytesperline	= pix->bytesperline;
+	icd->sizeimage		= pix->sizeimage;
+	icd->colorspace		= pix->colorspace;
+	icd->field		= pix->field;
+	if (ici->ops->init_videobuf)
+		icd->vb_vidq.field = pix->field;
+
+	dev_dbg(icd->pdev, "set width: %d height: %d\n",
+		icd->user_width, icd->user_height);
+
+	/* set physical bus parameters */
+	return ici->ops->set_bus_param(icd);
+}
+
+static int soc_camera_add_device(struct soc_camera_device *icd)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	int ret;
+
+	if (ici->icd)
+		return -EBUSY;
+
+	if (!icd->clk) {
+		ret = soc_camera_clock_start(ici);
+		if (ret < 0)
+			return ret;
+	}
+
+	if (ici->ops->add) {
+		ret = ici->ops->add(icd);
+		if (ret < 0)
+			goto eadd;
+	}
+
+	ici->icd = icd;
+
+	return 0;
+
+eadd:
+	if (!icd->clk)
+		soc_camera_clock_stop(ici);
+	return ret;
+}
+
+static void soc_camera_remove_device(struct soc_camera_device *icd)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+
+	if (WARN_ON(icd != ici->icd))
+		return;
+
+	if (ici->ops->remove)
+		ici->ops->remove(icd);
+	if (!icd->clk)
+		soc_camera_clock_stop(ici);
+	ici->icd = NULL;
+}
+
+static int soc_camera_open(struct file *file)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct soc_camera_device *icd;
+	struct soc_camera_host *ici;
+	int ret;
+
+	/*
+	 * Don't mess with the host during probe: wait until the loop in
+	 * scan_add_host() completes. Also protect against a race with
+	 * soc_camera_host_unregister().
+	 */
+	if (mutex_lock_interruptible(&list_lock))
+		return -ERESTARTSYS;
+
+	if (!vdev || !video_is_registered(vdev)) {
+		mutex_unlock(&list_lock);
+		return -ENODEV;
+	}
+
+	icd = video_get_drvdata(vdev);
+	ici = to_soc_camera_host(icd->parent);
+
+	ret = try_module_get(ici->ops->owner) ? 0 : -ENODEV;
+	mutex_unlock(&list_lock);
+
+	if (ret < 0) {
+		dev_err(icd->pdev, "Couldn't lock capture bus driver.\n");
+		return ret;
+	}
+
+	if (!to_soc_camera_control(icd)) {
+		/* No device driver attached */
+		ret = -ENODEV;
+		goto econtrol;
+	}
+
+	if (mutex_lock_interruptible(&ici->host_lock)) {
+		ret = -ERESTARTSYS;
+		goto elockhost;
+	}
+	icd->use_count++;
+
+	/* Now we really have to activate the camera */
+	if (icd->use_count == 1) {
+		struct soc_camera_desc *sdesc = to_soc_camera_desc(icd);
+		/* Restore parameters before the last close() per V4L2 API */
+		struct v4l2_format f = {
+			.type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
+			.fmt.pix = {
+				.width		= icd->user_width,
+				.height		= icd->user_height,
+				.field		= icd->field,
+				.colorspace	= icd->colorspace,
+				.pixelformat	=
+					icd->current_fmt->host_fmt->fourcc,
+			},
+		};
+
+		/* The camera could have been already on, try to reset */
+		if (sdesc->subdev_desc.reset)
+			if (icd->control)
+				sdesc->subdev_desc.reset(icd->control);
+
+		ret = soc_camera_add_device(icd);
+		if (ret < 0) {
+			dev_err(icd->pdev, "Couldn't activate the camera: %d\n", ret);
+			goto eiciadd;
+		}
+
+		ret = __soc_camera_power_on(icd);
+		if (ret < 0)
+			goto epower;
+
+		pm_runtime_enable(&icd->vdev->dev);
+		ret = pm_runtime_resume(&icd->vdev->dev);
+		if (ret < 0 && ret != -ENOSYS)
+			goto eresume;
+
+		/*
+		 * Try to configure with default parameters. Notice: this is the
+		 * very first open, so, we cannot race against other calls,
+		 * apart from someone else calling open() simultaneously, but
+		 * .host_lock is protecting us against it.
+		 */
+		ret = soc_camera_set_fmt(icd, &f);
+		if (ret < 0)
+			goto esfmt;
+
+		if (ici->ops->init_videobuf) {
+			ici->ops->init_videobuf(&icd->vb_vidq, icd);
+		} else {
+			ret = ici->ops->init_videobuf2(&icd->vb2_vidq, icd);
+			if (ret < 0)
+				goto einitvb;
+		}
+		v4l2_ctrl_handler_setup(&icd->ctrl_handler);
+	}
+	mutex_unlock(&ici->host_lock);
+
+	file->private_data = icd;
+	dev_dbg(icd->pdev, "camera device open\n");
+
+	return 0;
+
+	/*
+	 * All errors are entered with the .host_lock held, first four also
+	 * with use_count == 1
+	 */
+einitvb:
+esfmt:
+	pm_runtime_disable(&icd->vdev->dev);
+eresume:
+	__soc_camera_power_off(icd);
+epower:
+	soc_camera_remove_device(icd);
+eiciadd:
+	icd->use_count--;
+	mutex_unlock(&ici->host_lock);
+elockhost:
+econtrol:
+	module_put(ici->ops->owner);
+
+	return ret;
+}
+
+static int soc_camera_close(struct file *file)
+{
+	struct soc_camera_device *icd = file->private_data;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+
+	mutex_lock(&ici->host_lock);
+	if (icd->streamer == file) {
+		if (ici->ops->init_videobuf2)
+			vb2_queue_release(&icd->vb2_vidq);
+		icd->streamer = NULL;
+	}
+	icd->use_count--;
+	if (!icd->use_count) {
+		pm_runtime_suspend(&icd->vdev->dev);
+		pm_runtime_disable(&icd->vdev->dev);
+
+		__soc_camera_power_off(icd);
+
+		soc_camera_remove_device(icd);
+	}
+
+	mutex_unlock(&ici->host_lock);
+
+	module_put(ici->ops->owner);
+
+	dev_dbg(icd->pdev, "camera device close\n");
+
+	return 0;
+}
+
+static ssize_t soc_camera_read(struct file *file, char __user *buf,
+			       size_t count, loff_t *ppos)
+{
+	struct soc_camera_device *icd = file->private_data;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+
+	dev_dbg(icd->pdev, "read called, buf %p\n", buf);
+
+	if (ici->ops->init_videobuf2 && icd->vb2_vidq.io_modes & VB2_READ)
+		return vb2_read(&icd->vb2_vidq, buf, count, ppos,
+				file->f_flags & O_NONBLOCK);
+
+	dev_err(icd->pdev, "camera device read not implemented\n");
+
+	return -EINVAL;
+}
+
+static int soc_camera_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct soc_camera_device *icd = file->private_data;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	int err;
+
+	dev_dbg(icd->pdev, "mmap called, vma=0x%08lx\n", (unsigned long)vma);
+
+	if (icd->streamer != file)
+		return -EBUSY;
+
+	if (mutex_lock_interruptible(&ici->host_lock))
+		return -ERESTARTSYS;
+	if (ici->ops->init_videobuf)
+		err = videobuf_mmap_mapper(&icd->vb_vidq, vma);
+	else
+		err = vb2_mmap(&icd->vb2_vidq, vma);
+	mutex_unlock(&ici->host_lock);
+
+	dev_dbg(icd->pdev, "vma start=0x%08lx, size=%ld, ret=%d\n",
+		(unsigned long)vma->vm_start,
+		(unsigned long)vma->vm_end - (unsigned long)vma->vm_start,
+		err);
+
+	return err;
+}
+
+static unsigned int soc_camera_poll(struct file *file, poll_table *pt)
+{
+	struct soc_camera_device *icd = file->private_data;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	unsigned res = POLLERR;
+
+	if (icd->streamer != file)
+		return POLLERR;
+
+	mutex_lock(&ici->host_lock);
+	if (ici->ops->init_videobuf && list_empty(&icd->vb_vidq.stream))
+		dev_err(icd->pdev, "Trying to poll with no queued buffers!\n");
+	else
+		res = ici->ops->poll(file, pt);
+	mutex_unlock(&ici->host_lock);
+	return res;
+}
+
+static struct v4l2_file_operations soc_camera_fops = {
+	.owner		= THIS_MODULE,
+	.open		= soc_camera_open,
+	.release	= soc_camera_close,
+	.unlocked_ioctl	= video_ioctl2,
+	.read		= soc_camera_read,
+	.mmap		= soc_camera_mmap,
+	.poll		= soc_camera_poll,
+};
+
+static int soc_camera_s_fmt_vid_cap(struct file *file, void *priv,
+				    struct v4l2_format *f)
+{
+	struct soc_camera_device *icd = file->private_data;
+	int ret;
+
+	WARN_ON(priv != file->private_data);
+
+	if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+		dev_warn(icd->pdev, "Wrong buf-type %d\n", f->type);
+		return -EINVAL;
+	}
+
+	if (icd->streamer && icd->streamer != file)
+		return -EBUSY;
+
+	if (is_streaming(to_soc_camera_host(icd->parent), icd)) {
+		dev_err(icd->pdev, "S_FMT denied: queue initialised\n");
+		return -EBUSY;
+	}
+
+	ret = soc_camera_set_fmt(icd, f);
+
+	if (!ret && !icd->streamer)
+		icd->streamer = file;
+
+	return ret;
+}
+
+static int soc_camera_enum_fmt_vid_cap(struct file *file, void  *priv,
+				       struct v4l2_fmtdesc *f)
+{
+	struct soc_camera_device *icd = file->private_data;
+	const struct soc_mbus_pixelfmt *format;
+
+	WARN_ON(priv != file->private_data);
+
+	if (f->index >= icd->num_user_formats)
+		return -EINVAL;
+
+	format = icd->user_formats[f->index].host_fmt;
+
+	if (format->name)
+		strlcpy(f->description, format->name, sizeof(f->description));
+	f->pixelformat = format->fourcc;
+	return 0;
+}
+
+static int soc_camera_g_fmt_vid_cap(struct file *file, void *priv,
+				    struct v4l2_format *f)
+{
+	struct soc_camera_device *icd = file->private_data;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+
+	WARN_ON(priv != file->private_data);
+
+	if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	pix->width		= icd->user_width;
+	pix->height		= icd->user_height;
+	pix->bytesperline	= icd->bytesperline;
+	pix->sizeimage		= icd->sizeimage;
+	pix->field		= icd->field;
+	pix->pixelformat	= icd->current_fmt->host_fmt->fourcc;
+	pix->colorspace		= icd->colorspace;
+	dev_dbg(icd->pdev, "current_fmt->fourcc: 0x%08x\n",
+		icd->current_fmt->host_fmt->fourcc);
+	return 0;
+}
+
+static int soc_camera_querycap(struct file *file, void  *priv,
+			       struct v4l2_capability *cap)
+{
+	struct soc_camera_device *icd = file->private_data;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+
+	WARN_ON(priv != file->private_data);
+
+	strlcpy(cap->driver, ici->drv_name, sizeof(cap->driver));
+	return ici->ops->querycap(ici, cap);
+}
+
+static int soc_camera_streamon(struct file *file, void *priv,
+			       enum v4l2_buf_type i)
+{
+	struct soc_camera_device *icd = file->private_data;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	int ret;
+
+	WARN_ON(priv != file->private_data);
+
+	if (i != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	if (icd->streamer != file)
+		return -EBUSY;
+
+	/* This calls buf_queue from host driver's videobuf_queue_ops */
+	if (ici->ops->init_videobuf)
+		ret = videobuf_streamon(&icd->vb_vidq);
+	else
+		ret = vb2_streamon(&icd->vb2_vidq, i);
+
+	if (!ret)
+		v4l2_subdev_call(sd, video, s_stream, 1);
+
+	return ret;
+}
+
+static int soc_camera_streamoff(struct file *file, void *priv,
+				enum v4l2_buf_type i)
+{
+	struct soc_camera_device *icd = file->private_data;
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	int ret;
+
+	WARN_ON(priv != file->private_data);
+
+	if (i != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	if (icd->streamer != file)
+		return -EBUSY;
+
+	/*
+	 * This calls buf_release from host driver's videobuf_queue_ops for all
+	 * remaining buffers. When the last buffer is freed, stop capture
+	 */
+	if (ici->ops->init_videobuf)
+		ret = videobuf_streamoff(&icd->vb_vidq);
+	else
+		ret = vb2_streamoff(&icd->vb2_vidq, i);
+
+	v4l2_subdev_call(sd, video, s_stream, 0);
+
+	return ret;
+}
+
+static int soc_camera_cropcap(struct file *file, void *fh,
+			      struct v4l2_cropcap *a)
+{
+	struct soc_camera_device *icd = file->private_data;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+
+	return ici->ops->cropcap(icd, a);
+}
+
+static int soc_camera_g_crop(struct file *file, void *fh,
+			     struct v4l2_crop *a)
+{
+	struct soc_camera_device *icd = file->private_data;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	int ret;
+
+	ret = ici->ops->get_crop(icd, a);
+
+	return ret;
+}
+
+/*
+ * According to the V4L2 API, drivers shall not update the struct v4l2_crop
+ * argument with the actual geometry, instead, the user shall use G_CROP to
+ * retrieve it.
+ */
+static int soc_camera_s_crop(struct file *file, void *fh,
+			     const struct v4l2_crop *a)
+{
+	struct soc_camera_device *icd = file->private_data;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	const struct v4l2_rect *rect = &a->c;
+	struct v4l2_crop current_crop;
+	int ret;
+
+	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	dev_dbg(icd->pdev, "S_CROP(%ux%u@%u:%u)\n",
+		rect->width, rect->height, rect->left, rect->top);
+
+	current_crop.type = a->type;
+
+	/* If get_crop fails, we'll let host and / or client drivers decide */
+	ret = ici->ops->get_crop(icd, &current_crop);
+
+	/* Prohibit window size change with initialised buffers */
+	if (ret < 0) {
+		dev_err(icd->pdev,
+			"S_CROP denied: getting current crop failed\n");
+	} else if ((a->c.width == current_crop.c.width &&
+		    a->c.height == current_crop.c.height) ||
+		   !is_streaming(ici, icd)) {
+		/* same size or not streaming - use .set_crop() */
+		ret = ici->ops->set_crop(icd, a);
+	} else if (ici->ops->set_livecrop) {
+		ret = ici->ops->set_livecrop(icd, a);
+	} else {
+		dev_err(icd->pdev,
+			"S_CROP denied: queue initialised and sizes differ\n");
+		ret = -EBUSY;
+	}
+
+	return ret;
+}
+
+static int soc_camera_g_selection(struct file *file, void *fh,
+				  struct v4l2_selection *s)
+{
+	struct soc_camera_device *icd = file->private_data;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+
+	/* With a wrong type no need to try to fall back to cropping */
+	if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	if (!ici->ops->get_selection)
+		return -ENOTTY;
+
+	return ici->ops->get_selection(icd, s);
+}
+
+static int soc_camera_s_selection(struct file *file, void *fh,
+				  struct v4l2_selection *s)
+{
+	struct soc_camera_device *icd = file->private_data;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	int ret;
+
+	/* In all these cases cropping emulation will not help */
+	if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
+	    (s->target != V4L2_SEL_TGT_COMPOSE &&
+	     s->target != V4L2_SEL_TGT_CROP))
+		return -EINVAL;
+
+	if (s->target == V4L2_SEL_TGT_COMPOSE) {
+		/* No output size change during a running capture! */
+		if (is_streaming(ici, icd) &&
+		    (icd->user_width != s->r.width ||
+		     icd->user_height != s->r.height))
+			return -EBUSY;
+
+		/*
+		 * Only one user is allowed to change the output format, touch
+		 * buffers, start / stop streaming, poll for data
+		 */
+		if (icd->streamer && icd->streamer != file)
+			return -EBUSY;
+	}
+
+	if (!ici->ops->set_selection)
+		return -ENOTTY;
+
+	ret = ici->ops->set_selection(icd, s);
+	if (!ret &&
+	    s->target == V4L2_SEL_TGT_COMPOSE) {
+		icd->user_width = s->r.width;
+		icd->user_height = s->r.height;
+		if (!icd->streamer)
+			icd->streamer = file;
+	}
+
+	return ret;
+}
+
+static int soc_camera_g_parm(struct file *file, void *fh,
+			     struct v4l2_streamparm *a)
+{
+	struct soc_camera_device *icd = file->private_data;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+
+	if (ici->ops->get_parm)
+		return ici->ops->get_parm(icd, a);
+
+	return -ENOIOCTLCMD;
+}
+
+static int soc_camera_s_parm(struct file *file, void *fh,
+			     struct v4l2_streamparm *a)
+{
+	struct soc_camera_device *icd = file->private_data;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+
+	if (ici->ops->set_parm)
+		return ici->ops->set_parm(icd, a);
+
+	return -ENOIOCTLCMD;
+}
+
+static int soc_camera_probe(struct soc_camera_host *ici,
+			    struct soc_camera_device *icd);
+
+/* So far this function cannot fail */
+static void scan_add_host(struct soc_camera_host *ici)
+{
+	struct soc_camera_device *icd;
+
+	mutex_lock(&list_lock);
+
+	list_for_each_entry(icd, &devices, list)
+		if (icd->iface == ici->nr) {
+			struct soc_camera_desc *sdesc = to_soc_camera_desc(icd);
+			struct soc_camera_subdev_desc *ssdd = &sdesc->subdev_desc;
+
+			/* The camera could have been already on, try to reset */
+			if (ssdd->reset)
+				if (icd->control)
+					ssdd->reset(icd->control);
+
+			icd->parent = ici->v4l2_dev.dev;
+
+			/* Ignore errors */
+			soc_camera_probe(ici, icd);
+		}
+
+	mutex_unlock(&list_lock);
+}
+
+/*
+ * It is invalid to call v4l2_clk_enable() after a successful probing
+ * asynchronously outside of V4L2 operations, i.e. with .host_lock not held.
+ */
+static int soc_camera_clk_enable(struct v4l2_clk *clk)
+{
+	struct soc_camera_device *icd = clk->priv;
+	struct soc_camera_host *ici;
+
+	if (!icd || !icd->parent)
+		return -ENODEV;
+
+	ici = to_soc_camera_host(icd->parent);
+
+	if (!try_module_get(ici->ops->owner))
+		return -ENODEV;
+
+	/*
+	 * If a different client is currently being probed, the host will tell
+	 * you to go
+	 */
+	return soc_camera_clock_start(ici);
+}
+
+static void soc_camera_clk_disable(struct v4l2_clk *clk)
+{
+	struct soc_camera_device *icd = clk->priv;
+	struct soc_camera_host *ici;
+
+	if (!icd || !icd->parent)
+		return;
+
+	ici = to_soc_camera_host(icd->parent);
+
+	soc_camera_clock_stop(ici);
+
+	module_put(ici->ops->owner);
+}
+
+/*
+ * Eventually, it would be more logical to make the respective host the clock
+ * owner, but then we would have to copy this struct for each ici. Besides, it
+ * would introduce the circular dependency problem, unless we port all client
+ * drivers to release the clock, when not in use.
+ */
+static const struct v4l2_clk_ops soc_camera_clk_ops = {
+	.owner = THIS_MODULE,
+	.enable = soc_camera_clk_enable,
+	.disable = soc_camera_clk_disable,
+};
+
+static int soc_camera_dyn_pdev(struct soc_camera_desc *sdesc,
+			       struct soc_camera_async_client *sasc)
+{
+	struct platform_device *pdev;
+	int ret, i;
+
+	mutex_lock(&list_lock);
+	i = find_first_zero_bit(device_map, MAP_MAX_NUM);
+	if (i < MAP_MAX_NUM)
+		set_bit(i, device_map);
+	mutex_unlock(&list_lock);
+	if (i >= MAP_MAX_NUM)
+		return -ENOMEM;
+
+	pdev = platform_device_alloc("soc-camera-pdrv", i);
+	if (!pdev)
+		return -ENOMEM;
+
+	ret = platform_device_add_data(pdev, sdesc, sizeof(*sdesc));
+	if (ret < 0) {
+		platform_device_put(pdev);
+		return ret;
+	}
+
+	sasc->pdev = pdev;
+
+	return 0;
+}
+
+static struct soc_camera_device *soc_camera_add_pdev(struct soc_camera_async_client *sasc)
+{
+	struct platform_device *pdev = sasc->pdev;
+	int ret;
+
+	ret = platform_device_add(pdev);
+	if (ret < 0 || !pdev->dev.driver)
+		return NULL;
+
+	return platform_get_drvdata(pdev);
+}
+
+/* Locking: called with .host_lock held */
+static int soc_camera_probe_finish(struct soc_camera_device *icd)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_mbus_framefmt *mf = &fmt.format;
+	int ret;
+
+	sd->grp_id = soc_camera_grp_id(icd);
+	v4l2_set_subdev_hostdata(sd, icd);
+
+	v4l2_subdev_call(sd, video, g_tvnorms, &icd->vdev->tvnorms);
+
+	ret = v4l2_ctrl_add_handler(&icd->ctrl_handler, sd->ctrl_handler, NULL);
+	if (ret < 0)
+		return ret;
+
+	ret = soc_camera_add_device(icd);
+	if (ret < 0) {
+		dev_err(icd->pdev, "Couldn't activate the camera: %d\n", ret);
+		return ret;
+	}
+
+	/* At this point client .probe() should have run already */
+	ret = soc_camera_init_user_formats(icd);
+	if (ret < 0)
+		goto eusrfmt;
+
+	icd->field = V4L2_FIELD_ANY;
+
+	ret = soc_camera_video_start(icd);
+	if (ret < 0)
+		goto evidstart;
+
+	/* Try to improve our guess of a reasonable window format */
+	if (!v4l2_subdev_call(sd, pad, get_fmt, NULL, &fmt)) {
+		icd->user_width		= mf->width;
+		icd->user_height	= mf->height;
+		icd->colorspace		= mf->colorspace;
+		icd->field		= mf->field;
+	}
+	soc_camera_remove_device(icd);
+
+	return 0;
+
+evidstart:
+	soc_camera_free_user_formats(icd);
+eusrfmt:
+	soc_camera_remove_device(icd);
+
+	return ret;
+}
+
+#ifdef CONFIG_I2C_BOARDINFO
+static int soc_camera_i2c_init(struct soc_camera_device *icd,
+			       struct soc_camera_desc *sdesc)
+{
+	struct soc_camera_subdev_desc *ssdd;
+	struct i2c_client *client;
+	struct soc_camera_host *ici;
+	struct soc_camera_host_desc *shd = &sdesc->host_desc;
+	struct i2c_adapter *adap;
+	struct v4l2_subdev *subdev;
+	char clk_name[V4L2_SUBDEV_NAME_SIZE];
+	int ret;
+
+	/* First find out how we link the main client */
+	if (icd->sasc) {
+		/* Async non-OF probing handled by the subdevice list */
+		return -EPROBE_DEFER;
+	}
+
+	ici = to_soc_camera_host(icd->parent);
+	adap = i2c_get_adapter(shd->i2c_adapter_id);
+	if (!adap) {
+		dev_err(icd->pdev, "Cannot get I2C adapter #%d. No driver?\n",
+			shd->i2c_adapter_id);
+		return -ENODEV;
+	}
+
+	ssdd = kmemdup(&sdesc->subdev_desc, sizeof(*ssdd), GFP_KERNEL);
+	if (!ssdd) {
+		ret = -ENOMEM;
+		goto ealloc;
+	}
+	/*
+	 * In synchronous case we request regulators ourselves in
+	 * soc_camera_pdrv_probe(), make sure the subdevice driver doesn't try
+	 * to allocate them again.
+	 */
+	ssdd->sd_pdata.num_regulators = 0;
+	ssdd->sd_pdata.regulators = NULL;
+	shd->board_info->platform_data = ssdd;
+
+	snprintf(clk_name, sizeof(clk_name), "%d-%04x",
+		 shd->i2c_adapter_id, shd->board_info->addr);
+
+	icd->clk = v4l2_clk_register(&soc_camera_clk_ops, clk_name, icd);
+	if (IS_ERR(icd->clk)) {
+		ret = PTR_ERR(icd->clk);
+		goto eclkreg;
+	}
+
+	subdev = v4l2_i2c_new_subdev_board(&ici->v4l2_dev, adap,
+				shd->board_info, NULL);
+	if (!subdev) {
+		ret = -ENODEV;
+		goto ei2cnd;
+	}
+
+	client = v4l2_get_subdevdata(subdev);
+
+	/* Use to_i2c_client(dev) to recover the i2c client */
+	icd->control = &client->dev;
+
+	return 0;
+ei2cnd:
+	v4l2_clk_unregister(icd->clk);
+	icd->clk = NULL;
+eclkreg:
+	kfree(ssdd);
+ealloc:
+	i2c_put_adapter(adap);
+	return ret;
+}
+
+static void soc_camera_i2c_free(struct soc_camera_device *icd)
+{
+	struct i2c_client *client =
+		to_i2c_client(to_soc_camera_control(icd));
+	struct i2c_adapter *adap;
+	struct soc_camera_subdev_desc *ssdd;
+
+	icd->control = NULL;
+	if (icd->sasc)
+		return;
+
+	adap = client->adapter;
+	ssdd = client->dev.platform_data;
+	v4l2_device_unregister_subdev(i2c_get_clientdata(client));
+	i2c_unregister_device(client);
+	i2c_put_adapter(adap);
+	kfree(ssdd);
+	v4l2_clk_unregister(icd->clk);
+	icd->clk = NULL;
+}
+
+/*
+ * V4L2 asynchronous notifier callbacks. They are all called under a v4l2-async
+ * internal global mutex, therefore cannot race against other asynchronous
+ * events. Until notifier->complete() (soc_camera_async_complete()) is called,
+ * the video device node is not registered and no V4L fops can occur. Unloading
+ * of the host driver also calls a v4l2-async function, so also there we're
+ * protected.
+ */
+static int soc_camera_async_bound(struct v4l2_async_notifier *notifier,
+				  struct v4l2_subdev *sd,
+				  struct v4l2_async_subdev *asd)
+{
+	struct soc_camera_async_client *sasc = container_of(notifier,
+					struct soc_camera_async_client, notifier);
+	struct soc_camera_device *icd = platform_get_drvdata(sasc->pdev);
+
+	if (asd == sasc->sensor && !WARN_ON(icd->control)) {
+		struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+		/*
+		 * Only now we get subdevice-specific information like
+		 * regulators, flags, callbacks, etc.
+		 */
+		if (client) {
+			struct soc_camera_desc *sdesc = to_soc_camera_desc(icd);
+			struct soc_camera_subdev_desc *ssdd =
+				soc_camera_i2c_to_desc(client);
+			if (ssdd) {
+				memcpy(&sdesc->subdev_desc, ssdd,
+				       sizeof(sdesc->subdev_desc));
+				if (ssdd->reset)
+					ssdd->reset(&client->dev);
+			}
+
+			icd->control = &client->dev;
+		}
+	}
+
+	return 0;
+}
+
+static void soc_camera_async_unbind(struct v4l2_async_notifier *notifier,
+				    struct v4l2_subdev *sd,
+				    struct v4l2_async_subdev *asd)
+{
+	struct soc_camera_async_client *sasc = container_of(notifier,
+					struct soc_camera_async_client, notifier);
+	struct soc_camera_device *icd = platform_get_drvdata(sasc->pdev);
+
+	if (icd->clk) {
+		v4l2_clk_unregister(icd->clk);
+		icd->clk = NULL;
+	}
+}
+
+static int soc_camera_async_complete(struct v4l2_async_notifier *notifier)
+{
+	struct soc_camera_async_client *sasc = container_of(notifier,
+					struct soc_camera_async_client, notifier);
+	struct soc_camera_device *icd = platform_get_drvdata(sasc->pdev);
+
+	if (to_soc_camera_control(icd)) {
+		struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+		int ret;
+
+		mutex_lock(&list_lock);
+		ret = soc_camera_probe(ici, icd);
+		mutex_unlock(&list_lock);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int scan_async_group(struct soc_camera_host *ici,
+			    struct v4l2_async_subdev **asd, unsigned int size)
+{
+	struct soc_camera_async_subdev *sasd;
+	struct soc_camera_async_client *sasc;
+	struct soc_camera_device *icd;
+	struct soc_camera_desc sdesc = {.host_desc.bus_id = ici->nr,};
+	char clk_name[V4L2_SUBDEV_NAME_SIZE];
+	unsigned int i;
+	int ret;
+
+	/* First look for a sensor */
+	for (i = 0; i < size; i++) {
+		sasd = container_of(asd[i], struct soc_camera_async_subdev, asd);
+		if (sasd->role == SOCAM_SUBDEV_DATA_SOURCE)
+			break;
+	}
+
+	if (i >= size || asd[i]->match_type != V4L2_ASYNC_MATCH_I2C) {
+		/* All useless */
+		dev_err(ici->v4l2_dev.dev, "No I2C data source found!\n");
+		return -ENODEV;
+	}
+
+	/* Or shall this be managed by the soc-camera device? */
+	sasc = devm_kzalloc(ici->v4l2_dev.dev, sizeof(*sasc), GFP_KERNEL);
+	if (!sasc)
+		return -ENOMEM;
+
+	/* HACK: just need a != NULL */
+	sdesc.host_desc.board_info = ERR_PTR(-ENODATA);
+
+	ret = soc_camera_dyn_pdev(&sdesc, sasc);
+	if (ret < 0)
+		goto eallocpdev;
+
+	sasc->sensor = &sasd->asd;
+
+	icd = soc_camera_add_pdev(sasc);
+	if (!icd) {
+		ret = -ENOMEM;
+		goto eaddpdev;
+	}
+
+	sasc->notifier.subdevs = asd;
+	sasc->notifier.num_subdevs = size;
+	sasc->notifier.bound = soc_camera_async_bound;
+	sasc->notifier.unbind = soc_camera_async_unbind;
+	sasc->notifier.complete = soc_camera_async_complete;
+
+	icd->sasc = sasc;
+	icd->parent = ici->v4l2_dev.dev;
+
+	snprintf(clk_name, sizeof(clk_name), "%d-%04x",
+		 sasd->asd.match.i2c.adapter_id, sasd->asd.match.i2c.address);
+
+	icd->clk = v4l2_clk_register(&soc_camera_clk_ops, clk_name, icd);
+	if (IS_ERR(icd->clk)) {
+		ret = PTR_ERR(icd->clk);
+		goto eclkreg;
+	}
+
+	ret = v4l2_async_notifier_register(&ici->v4l2_dev, &sasc->notifier);
+	if (!ret)
+		return 0;
+
+	v4l2_clk_unregister(icd->clk);
+eclkreg:
+	icd->clk = NULL;
+	platform_device_del(sasc->pdev);
+eaddpdev:
+	platform_device_put(sasc->pdev);
+eallocpdev:
+	devm_kfree(ici->v4l2_dev.dev, sasc);
+	dev_err(ici->v4l2_dev.dev, "group probe failed: %d\n", ret);
+
+	return ret;
+}
+
+static void scan_async_host(struct soc_camera_host *ici)
+{
+	struct v4l2_async_subdev **asd;
+	int j;
+
+	for (j = 0, asd = ici->asd; ici->asd_sizes[j]; j++) {
+		scan_async_group(ici, asd, ici->asd_sizes[j]);
+		asd += ici->asd_sizes[j];
+	}
+}
+#else
+#define soc_camera_i2c_init(icd, sdesc)	(-ENODEV)
+#define soc_camera_i2c_free(icd)	do {} while (0)
+#define scan_async_host(ici)		do {} while (0)
+#endif
+
+#ifdef CONFIG_OF
+
+struct soc_of_info {
+	struct soc_camera_async_subdev	sasd;
+	struct soc_camera_async_client	sasc;
+	struct v4l2_async_subdev	*subdev;
+};
+
+static int soc_of_bind(struct soc_camera_host *ici,
+		       struct device_node *ep,
+		       struct device_node *remote)
+{
+	struct soc_camera_device *icd;
+	struct soc_camera_desc sdesc = {.host_desc.bus_id = ici->nr,};
+	struct soc_camera_async_client *sasc;
+	struct soc_of_info *info;
+	struct i2c_client *client;
+	char clk_name[V4L2_SUBDEV_NAME_SIZE + 32];
+	int ret;
+
+	/* allocate a new subdev and add match info to it */
+	info = devm_kzalloc(ici->v4l2_dev.dev, sizeof(struct soc_of_info),
+			    GFP_KERNEL);
+	if (!info)
+		return -ENOMEM;
+
+	info->sasd.asd.match.of.node = remote;
+	info->sasd.asd.match_type = V4L2_ASYNC_MATCH_OF;
+	info->subdev = &info->sasd.asd;
+
+	/* Or shall this be managed by the soc-camera device? */
+	sasc = &info->sasc;
+
+	/* HACK: just need a != NULL */
+	sdesc.host_desc.board_info = ERR_PTR(-ENODATA);
+
+	ret = soc_camera_dyn_pdev(&sdesc, sasc);
+	if (ret < 0)
+		goto eallocpdev;
+
+	sasc->sensor = &info->sasd.asd;
+
+	icd = soc_camera_add_pdev(sasc);
+	if (!icd) {
+		ret = -ENOMEM;
+		goto eaddpdev;
+	}
+
+	sasc->notifier.subdevs = &info->subdev;
+	sasc->notifier.num_subdevs = 1;
+	sasc->notifier.bound = soc_camera_async_bound;
+	sasc->notifier.unbind = soc_camera_async_unbind;
+	sasc->notifier.complete = soc_camera_async_complete;
+
+	icd->sasc = sasc;
+	icd->parent = ici->v4l2_dev.dev;
+
+	client = of_find_i2c_device_by_node(remote);
+
+	if (client)
+		snprintf(clk_name, sizeof(clk_name), "%d-%04x",
+			 client->adapter->nr, client->addr);
+	else
+		snprintf(clk_name, sizeof(clk_name), "of-%s",
+			 of_node_full_name(remote));
+
+	icd->clk = v4l2_clk_register(&soc_camera_clk_ops, clk_name, icd);
+	if (IS_ERR(icd->clk)) {
+		ret = PTR_ERR(icd->clk);
+		goto eclkreg;
+	}
+
+	ret = v4l2_async_notifier_register(&ici->v4l2_dev, &sasc->notifier);
+	if (!ret)
+		return 0;
+
+	v4l2_clk_unregister(icd->clk);
+eclkreg:
+	icd->clk = NULL;
+	platform_device_del(sasc->pdev);
+eaddpdev:
+	platform_device_put(sasc->pdev);
+eallocpdev:
+	devm_kfree(ici->v4l2_dev.dev, info);
+	dev_err(ici->v4l2_dev.dev, "group probe failed: %d\n", ret);
+
+	return ret;
+}
+
+static void scan_of_host(struct soc_camera_host *ici)
+{
+	struct device *dev = ici->v4l2_dev.dev;
+	struct device_node *np = dev->of_node;
+	struct device_node *epn = NULL, *ren;
+	unsigned int i;
+
+	for (i = 0; ; i++) {
+		epn = of_graph_get_next_endpoint(np, epn);
+		if (!epn)
+			break;
+
+		ren = of_graph_get_remote_port(epn);
+		if (!ren) {
+			dev_notice(dev, "no remote for %s\n",
+				   of_node_full_name(epn));
+			continue;
+		}
+
+		/* so we now have a remote node to connect */
+		if (!i)
+			soc_of_bind(ici, epn, ren->parent);
+
+		of_node_put(ren);
+
+		if (i) {
+			dev_err(dev, "multiple subdevices aren't supported yet!\n");
+			break;
+		}
+	}
+
+	of_node_put(epn);
+}
+
+#else
+static inline void scan_of_host(struct soc_camera_host *ici) { }
+#endif
+
+/* Called during host-driver probe */
+static int soc_camera_probe(struct soc_camera_host *ici,
+			    struct soc_camera_device *icd)
+{
+	struct soc_camera_desc *sdesc = to_soc_camera_desc(icd);
+	struct soc_camera_host_desc *shd = &sdesc->host_desc;
+	struct device *control = NULL;
+	int ret;
+
+	dev_info(icd->pdev, "Probing %s\n", dev_name(icd->pdev));
+
+	/*
+	 * Currently the subdev with the largest number of controls (13) is
+	 * ov6550. So let's pick 16 as a hint for the control handler. Note
+	 * that this is a hint only: too large and you waste some memory, too
+	 * small and there is a (very) small performance hit when looking up
+	 * controls in the internal hash.
+	 */
+	ret = v4l2_ctrl_handler_init(&icd->ctrl_handler, 16);
+	if (ret < 0)
+		return ret;
+
+	/* Must have icd->vdev before registering the device */
+	ret = video_dev_create(icd);
+	if (ret < 0)
+		goto evdc;
+
+	/*
+	 * ..._video_start() will create a device node, video_register_device()
+	 * itself is protected against concurrent open() calls, but we also have
+	 * to protect our data also during client probing.
+	 */
+
+	/* Non-i2c cameras, e.g., soc_camera_platform, have no board_info */
+	if (shd->board_info) {
+		ret = soc_camera_i2c_init(icd, sdesc);
+		if (ret < 0 && ret != -EPROBE_DEFER)
+			goto eadd;
+	} else if (!shd->add_device || !shd->del_device) {
+		ret = -EINVAL;
+		goto eadd;
+	} else {
+		ret = soc_camera_clock_start(ici);
+		if (ret < 0)
+			goto eadd;
+
+		if (shd->module_name)
+			ret = request_module(shd->module_name);
+
+		ret = shd->add_device(icd);
+		if (ret < 0)
+			goto eadddev;
+
+		/*
+		 * FIXME: this is racy, have to use driver-binding notification,
+		 * when it is available
+		 */
+		control = to_soc_camera_control(icd);
+		if (!control || !control->driver || !dev_get_drvdata(control) ||
+		    !try_module_get(control->driver->owner)) {
+			shd->del_device(icd);
+			ret = -ENODEV;
+			goto enodrv;
+		}
+	}
+
+	mutex_lock(&ici->host_lock);
+	ret = soc_camera_probe_finish(icd);
+	mutex_unlock(&ici->host_lock);
+	if (ret < 0)
+		goto efinish;
+
+	return 0;
+
+efinish:
+	if (shd->board_info) {
+		soc_camera_i2c_free(icd);
+	} else {
+		shd->del_device(icd);
+		module_put(control->driver->owner);
+enodrv:
+eadddev:
+		soc_camera_clock_stop(ici);
+	}
+eadd:
+	if (icd->vdev) {
+		video_device_release(icd->vdev);
+		icd->vdev = NULL;
+	}
+evdc:
+	v4l2_ctrl_handler_free(&icd->ctrl_handler);
+	return ret;
+}
+
+/*
+ * This is called on device_unregister, which only means we have to disconnect
+ * from the host, but not remove ourselves from the device list. With
+ * asynchronous client probing this can also be called without
+ * soc_camera_probe_finish() having run. Careful with clean up.
+ */
+static int soc_camera_remove(struct soc_camera_device *icd)
+{
+	struct soc_camera_desc *sdesc = to_soc_camera_desc(icd);
+	struct video_device *vdev = icd->vdev;
+
+	v4l2_ctrl_handler_free(&icd->ctrl_handler);
+	if (vdev) {
+		video_unregister_device(vdev);
+		icd->vdev = NULL;
+	}
+
+	if (sdesc->host_desc.board_info) {
+		soc_camera_i2c_free(icd);
+	} else {
+		struct device *dev = to_soc_camera_control(icd);
+		struct device_driver *drv = dev ? dev->driver : NULL;
+		if (drv) {
+			sdesc->host_desc.del_device(icd);
+			module_put(drv->owner);
+		}
+	}
+
+	if (icd->num_user_formats)
+		soc_camera_free_user_formats(icd);
+
+	if (icd->clk) {
+		/* For the synchronous case */
+		v4l2_clk_unregister(icd->clk);
+		icd->clk = NULL;
+	}
+
+	if (icd->sasc)
+		platform_device_unregister(icd->sasc->pdev);
+
+	return 0;
+}
+
+static int default_cropcap(struct soc_camera_device *icd,
+			   struct v4l2_cropcap *a)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	return v4l2_subdev_call(sd, video, cropcap, a);
+}
+
+static int default_g_crop(struct soc_camera_device *icd, struct v4l2_crop *a)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	return v4l2_subdev_call(sd, video, g_crop, a);
+}
+
+static int default_s_crop(struct soc_camera_device *icd, const struct v4l2_crop *a)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	return v4l2_subdev_call(sd, video, s_crop, a);
+}
+
+static int default_g_parm(struct soc_camera_device *icd,
+			  struct v4l2_streamparm *parm)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	return v4l2_subdev_call(sd, video, g_parm, parm);
+}
+
+static int default_s_parm(struct soc_camera_device *icd,
+			  struct v4l2_streamparm *parm)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	return v4l2_subdev_call(sd, video, s_parm, parm);
+}
+
+static int default_enum_framesizes(struct soc_camera_device *icd,
+				   struct v4l2_frmsizeenum *fsize)
+{
+	int ret;
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	const struct soc_camera_format_xlate *xlate;
+	struct v4l2_subdev_frame_size_enum fse = {
+		.index = fsize->index,
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+
+	xlate = soc_camera_xlate_by_fourcc(icd, fsize->pixel_format);
+	if (!xlate)
+		return -EINVAL;
+	fse.code = xlate->code;
+
+	ret = v4l2_subdev_call(sd, pad, enum_frame_size, NULL, &fse);
+	if (ret < 0)
+		return ret;
+
+	if (fse.min_width == fse.max_width &&
+	    fse.min_height == fse.max_height) {
+		fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+		fsize->discrete.width = fse.min_width;
+		fsize->discrete.height = fse.min_height;
+		return 0;
+	}
+	fsize->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
+	fsize->stepwise.min_width = fse.min_width;
+	fsize->stepwise.max_width = fse.max_width;
+	fsize->stepwise.min_height = fse.min_height;
+	fsize->stepwise.max_height = fse.max_height;
+	fsize->stepwise.step_width = 1;
+	fsize->stepwise.step_height = 1;
+	return 0;
+}
+
+int soc_camera_host_register(struct soc_camera_host *ici)
+{
+	struct soc_camera_host *ix;
+	int ret;
+
+	if (!ici || !ici->ops ||
+	    !ici->ops->try_fmt ||
+	    !ici->ops->set_fmt ||
+	    !ici->ops->set_bus_param ||
+	    !ici->ops->querycap ||
+	    ((!ici->ops->init_videobuf ||
+	      !ici->ops->reqbufs) &&
+	     !ici->ops->init_videobuf2) ||
+	    !ici->ops->poll ||
+	    !ici->v4l2_dev.dev)
+		return -EINVAL;
+
+	if (!ici->ops->set_crop)
+		ici->ops->set_crop = default_s_crop;
+	if (!ici->ops->get_crop)
+		ici->ops->get_crop = default_g_crop;
+	if (!ici->ops->cropcap)
+		ici->ops->cropcap = default_cropcap;
+	if (!ici->ops->set_parm)
+		ici->ops->set_parm = default_s_parm;
+	if (!ici->ops->get_parm)
+		ici->ops->get_parm = default_g_parm;
+	if (!ici->ops->enum_framesizes)
+		ici->ops->enum_framesizes = default_enum_framesizes;
+
+	mutex_lock(&list_lock);
+	list_for_each_entry(ix, &hosts, list) {
+		if (ix->nr == ici->nr) {
+			ret = -EBUSY;
+			goto edevreg;
+		}
+	}
+
+	ret = v4l2_device_register(ici->v4l2_dev.dev, &ici->v4l2_dev);
+	if (ret < 0)
+		goto edevreg;
+
+	list_add_tail(&ici->list, &hosts);
+	mutex_unlock(&list_lock);
+
+	mutex_init(&ici->host_lock);
+	mutex_init(&ici->clk_lock);
+
+	if (ici->v4l2_dev.dev->of_node)
+		scan_of_host(ici);
+	else if (ici->asd_sizes)
+		/*
+		 * No OF, host with a list of subdevices. Don't try to mix
+		 * modes by initialising some groups statically and some
+		 * dynamically!
+		 */
+		scan_async_host(ici);
+	else
+		/* Legacy: static platform devices from board data */
+		scan_add_host(ici);
+
+	return 0;
+
+edevreg:
+	mutex_unlock(&list_lock);
+	return ret;
+}
+EXPORT_SYMBOL(soc_camera_host_register);
+
+/* Unregister all clients! */
+void soc_camera_host_unregister(struct soc_camera_host *ici)
+{
+	struct soc_camera_device *icd, *tmp;
+	struct soc_camera_async_client *sasc;
+	LIST_HEAD(notifiers);
+
+	mutex_lock(&list_lock);
+	list_del(&ici->list);
+	list_for_each_entry(icd, &devices, list)
+		if (icd->iface == ici->nr && icd->sasc) {
+			/* as long as we hold the device, sasc won't be freed */
+			get_device(icd->pdev);
+			list_add(&icd->sasc->list, &notifiers);
+		}
+	mutex_unlock(&list_lock);
+
+	list_for_each_entry(sasc, &notifiers, list) {
+		/* Must call unlocked to avoid AB-BA dead-lock */
+		v4l2_async_notifier_unregister(&sasc->notifier);
+		put_device(&sasc->pdev->dev);
+	}
+
+	mutex_lock(&list_lock);
+
+	list_for_each_entry_safe(icd, tmp, &devices, list)
+		if (icd->iface == ici->nr)
+			soc_camera_remove(icd);
+
+	mutex_unlock(&list_lock);
+
+	v4l2_device_unregister(&ici->v4l2_dev);
+}
+EXPORT_SYMBOL(soc_camera_host_unregister);
+
+/* Image capture device */
+static int soc_camera_device_register(struct soc_camera_device *icd)
+{
+	struct soc_camera_device *ix;
+	int num = -1, i;
+
+	mutex_lock(&list_lock);
+	for (i = 0; i < 256 && num < 0; i++) {
+		num = i;
+		/* Check if this index is available on this interface */
+		list_for_each_entry(ix, &devices, list) {
+			if (ix->iface == icd->iface && ix->devnum == i) {
+				num = -1;
+				break;
+			}
+		}
+	}
+
+	if (num < 0) {
+		/*
+		 * ok, we have 256 cameras on this host...
+		 * man, stay reasonable...
+		 */
+		mutex_unlock(&list_lock);
+		return -ENOMEM;
+	}
+
+	icd->devnum		= num;
+	icd->use_count		= 0;
+	icd->host_priv		= NULL;
+
+	/*
+	 * Dynamically allocated devices set the bit earlier, but it doesn't hurt setting
+	 * it again
+	 */
+	i = to_platform_device(icd->pdev)->id;
+	if (i < 0)
+		/* One static (legacy) soc-camera platform device */
+		i = 0;
+	if (i >= MAP_MAX_NUM) {
+		mutex_unlock(&list_lock);
+		return -EBUSY;
+	}
+	set_bit(i, device_map);
+	list_add_tail(&icd->list, &devices);
+	mutex_unlock(&list_lock);
+
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops soc_camera_ioctl_ops = {
+	.vidioc_querycap	 = soc_camera_querycap,
+	.vidioc_try_fmt_vid_cap  = soc_camera_try_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap    = soc_camera_g_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap    = soc_camera_s_fmt_vid_cap,
+	.vidioc_enum_fmt_vid_cap = soc_camera_enum_fmt_vid_cap,
+	.vidioc_enum_input	 = soc_camera_enum_input,
+	.vidioc_g_input		 = soc_camera_g_input,
+	.vidioc_s_input		 = soc_camera_s_input,
+	.vidioc_s_std		 = soc_camera_s_std,
+	.vidioc_g_std		 = soc_camera_g_std,
+	.vidioc_enum_framesizes  = soc_camera_enum_framesizes,
+	.vidioc_reqbufs		 = soc_camera_reqbufs,
+	.vidioc_querybuf	 = soc_camera_querybuf,
+	.vidioc_qbuf		 = soc_camera_qbuf,
+	.vidioc_dqbuf		 = soc_camera_dqbuf,
+	.vidioc_create_bufs	 = soc_camera_create_bufs,
+	.vidioc_prepare_buf	 = soc_camera_prepare_buf,
+	.vidioc_expbuf		 = soc_camera_expbuf,
+	.vidioc_streamon	 = soc_camera_streamon,
+	.vidioc_streamoff	 = soc_camera_streamoff,
+	.vidioc_cropcap		 = soc_camera_cropcap,
+	.vidioc_g_crop		 = soc_camera_g_crop,
+	.vidioc_s_crop		 = soc_camera_s_crop,
+	.vidioc_g_selection	 = soc_camera_g_selection,
+	.vidioc_s_selection	 = soc_camera_s_selection,
+	.vidioc_g_parm		 = soc_camera_g_parm,
+	.vidioc_s_parm		 = soc_camera_s_parm,
+};
+
+static int video_dev_create(struct soc_camera_device *icd)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct video_device *vdev = video_device_alloc();
+
+	if (!vdev)
+		return -ENOMEM;
+
+	strlcpy(vdev->name, ici->drv_name, sizeof(vdev->name));
+
+	vdev->v4l2_dev		= &ici->v4l2_dev;
+	vdev->fops		= &soc_camera_fops;
+	vdev->ioctl_ops		= &soc_camera_ioctl_ops;
+	vdev->release		= video_device_release;
+	vdev->ctrl_handler	= &icd->ctrl_handler;
+	vdev->lock		= &ici->host_lock;
+
+	icd->vdev = vdev;
+
+	return 0;
+}
+
+/*
+ * Called from soc_camera_probe() above with .host_lock held
+ */
+static int soc_camera_video_start(struct soc_camera_device *icd)
+{
+	const struct device_type *type = icd->vdev->dev.type;
+	int ret;
+
+	if (!icd->parent)
+		return -ENODEV;
+
+	video_set_drvdata(icd->vdev, icd);
+	if (icd->vdev->tvnorms == 0) {
+		/* disable the STD API if there are no tvnorms defined */
+		v4l2_disable_ioctl(icd->vdev, VIDIOC_G_STD);
+		v4l2_disable_ioctl(icd->vdev, VIDIOC_S_STD);
+		v4l2_disable_ioctl(icd->vdev, VIDIOC_ENUMSTD);
+	}
+	ret = video_register_device(icd->vdev, VFL_TYPE_GRABBER, -1);
+	if (ret < 0) {
+		dev_err(icd->pdev, "video_register_device failed: %d\n", ret);
+		return ret;
+	}
+
+	/* Restore device type, possibly set by the subdevice driver */
+	icd->vdev->dev.type = type;
+
+	return 0;
+}
+
+static int soc_camera_pdrv_probe(struct platform_device *pdev)
+{
+	struct soc_camera_desc *sdesc = pdev->dev.platform_data;
+	struct soc_camera_subdev_desc *ssdd = &sdesc->subdev_desc;
+	struct soc_camera_device *icd;
+	int ret;
+
+	if (!sdesc)
+		return -EINVAL;
+
+	icd = devm_kzalloc(&pdev->dev, sizeof(*icd), GFP_KERNEL);
+	if (!icd)
+		return -ENOMEM;
+
+	/*
+	 * In the asynchronous case ssdd->num_regulators == 0 yet, so, the below
+	 * regulator allocation is a dummy. They are actually requested by the
+	 * subdevice driver, using soc_camera_power_init(). Also note, that in
+	 * that case regulators are attached to the I2C device and not to the
+	 * camera platform device.
+	 */
+	ret = devm_regulator_bulk_get(&pdev->dev, ssdd->sd_pdata.num_regulators,
+				      ssdd->sd_pdata.regulators);
+	if (ret < 0)
+		return ret;
+
+	icd->iface = sdesc->host_desc.bus_id;
+	icd->sdesc = sdesc;
+	icd->pdev = &pdev->dev;
+	platform_set_drvdata(pdev, icd);
+
+	icd->user_width		= DEFAULT_WIDTH;
+	icd->user_height	= DEFAULT_HEIGHT;
+
+	return soc_camera_device_register(icd);
+}
+
+/*
+ * Only called on rmmod for each platform device, since they are not
+ * hot-pluggable. Now we know, that all our users - hosts and devices have
+ * been unloaded already
+ */
+static int soc_camera_pdrv_remove(struct platform_device *pdev)
+{
+	struct soc_camera_device *icd = platform_get_drvdata(pdev);
+	int i;
+
+	if (!icd)
+		return -EINVAL;
+
+	i = pdev->id;
+	if (i < 0)
+		i = 0;
+
+	/*
+	 * In synchronous mode with static platform devices this is called in a
+	 * loop from drivers/base/dd.c::driver_detach(), no parallel execution,
+	 * no need to lock. In asynchronous case the caller -
+	 * soc_camera_host_unregister() - already holds the lock
+	 */
+	if (test_bit(i, device_map)) {
+		clear_bit(i, device_map);
+		list_del(&icd->list);
+	}
+
+	return 0;
+}
+
+static struct platform_driver __refdata soc_camera_pdrv = {
+	.probe = soc_camera_pdrv_probe,
+	.remove  = soc_camera_pdrv_remove,
+	.driver  = {
+		.name	= "soc-camera-pdrv",
+	},
+};
+
+module_platform_driver(soc_camera_pdrv);
+
+MODULE_DESCRIPTION("Image capture bus driver");
+MODULE_AUTHOR("Guennadi Liakhovetski <kernel@pengutronix.de>");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:soc-camera-pdrv");
diff --git a/drivers/media/platform/soc_camera/soc_camera_platform.c b/drivers/media/platform/soc_camera/soc_camera_platform.c
new file mode 100644
index 0000000..cc8eb07
--- /dev/null
+++ b/drivers/media/platform/soc_camera/soc_camera_platform.c
@@ -0,0 +1,199 @@
+/*
+ * Generic Platform Camera Driver
+ *
+ * Copyright (C) 2008 Magnus Damm
+ * Based on mt9m001 driver,
+ * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-subdev.h>
+#include <media/soc_camera.h>
+#include <media/soc_camera_platform.h>
+
+struct soc_camera_platform_priv {
+	struct v4l2_subdev subdev;
+};
+
+static struct soc_camera_platform_priv *get_priv(struct platform_device *pdev)
+{
+	struct v4l2_subdev *subdev = platform_get_drvdata(pdev);
+	return container_of(subdev, struct soc_camera_platform_priv, subdev);
+}
+
+static int soc_camera_platform_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct soc_camera_platform_info *p = v4l2_get_subdevdata(sd);
+	return p->set_capture(p, enable);
+}
+
+static int soc_camera_platform_fill_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
+{
+	struct soc_camera_platform_info *p = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *mf = &format->format;
+
+	mf->width	= p->format.width;
+	mf->height	= p->format.height;
+	mf->code	= p->format.code;
+	mf->colorspace	= p->format.colorspace;
+	mf->field	= p->format.field;
+
+	return 0;
+}
+
+static int soc_camera_platform_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct soc_camera_platform_info *p = v4l2_get_subdevdata(sd);
+
+	return soc_camera_set_power(p->icd->control, &p->icd->sdesc->subdev_desc, NULL, on);
+}
+
+static struct v4l2_subdev_core_ops platform_subdev_core_ops = {
+	.s_power = soc_camera_platform_s_power,
+};
+
+static int soc_camera_platform_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct soc_camera_platform_info *p = v4l2_get_subdevdata(sd);
+
+	if (code->pad || code->index)
+		return -EINVAL;
+
+	code->code = p->format.code;
+	return 0;
+}
+
+static int soc_camera_platform_g_crop(struct v4l2_subdev *sd,
+				      struct v4l2_crop *a)
+{
+	struct soc_camera_platform_info *p = v4l2_get_subdevdata(sd);
+
+	a->c.left	= 0;
+	a->c.top	= 0;
+	a->c.width	= p->format.width;
+	a->c.height	= p->format.height;
+	a->type		= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	return 0;
+}
+
+static int soc_camera_platform_cropcap(struct v4l2_subdev *sd,
+				       struct v4l2_cropcap *a)
+{
+	struct soc_camera_platform_info *p = v4l2_get_subdevdata(sd);
+
+	a->bounds.left			= 0;
+	a->bounds.top			= 0;
+	a->bounds.width			= p->format.width;
+	a->bounds.height		= p->format.height;
+	a->defrect			= a->bounds;
+	a->type				= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	a->pixelaspect.numerator	= 1;
+	a->pixelaspect.denominator	= 1;
+
+	return 0;
+}
+
+static int soc_camera_platform_g_mbus_config(struct v4l2_subdev *sd,
+					     struct v4l2_mbus_config *cfg)
+{
+	struct soc_camera_platform_info *p = v4l2_get_subdevdata(sd);
+
+	cfg->flags = p->mbus_param;
+	cfg->type = p->mbus_type;
+
+	return 0;
+}
+
+static struct v4l2_subdev_video_ops platform_subdev_video_ops = {
+	.s_stream	= soc_camera_platform_s_stream,
+	.cropcap	= soc_camera_platform_cropcap,
+	.g_crop		= soc_camera_platform_g_crop,
+	.g_mbus_config	= soc_camera_platform_g_mbus_config,
+};
+
+static const struct v4l2_subdev_pad_ops platform_subdev_pad_ops = {
+	.enum_mbus_code = soc_camera_platform_enum_mbus_code,
+	.get_fmt	= soc_camera_platform_fill_fmt,
+	.set_fmt	= soc_camera_platform_fill_fmt,
+};
+
+static struct v4l2_subdev_ops platform_subdev_ops = {
+	.core	= &platform_subdev_core_ops,
+	.video	= &platform_subdev_video_ops,
+	.pad	= &platform_subdev_pad_ops,
+};
+
+static int soc_camera_platform_probe(struct platform_device *pdev)
+{
+	struct soc_camera_host *ici;
+	struct soc_camera_platform_priv *priv;
+	struct soc_camera_platform_info *p = pdev->dev.platform_data;
+	struct soc_camera_device *icd;
+
+	if (!p)
+		return -EINVAL;
+
+	if (!p->icd) {
+		dev_err(&pdev->dev,
+			"Platform has not set soc_camera_device pointer!\n");
+		return -EINVAL;
+	}
+
+	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	icd = p->icd;
+
+	/* soc-camera convention: control's drvdata points to the subdev */
+	platform_set_drvdata(pdev, &priv->subdev);
+	/* Set the control device reference */
+	icd->control = &pdev->dev;
+
+	ici = to_soc_camera_host(icd->parent);
+
+	v4l2_subdev_init(&priv->subdev, &platform_subdev_ops);
+	v4l2_set_subdevdata(&priv->subdev, p);
+	strncpy(priv->subdev.name, dev_name(&pdev->dev), V4L2_SUBDEV_NAME_SIZE);
+
+	return v4l2_device_register_subdev(&ici->v4l2_dev, &priv->subdev);
+}
+
+static int soc_camera_platform_remove(struct platform_device *pdev)
+{
+	struct soc_camera_platform_priv *priv = get_priv(pdev);
+	struct soc_camera_platform_info *p = v4l2_get_subdevdata(&priv->subdev);
+
+	p->icd->control = NULL;
+	v4l2_device_unregister_subdev(&priv->subdev);
+	return 0;
+}
+
+static struct platform_driver soc_camera_platform_driver = {
+	.driver		= {
+		.name	= "soc_camera_platform",
+	},
+	.probe		= soc_camera_platform_probe,
+	.remove		= soc_camera_platform_remove,
+};
+
+module_platform_driver(soc_camera_platform_driver);
+
+MODULE_DESCRIPTION("SoC Camera Platform driver");
+MODULE_AUTHOR("Magnus Damm");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:soc_camera_platform");
diff --git a/drivers/media/platform/soc_camera/soc_mediabus.c b/drivers/media/platform/soc_camera/soc_mediabus.c
new file mode 100644
index 0000000..1dbcd42
--- /dev/null
+++ b/drivers/media/platform/soc_camera/soc_mediabus.c
@@ -0,0 +1,529 @@
+/*
+ * soc-camera media bus helper routines
+ *
+ * Copyright (C) 2009, Guennadi Liakhovetski <g.liakhovetski@gmx.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+
+#include <media/v4l2-device.h>
+#include <media/v4l2-mediabus.h>
+#include <media/soc_mediabus.h>
+
+static const struct soc_mbus_lookup mbus_fmt[] = {
+{
+	.code = MEDIA_BUS_FMT_YUYV8_2X8,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_YUYV,
+		.name			= "YUYV",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_YVYU8_2X8,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_YVYU,
+		.name			= "YVYU",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_UYVY8_2X8,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_UYVY,
+		.name			= "UYVY",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_VYUY8_2X8,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_VYUY,
+		.name			= "VYUY",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_RGB555,
+		.name			= "RGB555",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_RGB555X,
+		.name			= "RGB555X",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
+		.order			= SOC_MBUS_ORDER_BE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_RGB565_2X8_LE,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_RGB565,
+		.name			= "RGB565",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_RGB565_2X8_BE,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_RGB565X,
+		.name			= "RGB565X",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
+		.order			= SOC_MBUS_ORDER_BE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_RGB666_1X18,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_RGB32,
+		.name			= "RGB666/32bpp",
+		.bits_per_sample	= 18,
+		.packing		= SOC_MBUS_PACKING_EXTEND32,
+		.order			= SOC_MBUS_ORDER_LE,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_RGB888_1X24,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_RGB32,
+		.name			= "RGB888/32bpp",
+		.bits_per_sample	= 24,
+		.packing		= SOC_MBUS_PACKING_EXTEND32,
+		.order			= SOC_MBUS_ORDER_LE,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_RGB888_2X12_BE,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_RGB32,
+		.name			= "RGB888/32bpp",
+		.bits_per_sample	= 12,
+		.packing		= SOC_MBUS_PACKING_EXTEND32,
+		.order			= SOC_MBUS_ORDER_BE,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_RGB888_2X12_LE,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_RGB32,
+		.name			= "RGB888/32bpp",
+		.bits_per_sample	= 12,
+		.packing		= SOC_MBUS_PACKING_EXTEND32,
+		.order			= SOC_MBUS_ORDER_LE,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_SBGGR8_1X8,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_SBGGR8,
+		.name			= "Bayer 8 BGGR",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_NONE,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_SBGGR10_1X10,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_SBGGR10,
+		.name			= "Bayer 10 BGGR",
+		.bits_per_sample	= 10,
+		.packing		= SOC_MBUS_PACKING_EXTEND16,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_Y8_1X8,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_GREY,
+		.name			= "Grey",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_NONE,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_Y10_1X10,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_Y10,
+		.name			= "Grey 10bit",
+		.bits_per_sample	= 10,
+		.packing		= SOC_MBUS_PACKING_EXTEND16,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_SBGGR10,
+		.name			= "Bayer 10 BGGR",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_SBGGR10_2X8_PADLO_LE,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_SBGGR10,
+		.name			= "Bayer 10 BGGR",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_2X8_PADLO,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_BE,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_SBGGR10,
+		.name			= "Bayer 10 BGGR",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
+		.order			= SOC_MBUS_ORDER_BE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_SBGGR10_2X8_PADLO_BE,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_SBGGR10,
+		.name			= "Bayer 10 BGGR",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_2X8_PADLO,
+		.order			= SOC_MBUS_ORDER_BE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_JPEG_1X8,
+	.fmt = {
+		.fourcc                 = V4L2_PIX_FMT_JPEG,
+		.name                   = "JPEG",
+		.bits_per_sample        = 8,
+		.packing                = SOC_MBUS_PACKING_VARIABLE,
+		.order                  = SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_RGB444_2X8_PADHI_BE,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_RGB444,
+		.name			= "RGB444",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
+		.order			= SOC_MBUS_ORDER_BE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_YUYV8_1_5X8,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_YUV420,
+		.name			= "YUYV 4:2:0",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_1_5X8,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_YVYU8_1_5X8,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_YVU420,
+		.name			= "YVYU 4:2:0",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_1_5X8,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_UYVY8_1X16,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_UYVY,
+		.name			= "UYVY 16bit",
+		.bits_per_sample	= 16,
+		.packing		= SOC_MBUS_PACKING_EXTEND16,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_VYUY8_1X16,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_VYUY,
+		.name			= "VYUY 16bit",
+		.bits_per_sample	= 16,
+		.packing		= SOC_MBUS_PACKING_EXTEND16,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_YUYV8_1X16,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_YUYV,
+		.name			= "YUYV 16bit",
+		.bits_per_sample	= 16,
+		.packing		= SOC_MBUS_PACKING_EXTEND16,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_YVYU8_1X16,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_YVYU,
+		.name			= "YVYU 16bit",
+		.bits_per_sample	= 16,
+		.packing		= SOC_MBUS_PACKING_EXTEND16,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_SGRBG8_1X8,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_SGRBG8,
+		.name			= "Bayer 8 GRBG",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_NONE,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_SGRBG10_DPCM8_1X8,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_SGRBG10DPCM8,
+		.name			= "Bayer 10 BGGR DPCM 8",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_NONE,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_SGBRG10_1X10,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_SGBRG10,
+		.name			= "Bayer 10 GBRG",
+		.bits_per_sample	= 10,
+		.packing		= SOC_MBUS_PACKING_EXTEND16,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_SGRBG10_1X10,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_SGRBG10,
+		.name			= "Bayer 10 GRBG",
+		.bits_per_sample	= 10,
+		.packing		= SOC_MBUS_PACKING_EXTEND16,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_SRGGB10_1X10,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_SRGGB10,
+		.name			= "Bayer 10 RGGB",
+		.bits_per_sample	= 10,
+		.packing		= SOC_MBUS_PACKING_EXTEND16,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_SBGGR12_1X12,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_SBGGR12,
+		.name			= "Bayer 12 BGGR",
+		.bits_per_sample	= 12,
+		.packing		= SOC_MBUS_PACKING_EXTEND16,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_SGBRG12_1X12,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_SGBRG12,
+		.name			= "Bayer 12 GBRG",
+		.bits_per_sample	= 12,
+		.packing		= SOC_MBUS_PACKING_EXTEND16,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_SGRBG12_1X12,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_SGRBG12,
+		.name			= "Bayer 12 GRBG",
+		.bits_per_sample	= 12,
+		.packing		= SOC_MBUS_PACKING_EXTEND16,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+}, {
+	.code = MEDIA_BUS_FMT_SRGGB12_1X12,
+	.fmt = {
+		.fourcc			= V4L2_PIX_FMT_SRGGB12,
+		.name			= "Bayer 12 RGGB",
+		.bits_per_sample	= 12,
+		.packing		= SOC_MBUS_PACKING_EXTEND16,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
+},
+};
+
+int soc_mbus_samples_per_pixel(const struct soc_mbus_pixelfmt *mf,
+			unsigned int *numerator, unsigned int *denominator)
+{
+	switch (mf->packing) {
+	case SOC_MBUS_PACKING_NONE:
+	case SOC_MBUS_PACKING_EXTEND16:
+		*numerator = 1;
+		*denominator = 1;
+		return 0;
+	case SOC_MBUS_PACKING_EXTEND32:
+		*numerator = 1;
+		*denominator = 1;
+		return 0;
+	case SOC_MBUS_PACKING_2X8_PADHI:
+	case SOC_MBUS_PACKING_2X8_PADLO:
+		*numerator = 2;
+		*denominator = 1;
+		return 0;
+	case SOC_MBUS_PACKING_1_5X8:
+		*numerator = 3;
+		*denominator = 2;
+		return 0;
+	case SOC_MBUS_PACKING_VARIABLE:
+		*numerator = 0;
+		*denominator = 1;
+		return 0;
+	}
+	return -EINVAL;
+}
+EXPORT_SYMBOL(soc_mbus_samples_per_pixel);
+
+s32 soc_mbus_bytes_per_line(u32 width, const struct soc_mbus_pixelfmt *mf)
+{
+	if (mf->layout != SOC_MBUS_LAYOUT_PACKED)
+		return width * mf->bits_per_sample / 8;
+
+	switch (mf->packing) {
+	case SOC_MBUS_PACKING_NONE:
+		return width * mf->bits_per_sample / 8;
+	case SOC_MBUS_PACKING_2X8_PADHI:
+	case SOC_MBUS_PACKING_2X8_PADLO:
+	case SOC_MBUS_PACKING_EXTEND16:
+		return width * 2;
+	case SOC_MBUS_PACKING_1_5X8:
+		return width * 3 / 2;
+	case SOC_MBUS_PACKING_VARIABLE:
+		return 0;
+	case SOC_MBUS_PACKING_EXTEND32:
+		return width * 4;
+	}
+	return -EINVAL;
+}
+EXPORT_SYMBOL(soc_mbus_bytes_per_line);
+
+s32 soc_mbus_image_size(const struct soc_mbus_pixelfmt *mf,
+			u32 bytes_per_line, u32 height)
+{
+	if (mf->layout == SOC_MBUS_LAYOUT_PACKED)
+		return bytes_per_line * height;
+
+	switch (mf->packing) {
+	case SOC_MBUS_PACKING_2X8_PADHI:
+	case SOC_MBUS_PACKING_2X8_PADLO:
+		return bytes_per_line * height * 2;
+	case SOC_MBUS_PACKING_1_5X8:
+		return bytes_per_line * height * 3 / 2;
+	default:
+		return -EINVAL;
+	}
+}
+EXPORT_SYMBOL(soc_mbus_image_size);
+
+const struct soc_mbus_pixelfmt *soc_mbus_find_fmtdesc(
+	u32 code,
+	const struct soc_mbus_lookup *lookup,
+	int n)
+{
+	int i;
+
+	for (i = 0; i < n; i++)
+		if (lookup[i].code == code)
+			return &lookup[i].fmt;
+
+	return NULL;
+}
+EXPORT_SYMBOL(soc_mbus_find_fmtdesc);
+
+const struct soc_mbus_pixelfmt *soc_mbus_get_fmtdesc(
+	u32 code)
+{
+	return soc_mbus_find_fmtdesc(code, mbus_fmt, ARRAY_SIZE(mbus_fmt));
+}
+EXPORT_SYMBOL(soc_mbus_get_fmtdesc);
+
+unsigned int soc_mbus_config_compatible(const struct v4l2_mbus_config *cfg,
+					unsigned int flags)
+{
+	unsigned long common_flags;
+	bool hsync = true, vsync = true, pclk, data, mode;
+	bool mipi_lanes, mipi_clock;
+
+	common_flags = cfg->flags & flags;
+
+	switch (cfg->type) {
+	case V4L2_MBUS_PARALLEL:
+		hsync = common_flags & (V4L2_MBUS_HSYNC_ACTIVE_HIGH |
+					V4L2_MBUS_HSYNC_ACTIVE_LOW);
+		vsync = common_flags & (V4L2_MBUS_VSYNC_ACTIVE_HIGH |
+					V4L2_MBUS_VSYNC_ACTIVE_LOW);
+	case V4L2_MBUS_BT656:
+		pclk = common_flags & (V4L2_MBUS_PCLK_SAMPLE_RISING |
+				       V4L2_MBUS_PCLK_SAMPLE_FALLING);
+		data = common_flags & (V4L2_MBUS_DATA_ACTIVE_HIGH |
+				       V4L2_MBUS_DATA_ACTIVE_LOW);
+		mode = common_flags & (V4L2_MBUS_MASTER | V4L2_MBUS_SLAVE);
+		return (!hsync || !vsync || !pclk || !data || !mode) ?
+			0 : common_flags;
+	case V4L2_MBUS_CSI2:
+		mipi_lanes = common_flags & V4L2_MBUS_CSI2_LANES;
+		mipi_clock = common_flags & (V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK |
+					     V4L2_MBUS_CSI2_CONTINUOUS_CLOCK);
+		return (!mipi_lanes || !mipi_clock) ? 0 : common_flags;
+	}
+	return 0;
+}
+EXPORT_SYMBOL(soc_mbus_config_compatible);
+
+static int __init soc_mbus_init(void)
+{
+	return 0;
+}
+
+static void __exit soc_mbus_exit(void)
+{
+}
+
+module_init(soc_mbus_init);
+module_exit(soc_mbus_exit);
+
+MODULE_DESCRIPTION("soc-camera media bus interface");
+MODULE_AUTHOR("Guennadi Liakhovetski <g.liakhovetski@gmx.de>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/platform/soc_camera/soc_scale_crop.c b/drivers/media/platform/soc_camera/soc_scale_crop.c
new file mode 100644
index 0000000..bda29bc
--- /dev/null
+++ b/drivers/media/platform/soc_camera/soc_scale_crop.c
@@ -0,0 +1,407 @@
+/*
+ * soc-camera generic scaling-cropping manipulation functions
+ *
+ * Copyright (C) 2013 Guennadi Liakhovetski <g.liakhovetski@gmx.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/device.h>
+#include <linux/module.h>
+
+#include <media/soc_camera.h>
+#include <media/v4l2-common.h>
+
+#include "soc_scale_crop.h"
+
+#ifdef DEBUG_GEOMETRY
+#define dev_geo	dev_info
+#else
+#define dev_geo	dev_dbg
+#endif
+
+/* Check if any dimension of r1 is smaller than respective one of r2 */
+static bool is_smaller(const struct v4l2_rect *r1, const struct v4l2_rect *r2)
+{
+	return r1->width < r2->width || r1->height < r2->height;
+}
+
+/* Check if r1 fails to cover r2 */
+static bool is_inside(const struct v4l2_rect *r1, const struct v4l2_rect *r2)
+{
+	return r1->left > r2->left || r1->top > r2->top ||
+		r1->left + r1->width < r2->left + r2->width ||
+		r1->top + r1->height < r2->top + r2->height;
+}
+
+/* Get and store current client crop */
+int soc_camera_client_g_rect(struct v4l2_subdev *sd, struct v4l2_rect *rect)
+{
+	struct v4l2_crop crop;
+	struct v4l2_cropcap cap;
+	int ret;
+
+	crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	ret = v4l2_subdev_call(sd, video, g_crop, &crop);
+	if (!ret) {
+		*rect = crop.c;
+		return ret;
+	}
+
+	/* Camera driver doesn't support .g_crop(), assume default rectangle */
+	cap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	ret = v4l2_subdev_call(sd, video, cropcap, &cap);
+	if (!ret)
+		*rect = cap.defrect;
+
+	return ret;
+}
+EXPORT_SYMBOL(soc_camera_client_g_rect);
+
+/* Client crop has changed, update our sub-rectangle to remain within the area */
+static void update_subrect(struct v4l2_rect *rect, struct v4l2_rect *subrect)
+{
+	if (rect->width < subrect->width)
+		subrect->width = rect->width;
+
+	if (rect->height < subrect->height)
+		subrect->height = rect->height;
+
+	if (rect->left > subrect->left)
+		subrect->left = rect->left;
+	else if (rect->left + rect->width >
+		 subrect->left + subrect->width)
+		subrect->left = rect->left + rect->width -
+			subrect->width;
+
+	if (rect->top > subrect->top)
+		subrect->top = rect->top;
+	else if (rect->top + rect->height >
+		 subrect->top + subrect->height)
+		subrect->top = rect->top + rect->height -
+			subrect->height;
+}
+
+/*
+ * The common for both scaling and cropping iterative approach is:
+ * 1. try if the client can produce exactly what requested by the user
+ * 2. if (1) failed, try to double the client image until we get one big enough
+ * 3. if (2) failed, try to request the maximum image
+ */
+int soc_camera_client_s_crop(struct v4l2_subdev *sd,
+			struct v4l2_crop *crop, struct v4l2_crop *cam_crop,
+			struct v4l2_rect *target_rect, struct v4l2_rect *subrect)
+{
+	struct v4l2_rect *rect = &crop->c, *cam_rect = &cam_crop->c;
+	struct device *dev = sd->v4l2_dev->dev;
+	struct v4l2_cropcap cap;
+	int ret;
+	unsigned int width, height;
+
+	v4l2_subdev_call(sd, video, s_crop, crop);
+	ret = soc_camera_client_g_rect(sd, cam_rect);
+	if (ret < 0)
+		return ret;
+
+	/*
+	 * Now cam_crop contains the current camera input rectangle, and it must
+	 * be within camera cropcap bounds
+	 */
+	if (!memcmp(rect, cam_rect, sizeof(*rect))) {
+		/* Even if camera S_CROP failed, but camera rectangle matches */
+		dev_dbg(dev, "Camera S_CROP successful for %dx%d@%d:%d\n",
+			rect->width, rect->height, rect->left, rect->top);
+		*target_rect = *cam_rect;
+		return 0;
+	}
+
+	/* Try to fix cropping, that camera hasn't managed to set */
+	dev_geo(dev, "Fix camera S_CROP for %dx%d@%d:%d to %dx%d@%d:%d\n",
+		cam_rect->width, cam_rect->height,
+		cam_rect->left, cam_rect->top,
+		rect->width, rect->height, rect->left, rect->top);
+
+	/* We need sensor maximum rectangle */
+	ret = v4l2_subdev_call(sd, video, cropcap, &cap);
+	if (ret < 0)
+		return ret;
+
+	/* Put user requested rectangle within sensor bounds */
+	soc_camera_limit_side(&rect->left, &rect->width, cap.bounds.left, 2,
+			      cap.bounds.width);
+	soc_camera_limit_side(&rect->top, &rect->height, cap.bounds.top, 4,
+			      cap.bounds.height);
+
+	/*
+	 * Popular special case - some cameras can only handle fixed sizes like
+	 * QVGA, VGA,... Take care to avoid infinite loop.
+	 */
+	width = max_t(unsigned int, cam_rect->width, 2);
+	height = max_t(unsigned int, cam_rect->height, 2);
+
+	/*
+	 * Loop as long as sensor is not covering the requested rectangle and
+	 * is still within its bounds
+	 */
+	while (!ret && (is_smaller(cam_rect, rect) ||
+			is_inside(cam_rect, rect)) &&
+	       (cap.bounds.width > width || cap.bounds.height > height)) {
+
+		width *= 2;
+		height *= 2;
+
+		cam_rect->width = width;
+		cam_rect->height = height;
+
+		/*
+		 * We do not know what capabilities the camera has to set up
+		 * left and top borders. We could try to be smarter in iterating
+		 * them, e.g., if camera current left is to the right of the
+		 * target left, set it to the middle point between the current
+		 * left and minimum left. But that would add too much
+		 * complexity: we would have to iterate each border separately.
+		 * Instead we just drop to the left and top bounds.
+		 */
+		if (cam_rect->left > rect->left)
+			cam_rect->left = cap.bounds.left;
+
+		if (cam_rect->left + cam_rect->width < rect->left + rect->width)
+			cam_rect->width = rect->left + rect->width -
+				cam_rect->left;
+
+		if (cam_rect->top > rect->top)
+			cam_rect->top = cap.bounds.top;
+
+		if (cam_rect->top + cam_rect->height < rect->top + rect->height)
+			cam_rect->height = rect->top + rect->height -
+				cam_rect->top;
+
+		v4l2_subdev_call(sd, video, s_crop, cam_crop);
+		ret = soc_camera_client_g_rect(sd, cam_rect);
+		dev_geo(dev, "Camera S_CROP %d for %dx%d@%d:%d\n", ret,
+			cam_rect->width, cam_rect->height,
+			cam_rect->left, cam_rect->top);
+	}
+
+	/* S_CROP must not modify the rectangle */
+	if (is_smaller(cam_rect, rect) || is_inside(cam_rect, rect)) {
+		/*
+		 * The camera failed to configure a suitable cropping,
+		 * we cannot use the current rectangle, set to max
+		 */
+		*cam_rect = cap.bounds;
+		v4l2_subdev_call(sd, video, s_crop, cam_crop);
+		ret = soc_camera_client_g_rect(sd, cam_rect);
+		dev_geo(dev, "Camera S_CROP %d for max %dx%d@%d:%d\n", ret,
+			cam_rect->width, cam_rect->height,
+			cam_rect->left, cam_rect->top);
+	}
+
+	if (!ret) {
+		*target_rect = *cam_rect;
+		update_subrect(target_rect, subrect);
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL(soc_camera_client_s_crop);
+
+/* Iterative set_fmt, also updates cached client crop on success */
+static int client_set_fmt(struct soc_camera_device *icd,
+			struct v4l2_rect *rect, struct v4l2_rect *subrect,
+			unsigned int max_width, unsigned int max_height,
+			struct v4l2_subdev_format *format, bool host_can_scale)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct device *dev = icd->parent;
+	struct v4l2_mbus_framefmt *mf = &format->format;
+	unsigned int width = mf->width, height = mf->height, tmp_w, tmp_h;
+	struct v4l2_cropcap cap;
+	bool host_1to1;
+	int ret;
+
+	ret = v4l2_device_call_until_err(sd->v4l2_dev,
+					 soc_camera_grp_id(icd), pad,
+					 set_fmt, NULL, format);
+	if (ret < 0)
+		return ret;
+
+	dev_geo(dev, "camera scaled to %ux%u\n", mf->width, mf->height);
+
+	if (width == mf->width && height == mf->height) {
+		/* Perfect! The client has done it all. */
+		host_1to1 = true;
+		goto update_cache;
+	}
+
+	host_1to1 = false;
+	if (!host_can_scale)
+		goto update_cache;
+
+	cap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	ret = v4l2_subdev_call(sd, video, cropcap, &cap);
+	if (ret < 0)
+		return ret;
+
+	if (max_width > cap.bounds.width)
+		max_width = cap.bounds.width;
+	if (max_height > cap.bounds.height)
+		max_height = cap.bounds.height;
+
+	/* Camera set a format, but geometry is not precise, try to improve */
+	tmp_w = mf->width;
+	tmp_h = mf->height;
+
+	/* width <= max_width && height <= max_height - guaranteed by try_fmt */
+	while ((width > tmp_w || height > tmp_h) &&
+	       tmp_w < max_width && tmp_h < max_height) {
+		tmp_w = min(2 * tmp_w, max_width);
+		tmp_h = min(2 * tmp_h, max_height);
+		mf->width = tmp_w;
+		mf->height = tmp_h;
+		ret = v4l2_device_call_until_err(sd->v4l2_dev,
+					soc_camera_grp_id(icd), pad,
+					set_fmt, NULL, format);
+		dev_geo(dev, "Camera scaled to %ux%u\n",
+			mf->width, mf->height);
+		if (ret < 0) {
+			/* This shouldn't happen */
+			dev_err(dev, "Client failed to set format: %d\n", ret);
+			return ret;
+		}
+	}
+
+update_cache:
+	/* Update cache */
+	ret = soc_camera_client_g_rect(sd, rect);
+	if (ret < 0)
+		return ret;
+
+	if (host_1to1)
+		*subrect = *rect;
+	else
+		update_subrect(rect, subrect);
+
+	return 0;
+}
+
+/**
+ * @icd		- soc-camera device
+ * @rect	- camera cropping window
+ * @subrect	- part of rect, sent to the user
+ * @mf		- in- / output camera output window
+ * @width	- on input: max host input width
+ *		  on output: user width, mapped back to input
+ * @height	- on input: max host input height
+ *		  on output: user height, mapped back to input
+ * @host_can_scale - host can scale this pixel format
+ * @shift	- shift, used for scaling
+ */
+int soc_camera_client_scale(struct soc_camera_device *icd,
+			struct v4l2_rect *rect, struct v4l2_rect *subrect,
+			struct v4l2_mbus_framefmt *mf,
+			unsigned int *width, unsigned int *height,
+			bool host_can_scale, unsigned int shift)
+{
+	struct device *dev = icd->parent;
+	struct v4l2_subdev_format fmt_tmp = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.format = *mf,
+	};
+	struct v4l2_mbus_framefmt *mf_tmp = &fmt_tmp.format;
+	unsigned int scale_h, scale_v;
+	int ret;
+
+	/*
+	 * 5. Apply iterative camera S_FMT for camera user window (also updates
+	 *    client crop cache and the imaginary sub-rectangle).
+	 */
+	ret = client_set_fmt(icd, rect, subrect, *width, *height,
+			   &fmt_tmp, host_can_scale);
+	if (ret < 0)
+		return ret;
+
+	dev_geo(dev, "5: camera scaled to %ux%u\n",
+		mf_tmp->width, mf_tmp->height);
+
+	/* 6. Retrieve camera output window (g_fmt) */
+
+	/* unneeded - it is already in "mf_tmp" */
+
+	/* 7. Calculate new client scales. */
+	scale_h = soc_camera_calc_scale(rect->width, shift, mf_tmp->width);
+	scale_v = soc_camera_calc_scale(rect->height, shift, mf_tmp->height);
+
+	mf->width	= mf_tmp->width;
+	mf->height	= mf_tmp->height;
+	mf->colorspace	= mf_tmp->colorspace;
+
+	/*
+	 * 8. Calculate new host crop - apply camera scales to previously
+	 *    updated "effective" crop.
+	 */
+	*width = soc_camera_shift_scale(subrect->width, shift, scale_h);
+	*height = soc_camera_shift_scale(subrect->height, shift, scale_v);
+
+	dev_geo(dev, "8: new client sub-window %ux%u\n", *width, *height);
+
+	return 0;
+}
+EXPORT_SYMBOL(soc_camera_client_scale);
+
+/*
+ * Calculate real client output window by applying new scales to the current
+ * client crop. New scales are calculated from the requested output format and
+ * host crop, mapped backed onto the client input (subrect).
+ */
+void soc_camera_calc_client_output(struct soc_camera_device *icd,
+		struct v4l2_rect *rect, struct v4l2_rect *subrect,
+		const struct v4l2_pix_format *pix, struct v4l2_mbus_framefmt *mf,
+		unsigned int shift)
+{
+	struct device *dev = icd->parent;
+	unsigned int scale_v, scale_h;
+
+	if (subrect->width == rect->width &&
+	    subrect->height == rect->height) {
+		/* No sub-cropping */
+		mf->width	= pix->width;
+		mf->height	= pix->height;
+		return;
+	}
+
+	/* 1.-2. Current camera scales and subwin - cached. */
+
+	dev_geo(dev, "2: subwin %ux%u@%u:%u\n",
+		subrect->width, subrect->height,
+		subrect->left, subrect->top);
+
+	/*
+	 * 3. Calculate new combined scales from input sub-window to requested
+	 *    user window.
+	 */
+
+	/*
+	 * TODO: CEU cannot scale images larger than VGA to smaller than SubQCIF
+	 * (128x96) or larger than VGA. This and similar limitations have to be
+	 * taken into account here.
+	 */
+	scale_h = soc_camera_calc_scale(subrect->width, shift, pix->width);
+	scale_v = soc_camera_calc_scale(subrect->height, shift, pix->height);
+
+	dev_geo(dev, "3: scales %u:%u\n", scale_h, scale_v);
+
+	/*
+	 * 4. Calculate desired client output window by applying combined scales
+	 *    to client (real) input window.
+	 */
+	mf->width = soc_camera_shift_scale(rect->width, shift, scale_h);
+	mf->height = soc_camera_shift_scale(rect->height, shift, scale_v);
+}
+EXPORT_SYMBOL(soc_camera_calc_client_output);
diff --git a/drivers/media/platform/soc_camera/soc_scale_crop.h b/drivers/media/platform/soc_camera/soc_scale_crop.h
new file mode 100644
index 0000000..184a30d
--- /dev/null
+++ b/drivers/media/platform/soc_camera/soc_scale_crop.h
@@ -0,0 +1,47 @@
+/*
+ * soc-camera generic scaling-cropping manipulation functions
+ *
+ * Copyright (C) 2013 Guennadi Liakhovetski <g.liakhovetski@gmx.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#ifndef SOC_SCALE_CROP_H
+#define SOC_SCALE_CROP_H
+
+#include <linux/kernel.h>
+
+struct soc_camera_device;
+
+struct v4l2_crop;
+struct v4l2_mbus_framefmt;
+struct v4l2_pix_format;
+struct v4l2_rect;
+struct v4l2_subdev;
+
+static inline unsigned int soc_camera_shift_scale(unsigned int size,
+				unsigned int shift, unsigned int scale)
+{
+	return DIV_ROUND_CLOSEST(size << shift, scale);
+}
+
+#define soc_camera_calc_scale(in, shift, out) soc_camera_shift_scale(in, shift, out)
+
+int soc_camera_client_g_rect(struct v4l2_subdev *sd, struct v4l2_rect *rect);
+int soc_camera_client_s_crop(struct v4l2_subdev *sd,
+			struct v4l2_crop *crop, struct v4l2_crop *cam_crop,
+			struct v4l2_rect *target_rect, struct v4l2_rect *subrect);
+int soc_camera_client_scale(struct soc_camera_device *icd,
+			struct v4l2_rect *rect, struct v4l2_rect *subrect,
+			struct v4l2_mbus_framefmt *mf,
+			unsigned int *width, unsigned int *height,
+			bool host_can_scale, unsigned int shift);
+void soc_camera_calc_client_output(struct soc_camera_device *icd,
+		struct v4l2_rect *rect, struct v4l2_rect *subrect,
+		const struct v4l2_pix_format *pix, struct v4l2_mbus_framefmt *mf,
+		unsigned int shift);
+
+#endif
diff --git a/drivers/media/platform/sti/bdisp/Makefile b/drivers/media/platform/sti/bdisp/Makefile
new file mode 100644
index 0000000..bc53496
--- /dev/null
+++ b/drivers/media/platform/sti/bdisp/Makefile
@@ -0,0 +1,3 @@
+obj-$(CONFIG_VIDEO_STI_BDISP) := bdisp.o
+
+bdisp-objs := bdisp-v4l2.o bdisp-hw.o bdisp-debug.o
diff --git a/drivers/media/platform/sti/bdisp/bdisp-debug.c b/drivers/media/platform/sti/bdisp/bdisp-debug.c
new file mode 100644
index 0000000..79c5635
--- /dev/null
+++ b/drivers/media/platform/sti/bdisp/bdisp-debug.c
@@ -0,0 +1,687 @@
+/*
+ * Copyright (C) STMicroelectronics SA 2014
+ * Authors: Fabien Dessenne <fabien.dessenne@st.com> for STMicroelectronics.
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+#include <linux/debugfs.h>
+#include <linux/pm_runtime.h>
+
+#include "bdisp.h"
+#include "bdisp-filter.h"
+#include "bdisp-reg.h"
+
+void bdisp_dbg_perf_begin(struct bdisp_dev *bdisp)
+{
+	bdisp->dbg.hw_start = ktime_get();
+}
+
+void bdisp_dbg_perf_end(struct bdisp_dev *bdisp)
+{
+	s64 time_us;
+
+	time_us = ktime_us_delta(ktime_get(), bdisp->dbg.hw_start);
+
+	if (!bdisp->dbg.min_duration)
+		bdisp->dbg.min_duration = time_us;
+	else
+		bdisp->dbg.min_duration = min(time_us, bdisp->dbg.min_duration);
+
+	bdisp->dbg.last_duration = time_us;
+	bdisp->dbg.max_duration = max(time_us, bdisp->dbg.max_duration);
+	bdisp->dbg.tot_duration += time_us;
+}
+
+static void bdisp_dbg_dump_ins(struct seq_file *s, u32 val)
+{
+	seq_printf(s, "INS\t0x%08X\t", val);
+
+	switch (val & BLT_INS_S1_MASK) {
+	case BLT_INS_S1_OFF:
+		break;
+	case BLT_INS_S1_MEM:
+		seq_puts(s, "SRC1=mem - ");
+		break;
+	case BLT_INS_S1_CF:
+		seq_puts(s, "SRC1=ColorFill - ");
+		break;
+	case BLT_INS_S1_COPY:
+		seq_puts(s, "SRC1=copy - ");
+		break;
+	case BLT_INS_S1_FILL:
+		seq_puts(s, "SRC1=fil - ");
+		break;
+	default:
+		seq_puts(s, "SRC1=??? - ");
+		break;
+	}
+
+	switch (val & BLT_INS_S2_MASK) {
+	case BLT_INS_S2_OFF:
+		break;
+	case BLT_INS_S2_MEM:
+		seq_puts(s, "SRC2=mem - ");
+		break;
+	case BLT_INS_S2_CF:
+		seq_puts(s, "SRC2=ColorFill - ");
+		break;
+	default:
+		seq_puts(s, "SRC2=??? - ");
+		break;
+	}
+
+	if ((val & BLT_INS_S3_MASK) == BLT_INS_S3_MEM)
+		seq_puts(s, "SRC3=mem - ");
+
+	if (val & BLT_INS_IVMX)
+		seq_puts(s, "IVMX - ");
+	if (val & BLT_INS_CLUT)
+		seq_puts(s, "CLUT - ");
+	if (val & BLT_INS_SCALE)
+		seq_puts(s, "Scale - ");
+	if (val & BLT_INS_FLICK)
+		seq_puts(s, "Flicker - ");
+	if (val & BLT_INS_CLIP)
+		seq_puts(s, "Clip - ");
+	if (val & BLT_INS_CKEY)
+		seq_puts(s, "ColorKey - ");
+	if (val & BLT_INS_OVMX)
+		seq_puts(s, "OVMX - ");
+	if (val & BLT_INS_DEI)
+		seq_puts(s, "Deint - ");
+	if (val & BLT_INS_PMASK)
+		seq_puts(s, "PlaneMask - ");
+	if (val & BLT_INS_VC1R)
+		seq_puts(s, "VC1R - ");
+	if (val & BLT_INS_ROTATE)
+		seq_puts(s, "Rotate - ");
+	if (val & BLT_INS_GRAD)
+		seq_puts(s, "GradFill - ");
+	if (val & BLT_INS_AQLOCK)
+		seq_puts(s, "AQLock - ");
+	if (val & BLT_INS_PACE)
+		seq_puts(s, "Pace - ");
+	if (val & BLT_INS_IRQ)
+		seq_puts(s, "IRQ - ");
+
+	seq_puts(s, "\n");
+}
+
+static void bdisp_dbg_dump_tty(struct seq_file *s, u32 val)
+{
+	seq_printf(s, "TTY\t0x%08X\t", val);
+	seq_printf(s, "Pitch=%d - ", val & 0xFFFF);
+
+	switch ((val & BLT_TTY_COL_MASK) >> BLT_TTY_COL_SHIFT) {
+	case BDISP_RGB565:
+		seq_puts(s, "RGB565 - ");
+		break;
+	case BDISP_RGB888:
+		seq_puts(s, "RGB888 - ");
+		break;
+	case BDISP_XRGB8888:
+		seq_puts(s, "xRGB888 - ");
+		break;
+	case BDISP_ARGB8888:
+		seq_puts(s, "ARGB8888 - ");
+		break;
+	case BDISP_NV12:
+		seq_puts(s, "NV12 - ");
+		break;
+	case BDISP_YUV_3B:
+		seq_puts(s, "YUV420P - ");
+		break;
+	default:
+		seq_puts(s, "ColorFormat ??? - ");
+		break;
+	}
+
+	if (val & BLT_TTY_ALPHA_R)
+		seq_puts(s, "AlphaRange - ");
+	if (val & BLT_TTY_CR_NOT_CB)
+		seq_puts(s, "CrNotCb - ");
+	if (val & BLT_TTY_MB)
+		seq_puts(s, "MB - ");
+	if (val & BLT_TTY_HSO)
+		seq_puts(s, "HSO inverse - ");
+	if (val & BLT_TTY_VSO)
+		seq_puts(s, "VSO inverse - ");
+	if (val & BLT_TTY_DITHER)
+		seq_puts(s, "Dither - ");
+	if (val & BLT_TTY_CHROMA)
+		seq_puts(s, "Write CHROMA - ");
+	if (val & BLT_TTY_BIG_END)
+		seq_puts(s, "BigEndian - ");
+
+	seq_puts(s, "\n");
+}
+
+static void bdisp_dbg_dump_xy(struct seq_file *s, u32 val, char *name)
+{
+	seq_printf(s, "%s\t0x%08X\t", name, val);
+	seq_printf(s, "(%d,%d)\n", val & 0xFFFF, (val >> 16));
+}
+
+static void bdisp_dbg_dump_sz(struct seq_file *s, u32 val, char *name)
+{
+	seq_printf(s, "%s\t0x%08X\t", name, val);
+	seq_printf(s, "%dx%d\n", val & 0x1FFF, (val >> 16) & 0x1FFF);
+}
+
+static void bdisp_dbg_dump_sty(struct seq_file *s,
+			       u32 val, u32 addr, char *name)
+{
+	bool s1, s2, s3;
+
+	seq_printf(s, "%s\t0x%08X\t", name, val);
+
+	if (!addr || !name || (strlen(name) < 2))
+		goto done;
+
+	s1 = name[strlen(name) - 1] == '1';
+	s2 = name[strlen(name) - 1] == '2';
+	s3 = name[strlen(name) - 1] == '3';
+
+	seq_printf(s, "Pitch=%d - ", val & 0xFFFF);
+
+	switch ((val & BLT_TTY_COL_MASK) >> BLT_TTY_COL_SHIFT) {
+	case BDISP_RGB565:
+		seq_puts(s, "RGB565 - ");
+		break;
+	case BDISP_RGB888:
+		seq_puts(s, "RGB888 - ");
+		break;
+	case BDISP_XRGB8888:
+		seq_puts(s, "xRGB888 - ");
+		break;
+	case BDISP_ARGB8888:
+		seq_puts(s, "ARGB888 - ");
+		break;
+	case BDISP_NV12:
+		seq_puts(s, "NV12 - ");
+		break;
+	case BDISP_YUV_3B:
+		seq_puts(s, "YUV420P - ");
+		break;
+	default:
+		seq_puts(s, "ColorFormat ??? - ");
+		break;
+	}
+
+	if ((val & BLT_TTY_ALPHA_R) && !s3)
+		seq_puts(s, "AlphaRange - ");
+	if ((val & BLT_S1TY_A1_SUBSET) && !s3)
+		seq_puts(s, "A1SubSet - ");
+	if ((val & BLT_TTY_MB) && !s1)
+		seq_puts(s, "MB - ");
+	if (val & BLT_TTY_HSO)
+		seq_puts(s, "HSO inverse - ");
+	if (val & BLT_TTY_VSO)
+		seq_puts(s, "VSO inverse - ");
+	if ((val & BLT_S1TY_CHROMA_EXT) && (s1 || s2))
+		seq_puts(s, "ChromaExt - ");
+	if ((val & BLT_S3TY_BLANK_ACC) && s3)
+		seq_puts(s, "Blank Acc - ");
+	if ((val & BTL_S1TY_SUBBYTE) && !s3)
+		seq_puts(s, "SubByte - ");
+	if ((val & BLT_S1TY_RGB_EXP) && !s3)
+		seq_puts(s, "RGBExpand - ");
+	if ((val & BLT_TTY_BIG_END) && !s3)
+		seq_puts(s, "BigEndian - ");
+
+done:
+	seq_puts(s, "\n");
+}
+
+static void bdisp_dbg_dump_fctl(struct seq_file *s, u32 val)
+{
+	seq_printf(s, "FCTL\t0x%08X\t", val);
+
+	if ((val & BLT_FCTL_Y_HV_SCALE) == BLT_FCTL_Y_HV_SCALE)
+		seq_puts(s, "Resize Luma - ");
+	else if ((val & BLT_FCTL_Y_HV_SCALE) == BLT_FCTL_Y_HV_SAMPLE)
+		seq_puts(s, "Sample Luma - ");
+
+	if ((val & BLT_FCTL_HV_SCALE) == BLT_FCTL_HV_SCALE)
+		seq_puts(s, "Resize Chroma");
+	else if ((val & BLT_FCTL_HV_SCALE) == BLT_FCTL_HV_SAMPLE)
+		seq_puts(s, "Sample Chroma");
+
+	seq_puts(s, "\n");
+}
+
+static void bdisp_dbg_dump_rsf(struct seq_file *s, u32 val, char *name)
+{
+	u32 inc;
+
+	seq_printf(s, "%s\t0x%08X\t", name, val);
+
+	if (!val)
+		goto done;
+
+	inc = val & 0xFFFF;
+	seq_printf(s, "H: %d(6.10) / scale~%dx0.1 - ", inc, 1024 * 10 / inc);
+
+	inc = val >> 16;
+	seq_printf(s, "V: %d(6.10) / scale~%dx0.1", inc, 1024 * 10 / inc);
+
+done:
+	seq_puts(s, "\n");
+}
+
+static void bdisp_dbg_dump_rzi(struct seq_file *s, u32 val, char *name)
+{
+	seq_printf(s, "%s\t0x%08X\t", name, val);
+
+	if (!val)
+		goto done;
+
+	seq_printf(s, "H: init=%d repeat=%d - ", val & 0x3FF, (val >> 12) & 7);
+	val >>= 16;
+	seq_printf(s, "V: init=%d repeat=%d", val & 0x3FF, (val >> 12) & 7);
+
+done:
+	seq_puts(s, "\n");
+}
+
+static void bdisp_dbg_dump_ivmx(struct seq_file *s,
+				u32 c0, u32 c1, u32 c2, u32 c3)
+{
+	seq_printf(s, "IVMX0\t0x%08X\n", c0);
+	seq_printf(s, "IVMX1\t0x%08X\n", c1);
+	seq_printf(s, "IVMX2\t0x%08X\n", c2);
+	seq_printf(s, "IVMX3\t0x%08X\t", c3);
+
+	if (!c0 && !c1 && !c2 && !c3) {
+		seq_puts(s, "\n");
+		return;
+	}
+
+	if ((c0 == bdisp_rgb_to_yuv[0]) &&
+	    (c1 == bdisp_rgb_to_yuv[1]) &&
+	    (c2 == bdisp_rgb_to_yuv[2]) &&
+	    (c3 == bdisp_rgb_to_yuv[3])) {
+		seq_puts(s, "RGB to YUV\n");
+		return;
+	}
+
+	if ((c0 == bdisp_yuv_to_rgb[0]) &&
+	    (c1 == bdisp_yuv_to_rgb[1]) &&
+	    (c2 == bdisp_yuv_to_rgb[2]) &&
+	    (c3 == bdisp_yuv_to_rgb[3])) {
+		seq_puts(s, "YUV to RGB\n");
+		return;
+	}
+	seq_puts(s, "Unknown conversion\n");
+}
+
+static int bdisp_dbg_last_nodes(struct seq_file *s, void *data)
+{
+	/* Not dumping all fields, focusing on significant ones */
+	struct bdisp_dev *bdisp = s->private;
+	struct bdisp_node *node;
+	int i = 0;
+
+	if (!bdisp->dbg.copy_node[0]) {
+		seq_puts(s, "No node built yet\n");
+		return 0;
+	}
+
+	do {
+		node = bdisp->dbg.copy_node[i];
+		if (!node)
+			break;
+		seq_printf(s, "--------\nNode %d:\n", i);
+		seq_puts(s, "-- General --\n");
+		seq_printf(s, "NIP\t0x%08X\n", node->nip);
+		seq_printf(s, "CIC\t0x%08X\n", node->cic);
+		bdisp_dbg_dump_ins(s, node->ins);
+		seq_printf(s, "ACK\t0x%08X\n", node->ack);
+		seq_puts(s, "-- Target --\n");
+		seq_printf(s, "TBA\t0x%08X\n", node->tba);
+		bdisp_dbg_dump_tty(s, node->tty);
+		bdisp_dbg_dump_xy(s, node->txy, "TXY");
+		bdisp_dbg_dump_sz(s, node->tsz, "TSZ");
+		/* Color Fill not dumped */
+		seq_puts(s, "-- Source 1 --\n");
+		seq_printf(s, "S1BA\t0x%08X\n", node->s1ba);
+		bdisp_dbg_dump_sty(s, node->s1ty, node->s1ba, "S1TY");
+		bdisp_dbg_dump_xy(s, node->s1xy, "S1XY");
+		seq_puts(s, "-- Source 2 --\n");
+		seq_printf(s, "S2BA\t0x%08X\n", node->s2ba);
+		bdisp_dbg_dump_sty(s, node->s2ty, node->s2ba, "S2TY");
+		bdisp_dbg_dump_xy(s, node->s2xy, "S2XY");
+		bdisp_dbg_dump_sz(s, node->s2sz, "S2SZ");
+		seq_puts(s, "-- Source 3 --\n");
+		seq_printf(s, "S3BA\t0x%08X\n", node->s3ba);
+		bdisp_dbg_dump_sty(s, node->s3ty, node->s3ba, "S3TY");
+		bdisp_dbg_dump_xy(s, node->s3xy, "S3XY");
+		bdisp_dbg_dump_sz(s, node->s3sz, "S3SZ");
+		/* Clipping not dumped */
+		/* CLUT not dumped */
+		seq_puts(s, "-- Filter & Mask --\n");
+		bdisp_dbg_dump_fctl(s, node->fctl);
+		/* PMK not dumped */
+		seq_puts(s, "-- Chroma Filter --\n");
+		bdisp_dbg_dump_rsf(s, node->rsf, "RSF");
+		bdisp_dbg_dump_rzi(s, node->rzi, "RZI");
+		seq_printf(s, "HFP\t0x%08X\n", node->hfp);
+		seq_printf(s, "VFP\t0x%08X\n", node->vfp);
+		seq_puts(s, "-- Luma Filter --\n");
+		bdisp_dbg_dump_rsf(s, node->y_rsf, "Y_RSF");
+		bdisp_dbg_dump_rzi(s, node->y_rzi, "Y_RZI");
+		seq_printf(s, "Y_HFP\t0x%08X\n", node->y_hfp);
+		seq_printf(s, "Y_VFP\t0x%08X\n", node->y_vfp);
+		/* Flicker not dumped */
+		/* Color key not dumped */
+		/* Reserved not dumped */
+		/* Static Address & User not dumped */
+		seq_puts(s, "-- Input Versatile Matrix --\n");
+		bdisp_dbg_dump_ivmx(s, node->ivmx0, node->ivmx1,
+				    node->ivmx2, node->ivmx3);
+		/* Output Versatile Matrix not dumped */
+		/* Pace not dumped */
+		/* VC1R & DEI not dumped */
+		/* Gradient Fill not dumped */
+	} while ((++i < MAX_NB_NODE) && node->nip);
+
+	return 0;
+}
+
+static int bdisp_dbg_last_nodes_raw(struct seq_file *s, void *data)
+{
+	struct bdisp_dev *bdisp = s->private;
+	struct bdisp_node *node;
+	u32 *val;
+	int j, i = 0;
+
+	if (!bdisp->dbg.copy_node[0]) {
+		seq_puts(s, "No node built yet\n");
+		return 0;
+	}
+
+	do {
+		node = bdisp->dbg.copy_node[i];
+		if (!node)
+			break;
+
+		seq_printf(s, "--------\nNode %d:\n", i);
+		val = (u32 *)node;
+		for (j = 0; j < sizeof(struct bdisp_node) / sizeof(u32); j++)
+			seq_printf(s, "0x%08X\n", *val++);
+	} while ((++i < MAX_NB_NODE) && node->nip);
+
+	return 0;
+}
+
+static const char *bdisp_fmt_to_str(struct bdisp_frame frame)
+{
+	switch (frame.fmt->pixelformat) {
+	case V4L2_PIX_FMT_YUV420:
+		return "YUV420P";
+	case V4L2_PIX_FMT_NV12:
+		if (frame.field == V4L2_FIELD_INTERLACED)
+			return "NV12 interlaced";
+		else
+			return "NV12";
+	case V4L2_PIX_FMT_RGB565:
+		return "RGB16";
+	case V4L2_PIX_FMT_RGB24:
+		return "RGB24";
+	case V4L2_PIX_FMT_XBGR32:
+		return "XRGB";
+	case V4L2_PIX_FMT_ABGR32:
+		return "ARGB";
+	default:
+		return "????";
+	}
+}
+
+static int bdisp_dbg_last_request(struct seq_file *s, void *data)
+{
+	struct bdisp_dev *bdisp = s->private;
+	struct bdisp_request *request = &bdisp->dbg.copy_request;
+	struct bdisp_frame src, dst;
+
+	if (!request->nb_req) {
+		seq_puts(s, "No request\n");
+		return 0;
+	}
+
+	src = request->src;
+	dst = request->dst;
+
+	seq_printf(s, "\nRequest #%d\n", request->nb_req);
+
+	seq_printf(s, "Format:    %s\t\t\t%s\n",
+		   bdisp_fmt_to_str(src), bdisp_fmt_to_str(dst));
+	seq_printf(s, "Crop area: %dx%d @ %d,%d  ==>\t%dx%d @ %d,%d\n",
+		   src.crop.width, src.crop.height,
+		   src.crop.left, src.crop.top,
+		   dst.crop.width, dst.crop.height,
+		   dst.crop.left, dst.crop.top);
+	seq_printf(s, "Buff size: %dx%d\t\t%dx%d\n\n",
+		   src.width, src.height, dst.width, dst.height);
+
+	if (request->hflip)
+		seq_puts(s, "Horizontal flip\n\n");
+
+	if (request->vflip)
+		seq_puts(s, "Vertical flip\n\n");
+
+	return 0;
+}
+
+#define DUMP(reg) seq_printf(s, #reg " \t0x%08X\n", readl(bdisp->regs + reg))
+
+static int bdisp_dbg_regs(struct seq_file *s, void *data)
+{
+	struct bdisp_dev *bdisp = s->private;
+	int ret;
+	unsigned int i;
+
+	ret = pm_runtime_get_sync(bdisp->dev);
+	if (ret < 0) {
+		seq_puts(s, "Cannot wake up IP\n");
+		return 0;
+	}
+
+	seq_printf(s, "Reg @ = 0x%p\n", bdisp->regs);
+
+	seq_puts(s, "\nStatic:\n");
+	DUMP(BLT_CTL);
+	DUMP(BLT_ITS);
+	DUMP(BLT_STA1);
+	DUMP(BLT_AQ1_CTL);
+	DUMP(BLT_AQ1_IP);
+	DUMP(BLT_AQ1_LNA);
+	DUMP(BLT_AQ1_STA);
+	DUMP(BLT_ITM0);
+
+	seq_puts(s, "\nPlugs:\n");
+	DUMP(BLT_PLUGS1_OP2);
+	DUMP(BLT_PLUGS1_CHZ);
+	DUMP(BLT_PLUGS1_MSZ);
+	DUMP(BLT_PLUGS1_PGZ);
+	DUMP(BLT_PLUGS2_OP2);
+	DUMP(BLT_PLUGS2_CHZ);
+	DUMP(BLT_PLUGS2_MSZ);
+	DUMP(BLT_PLUGS2_PGZ);
+	DUMP(BLT_PLUGS3_OP2);
+	DUMP(BLT_PLUGS3_CHZ);
+	DUMP(BLT_PLUGS3_MSZ);
+	DUMP(BLT_PLUGS3_PGZ);
+	DUMP(BLT_PLUGT_OP2);
+	DUMP(BLT_PLUGT_CHZ);
+	DUMP(BLT_PLUGT_MSZ);
+	DUMP(BLT_PLUGT_PGZ);
+
+	seq_puts(s, "\nNode:\n");
+	DUMP(BLT_NIP);
+	DUMP(BLT_CIC);
+	DUMP(BLT_INS);
+	DUMP(BLT_ACK);
+	DUMP(BLT_TBA);
+	DUMP(BLT_TTY);
+	DUMP(BLT_TXY);
+	DUMP(BLT_TSZ);
+	DUMP(BLT_S1BA);
+	DUMP(BLT_S1TY);
+	DUMP(BLT_S1XY);
+	DUMP(BLT_S2BA);
+	DUMP(BLT_S2TY);
+	DUMP(BLT_S2XY);
+	DUMP(BLT_S2SZ);
+	DUMP(BLT_S3BA);
+	DUMP(BLT_S3TY);
+	DUMP(BLT_S3XY);
+	DUMP(BLT_S3SZ);
+	DUMP(BLT_FCTL);
+	DUMP(BLT_RSF);
+	DUMP(BLT_RZI);
+	DUMP(BLT_HFP);
+	DUMP(BLT_VFP);
+	DUMP(BLT_Y_RSF);
+	DUMP(BLT_Y_RZI);
+	DUMP(BLT_Y_HFP);
+	DUMP(BLT_Y_VFP);
+	DUMP(BLT_IVMX0);
+	DUMP(BLT_IVMX1);
+	DUMP(BLT_IVMX2);
+	DUMP(BLT_IVMX3);
+	DUMP(BLT_OVMX0);
+	DUMP(BLT_OVMX1);
+	DUMP(BLT_OVMX2);
+	DUMP(BLT_OVMX3);
+	DUMP(BLT_DEI);
+
+	seq_puts(s, "\nFilter:\n");
+	for (i = 0; i < BLT_NB_H_COEF; i++) {
+		seq_printf(s, "BLT_HFC%d \t0x%08X\n", i,
+			   readl(bdisp->regs + BLT_HFC_N + i * 4));
+	}
+	for (i = 0; i < BLT_NB_V_COEF; i++) {
+		seq_printf(s, "BLT_VFC%d \t0x%08X\n", i,
+			   readl(bdisp->regs + BLT_VFC_N + i * 4));
+	}
+
+	seq_puts(s, "\nLuma filter:\n");
+	for (i = 0; i < BLT_NB_H_COEF; i++) {
+		seq_printf(s, "BLT_Y_HFC%d \t0x%08X\n", i,
+			   readl(bdisp->regs + BLT_Y_HFC_N + i * 4));
+	}
+	for (i = 0; i < BLT_NB_V_COEF; i++) {
+		seq_printf(s, "BLT_Y_VFC%d \t0x%08X\n", i,
+			   readl(bdisp->regs + BLT_Y_VFC_N + i * 4));
+	}
+
+	pm_runtime_put(bdisp->dev);
+
+	return 0;
+}
+
+#define SECOND 1000000
+
+static int bdisp_dbg_perf(struct seq_file *s, void *data)
+{
+	struct bdisp_dev *bdisp = s->private;
+	struct bdisp_request *request = &bdisp->dbg.copy_request;
+	s64 avg_time_us;
+	int avg_fps, min_fps, max_fps, last_fps;
+
+	if (!request->nb_req) {
+		seq_puts(s, "No request\n");
+		return 0;
+	}
+
+	avg_time_us = div64_s64(bdisp->dbg.tot_duration, request->nb_req);
+	if (avg_time_us > SECOND)
+		avg_fps = 0;
+	else
+		avg_fps = SECOND / (s32)avg_time_us;
+
+	if (bdisp->dbg.min_duration > SECOND)
+		min_fps = 0;
+	else
+		min_fps = SECOND / (s32)bdisp->dbg.min_duration;
+
+	if (bdisp->dbg.max_duration > SECOND)
+		max_fps = 0;
+	else
+		max_fps = SECOND / (s32)bdisp->dbg.max_duration;
+
+	if (bdisp->dbg.last_duration > SECOND)
+		last_fps = 0;
+	else
+		last_fps = SECOND / (s32)bdisp->dbg.last_duration;
+
+	seq_printf(s, "HW processing (%d requests):\n", request->nb_req);
+	seq_printf(s, " Average: %5lld us  (%3d fps)\n",
+		   avg_time_us, avg_fps);
+	seq_printf(s, " Min-Max: %5lld us  (%3d fps) - %5lld us  (%3d fps)\n",
+		   bdisp->dbg.min_duration, min_fps,
+		   bdisp->dbg.max_duration, max_fps);
+	seq_printf(s, " Last:    %5lld us  (%3d fps)\n",
+		   bdisp->dbg.last_duration, last_fps);
+
+	return 0;
+}
+
+#define bdisp_dbg_declare(name) \
+	static int bdisp_dbg_##name##_open(struct inode *i, struct file *f) \
+	{ \
+		return single_open(f, bdisp_dbg_##name, i->i_private); \
+	} \
+	static const struct file_operations bdisp_dbg_##name##_fops = { \
+		.open           = bdisp_dbg_##name##_open, \
+		.read           = seq_read, \
+		.llseek         = seq_lseek, \
+		.release        = single_release, \
+	}
+
+#define bdisp_dbg_create_entry(name) \
+	debugfs_create_file(#name, S_IRUGO, bdisp->dbg.debugfs_entry, bdisp, \
+			    &bdisp_dbg_##name##_fops)
+
+bdisp_dbg_declare(regs);
+bdisp_dbg_declare(last_nodes);
+bdisp_dbg_declare(last_nodes_raw);
+bdisp_dbg_declare(last_request);
+bdisp_dbg_declare(perf);
+
+int bdisp_debugfs_create(struct bdisp_dev *bdisp)
+{
+	char dirname[16];
+
+	snprintf(dirname, sizeof(dirname), "%s%d", BDISP_NAME, bdisp->id);
+	bdisp->dbg.debugfs_entry = debugfs_create_dir(dirname, NULL);
+	if (!bdisp->dbg.debugfs_entry)
+		goto err;
+
+	if (!bdisp_dbg_create_entry(regs))
+		goto err;
+
+	if (!bdisp_dbg_create_entry(last_nodes))
+		goto err;
+
+	if (!bdisp_dbg_create_entry(last_nodes_raw))
+		goto err;
+
+	if (!bdisp_dbg_create_entry(last_request))
+		goto err;
+
+	if (!bdisp_dbg_create_entry(perf))
+		goto err;
+
+	return 0;
+
+err:
+	bdisp_debugfs_remove(bdisp);
+	return 0;
+}
+
+void bdisp_debugfs_remove(struct bdisp_dev *bdisp)
+{
+	debugfs_remove_recursive(bdisp->dbg.debugfs_entry);
+	bdisp->dbg.debugfs_entry = NULL;
+}
diff --git a/drivers/media/platform/sti/bdisp/bdisp-filter.h b/drivers/media/platform/sti/bdisp/bdisp-filter.h
new file mode 100644
index 0000000..fc8c54f
--- /dev/null
+++ b/drivers/media/platform/sti/bdisp/bdisp-filter.h
@@ -0,0 +1,346 @@
+/*
+ * Copyright (C) STMicroelectronics SA 2014
+ * Authors: Fabien Dessenne <fabien.dessenne@st.com> for STMicroelectronics.
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+#define BDISP_HF_NB             64
+#define BDISP_VF_NB             40
+
+/**
+ * struct bdisp_filter_h_spec - Horizontal filter specification
+ *
+ * @min:        min scale factor for this filter (6.10 fixed point)
+ * @max:        max scale factor for this filter (6.10 fixed point)
+ * coef:        filter coefficients
+ */
+struct bdisp_filter_h_spec {
+	const u16 min;
+	const u16 max;
+	const u8 coef[BDISP_HF_NB];
+};
+
+static const struct bdisp_filter_h_spec bdisp_h_spec[] = {
+	{
+		.min = 0,
+		.max = 921,
+		.coef = {
+			0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00,
+			0x00, 0x00, 0xff, 0x07, 0x3d, 0xfc, 0x01, 0x00,
+			0x00, 0x01, 0xfd, 0x11, 0x36, 0xf9, 0x02, 0x00,
+			0x00, 0x01, 0xfb, 0x1b, 0x2e, 0xf9, 0x02, 0x00,
+			0x00, 0x01, 0xf9, 0x26, 0x26, 0xf9, 0x01, 0x00,
+			0x00, 0x02, 0xf9, 0x30, 0x19, 0xfb, 0x01, 0x00,
+			0x00, 0x02, 0xf9, 0x39, 0x0e, 0xfd, 0x01, 0x00,
+			0x00, 0x01, 0xfc, 0x3e, 0x06, 0xff, 0x00, 0x00
+		}
+	},
+	{
+		.min = 921,
+		.max = 1024,
+		.coef = {
+			0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00,
+			0xff, 0x03, 0xfd, 0x08, 0x3e, 0xf9, 0x04, 0xfe,
+			0xfd, 0x06, 0xf8, 0x13, 0x3b, 0xf4, 0x07, 0xfc,
+			0xfb, 0x08, 0xf5, 0x1f, 0x34, 0xf1, 0x09, 0xfb,
+			0xfb, 0x09, 0xf2, 0x2b, 0x2a, 0xf1, 0x09, 0xfb,
+			0xfb, 0x09, 0xf2, 0x35, 0x1e, 0xf4, 0x08, 0xfb,
+			0xfc, 0x07, 0xf5, 0x3c, 0x12, 0xf7, 0x06, 0xfd,
+			0xfe, 0x04, 0xfa, 0x3f, 0x07, 0xfc, 0x03, 0xff
+		}
+	},
+	{
+		.min = 1024,
+		.max = 1126,
+		.coef = {
+			0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00,
+			0xff, 0x03, 0xfd, 0x08, 0x3e, 0xf9, 0x04, 0xfe,
+			0xfd, 0x06, 0xf8, 0x13, 0x3b, 0xf4, 0x07, 0xfc,
+			0xfb, 0x08, 0xf5, 0x1f, 0x34, 0xf1, 0x09, 0xfb,
+			0xfb, 0x09, 0xf2, 0x2b, 0x2a, 0xf1, 0x09, 0xfb,
+			0xfb, 0x09, 0xf2, 0x35, 0x1e, 0xf4, 0x08, 0xfb,
+			0xfc, 0x07, 0xf5, 0x3c, 0x12, 0xf7, 0x06, 0xfd,
+			0xfe, 0x04, 0xfa, 0x3f, 0x07, 0xfc, 0x03, 0xff
+		}
+	},
+	{
+		.min = 1126,
+		.max = 1228,
+		.coef = {
+			0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00,
+			0xff, 0x03, 0xfd, 0x08, 0x3e, 0xf9, 0x04, 0xfe,
+			0xfd, 0x06, 0xf8, 0x13, 0x3b, 0xf4, 0x07, 0xfc,
+			0xfb, 0x08, 0xf5, 0x1f, 0x34, 0xf1, 0x09, 0xfb,
+			0xfb, 0x09, 0xf2, 0x2b, 0x2a, 0xf1, 0x09, 0xfb,
+			0xfb, 0x09, 0xf2, 0x35, 0x1e, 0xf4, 0x08, 0xfb,
+			0xfc, 0x07, 0xf5, 0x3c, 0x12, 0xf7, 0x06, 0xfd,
+			0xfe, 0x04, 0xfa, 0x3f, 0x07, 0xfc, 0x03, 0xff
+		}
+	},
+	{
+		.min = 1228,
+		.max = 1331,
+		.coef = {
+			0xfd, 0x04, 0xfc, 0x05, 0x39, 0x05, 0xfc, 0x04,
+			0xfc, 0x06, 0xf9, 0x0c, 0x39, 0xfe, 0x00, 0x02,
+			0xfb, 0x08, 0xf6, 0x17, 0x35, 0xf9, 0x02, 0x00,
+			0xfc, 0x08, 0xf4, 0x20, 0x30, 0xf4, 0x05, 0xff,
+			0xfd, 0x07, 0xf4, 0x29, 0x28, 0xf3, 0x07, 0xfd,
+			0xff, 0x05, 0xf5, 0x31, 0x1f, 0xf3, 0x08, 0xfc,
+			0x00, 0x02, 0xf9, 0x38, 0x14, 0xf6, 0x08, 0xfb,
+			0x02, 0x00, 0xff, 0x3a, 0x0b, 0xf8, 0x06, 0xfc
+		}
+	},
+	{
+		.min = 1331,
+		.max = 1433,
+		.coef = {
+			0xfc, 0x06, 0xf9, 0x09, 0x34, 0x09, 0xf9, 0x06,
+			0xfd, 0x07, 0xf7, 0x10, 0x32, 0x02, 0xfc, 0x05,
+			0xfe, 0x07, 0xf6, 0x17, 0x2f, 0xfc, 0xff, 0x04,
+			0xff, 0x06, 0xf5, 0x20, 0x2a, 0xf9, 0x01, 0x02,
+			0x00, 0x04, 0xf6, 0x27, 0x25, 0xf6, 0x04, 0x00,
+			0x02, 0x01, 0xf9, 0x2d, 0x1d, 0xf5, 0x06, 0xff,
+			0x04, 0xff, 0xfd, 0x31, 0x15, 0xf5, 0x07, 0xfe,
+			0x05, 0xfc, 0x02, 0x35, 0x0d, 0xf7, 0x07, 0xfd
+		}
+	},
+	{
+		.min = 1433,
+		.max = 1536,
+		.coef = {
+			0xfe, 0x06, 0xf8, 0x0b, 0x30, 0x0b, 0xf8, 0x06,
+			0xff, 0x06, 0xf7, 0x12, 0x2d, 0x05, 0xfa, 0x06,
+			0x00, 0x04, 0xf6, 0x18, 0x2c, 0x00, 0xfc, 0x06,
+			0x01, 0x02, 0xf7, 0x1f, 0x27, 0xfd, 0xff, 0x04,
+			0x03, 0x00, 0xf9, 0x24, 0x24, 0xf9, 0x00, 0x03,
+			0x04, 0xff, 0xfd, 0x29, 0x1d, 0xf7, 0x02, 0x01,
+			0x06, 0xfc, 0x00, 0x2d, 0x17, 0xf6, 0x04, 0x00,
+			0x06, 0xfa, 0x05, 0x30, 0x0f, 0xf7, 0x06, 0xff
+		}
+	},
+	{
+		.min = 1536,
+		.max = 2048,
+		.coef = {
+			0x05, 0xfd, 0xfb, 0x13, 0x25, 0x13, 0xfb, 0xfd,
+			0x05, 0xfc, 0xfd, 0x17, 0x24, 0x0f, 0xf9, 0xff,
+			0x04, 0xfa, 0xff, 0x1b, 0x24, 0x0b, 0xf9, 0x00,
+			0x03, 0xf9, 0x01, 0x1f, 0x23, 0x08, 0xf8, 0x01,
+			0x02, 0xf9, 0x04, 0x22, 0x20, 0x04, 0xf9, 0x02,
+			0x01, 0xf8, 0x08, 0x25, 0x1d, 0x01, 0xf9, 0x03,
+			0x00, 0xf9, 0x0c, 0x25, 0x1a, 0xfe, 0xfa, 0x04,
+			0xff, 0xf9, 0x10, 0x26, 0x15, 0xfc, 0xfc, 0x05
+		}
+	},
+	{
+		.min = 2048,
+		.max = 3072,
+		.coef = {
+			0xfc, 0xfd, 0x06, 0x13, 0x18, 0x13, 0x06, 0xfd,
+			0xfc, 0xfe, 0x08, 0x15, 0x17, 0x12, 0x04, 0xfc,
+			0xfb, 0xfe, 0x0a, 0x16, 0x18, 0x10, 0x03, 0xfc,
+			0xfb, 0x00, 0x0b, 0x18, 0x17, 0x0f, 0x01, 0xfb,
+			0xfb, 0x00, 0x0d, 0x19, 0x17, 0x0d, 0x00, 0xfb,
+			0xfb, 0x01, 0x0f, 0x19, 0x16, 0x0b, 0x00, 0xfb,
+			0xfc, 0x03, 0x11, 0x19, 0x15, 0x09, 0xfe, 0xfb,
+			0xfc, 0x04, 0x12, 0x1a, 0x12, 0x08, 0xfe, 0xfc
+		}
+	},
+	{
+		.min = 3072,
+		.max = 4096,
+		.coef = {
+			0xfe, 0x02, 0x09, 0x0f, 0x0e, 0x0f, 0x09, 0x02,
+			0xff, 0x02, 0x09, 0x0f, 0x10, 0x0e, 0x08, 0x01,
+			0xff, 0x03, 0x0a, 0x10, 0x10, 0x0d, 0x07, 0x00,
+			0x00, 0x04, 0x0b, 0x10, 0x0f, 0x0c, 0x06, 0x00,
+			0x00, 0x05, 0x0c, 0x10, 0x0e, 0x0c, 0x05, 0x00,
+			0x00, 0x06, 0x0c, 0x11, 0x0e, 0x0b, 0x04, 0x00,
+			0x00, 0x07, 0x0d, 0x11, 0x0f, 0x0a, 0x03, 0xff,
+			0x01, 0x08, 0x0e, 0x11, 0x0e, 0x09, 0x02, 0xff
+		}
+	},
+	{
+		.min = 4096,
+		.max = 5120,
+		.coef = {
+			0x00, 0x04, 0x09, 0x0c, 0x0e, 0x0c, 0x09, 0x04,
+			0x01, 0x05, 0x09, 0x0c, 0x0d, 0x0c, 0x08, 0x04,
+			0x01, 0x05, 0x0a, 0x0c, 0x0e, 0x0b, 0x08, 0x03,
+			0x02, 0x06, 0x0a, 0x0d, 0x0c, 0x0b, 0x07, 0x03,
+			0x02, 0x07, 0x0a, 0x0d, 0x0d, 0x0a, 0x07, 0x02,
+			0x03, 0x07, 0x0b, 0x0d, 0x0c, 0x0a, 0x06, 0x02,
+			0x03, 0x08, 0x0b, 0x0d, 0x0d, 0x0a, 0x05, 0x01,
+			0x04, 0x08, 0x0c, 0x0d, 0x0c, 0x09, 0x05, 0x01
+		}
+	},
+	{
+		.min = 5120,
+		.max = 65535,
+		.coef = {
+			0x03, 0x06, 0x09, 0x0b, 0x09, 0x0b, 0x09, 0x06,
+			0x03, 0x06, 0x09, 0x0b, 0x0c, 0x0a, 0x08, 0x05,
+			0x03, 0x06, 0x09, 0x0b, 0x0c, 0x0a, 0x08, 0x05,
+			0x04, 0x07, 0x09, 0x0b, 0x0b, 0x0a, 0x08, 0x04,
+			0x04, 0x07, 0x0a, 0x0b, 0x0b, 0x0a, 0x07, 0x04,
+			0x04, 0x08, 0x0a, 0x0b, 0x0b, 0x09, 0x07, 0x04,
+			0x05, 0x08, 0x0a, 0x0b, 0x0c, 0x09, 0x06, 0x03,
+			0x05, 0x08, 0x0a, 0x0b, 0x0c, 0x09, 0x06, 0x03
+		}
+	}
+};
+
+/**
+ * struct bdisp_filter_v_spec - Vertical filter specification
+ *
+ * @min:	min scale factor for this filter (6.10 fixed point)
+ * @max:	max scale factor for this filter (6.10 fixed point)
+ * coef:	filter coefficients
+ */
+struct bdisp_filter_v_spec {
+	const u16 min;
+	const u16 max;
+	const u8 coef[BDISP_VF_NB];
+};
+
+static const struct bdisp_filter_v_spec bdisp_v_spec[] = {
+	{
+		.min = 0,
+		.max = 1024,
+		.coef = {
+			0x00, 0x00, 0x40, 0x00, 0x00,
+			0x00, 0x06, 0x3d, 0xfd, 0x00,
+			0xfe, 0x0f, 0x38, 0xfb, 0x00,
+			0xfd, 0x19, 0x2f, 0xfb, 0x00,
+			0xfc, 0x24, 0x24, 0xfc, 0x00,
+			0xfb, 0x2f, 0x19, 0xfd, 0x00,
+			0xfb, 0x38, 0x0f, 0xfe, 0x00,
+			0xfd, 0x3d, 0x06, 0x00, 0x00
+		}
+	},
+	{
+		.min = 1024,
+		.max = 1331,
+		.coef = {
+			0xfc, 0x05, 0x3e, 0x05, 0xfc,
+			0xf8, 0x0e, 0x3b, 0xff, 0x00,
+			0xf5, 0x18, 0x38, 0xf9, 0x02,
+			0xf4, 0x21, 0x31, 0xf5, 0x05,
+			0xf4, 0x2a, 0x27, 0xf4, 0x07,
+			0xf6, 0x30, 0x1e, 0xf4, 0x08,
+			0xf9, 0x35, 0x15, 0xf6, 0x07,
+			0xff, 0x37, 0x0b, 0xf9, 0x06
+		}
+	},
+	{
+		.min = 1331,
+		.max = 1433,
+		.coef = {
+			0xf8, 0x0a, 0x3c, 0x0a, 0xf8,
+			0xf6, 0x12, 0x3b, 0x02, 0xfb,
+			0xf4, 0x1b, 0x35, 0xfd, 0xff,
+			0xf4, 0x23, 0x30, 0xf8, 0x01,
+			0xf6, 0x29, 0x27, 0xf6, 0x04,
+			0xf9, 0x2e, 0x1e, 0xf5, 0x06,
+			0xfd, 0x31, 0x16, 0xf6, 0x06,
+			0x02, 0x32, 0x0d, 0xf8, 0x07
+		}
+	},
+	{
+		.min = 1433,
+		.max = 1536,
+		.coef = {
+			0xf6, 0x0e, 0x38, 0x0e, 0xf6,
+			0xf5, 0x15, 0x38, 0x06, 0xf8,
+			0xf5, 0x1d, 0x33, 0x00, 0xfb,
+			0xf6, 0x23, 0x2d, 0xfc, 0xfe,
+			0xf9, 0x28, 0x26, 0xf9, 0x00,
+			0xfc, 0x2c, 0x1e, 0xf7, 0x03,
+			0x00, 0x2e, 0x18, 0xf6, 0x04,
+			0x05, 0x2e, 0x11, 0xf7, 0x05
+		}
+	},
+	{
+		.min = 1536,
+		.max = 2048,
+		.coef = {
+			0xfb, 0x13, 0x24, 0x13, 0xfb,
+			0xfd, 0x17, 0x23, 0x0f, 0xfa,
+			0xff, 0x1a, 0x23, 0x0b, 0xf9,
+			0x01, 0x1d, 0x22, 0x07, 0xf9,
+			0x04, 0x20, 0x1f, 0x04, 0xf9,
+			0x07, 0x22, 0x1c, 0x01, 0xfa,
+			0x0b, 0x24, 0x17, 0xff, 0xfb,
+			0x0f, 0x24, 0x14, 0xfd, 0xfc
+		}
+	},
+	{
+		.min = 2048,
+		.max = 3072,
+		.coef = {
+			0x05, 0x10, 0x16, 0x10, 0x05,
+			0x06, 0x11, 0x16, 0x0f, 0x04,
+			0x08, 0x13, 0x15, 0x0e, 0x02,
+			0x09, 0x14, 0x16, 0x0c, 0x01,
+			0x0b, 0x15, 0x15, 0x0b, 0x00,
+			0x0d, 0x16, 0x13, 0x0a, 0x00,
+			0x0f, 0x17, 0x13, 0x08, 0xff,
+			0x11, 0x18, 0x12, 0x07, 0xfe
+		}
+	},
+	{
+		.min = 3072,
+		.max = 4096,
+		.coef = {
+			0x09, 0x0f, 0x10, 0x0f, 0x09,
+			0x09, 0x0f, 0x12, 0x0e, 0x08,
+			0x0a, 0x10, 0x11, 0x0e, 0x07,
+			0x0b, 0x11, 0x11, 0x0d, 0x06,
+			0x0c, 0x11, 0x12, 0x0c, 0x05,
+			0x0d, 0x12, 0x11, 0x0c, 0x04,
+			0x0e, 0x12, 0x11, 0x0b, 0x04,
+			0x0f, 0x13, 0x11, 0x0a, 0x03
+		}
+	},
+	{
+		.min = 4096,
+		.max = 5120,
+		.coef = {
+			0x0a, 0x0e, 0x10, 0x0e, 0x0a,
+			0x0b, 0x0e, 0x0f, 0x0e, 0x0a,
+			0x0b, 0x0f, 0x10, 0x0d, 0x09,
+			0x0c, 0x0f, 0x10, 0x0d, 0x08,
+			0x0d, 0x0f, 0x0f, 0x0d, 0x08,
+			0x0d, 0x10, 0x10, 0x0c, 0x07,
+			0x0e, 0x10, 0x0f, 0x0c, 0x07,
+			0x0f, 0x10, 0x10, 0x0b, 0x06
+		}
+	},
+	{
+		.min = 5120,
+		.max = 65535,
+		.coef = {
+			0x0b, 0x0e, 0x0e, 0x0e, 0x0b,
+			0x0b, 0x0e, 0x0f, 0x0d, 0x0b,
+			0x0c, 0x0e, 0x0f, 0x0d, 0x0a,
+			0x0c, 0x0e, 0x0f, 0x0d, 0x0a,
+			0x0d, 0x0f, 0x0e, 0x0d, 0x09,
+			0x0d, 0x0f, 0x0f, 0x0c, 0x09,
+			0x0e, 0x0f, 0x0e, 0x0c, 0x09,
+			0x0e, 0x0f, 0x0f, 0x0c, 0x08
+		}
+	}
+};
+
+#define NB_H_FILTER ARRAY_SIZE(bdisp_h_spec)
+#define NB_V_FILTER ARRAY_SIZE(bdisp_v_spec)
+
+/* RGB YUV 601 standard conversion */
+static const u32 bdisp_rgb_to_yuv[] = {
+		0x0e1e8bee, 0x08420419, 0xfb5ed471, 0x08004080,
+};
+
+static const u32 bdisp_yuv_to_rgb[] = {
+		0x3324a800, 0xe604ab9c, 0x0004a957, 0x32121eeb,
+};
diff --git a/drivers/media/platform/sti/bdisp/bdisp-hw.c b/drivers/media/platform/sti/bdisp/bdisp-hw.c
new file mode 100644
index 0000000..052c932
--- /dev/null
+++ b/drivers/media/platform/sti/bdisp/bdisp-hw.c
@@ -0,0 +1,823 @@
+/*
+ * Copyright (C) STMicroelectronics SA 2014
+ * Authors: Fabien Dessenne <fabien.dessenne@st.com> for STMicroelectronics.
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+#include <linux/delay.h>
+
+#include "bdisp.h"
+#include "bdisp-filter.h"
+#include "bdisp-reg.h"
+
+/* Max width of the source frame in a single node */
+#define MAX_SRC_WIDTH           2048
+
+/* Reset & boot poll config */
+#define POLL_RST_MAX            50
+#define POLL_RST_DELAY_MS       20
+
+enum bdisp_target_plan {
+	BDISP_RGB,
+	BDISP_Y,
+	BDISP_CBCR
+};
+
+struct bdisp_op_cfg {
+	bool cconv;          /* RGB - YUV conversion */
+	bool hflip;          /* Horizontal flip */
+	bool vflip;          /* Vertical flip */
+	bool wide;           /* Wide (>MAX_SRC_WIDTH) */
+	bool scale;          /* Scale */
+	u16  h_inc;          /* Horizontal increment in 6.10 format */
+	u16  v_inc;          /* Vertical increment in 6.10 format */
+	bool src_interlaced; /* is the src an interlaced buffer */
+	u8   src_nbp;        /* nb of planes of the src */
+	bool src_yuv;        /* is the src a YUV color format */
+	bool src_420;        /* is the src 4:2:0 chroma subsampled */
+	u8   dst_nbp;        /* nb of planes of the dst */
+	bool dst_yuv;        /* is the dst a YUV color format */
+	bool dst_420;        /* is the dst 4:2:0 chroma subsampled */
+};
+
+struct bdisp_filter_addr {
+	u16 min;             /* Filter min scale factor (6.10 fixed point) */
+	u16 max;             /* Filter max scale factor (6.10 fixed point) */
+	void *virt;          /* Virtual address for filter table */
+	dma_addr_t paddr;    /* Physical address for filter table */
+};
+
+static struct bdisp_filter_addr bdisp_h_filter[NB_H_FILTER];
+static struct bdisp_filter_addr bdisp_v_filter[NB_V_FILTER];
+
+/**
+ * bdisp_hw_reset
+ * @bdisp:      bdisp entity
+ *
+ * Resets HW
+ *
+ * RETURNS:
+ * 0 on success.
+ */
+int bdisp_hw_reset(struct bdisp_dev *bdisp)
+{
+	unsigned int i;
+
+	dev_dbg(bdisp->dev, "%s\n", __func__);
+
+	/* Mask Interrupt */
+	writel(0, bdisp->regs + BLT_ITM0);
+
+	/* Reset */
+	writel(readl(bdisp->regs + BLT_CTL) | BLT_CTL_RESET,
+	       bdisp->regs + BLT_CTL);
+	writel(0, bdisp->regs + BLT_CTL);
+
+	/* Wait for reset done */
+	for (i = 0; i < POLL_RST_MAX; i++) {
+		if (readl(bdisp->regs + BLT_STA1) & BLT_STA1_IDLE)
+			break;
+		msleep(POLL_RST_DELAY_MS);
+	}
+	if (i == POLL_RST_MAX)
+		dev_err(bdisp->dev, "Reset timeout\n");
+
+	return (i == POLL_RST_MAX) ? -EAGAIN : 0;
+}
+
+/**
+ * bdisp_hw_get_and_clear_irq
+ * @bdisp:      bdisp entity
+ *
+ * Read then reset interrupt status
+ *
+ * RETURNS:
+ * 0 if expected interrupt was raised.
+ */
+int bdisp_hw_get_and_clear_irq(struct bdisp_dev *bdisp)
+{
+	u32 its;
+
+	its = readl(bdisp->regs + BLT_ITS);
+
+	/* Check for the only expected IT: LastNode of AQ1 */
+	if (!(its & BLT_ITS_AQ1_LNA)) {
+		dev_dbg(bdisp->dev, "Unexpected IT status: 0x%08X\n", its);
+		writel(its, bdisp->regs + BLT_ITS);
+		return -1;
+	}
+
+	/* Clear and mask */
+	writel(its, bdisp->regs + BLT_ITS);
+	writel(0, bdisp->regs + BLT_ITM0);
+
+	return 0;
+}
+
+/**
+ * bdisp_hw_free_nodes
+ * @ctx:        bdisp context
+ *
+ * Free node memory
+ *
+ * RETURNS:
+ * None
+ */
+void bdisp_hw_free_nodes(struct bdisp_ctx *ctx)
+{
+	if (ctx && ctx->node[0]) {
+		DEFINE_DMA_ATTRS(attrs);
+
+		dma_set_attr(DMA_ATTR_WRITE_COMBINE, &attrs);
+		dma_free_attrs(ctx->bdisp_dev->dev,
+			       sizeof(struct bdisp_node) * MAX_NB_NODE,
+			       ctx->node[0], ctx->node_paddr[0], &attrs);
+	}
+}
+
+/**
+ * bdisp_hw_alloc_nodes
+ * @ctx:        bdisp context
+ *
+ * Allocate dma memory for nodes
+ *
+ * RETURNS:
+ * 0 on success
+ */
+int bdisp_hw_alloc_nodes(struct bdisp_ctx *ctx)
+{
+	struct device *dev = ctx->bdisp_dev->dev;
+	unsigned int i, node_size = sizeof(struct bdisp_node);
+	void *base;
+	dma_addr_t paddr;
+	DEFINE_DMA_ATTRS(attrs);
+
+	/* Allocate all the nodes within a single memory page */
+	dma_set_attr(DMA_ATTR_WRITE_COMBINE, &attrs);
+	base = dma_alloc_attrs(dev, node_size * MAX_NB_NODE, &paddr,
+			       GFP_KERNEL | GFP_DMA, &attrs);
+	if (!base) {
+		dev_err(dev, "%s no mem\n", __func__);
+		return -ENOMEM;
+	}
+
+	memset(base, 0, node_size * MAX_NB_NODE);
+
+	for (i = 0; i < MAX_NB_NODE; i++) {
+		ctx->node[i] = base;
+		ctx->node_paddr[i] = paddr;
+		dev_dbg(dev, "node[%d]=0x%p (paddr=%pad)\n", i, ctx->node[i],
+			&paddr);
+		base += node_size;
+		paddr += node_size;
+	}
+
+	return 0;
+}
+
+/**
+ * bdisp_hw_free_filters
+ * @dev:        device
+ *
+ * Free filters memory
+ *
+ * RETURNS:
+ * None
+ */
+void bdisp_hw_free_filters(struct device *dev)
+{
+	int size = (BDISP_HF_NB * NB_H_FILTER) + (BDISP_VF_NB * NB_V_FILTER);
+
+	if (bdisp_h_filter[0].virt) {
+		DEFINE_DMA_ATTRS(attrs);
+
+		dma_set_attr(DMA_ATTR_WRITE_COMBINE, &attrs);
+		dma_free_attrs(dev, size, bdisp_h_filter[0].virt,
+			       bdisp_h_filter[0].paddr, &attrs);
+	}
+}
+
+/**
+ * bdisp_hw_alloc_filters
+ * @dev:        device
+ *
+ * Allocate dma memory for filters
+ *
+ * RETURNS:
+ * 0 on success
+ */
+int bdisp_hw_alloc_filters(struct device *dev)
+{
+	unsigned int i, size;
+	void *base;
+	dma_addr_t paddr;
+	DEFINE_DMA_ATTRS(attrs);
+
+	/* Allocate all the filters within a single memory page */
+	size = (BDISP_HF_NB * NB_H_FILTER) + (BDISP_VF_NB * NB_V_FILTER);
+	dma_set_attr(DMA_ATTR_WRITE_COMBINE, &attrs);
+	base = dma_alloc_attrs(dev, size, &paddr, GFP_KERNEL | GFP_DMA, &attrs);
+	if (!base)
+		return -ENOMEM;
+
+	/* Setup filter addresses */
+	for (i = 0; i < NB_H_FILTER; i++) {
+		bdisp_h_filter[i].min = bdisp_h_spec[i].min;
+		bdisp_h_filter[i].max = bdisp_h_spec[i].max;
+		memcpy(base, bdisp_h_spec[i].coef, BDISP_HF_NB);
+		bdisp_h_filter[i].virt = base;
+		bdisp_h_filter[i].paddr = paddr;
+		base += BDISP_HF_NB;
+		paddr += BDISP_HF_NB;
+	}
+
+	for (i = 0; i < NB_V_FILTER; i++) {
+		bdisp_v_filter[i].min = bdisp_v_spec[i].min;
+		bdisp_v_filter[i].max = bdisp_v_spec[i].max;
+		memcpy(base, bdisp_v_spec[i].coef, BDISP_VF_NB);
+		bdisp_v_filter[i].virt = base;
+		bdisp_v_filter[i].paddr = paddr;
+		base += BDISP_VF_NB;
+		paddr += BDISP_VF_NB;
+	}
+
+	return 0;
+}
+
+/**
+ * bdisp_hw_get_hf_addr
+ * @inc:        resize increment
+ *
+ * Find the horizontal filter table that fits the resize increment
+ *
+ * RETURNS:
+ * table physical address
+ */
+static dma_addr_t bdisp_hw_get_hf_addr(u16 inc)
+{
+	unsigned int i;
+
+	for (i = NB_H_FILTER - 1; i > 0; i--)
+		if ((bdisp_h_filter[i].min < inc) &&
+		    (inc <= bdisp_h_filter[i].max))
+			break;
+
+	return bdisp_h_filter[i].paddr;
+}
+
+/**
+ * bdisp_hw_get_vf_addr
+ * @inc:        resize increment
+ *
+ * Find the vertical filter table that fits the resize increment
+ *
+ * RETURNS:
+ * table physical address
+ */
+static dma_addr_t bdisp_hw_get_vf_addr(u16 inc)
+{
+	unsigned int i;
+
+	for (i = NB_V_FILTER - 1; i > 0; i--)
+		if ((bdisp_v_filter[i].min < inc) &&
+		    (inc <= bdisp_v_filter[i].max))
+			break;
+
+	return bdisp_v_filter[i].paddr;
+}
+
+/**
+ * bdisp_hw_get_inc
+ * @from:       input size
+ * @to:         output size
+ * @inc:        resize increment in 6.10 format
+ *
+ * Computes the increment (inverse of scale) in 6.10 format
+ *
+ * RETURNS:
+ * 0 on success
+ */
+static int bdisp_hw_get_inc(u32 from, u32 to, u16 *inc)
+{
+	u32 tmp;
+
+	if (!to)
+		return -EINVAL;
+
+	if (to == from) {
+		*inc = 1 << 10;
+		return 0;
+	}
+
+	tmp = (from << 10) / to;
+	if ((tmp > 0xFFFF) || (!tmp))
+		/* overflow (downscale x 63) or too small (upscale x 1024) */
+		return -EINVAL;
+
+	*inc = (u16)tmp;
+
+	return 0;
+}
+
+/**
+ * bdisp_hw_get_hv_inc
+ * @ctx:        device context
+ * @h_inc:      horizontal increment
+ * @v_inc:      vertical increment
+ *
+ * Computes the horizontal & vertical increments (inverse of scale)
+ *
+ * RETURNS:
+ * 0 on success
+ */
+static int bdisp_hw_get_hv_inc(struct bdisp_ctx *ctx, u16 *h_inc, u16 *v_inc)
+{
+	u32 src_w, src_h, dst_w, dst_h;
+
+	src_w = ctx->src.crop.width;
+	src_h = ctx->src.crop.height;
+	dst_w = ctx->dst.crop.width;
+	dst_h = ctx->dst.crop.height;
+
+	if (bdisp_hw_get_inc(src_w, dst_w, h_inc) ||
+	    bdisp_hw_get_inc(src_h, dst_h, v_inc)) {
+		dev_err(ctx->bdisp_dev->dev,
+			"scale factors failed (%dx%d)->(%dx%d)\n",
+			src_w, src_h, dst_w, dst_h);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/**
+ * bdisp_hw_get_op_cfg
+ * @ctx:        device context
+ * @c:          operation configuration
+ *
+ * Check which blitter operations are expected and sets the scaling increments
+ *
+ * RETURNS:
+ * 0 on success
+ */
+static int bdisp_hw_get_op_cfg(struct bdisp_ctx *ctx, struct bdisp_op_cfg *c)
+{
+	struct device *dev = ctx->bdisp_dev->dev;
+	struct bdisp_frame *src = &ctx->src;
+	struct bdisp_frame *dst = &ctx->dst;
+
+	if (src->width > MAX_SRC_WIDTH * MAX_VERTICAL_STRIDES) {
+		dev_err(dev, "Image width out of HW caps\n");
+		return -EINVAL;
+	}
+
+	c->wide = src->width > MAX_SRC_WIDTH;
+
+	c->hflip = ctx->hflip;
+	c->vflip = ctx->vflip;
+
+	c->src_interlaced = (src->field == V4L2_FIELD_INTERLACED);
+
+	c->src_nbp = src->fmt->nb_planes;
+	c->src_yuv = (src->fmt->pixelformat == V4L2_PIX_FMT_NV12) ||
+			(src->fmt->pixelformat == V4L2_PIX_FMT_YUV420);
+	c->src_420 = c->src_yuv;
+
+	c->dst_nbp = dst->fmt->nb_planes;
+	c->dst_yuv = (dst->fmt->pixelformat == V4L2_PIX_FMT_NV12) ||
+			(dst->fmt->pixelformat == V4L2_PIX_FMT_YUV420);
+	c->dst_420 = c->dst_yuv;
+
+	c->cconv = (c->src_yuv != c->dst_yuv);
+
+	if (bdisp_hw_get_hv_inc(ctx, &c->h_inc, &c->v_inc)) {
+		dev_err(dev, "Scale factor out of HW caps\n");
+		return -EINVAL;
+	}
+
+	/* Deinterlacing adjustment : stretch a field to a frame */
+	if (c->src_interlaced)
+		c->v_inc /= 2;
+
+	if ((c->h_inc != (1 << 10)) || (c->v_inc != (1 << 10)))
+		c->scale = true;
+	else
+		c->scale = false;
+
+	return 0;
+}
+
+/**
+ * bdisp_hw_color_format
+ * @pixelformat: v4l2 pixel format
+ *
+ * v4l2 to bdisp pixel format convert
+ *
+ * RETURNS:
+ * bdisp pixel format
+ */
+static u32 bdisp_hw_color_format(u32 pixelformat)
+{
+	u32 ret;
+
+	switch (pixelformat) {
+	case V4L2_PIX_FMT_YUV420:
+		ret = (BDISP_YUV_3B << BLT_TTY_COL_SHIFT);
+		break;
+	case V4L2_PIX_FMT_NV12:
+		ret = (BDISP_NV12 << BLT_TTY_COL_SHIFT) | BLT_TTY_BIG_END;
+		break;
+	case V4L2_PIX_FMT_RGB565:
+		ret = (BDISP_RGB565 << BLT_TTY_COL_SHIFT);
+		break;
+	case V4L2_PIX_FMT_XBGR32: /* This V4L format actually refers to xRGB */
+		ret = (BDISP_XRGB8888 << BLT_TTY_COL_SHIFT);
+		break;
+	case V4L2_PIX_FMT_RGB24:  /* RGB888 format */
+		ret = (BDISP_RGB888 << BLT_TTY_COL_SHIFT) | BLT_TTY_BIG_END;
+		break;
+	case V4L2_PIX_FMT_ABGR32: /* This V4L format actually refers to ARGB */
+
+	default:
+		ret = (BDISP_ARGB8888 << BLT_TTY_COL_SHIFT) | BLT_TTY_ALPHA_R;
+		break;
+	}
+
+	return ret;
+}
+
+/**
+ * bdisp_hw_build_node
+ * @ctx:        device context
+ * @cfg:        operation configuration
+ * @node:       node to be set
+ * @t_plan:     whether the node refers to a RGB/Y or a CbCr plane
+ * @src_x_offset: x offset in the source image
+ *
+ * Build a node
+ *
+ * RETURNS:
+ * None
+ */
+static void bdisp_hw_build_node(struct bdisp_ctx *ctx,
+				struct bdisp_op_cfg *cfg,
+				struct bdisp_node *node,
+				enum bdisp_target_plan t_plan, int src_x_offset)
+{
+	struct bdisp_frame *src = &ctx->src;
+	struct bdisp_frame *dst = &ctx->dst;
+	u16 h_inc, v_inc, yh_inc, yv_inc;
+	struct v4l2_rect src_rect = src->crop;
+	struct v4l2_rect dst_rect = dst->crop;
+	int dst_x_offset;
+	s32 dst_width = dst->crop.width;
+	u32 src_fmt, dst_fmt;
+	const u32 *ivmx;
+
+	dev_dbg(ctx->bdisp_dev->dev, "%s\n", __func__);
+
+	memset(node, 0, sizeof(*node));
+
+	/* Adjust src and dst areas wrt src_x_offset */
+	src_rect.left += src_x_offset;
+	src_rect.width -= src_x_offset;
+	src_rect.width = min_t(__s32, MAX_SRC_WIDTH, src_rect.width);
+
+	dst_x_offset = (src_x_offset * dst_width) / ctx->src.crop.width;
+	dst_rect.left += dst_x_offset;
+	dst_rect.width = (src_rect.width * dst_width) / ctx->src.crop.width;
+
+	/* General */
+	src_fmt = src->fmt->pixelformat;
+	dst_fmt = dst->fmt->pixelformat;
+
+	node->nip = 0;
+	node->cic = BLT_CIC_ALL_GRP;
+	node->ack = BLT_ACK_BYPASS_S2S3;
+
+	switch (cfg->src_nbp) {
+	case 1:
+		/* Src2 = RGB / Src1 = Src3 = off */
+		node->ins = BLT_INS_S1_OFF | BLT_INS_S2_MEM | BLT_INS_S3_OFF;
+		break;
+	case 2:
+		/* Src3 = Y
+		 * Src2 = CbCr or ColorFill if writing the Y plane
+		 * Src1 = off */
+		node->ins = BLT_INS_S1_OFF | BLT_INS_S3_MEM;
+		if (t_plan == BDISP_Y)
+			node->ins |= BLT_INS_S2_CF;
+		else
+			node->ins |= BLT_INS_S2_MEM;
+		break;
+	case 3:
+	default:
+		/* Src3 = Y
+		 * Src2 = Cb or ColorFill if writing the Y plane
+		 * Src1 = Cr or ColorFill if writing the Y plane */
+		node->ins = BLT_INS_S3_MEM;
+		if (t_plan == BDISP_Y)
+			node->ins |= BLT_INS_S2_CF | BLT_INS_S1_CF;
+		else
+			node->ins |= BLT_INS_S2_MEM | BLT_INS_S1_MEM;
+		break;
+	}
+
+	/* Color convert */
+	node->ins |= cfg->cconv ? BLT_INS_IVMX : 0;
+	/* Scale needed if scaling OR 4:2:0 up/downsampling */
+	node->ins |= (cfg->scale || cfg->src_420 || cfg->dst_420) ?
+			BLT_INS_SCALE : 0;
+
+	/* Target */
+	node->tba = (t_plan == BDISP_CBCR) ? dst->paddr[1] : dst->paddr[0];
+
+	node->tty = dst->bytesperline;
+	node->tty |= bdisp_hw_color_format(dst_fmt);
+	node->tty |= BLT_TTY_DITHER;
+	node->tty |= (t_plan == BDISP_CBCR) ? BLT_TTY_CHROMA : 0;
+	node->tty |= cfg->hflip ? BLT_TTY_HSO : 0;
+	node->tty |= cfg->vflip ? BLT_TTY_VSO : 0;
+
+	if (cfg->dst_420 && (t_plan == BDISP_CBCR)) {
+		/* 420 chroma downsampling */
+		dst_rect.height /= 2;
+		dst_rect.width /= 2;
+		dst_rect.left /= 2;
+		dst_rect.top /= 2;
+		dst_x_offset /= 2;
+		dst_width /= 2;
+	}
+
+	node->txy = cfg->vflip ? (dst_rect.height - 1) : dst_rect.top;
+	node->txy <<= 16;
+	node->txy |= cfg->hflip ? (dst_width - dst_x_offset - 1) :
+			dst_rect.left;
+
+	node->tsz = dst_rect.height << 16 | dst_rect.width;
+
+	if (cfg->src_interlaced) {
+		/* handle only the top field which is half height of a frame */
+		src_rect.top /= 2;
+		src_rect.height /= 2;
+	}
+
+	if (cfg->src_nbp == 1) {
+		/* Src 2 : RGB */
+		node->s2ba = src->paddr[0];
+
+		node->s2ty = src->bytesperline;
+		if (cfg->src_interlaced)
+			node->s2ty *= 2;
+
+		node->s2ty |= bdisp_hw_color_format(src_fmt);
+
+		node->s2xy = src_rect.top << 16 | src_rect.left;
+		node->s2sz = src_rect.height << 16 | src_rect.width;
+	} else {
+		/* Src 2 : Cb or CbCr */
+		if (cfg->src_420) {
+			/* 420 chroma upsampling */
+			src_rect.top /= 2;
+			src_rect.left /= 2;
+			src_rect.width /= 2;
+			src_rect.height /= 2;
+		}
+
+		node->s2ba = src->paddr[1];
+
+		node->s2ty = src->bytesperline;
+		if (cfg->src_nbp == 3)
+			node->s2ty /= 2;
+		if (cfg->src_interlaced)
+			node->s2ty *= 2;
+
+		node->s2ty |= bdisp_hw_color_format(src_fmt);
+
+		node->s2xy = src_rect.top << 16 | src_rect.left;
+		node->s2sz = src_rect.height << 16 | src_rect.width;
+
+		if (cfg->src_nbp == 3) {
+			/* Src 1 : Cr */
+			node->s1ba = src->paddr[2];
+
+			node->s1ty = node->s2ty;
+			node->s1xy = node->s2xy;
+		}
+
+		/* Src 3 : Y */
+		node->s3ba = src->paddr[0];
+
+		node->s3ty = src->bytesperline;
+		if (cfg->src_interlaced)
+			node->s3ty *= 2;
+		node->s3ty |= bdisp_hw_color_format(src_fmt);
+
+		if ((t_plan != BDISP_CBCR) && cfg->src_420) {
+			/* No chroma upsampling for output RGB / Y plane */
+			node->s3xy = node->s2xy * 2;
+			node->s3sz = node->s2sz * 2;
+		} else {
+			/* No need to read Y (Src3) when writing Chroma */
+			node->s3ty |= BLT_S3TY_BLANK_ACC;
+			node->s3xy = node->s2xy;
+			node->s3sz = node->s2sz;
+		}
+	}
+
+	/* Resize (scale OR 4:2:0: chroma up/downsampling) */
+	if (node->ins & BLT_INS_SCALE) {
+		/* no need to compute Y when writing CbCr from RGB input */
+		bool skip_y = (t_plan == BDISP_CBCR) && !cfg->src_yuv;
+
+		/* FCTL */
+		if (cfg->scale) {
+			node->fctl = BLT_FCTL_HV_SCALE;
+			if (!skip_y)
+				node->fctl |= BLT_FCTL_Y_HV_SCALE;
+		} else {
+			node->fctl = BLT_FCTL_HV_SAMPLE;
+			if (!skip_y)
+				node->fctl |= BLT_FCTL_Y_HV_SAMPLE;
+		}
+
+		/* RSF - Chroma may need to be up/downsampled */
+		h_inc = cfg->h_inc;
+		v_inc = cfg->v_inc;
+		if (!cfg->src_420 && cfg->dst_420 && (t_plan == BDISP_CBCR)) {
+			/* RGB to 4:2:0 for Chroma: downsample */
+			h_inc *= 2;
+			v_inc *= 2;
+		} else if (cfg->src_420 && !cfg->dst_420) {
+			/* 4:2:0: to RGB: upsample*/
+			h_inc /= 2;
+			v_inc /= 2;
+		}
+		node->rsf = v_inc << 16 | h_inc;
+
+		/* RZI */
+		node->rzi = BLT_RZI_DEFAULT;
+
+		/* Filter table physical addr */
+		node->hfp = bdisp_hw_get_hf_addr(h_inc);
+		node->vfp = bdisp_hw_get_vf_addr(v_inc);
+
+		/* Y version */
+		if (!skip_y) {
+			yh_inc = cfg->h_inc;
+			yv_inc = cfg->v_inc;
+
+			node->y_rsf = yv_inc << 16 | yh_inc;
+			node->y_rzi = BLT_RZI_DEFAULT;
+			node->y_hfp = bdisp_hw_get_hf_addr(yh_inc);
+			node->y_vfp = bdisp_hw_get_vf_addr(yv_inc);
+		}
+	}
+
+	/* Versatile matrix for RGB / YUV conversion */
+	if (cfg->cconv) {
+		ivmx = cfg->src_yuv ? bdisp_yuv_to_rgb : bdisp_rgb_to_yuv;
+
+		node->ivmx0 = ivmx[0];
+		node->ivmx1 = ivmx[1];
+		node->ivmx2 = ivmx[2];
+		node->ivmx3 = ivmx[3];
+	}
+}
+
+/**
+ * bdisp_hw_build_all_nodes
+ * @ctx:        device context
+ *
+ * Build all the nodes for the blitter operation
+ *
+ * RETURNS:
+ * 0 on success
+ */
+static int bdisp_hw_build_all_nodes(struct bdisp_ctx *ctx)
+{
+	struct bdisp_op_cfg cfg;
+	unsigned int i, nid = 0;
+	int src_x_offset = 0;
+
+	for (i = 0; i < MAX_NB_NODE; i++)
+		if (!ctx->node[i]) {
+			dev_err(ctx->bdisp_dev->dev, "node %d is null\n", i);
+			return -EINVAL;
+		}
+
+	/* Get configuration (scale, flip, ...) */
+	if (bdisp_hw_get_op_cfg(ctx, &cfg))
+		return -EINVAL;
+
+	/* Split source in vertical strides (HW constraint) */
+	for (i = 0; i < MAX_VERTICAL_STRIDES; i++) {
+		/* Build RGB/Y node and link it to the previous node */
+		bdisp_hw_build_node(ctx, &cfg, ctx->node[nid],
+				    cfg.dst_nbp == 1 ? BDISP_RGB : BDISP_Y,
+				    src_x_offset);
+		if (nid)
+			ctx->node[nid - 1]->nip = ctx->node_paddr[nid];
+		nid++;
+
+		/* Build additional Cb(Cr) node, link it to the previous one */
+		if (cfg.dst_nbp > 1) {
+			bdisp_hw_build_node(ctx, &cfg, ctx->node[nid],
+					    BDISP_CBCR, src_x_offset);
+			ctx->node[nid - 1]->nip = ctx->node_paddr[nid];
+			nid++;
+		}
+
+		/* Next stride until full width covered */
+		src_x_offset += MAX_SRC_WIDTH;
+		if (src_x_offset >= ctx->src.crop.width)
+			break;
+	}
+
+	/* Mark last node as the last */
+	ctx->node[nid - 1]->nip = 0;
+
+	return 0;
+}
+
+/**
+ * bdisp_hw_save_request
+ * @ctx:        device context
+ *
+ * Save a copy of the request and of the built nodes
+ *
+ * RETURNS:
+ * None
+ */
+static void bdisp_hw_save_request(struct bdisp_ctx *ctx)
+{
+	struct bdisp_node **copy_node = ctx->bdisp_dev->dbg.copy_node;
+	struct bdisp_request *request = &ctx->bdisp_dev->dbg.copy_request;
+	struct bdisp_node **node = ctx->node;
+	int i;
+
+	/* Request copy */
+	request->src = ctx->src;
+	request->dst = ctx->dst;
+	request->hflip = ctx->hflip;
+	request->vflip = ctx->vflip;
+	request->nb_req++;
+
+	/* Nodes copy */
+	for (i = 0; i < MAX_NB_NODE; i++) {
+		/* Allocate memory if not done yet */
+		if (!copy_node[i]) {
+			copy_node[i] = devm_kzalloc(ctx->bdisp_dev->dev,
+						    sizeof(*copy_node[i]),
+						    GFP_KERNEL);
+			if (!copy_node[i])
+				return;
+		}
+		*copy_node[i] = *node[i];
+	}
+}
+
+/**
+ * bdisp_hw_update
+ * @ctx:        device context
+ *
+ * Send the request to the HW
+ *
+ * RETURNS:
+ * 0 on success
+ */
+int bdisp_hw_update(struct bdisp_ctx *ctx)
+{
+	int ret;
+	struct bdisp_dev *bdisp = ctx->bdisp_dev;
+	struct device *dev = bdisp->dev;
+	unsigned int node_id;
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	/* build nodes */
+	ret = bdisp_hw_build_all_nodes(ctx);
+	if (ret) {
+		dev_err(dev, "cannot build nodes (%d)\n", ret);
+		return ret;
+	}
+
+	/* Save a copy of the request */
+	bdisp_hw_save_request(ctx);
+
+	/* Configure interrupt to 'Last Node Reached for AQ1' */
+	writel(BLT_AQ1_CTL_CFG, bdisp->regs + BLT_AQ1_CTL);
+	writel(BLT_ITS_AQ1_LNA, bdisp->regs + BLT_ITM0);
+
+	/* Write first node addr */
+	writel(ctx->node_paddr[0], bdisp->regs + BLT_AQ1_IP);
+
+	/* Find and write last node addr : this starts the HW processing */
+	for (node_id = 0; node_id < MAX_NB_NODE - 1; node_id++) {
+		if (!ctx->node[node_id]->nip)
+			break;
+	}
+	writel(ctx->node_paddr[node_id], bdisp->regs + BLT_AQ1_LNA);
+
+	return 0;
+}
diff --git a/drivers/media/platform/sti/bdisp/bdisp-reg.h b/drivers/media/platform/sti/bdisp/bdisp-reg.h
new file mode 100644
index 0000000..e7e1a42
--- /dev/null
+++ b/drivers/media/platform/sti/bdisp/bdisp-reg.h
@@ -0,0 +1,235 @@
+/*
+ * Copyright (C) STMicroelectronics SA 2014
+ * Authors: Fabien Dessenne <fabien.dessenne@st.com> for STMicroelectronics.
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+struct bdisp_node {
+	/* 0 - General */
+	u32 nip;
+	u32 cic;
+	u32 ins;
+	u32 ack;
+	/* 1 - Target */
+	u32 tba;
+	u32 tty;
+	u32 txy;
+	u32 tsz;
+	/* 2 - Color Fill */
+	u32 s1cf;
+	u32 s2cf;
+	/* 3 - Source 1 */
+	u32 s1ba;
+	u32 s1ty;
+	u32 s1xy;
+	u32 s1sz_tsz;
+	/* 4 - Source 2 */
+	u32 s2ba;
+	u32 s2ty;
+	u32 s2xy;
+	u32 s2sz;
+	/* 5 - Source 3 */
+	u32 s3ba;
+	u32 s3ty;
+	u32 s3xy;
+	u32 s3sz;
+	/* 6 - Clipping */
+	u32 cwo;
+	u32 cws;
+	/* 7 - CLUT */
+	u32 cco;
+	u32 cml;
+	/* 8 - Filter & Mask */
+	u32 fctl;
+	u32 pmk;
+	/* 9 - Chroma Filter */
+	u32 rsf;
+	u32 rzi;
+	u32 hfp;
+	u32 vfp;
+	/* 10 - Luma Filter */
+	u32 y_rsf;
+	u32 y_rzi;
+	u32 y_hfp;
+	u32 y_vfp;
+	/* 11 - Flicker */
+	u32 ff0;
+	u32 ff1;
+	u32 ff2;
+	u32 ff3;
+	/* 12 - Color Key */
+	u32 key1;
+	u32 key2;
+	/* 14 - Static Address & User */
+	u32 sar;
+	u32 usr;
+	/* 15 - Input Versatile Matrix */
+	u32 ivmx0;
+	u32 ivmx1;
+	u32 ivmx2;
+	u32 ivmx3;
+	/* 16 - Output Versatile Matrix */
+	u32 ovmx0;
+	u32 ovmx1;
+	u32 ovmx2;
+	u32 ovmx3;
+	/* 17 - Pace */
+	u32 pace;
+	/* 18 - VC1R & DEI */
+	u32 vc1r;
+	u32 dei;
+	/* 19 - Gradient Fill */
+	u32 hgf;
+	u32 vgf;
+};
+
+/* HW registers : static */
+#define BLT_CTL                 0x0A00
+#define BLT_ITS                 0x0A04
+#define BLT_STA1                0x0A08
+#define BLT_AQ1_CTL             0x0A60
+#define BLT_AQ1_IP              0x0A64
+#define BLT_AQ1_LNA             0x0A68
+#define BLT_AQ1_STA             0x0A6C
+#define BLT_ITM0                0x0AD0
+/* HW registers : plugs */
+#define BLT_PLUGS1_OP2          0x0B04
+#define BLT_PLUGS1_CHZ          0x0B08
+#define BLT_PLUGS1_MSZ          0x0B0C
+#define BLT_PLUGS1_PGZ          0x0B10
+#define BLT_PLUGS2_OP2          0x0B24
+#define BLT_PLUGS2_CHZ          0x0B28
+#define BLT_PLUGS2_MSZ          0x0B2C
+#define BLT_PLUGS2_PGZ          0x0B30
+#define BLT_PLUGS3_OP2          0x0B44
+#define BLT_PLUGS3_CHZ          0x0B48
+#define BLT_PLUGS3_MSZ          0x0B4C
+#define BLT_PLUGS3_PGZ          0x0B50
+#define BLT_PLUGT_OP2           0x0B84
+#define BLT_PLUGT_CHZ           0x0B88
+#define BLT_PLUGT_MSZ           0x0B8C
+#define BLT_PLUGT_PGZ           0x0B90
+/* HW registers : node */
+#define BLT_NIP                 0x0C00
+#define BLT_CIC                 0x0C04
+#define BLT_INS                 0x0C08
+#define BLT_ACK                 0x0C0C
+#define BLT_TBA                 0x0C10
+#define BLT_TTY                 0x0C14
+#define BLT_TXY                 0x0C18
+#define BLT_TSZ                 0x0C1C
+#define BLT_S1BA                0x0C28
+#define BLT_S1TY                0x0C2C
+#define BLT_S1XY                0x0C30
+#define BLT_S2BA                0x0C38
+#define BLT_S2TY                0x0C3C
+#define BLT_S2XY                0x0C40
+#define BLT_S2SZ                0x0C44
+#define BLT_S3BA                0x0C48
+#define BLT_S3TY                0x0C4C
+#define BLT_S3XY                0x0C50
+#define BLT_S3SZ                0x0C54
+#define BLT_FCTL                0x0C68
+#define BLT_RSF                 0x0C70
+#define BLT_RZI                 0x0C74
+#define BLT_HFP                 0x0C78
+#define BLT_VFP                 0x0C7C
+#define BLT_Y_RSF               0x0C80
+#define BLT_Y_RZI               0x0C84
+#define BLT_Y_HFP               0x0C88
+#define BLT_Y_VFP               0x0C8C
+#define BLT_IVMX0               0x0CC0
+#define BLT_IVMX1               0x0CC4
+#define BLT_IVMX2               0x0CC8
+#define BLT_IVMX3               0x0CCC
+#define BLT_OVMX0               0x0CD0
+#define BLT_OVMX1               0x0CD4
+#define BLT_OVMX2               0x0CD8
+#define BLT_OVMX3               0x0CDC
+#define BLT_DEI                 0x0CEC
+/* HW registers : filters */
+#define BLT_HFC_N               0x0D00
+#define BLT_VFC_N               0x0D90
+#define BLT_Y_HFC_N             0x0E00
+#define BLT_Y_VFC_N             0x0E90
+#define BLT_NB_H_COEF           16
+#define BLT_NB_V_COEF           10
+
+/* Registers values */
+#define BLT_CTL_RESET           BIT(31)         /* Global soft reset */
+
+#define BLT_ITS_AQ1_LNA         BIT(12)         /* AQ1 LNA reached */
+
+#define BLT_STA1_IDLE           BIT(0)          /* BDISP idle */
+
+#define BLT_AQ1_CTL_CFG         0x80400003      /* Enable, P3, LNA reached */
+
+#define BLT_INS_S1_MASK         (BIT(0) | BIT(1) | BIT(2))
+#define BLT_INS_S1_OFF          0x00000000      /* src1 disabled */
+#define BLT_INS_S1_MEM          0x00000001      /* src1 fetched from memory */
+#define BLT_INS_S1_CF           0x00000003      /* src1 color fill */
+#define BLT_INS_S1_COPY         0x00000004      /* src1 direct copy */
+#define BLT_INS_S1_FILL         0x00000007      /* src1 firect fill */
+#define BLT_INS_S2_MASK         (BIT(3) | BIT(4))
+#define BLT_INS_S2_OFF          0x00000000      /* src2 disabled */
+#define BLT_INS_S2_MEM          0x00000008      /* src2 fetched from memory */
+#define BLT_INS_S2_CF           0x00000018      /* src2 color fill */
+#define BLT_INS_S3_MASK         BIT(5)
+#define BLT_INS_S3_OFF          0x00000000      /* src3 disabled */
+#define BLT_INS_S3_MEM          0x00000020      /* src3 fetched from memory */
+#define BLT_INS_IVMX            BIT(6)          /* Input versatile matrix */
+#define BLT_INS_CLUT            BIT(7)          /* Color Look Up Table */
+#define BLT_INS_SCALE           BIT(8)          /* Scaling */
+#define BLT_INS_FLICK           BIT(9)          /* Flicker filter */
+#define BLT_INS_CLIP            BIT(10)         /* Clipping */
+#define BLT_INS_CKEY            BIT(11)         /* Color key */
+#define BLT_INS_OVMX            BIT(12)         /* Output versatile matrix */
+#define BLT_INS_DEI             BIT(13)         /* Deinterlace */
+#define BLT_INS_PMASK           BIT(14)         /* Plane mask */
+#define BLT_INS_VC1R            BIT(17)         /* VC1 Range mapping */
+#define BLT_INS_ROTATE          BIT(18)         /* Rotation */
+#define BLT_INS_GRAD            BIT(19)         /* Gradient fill */
+#define BLT_INS_AQLOCK          BIT(29)         /* AQ lock */
+#define BLT_INS_PACE            BIT(30)         /* Pace down */
+#define BLT_INS_IRQ             BIT(31)         /* Raise IRQ when node done */
+#define BLT_CIC_ALL_GRP         0x000FDFFC      /* all valid groups present */
+#define BLT_ACK_BYPASS_S2S3     0x00000007      /* Bypass src2 and src3 */
+
+#define BLT_TTY_COL_SHIFT       16              /* Color format */
+#define BLT_TTY_COL_MASK        0x001F0000      /* Color format mask */
+#define BLT_TTY_ALPHA_R         BIT(21)         /* Alpha range */
+#define BLT_TTY_CR_NOT_CB       BIT(22)         /* CR not Cb */
+#define BLT_TTY_MB              BIT(23)         /* MB frame / field*/
+#define BLT_TTY_HSO             BIT(24)         /* H scan order */
+#define BLT_TTY_VSO             BIT(25)         /* V scan order */
+#define BLT_TTY_DITHER          BIT(26)         /* Dithering */
+#define BLT_TTY_CHROMA          BIT(27)         /* Write chroma / luma */
+#define BLT_TTY_BIG_END         BIT(30)         /* Big endianness */
+
+#define BLT_S1TY_A1_SUBSET      BIT(22)         /* A1 subset */
+#define BLT_S1TY_CHROMA_EXT     BIT(26)         /* Chroma Extended */
+#define BTL_S1TY_SUBBYTE        BIT(28)         /* Sub-byte fmt, pixel order */
+#define BLT_S1TY_RGB_EXP        BIT(29)         /* RGB expansion mode */
+
+#define BLT_S2TY_A1_SUBSET      BIT(22)         /* A1 subset */
+#define BLT_S2TY_CHROMA_EXT     BIT(26)         /* Chroma Extended */
+#define BTL_S2TY_SUBBYTE        BIT(28)         /* Sub-byte fmt, pixel order */
+#define BLT_S2TY_RGB_EXP        BIT(29)         /* RGB expansion mode */
+
+#define BLT_S3TY_BLANK_ACC      BIT(26)         /* Blank access */
+
+#define BLT_FCTL_HV_SCALE       0x00000055      /* H/V resize + color filter */
+#define BLT_FCTL_Y_HV_SCALE     0x33000000      /* Luma version */
+
+#define BLT_FCTL_HV_SAMPLE      0x00000044      /* H/V resize */
+#define BLT_FCTL_Y_HV_SAMPLE    0x22000000      /* Luma version */
+
+#define BLT_RZI_DEFAULT         0x20003000      /* H/VNB_repeat = 3/2 */
+
+/* Color format */
+#define BDISP_RGB565            0x00            /* RGB565 */
+#define BDISP_RGB888            0x01            /* RGB888 */
+#define BDISP_XRGB8888          0x02            /* RGB888_32 */
+#define BDISP_ARGB8888          0x05            /* ARGB888 */
+#define BDISP_NV12              0x16            /* YCbCr42x R2B */
+#define BDISP_YUV_3B            0x1E            /* YUV (3 buffer) */
diff --git a/drivers/media/platform/sti/bdisp/bdisp-v4l2.c b/drivers/media/platform/sti/bdisp/bdisp-v4l2.c
new file mode 100644
index 0000000..a0d267e
--- /dev/null
+++ b/drivers/media/platform/sti/bdisp/bdisp-v4l2.c
@@ -0,0 +1,1444 @@
+/*
+ * Copyright (C) STMicroelectronics SA 2014
+ * Authors: Fabien Dessenne <fabien.dessenne@st.com> for STMicroelectronics.
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+#include <linux/errno.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+
+#include <media/v4l2-event.h>
+#include <media/v4l2-ioctl.h>
+
+#include "bdisp.h"
+
+#define BDISP_MAX_CTRL_NUM      10
+
+#define BDISP_WORK_TIMEOUT      ((100 * HZ) / 1000)
+
+/* User configuration change */
+#define BDISP_PARAMS            BIT(0) /* Config updated */
+#define BDISP_SRC_FMT           BIT(1) /* Source set */
+#define BDISP_DST_FMT           BIT(2) /* Destination set */
+#define BDISP_CTX_STOP_REQ      BIT(3) /* Stop request */
+#define BDISP_CTX_ABORT         BIT(4) /* Abort while device run */
+
+#define BDISP_MIN_W             1
+#define BDISP_MAX_W             8191
+#define BDISP_MIN_H             1
+#define BDISP_MAX_H             8191
+
+#define fh_to_ctx(__fh) container_of(__fh, struct bdisp_ctx, fh)
+
+enum bdisp_dev_flags {
+	ST_M2M_OPEN,            /* Driver opened */
+	ST_M2M_RUNNING,         /* HW device running */
+	ST_M2M_SUSPENDED,       /* Driver suspended */
+	ST_M2M_SUSPENDING,      /* Driver being suspended */
+};
+
+static const struct bdisp_fmt bdisp_formats[] = {
+	/* ARGB888. [31:0] A:R:G:B 8:8:8:8 little endian */
+	{
+		.pixelformat    = V4L2_PIX_FMT_ABGR32, /* is actually ARGB */
+		.nb_planes      = 1,
+		.bpp            = 32,
+		.bpp_plane0     = 32,
+		.w_align        = 1,
+		.h_align        = 1
+	},
+	/* XRGB888. [31:0] x:R:G:B 8:8:8:8 little endian */
+	{
+		.pixelformat    = V4L2_PIX_FMT_XBGR32, /* is actually xRGB */
+		.nb_planes      = 1,
+		.bpp            = 32,
+		.bpp_plane0     = 32,
+		.w_align        = 1,
+		.h_align        = 1
+	},
+	/* RGB565. [15:0] R:G:B 5:6:5 little endian */
+	{
+		.pixelformat    = V4L2_PIX_FMT_RGB565,
+		.nb_planes      = 1,
+		.bpp            = 16,
+		.bpp_plane0     = 16,
+		.w_align        = 1,
+		.h_align        = 1
+	},
+	/* NV12. YUV420SP - 1 plane for Y + 1 plane for (CbCr) */
+	{
+		.pixelformat    = V4L2_PIX_FMT_NV12,
+		.nb_planes      = 2,
+		.bpp            = 12,
+		.bpp_plane0     = 8,
+		.w_align        = 2,
+		.h_align        = 2
+	},
+	/* RGB888. [23:0] B:G:R 8:8:8 little endian */
+	{
+		.pixelformat    = V4L2_PIX_FMT_RGB24,
+		.nb_planes      = 1,
+		.bpp            = 24,
+		.bpp_plane0     = 24,
+		.w_align        = 1,
+		.h_align        = 1
+	},
+	/* YU12. YUV420P - 1 plane for Y + 1 plane for Cb + 1 plane for Cr
+	 * To keep as the LAST element of this table (no support on capture)
+	 */
+	{
+		.pixelformat    = V4L2_PIX_FMT_YUV420,
+		.nb_planes      = 3,
+		.bpp            = 12,
+		.bpp_plane0     = 8,
+		.w_align        = 2,
+		.h_align        = 2
+	}
+};
+
+/* Default format : HD ARGB32*/
+#define BDISP_DEF_WIDTH         1920
+#define BDISP_DEF_HEIGHT        1080
+
+static const struct bdisp_frame bdisp_dflt_fmt = {
+		.width          = BDISP_DEF_WIDTH,
+		.height         = BDISP_DEF_HEIGHT,
+		.fmt            = &bdisp_formats[0],
+		.field          = V4L2_FIELD_NONE,
+		.bytesperline   = BDISP_DEF_WIDTH * 4,
+		.sizeimage      = BDISP_DEF_WIDTH * BDISP_DEF_HEIGHT * 4,
+		.colorspace     = V4L2_COLORSPACE_REC709,
+		.crop           = {0, 0, BDISP_DEF_WIDTH, BDISP_DEF_HEIGHT},
+		.paddr          = {0, 0, 0, 0}
+};
+
+static inline void bdisp_ctx_state_lock_set(u32 state, struct bdisp_ctx *ctx)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&ctx->bdisp_dev->slock, flags);
+	ctx->state |= state;
+	spin_unlock_irqrestore(&ctx->bdisp_dev->slock, flags);
+}
+
+static inline void bdisp_ctx_state_lock_clear(u32 state, struct bdisp_ctx *ctx)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&ctx->bdisp_dev->slock, flags);
+	ctx->state &= ~state;
+	spin_unlock_irqrestore(&ctx->bdisp_dev->slock, flags);
+}
+
+static inline bool bdisp_ctx_state_is_set(u32 mask, struct bdisp_ctx *ctx)
+{
+	unsigned long flags;
+	bool ret;
+
+	spin_lock_irqsave(&ctx->bdisp_dev->slock, flags);
+	ret = (ctx->state & mask) == mask;
+	spin_unlock_irqrestore(&ctx->bdisp_dev->slock, flags);
+
+	return ret;
+}
+
+static const struct bdisp_fmt *bdisp_find_fmt(u32 pixelformat)
+{
+	const struct bdisp_fmt *fmt;
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(bdisp_formats); i++) {
+		fmt = &bdisp_formats[i];
+		if (fmt->pixelformat == pixelformat)
+			return fmt;
+	}
+
+	return NULL;
+}
+
+static struct bdisp_frame *ctx_get_frame(struct bdisp_ctx *ctx,
+					 enum v4l2_buf_type type)
+{
+	switch (type) {
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+		return &ctx->src;
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		return &ctx->dst;
+	default:
+		dev_err(ctx->bdisp_dev->dev,
+			"Wrong buffer/video queue type (%d)\n", type);
+		break;
+	}
+
+	return ERR_PTR(-EINVAL);
+}
+
+static void bdisp_job_finish(struct bdisp_ctx *ctx, int vb_state)
+{
+	struct vb2_v4l2_buffer *src_vb, *dst_vb;
+
+	if (WARN(!ctx || !ctx->fh.m2m_ctx, "Null hardware context\n"))
+		return;
+
+	dev_dbg(ctx->bdisp_dev->dev, "%s\n", __func__);
+
+	src_vb = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+	dst_vb = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
+
+	if (src_vb && dst_vb) {
+		dst_vb->timestamp = src_vb->timestamp;
+		dst_vb->timecode = src_vb->timecode;
+		dst_vb->flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+		dst_vb->flags |= src_vb->flags &
+					  V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+
+		v4l2_m2m_buf_done(src_vb, vb_state);
+		v4l2_m2m_buf_done(dst_vb, vb_state);
+
+		v4l2_m2m_job_finish(ctx->bdisp_dev->m2m.m2m_dev,
+				    ctx->fh.m2m_ctx);
+	}
+}
+
+static int bdisp_ctx_stop_req(struct bdisp_ctx *ctx)
+{
+	struct bdisp_ctx *curr_ctx;
+	struct bdisp_dev *bdisp = ctx->bdisp_dev;
+	int ret;
+
+	dev_dbg(ctx->bdisp_dev->dev, "%s\n", __func__);
+
+	cancel_delayed_work(&bdisp->timeout_work);
+
+	curr_ctx = v4l2_m2m_get_curr_priv(bdisp->m2m.m2m_dev);
+	if (!test_bit(ST_M2M_RUNNING, &bdisp->state) || (curr_ctx != ctx))
+		return 0;
+
+	bdisp_ctx_state_lock_set(BDISP_CTX_STOP_REQ, ctx);
+
+	ret = wait_event_timeout(bdisp->irq_queue,
+			!bdisp_ctx_state_is_set(BDISP_CTX_STOP_REQ, ctx),
+			BDISP_WORK_TIMEOUT);
+
+	if (!ret) {
+		dev_err(ctx->bdisp_dev->dev, "%s IRQ timeout\n", __func__);
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static void __bdisp_job_abort(struct bdisp_ctx *ctx)
+{
+	int ret;
+
+	ret = bdisp_ctx_stop_req(ctx);
+	if ((ret == -ETIMEDOUT) || (ctx->state & BDISP_CTX_ABORT)) {
+		bdisp_ctx_state_lock_clear(BDISP_CTX_STOP_REQ | BDISP_CTX_ABORT,
+					   ctx);
+		bdisp_job_finish(ctx, VB2_BUF_STATE_ERROR);
+	}
+}
+
+static void bdisp_job_abort(void *priv)
+{
+	__bdisp_job_abort((struct bdisp_ctx *)priv);
+}
+
+static int bdisp_get_addr(struct bdisp_ctx *ctx, struct vb2_buffer *vb,
+			  struct bdisp_frame *frame, dma_addr_t *paddr)
+{
+	if (!vb || !frame)
+		return -EINVAL;
+
+	paddr[0] = vb2_dma_contig_plane_dma_addr(vb, 0);
+
+	if (frame->fmt->nb_planes > 1)
+		/* UV (NV12) or U (420P) */
+		paddr[1] = (dma_addr_t)(paddr[0] +
+				frame->bytesperline * frame->height);
+
+	if (frame->fmt->nb_planes > 2)
+		/* V (420P) */
+		paddr[2] = (dma_addr_t)(paddr[1] +
+				(frame->bytesperline * frame->height) / 4);
+
+	if (frame->fmt->nb_planes > 3)
+		dev_dbg(ctx->bdisp_dev->dev, "ignoring some planes\n");
+
+	dev_dbg(ctx->bdisp_dev->dev,
+		"%s plane[0]=%pad plane[1]=%pad plane[2]=%pad\n",
+		__func__, &paddr[0], &paddr[1], &paddr[2]);
+
+	return 0;
+}
+
+static int bdisp_get_bufs(struct bdisp_ctx *ctx)
+{
+	struct bdisp_frame *src, *dst;
+	struct vb2_v4l2_buffer *src_vb, *dst_vb;
+	int ret;
+
+	src = &ctx->src;
+	dst = &ctx->dst;
+
+	src_vb = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+	ret = bdisp_get_addr(ctx, &src_vb->vb2_buf, src, src->paddr);
+	if (ret)
+		return ret;
+
+	dst_vb = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
+	ret = bdisp_get_addr(ctx, &dst_vb->vb2_buf, dst, dst->paddr);
+	if (ret)
+		return ret;
+
+	dst_vb->timestamp = src_vb->timestamp;
+
+	return 0;
+}
+
+static void bdisp_device_run(void *priv)
+{
+	struct bdisp_ctx *ctx = priv;
+	struct bdisp_dev *bdisp;
+	unsigned long flags;
+	int err = 0;
+
+	if (WARN(!ctx, "Null hardware context\n"))
+		return;
+
+	bdisp = ctx->bdisp_dev;
+	dev_dbg(bdisp->dev, "%s\n", __func__);
+	spin_lock_irqsave(&bdisp->slock, flags);
+
+	if (bdisp->m2m.ctx != ctx) {
+		dev_dbg(bdisp->dev, "ctx updated: %p -> %p\n",
+			bdisp->m2m.ctx, ctx);
+		ctx->state |= BDISP_PARAMS;
+		bdisp->m2m.ctx = ctx;
+	}
+
+	if (ctx->state & BDISP_CTX_STOP_REQ) {
+		ctx->state &= ~BDISP_CTX_STOP_REQ;
+		ctx->state |= BDISP_CTX_ABORT;
+		wake_up(&bdisp->irq_queue);
+		goto out;
+	}
+
+	err = bdisp_get_bufs(ctx);
+	if (err) {
+		dev_err(bdisp->dev, "cannot get address\n");
+		goto out;
+	}
+
+	bdisp_dbg_perf_begin(bdisp);
+
+	err = bdisp_hw_reset(bdisp);
+	if (err) {
+		dev_err(bdisp->dev, "could not get HW ready\n");
+		goto out;
+	}
+
+	err = bdisp_hw_update(ctx);
+	if (err) {
+		dev_err(bdisp->dev, "could not send HW request\n");
+		goto out;
+	}
+
+	queue_delayed_work(bdisp->work_queue, &bdisp->timeout_work,
+			   BDISP_WORK_TIMEOUT);
+	set_bit(ST_M2M_RUNNING, &bdisp->state);
+out:
+	ctx->state &= ~BDISP_PARAMS;
+	spin_unlock_irqrestore(&bdisp->slock, flags);
+	if (err)
+		bdisp_job_finish(ctx, VB2_BUF_STATE_ERROR);
+}
+
+static struct v4l2_m2m_ops bdisp_m2m_ops = {
+	.device_run     = bdisp_device_run,
+	.job_abort      = bdisp_job_abort,
+};
+
+static int __bdisp_s_ctrl(struct bdisp_ctx *ctx, struct v4l2_ctrl *ctrl)
+{
+	if (ctrl->flags & V4L2_CTRL_FLAG_INACTIVE)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_HFLIP:
+		ctx->hflip = ctrl->val;
+		break;
+	case V4L2_CID_VFLIP:
+		ctx->vflip = ctrl->val;
+		break;
+	default:
+		dev_err(ctx->bdisp_dev->dev, "unknown control %d\n", ctrl->id);
+		return -EINVAL;
+	}
+
+	ctx->state |= BDISP_PARAMS;
+
+	return 0;
+}
+
+static int bdisp_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct bdisp_ctx *ctx = container_of(ctrl->handler, struct bdisp_ctx,
+						ctrl_handler);
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&ctx->bdisp_dev->slock, flags);
+	ret = __bdisp_s_ctrl(ctx, ctrl);
+	spin_unlock_irqrestore(&ctx->bdisp_dev->slock, flags);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops bdisp_c_ops = {
+	.s_ctrl = bdisp_s_ctrl,
+};
+
+static int bdisp_ctrls_create(struct bdisp_ctx *ctx)
+{
+	if (ctx->ctrls_rdy)
+		return 0;
+
+	v4l2_ctrl_handler_init(&ctx->ctrl_handler, BDISP_MAX_CTRL_NUM);
+
+	ctx->bdisp_ctrls.hflip = v4l2_ctrl_new_std(&ctx->ctrl_handler,
+				&bdisp_c_ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
+	ctx->bdisp_ctrls.vflip = v4l2_ctrl_new_std(&ctx->ctrl_handler,
+				&bdisp_c_ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
+
+	if (ctx->ctrl_handler.error) {
+		int err = ctx->ctrl_handler.error;
+
+		v4l2_ctrl_handler_free(&ctx->ctrl_handler);
+		return err;
+	}
+
+	ctx->ctrls_rdy = true;
+
+	return 0;
+}
+
+static void bdisp_ctrls_delete(struct bdisp_ctx *ctx)
+{
+	if (ctx->ctrls_rdy) {
+		v4l2_ctrl_handler_free(&ctx->ctrl_handler);
+		ctx->ctrls_rdy = false;
+	}
+}
+
+static int bdisp_queue_setup(struct vb2_queue *vq,
+			     const void *parg,
+			     unsigned int *nb_buf, unsigned int *nb_planes,
+			     unsigned int sizes[], void *allocators[])
+{
+	const struct v4l2_format *fmt = parg;
+	struct bdisp_ctx *ctx = vb2_get_drv_priv(vq);
+	struct bdisp_frame *frame = ctx_get_frame(ctx, vq->type);
+
+	if (IS_ERR(frame)) {
+		dev_err(ctx->bdisp_dev->dev, "Invalid frame (%p)\n", frame);
+		return PTR_ERR(frame);
+	}
+
+	if (!frame->fmt) {
+		dev_err(ctx->bdisp_dev->dev, "Invalid format\n");
+		return -EINVAL;
+	}
+
+	if (fmt && fmt->fmt.pix.sizeimage < frame->sizeimage)
+		return -EINVAL;
+
+	*nb_planes = 1;
+	sizes[0] = fmt ? fmt->fmt.pix.sizeimage : frame->sizeimage;
+	allocators[0] = ctx->bdisp_dev->alloc_ctx;
+
+	return 0;
+}
+
+static int bdisp_buf_prepare(struct vb2_buffer *vb)
+{
+	struct bdisp_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct bdisp_frame *frame = ctx_get_frame(ctx, vb->vb2_queue->type);
+
+	if (IS_ERR(frame)) {
+		dev_err(ctx->bdisp_dev->dev, "Invalid frame (%p)\n", frame);
+		return PTR_ERR(frame);
+	}
+
+	if (vb->vb2_queue->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		vb2_set_plane_payload(vb, 0, frame->sizeimage);
+
+	return 0;
+}
+
+static void bdisp_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct bdisp_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+
+	/* return to V4L2 any 0-size buffer so it can be dequeued by user */
+	if (!vb2_get_plane_payload(vb, 0)) {
+		dev_dbg(ctx->bdisp_dev->dev, "0 data buffer, skip it\n");
+		vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
+		return;
+	}
+
+	if (ctx->fh.m2m_ctx)
+		v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
+}
+
+static int bdisp_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct bdisp_ctx *ctx = q->drv_priv;
+	struct vb2_v4l2_buffer *buf;
+	int ret = pm_runtime_get_sync(ctx->bdisp_dev->dev);
+
+	if (ret < 0) {
+		dev_err(ctx->bdisp_dev->dev, "failed to set runtime PM\n");
+
+		if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+			while ((buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx)))
+				v4l2_m2m_buf_done(buf, VB2_BUF_STATE_QUEUED);
+		} else {
+			while ((buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx)))
+				v4l2_m2m_buf_done(buf, VB2_BUF_STATE_QUEUED);
+		}
+
+		return ret;
+	}
+
+	return 0;
+}
+
+static void bdisp_stop_streaming(struct vb2_queue *q)
+{
+	struct bdisp_ctx *ctx = q->drv_priv;
+
+	__bdisp_job_abort(ctx);
+
+	pm_runtime_put(ctx->bdisp_dev->dev);
+}
+
+static struct vb2_ops bdisp_qops = {
+	.queue_setup     = bdisp_queue_setup,
+	.buf_prepare     = bdisp_buf_prepare,
+	.buf_queue       = bdisp_buf_queue,
+	.wait_prepare    = vb2_ops_wait_prepare,
+	.wait_finish     = vb2_ops_wait_finish,
+	.stop_streaming  = bdisp_stop_streaming,
+	.start_streaming = bdisp_start_streaming,
+};
+
+static int queue_init(void *priv,
+		      struct vb2_queue *src_vq, struct vb2_queue *dst_vq)
+{
+	struct bdisp_ctx *ctx = priv;
+	int ret;
+
+	memset(src_vq, 0, sizeof(*src_vq));
+	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	src_vq->io_modes = VB2_MMAP | VB2_DMABUF;
+	src_vq->drv_priv = ctx;
+	src_vq->ops = &bdisp_qops;
+	src_vq->mem_ops = &vb2_dma_contig_memops;
+	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	src_vq->lock = &ctx->bdisp_dev->lock;
+
+	ret = vb2_queue_init(src_vq);
+	if (ret)
+		return ret;
+
+	memset(dst_vq, 0, sizeof(*dst_vq));
+	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	dst_vq->io_modes = VB2_MMAP | VB2_DMABUF;
+	dst_vq->drv_priv = ctx;
+	dst_vq->ops = &bdisp_qops;
+	dst_vq->mem_ops = &vb2_dma_contig_memops;
+	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	dst_vq->lock = &ctx->bdisp_dev->lock;
+
+	return vb2_queue_init(dst_vq);
+}
+
+static int bdisp_open(struct file *file)
+{
+	struct bdisp_dev *bdisp = video_drvdata(file);
+	struct bdisp_ctx *ctx = NULL;
+	int ret;
+
+	if (mutex_lock_interruptible(&bdisp->lock))
+		return -ERESTARTSYS;
+
+	/* Allocate memory for both context and node */
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx) {
+		ret = -ENOMEM;
+		goto unlock;
+	}
+	ctx->bdisp_dev = bdisp;
+
+	if (bdisp_hw_alloc_nodes(ctx)) {
+		dev_err(bdisp->dev, "no memory for nodes\n");
+		ret = -ENOMEM;
+		goto mem_ctx;
+	}
+
+	v4l2_fh_init(&ctx->fh, bdisp->m2m.vdev);
+
+	ret = bdisp_ctrls_create(ctx);
+	if (ret) {
+		dev_err(bdisp->dev, "Failed to create control\n");
+		goto error_fh;
+	}
+
+	/* Use separate control handler per file handle */
+	ctx->fh.ctrl_handler = &ctx->ctrl_handler;
+	file->private_data = &ctx->fh;
+	v4l2_fh_add(&ctx->fh);
+
+	/* Default format */
+	ctx->src = bdisp_dflt_fmt;
+	ctx->dst = bdisp_dflt_fmt;
+
+	/* Setup the device context for mem2mem mode. */
+	ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(bdisp->m2m.m2m_dev, ctx,
+					    queue_init);
+	if (IS_ERR(ctx->fh.m2m_ctx)) {
+		dev_err(bdisp->dev, "Failed to initialize m2m context\n");
+		ret = PTR_ERR(ctx->fh.m2m_ctx);
+		goto error_ctrls;
+	}
+
+	bdisp->m2m.refcnt++;
+	set_bit(ST_M2M_OPEN, &bdisp->state);
+
+	dev_dbg(bdisp->dev, "driver opened, ctx = 0x%p\n", ctx);
+
+	mutex_unlock(&bdisp->lock);
+
+	return 0;
+
+error_ctrls:
+	bdisp_ctrls_delete(ctx);
+error_fh:
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	bdisp_hw_free_nodes(ctx);
+mem_ctx:
+	kfree(ctx);
+unlock:
+	mutex_unlock(&bdisp->lock);
+
+	return ret;
+}
+
+static int bdisp_release(struct file *file)
+{
+	struct bdisp_ctx *ctx = fh_to_ctx(file->private_data);
+	struct bdisp_dev *bdisp = ctx->bdisp_dev;
+
+	dev_dbg(bdisp->dev, "%s\n", __func__);
+
+	if (mutex_lock_interruptible(&bdisp->lock))
+		return -ERESTARTSYS;
+
+	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
+
+	bdisp_ctrls_delete(ctx);
+
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+
+	if (--bdisp->m2m.refcnt <= 0)
+		clear_bit(ST_M2M_OPEN, &bdisp->state);
+
+	bdisp_hw_free_nodes(ctx);
+
+	kfree(ctx);
+
+	mutex_unlock(&bdisp->lock);
+
+	return 0;
+}
+
+static const struct v4l2_file_operations bdisp_fops = {
+	.owner          = THIS_MODULE,
+	.open           = bdisp_open,
+	.release        = bdisp_release,
+	.poll           = v4l2_m2m_fop_poll,
+	.unlocked_ioctl = video_ioctl2,
+	.mmap           = v4l2_m2m_fop_mmap,
+};
+
+static int bdisp_querycap(struct file *file, void *fh,
+			  struct v4l2_capability *cap)
+{
+	struct bdisp_ctx *ctx = fh_to_ctx(fh);
+	struct bdisp_dev *bdisp = ctx->bdisp_dev;
+
+	strlcpy(cap->driver, bdisp->pdev->name, sizeof(cap->driver));
+	strlcpy(cap->card, bdisp->pdev->name, sizeof(cap->card));
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s%d",
+		 BDISP_NAME, bdisp->id);
+
+	cap->device_caps = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_M2M;
+
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+
+	return 0;
+}
+
+static int bdisp_enum_fmt(struct file *file, void *fh, struct v4l2_fmtdesc *f)
+{
+	struct bdisp_ctx *ctx = fh_to_ctx(fh);
+	const struct bdisp_fmt *fmt;
+
+	if (f->index >= ARRAY_SIZE(bdisp_formats))
+		return -EINVAL;
+
+	fmt = &bdisp_formats[f->index];
+
+	if ((fmt->pixelformat == V4L2_PIX_FMT_YUV420) &&
+	    (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)) {
+		dev_dbg(ctx->bdisp_dev->dev, "No YU12 on capture\n");
+		return -EINVAL;
+	}
+	f->pixelformat = fmt->pixelformat;
+
+	return 0;
+}
+
+static int bdisp_g_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct bdisp_ctx *ctx = fh_to_ctx(fh);
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct bdisp_frame *frame  = ctx_get_frame(ctx, f->type);
+
+	if (IS_ERR(frame)) {
+		dev_err(ctx->bdisp_dev->dev, "Invalid frame (%p)\n", frame);
+		return PTR_ERR(frame);
+	}
+
+	pix = &f->fmt.pix;
+	pix->width = frame->width;
+	pix->height = frame->height;
+	pix->pixelformat = frame->fmt->pixelformat;
+	pix->field = frame->field;
+	pix->bytesperline = frame->bytesperline;
+	pix->sizeimage = frame->sizeimage;
+	pix->colorspace = (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) ?
+				frame->colorspace : bdisp_dflt_fmt.colorspace;
+
+	return 0;
+}
+
+static int bdisp_try_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct bdisp_ctx *ctx = fh_to_ctx(fh);
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	const struct bdisp_fmt *format;
+	u32 in_w, in_h;
+
+	format = bdisp_find_fmt(pix->pixelformat);
+	if (!format) {
+		dev_dbg(ctx->bdisp_dev->dev, "Unknown format 0x%x\n",
+			pix->pixelformat);
+		return -EINVAL;
+	}
+
+	/* YUV420P only supported for VIDEO_OUTPUT */
+	if ((format->pixelformat == V4L2_PIX_FMT_YUV420) &&
+	    (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)) {
+		dev_dbg(ctx->bdisp_dev->dev, "No YU12 on capture\n");
+		return -EINVAL;
+	}
+
+	/* Field (interlaced only supported on OUTPUT) */
+	if ((f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) ||
+	    (pix->field != V4L2_FIELD_INTERLACED))
+		pix->field = V4L2_FIELD_NONE;
+
+	/* Adjust width & height */
+	in_w = pix->width;
+	in_h = pix->height;
+	v4l_bound_align_image(&pix->width,
+			      BDISP_MIN_W, BDISP_MAX_W,
+			      ffs(format->w_align) - 1,
+			      &pix->height,
+			      BDISP_MIN_H, BDISP_MAX_H,
+			      ffs(format->h_align) - 1,
+			      0);
+	if ((pix->width != in_w) || (pix->height != in_h))
+		dev_dbg(ctx->bdisp_dev->dev,
+			"%s size updated: %dx%d -> %dx%d\n", __func__,
+			in_w, in_h, pix->width, pix->height);
+
+	pix->bytesperline = (pix->width * format->bpp_plane0) / 8;
+	pix->sizeimage = (pix->width * pix->height * format->bpp) / 8;
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		pix->colorspace = bdisp_dflt_fmt.colorspace;
+
+	return 0;
+}
+
+static int bdisp_s_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct bdisp_ctx *ctx = fh_to_ctx(fh);
+	struct vb2_queue *vq;
+	struct bdisp_frame *frame;
+	struct v4l2_pix_format *pix;
+	int ret;
+	u32 state;
+
+	ret = bdisp_try_fmt(file, fh, f);
+	if (ret) {
+		dev_err(ctx->bdisp_dev->dev, "Cannot set format\n");
+		return ret;
+	}
+
+	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
+	if (vb2_is_streaming(vq)) {
+		dev_err(ctx->bdisp_dev->dev, "queue (%d) busy\n", f->type);
+		return -EBUSY;
+	}
+
+	frame = (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) ?
+			&ctx->src : &ctx->dst;
+	pix = &f->fmt.pix;
+	frame->fmt = bdisp_find_fmt(pix->pixelformat);
+	if (!frame->fmt) {
+		dev_err(ctx->bdisp_dev->dev, "Unknown format 0x%x\n",
+			pix->pixelformat);
+		return -EINVAL;
+	}
+
+	frame->width = pix->width;
+	frame->height = pix->height;
+	frame->bytesperline = pix->bytesperline;
+	frame->sizeimage = pix->sizeimage;
+	frame->field = pix->field;
+	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		frame->colorspace = pix->colorspace;
+
+	frame->crop.width = frame->width;
+	frame->crop.height = frame->height;
+	frame->crop.left = 0;
+	frame->crop.top = 0;
+
+	state = BDISP_PARAMS;
+	state |= (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) ?
+			BDISP_DST_FMT : BDISP_SRC_FMT;
+	bdisp_ctx_state_lock_set(state, ctx);
+
+	return 0;
+}
+
+static int bdisp_g_selection(struct file *file, void *fh,
+			     struct v4l2_selection *s)
+{
+	struct bdisp_frame *frame;
+	struct bdisp_ctx *ctx = fh_to_ctx(fh);
+
+	frame = ctx_get_frame(ctx, s->type);
+	if (IS_ERR(frame)) {
+		dev_err(ctx->bdisp_dev->dev, "Invalid frame (%p)\n", frame);
+		return PTR_ERR(frame);
+	}
+
+	switch (s->type) {
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+		switch (s->target) {
+		case V4L2_SEL_TGT_CROP:
+			/* cropped frame */
+			s->r = frame->crop;
+			break;
+		case V4L2_SEL_TGT_CROP_DEFAULT:
+		case V4L2_SEL_TGT_CROP_BOUNDS:
+			/* complete frame */
+			s->r.left = 0;
+			s->r.top = 0;
+			s->r.width = frame->width;
+			s->r.height = frame->height;
+			break;
+		default:
+			dev_err(ctx->bdisp_dev->dev, "Invalid target\n");
+			return -EINVAL;
+		}
+		break;
+
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		switch (s->target) {
+		case V4L2_SEL_TGT_COMPOSE:
+		case V4L2_SEL_TGT_COMPOSE_PADDED:
+			/* composed (cropped) frame */
+			s->r = frame->crop;
+			break;
+		case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+		case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+			/* complete frame */
+			s->r.left = 0;
+			s->r.top = 0;
+			s->r.width = frame->width;
+			s->r.height = frame->height;
+			break;
+		default:
+			dev_err(ctx->bdisp_dev->dev, "Invalid target\n");
+			return -EINVAL;
+		}
+		break;
+
+	default:
+		dev_err(ctx->bdisp_dev->dev, "Invalid type\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int is_rect_enclosed(struct v4l2_rect *a, struct v4l2_rect *b)
+{
+	/* Return 1 if a is enclosed in b, or 0 otherwise. */
+
+	if (a->left < b->left || a->top < b->top)
+		return 0;
+
+	if (a->left + a->width > b->left + b->width)
+		return 0;
+
+	if (a->top + a->height > b->top + b->height)
+		return 0;
+
+	return 1;
+}
+
+static int bdisp_s_selection(struct file *file, void *fh,
+			     struct v4l2_selection *s)
+{
+	struct bdisp_frame *frame;
+	struct bdisp_ctx *ctx = fh_to_ctx(fh);
+	struct v4l2_rect *in, out;
+	bool valid = false;
+
+	if ((s->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) &&
+	    (s->target == V4L2_SEL_TGT_CROP))
+		valid = true;
+
+	if ((s->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) &&
+	    (s->target == V4L2_SEL_TGT_COMPOSE))
+		valid = true;
+
+	if (!valid) {
+		dev_err(ctx->bdisp_dev->dev, "Invalid type / target\n");
+		return -EINVAL;
+	}
+
+	frame = ctx_get_frame(ctx, s->type);
+	if (IS_ERR(frame)) {
+		dev_err(ctx->bdisp_dev->dev, "Invalid frame (%p)\n", frame);
+		return PTR_ERR(frame);
+	}
+
+	in = &s->r;
+	out = *in;
+
+	/* Align and check origin */
+	out.left = ALIGN(in->left, frame->fmt->w_align);
+	out.top = ALIGN(in->top, frame->fmt->h_align);
+
+	if ((out.left < 0) || (out.left >= frame->width) ||
+	    (out.top < 0) || (out.top >= frame->height)) {
+		dev_err(ctx->bdisp_dev->dev,
+			"Invalid crop: %dx%d@(%d,%d) vs frame: %dx%d\n",
+			out.width, out.height, out.left, out.top,
+			frame->width, frame->height);
+		return -EINVAL;
+	}
+
+	/* Align and check size */
+	out.width = ALIGN(in->width, frame->fmt->w_align);
+	out.height = ALIGN(in->height, frame->fmt->w_align);
+
+	if (((out.left + out.width) > frame->width) ||
+	    ((out.top + out.height) > frame->height)) {
+		dev_err(ctx->bdisp_dev->dev,
+			"Invalid crop: %dx%d@(%d,%d) vs frame: %dx%d\n",
+			out.width, out.height, out.left, out.top,
+			frame->width, frame->height);
+		return -EINVAL;
+	}
+
+	/* Checks adjust constraints flags */
+	if (s->flags & V4L2_SEL_FLAG_LE && !is_rect_enclosed(&out, in))
+		return -ERANGE;
+
+	if (s->flags & V4L2_SEL_FLAG_GE && !is_rect_enclosed(in, &out))
+		return -ERANGE;
+
+	if ((out.left != in->left) || (out.top != in->top) ||
+	    (out.width != in->width) || (out.height != in->height)) {
+		dev_dbg(ctx->bdisp_dev->dev,
+			"%s crop updated: %dx%d@(%d,%d) -> %dx%d@(%d,%d)\n",
+			__func__, in->width, in->height, in->left, in->top,
+			out.width, out.height, out.left, out.top);
+		*in = out;
+	}
+
+	frame->crop = out;
+
+	bdisp_ctx_state_lock_set(BDISP_PARAMS, ctx);
+
+	return 0;
+}
+
+static int bdisp_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
+{
+	struct bdisp_ctx *ctx = fh_to_ctx(fh);
+
+	if ((type == V4L2_BUF_TYPE_VIDEO_OUTPUT) &&
+	    !bdisp_ctx_state_is_set(BDISP_SRC_FMT, ctx)) {
+		dev_err(ctx->bdisp_dev->dev, "src not defined\n");
+		return -EINVAL;
+	}
+
+	if ((type == V4L2_BUF_TYPE_VIDEO_CAPTURE) &&
+	    !bdisp_ctx_state_is_set(BDISP_DST_FMT, ctx)) {
+		dev_err(ctx->bdisp_dev->dev, "dst not defined\n");
+		return -EINVAL;
+	}
+
+	return v4l2_m2m_streamon(file, ctx->fh.m2m_ctx, type);
+}
+
+static const struct v4l2_ioctl_ops bdisp_ioctl_ops = {
+	.vidioc_querycap                = bdisp_querycap,
+	.vidioc_enum_fmt_vid_cap        = bdisp_enum_fmt,
+	.vidioc_enum_fmt_vid_out        = bdisp_enum_fmt,
+	.vidioc_g_fmt_vid_cap           = bdisp_g_fmt,
+	.vidioc_g_fmt_vid_out           = bdisp_g_fmt,
+	.vidioc_try_fmt_vid_cap         = bdisp_try_fmt,
+	.vidioc_try_fmt_vid_out         = bdisp_try_fmt,
+	.vidioc_s_fmt_vid_cap           = bdisp_s_fmt,
+	.vidioc_s_fmt_vid_out           = bdisp_s_fmt,
+	.vidioc_g_selection		= bdisp_g_selection,
+	.vidioc_s_selection		= bdisp_s_selection,
+	.vidioc_reqbufs                 = v4l2_m2m_ioctl_reqbufs,
+	.vidioc_create_bufs             = v4l2_m2m_ioctl_create_bufs,
+	.vidioc_expbuf                  = v4l2_m2m_ioctl_expbuf,
+	.vidioc_querybuf                = v4l2_m2m_ioctl_querybuf,
+	.vidioc_qbuf                    = v4l2_m2m_ioctl_qbuf,
+	.vidioc_dqbuf                   = v4l2_m2m_ioctl_dqbuf,
+	.vidioc_streamon                = bdisp_streamon,
+	.vidioc_streamoff               = v4l2_m2m_ioctl_streamoff,
+	.vidioc_subscribe_event         = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event       = v4l2_event_unsubscribe,
+};
+
+static int bdisp_register_device(struct bdisp_dev *bdisp)
+{
+	int ret;
+
+	if (!bdisp)
+		return -ENODEV;
+
+	bdisp->vdev.fops        = &bdisp_fops;
+	bdisp->vdev.ioctl_ops   = &bdisp_ioctl_ops;
+	bdisp->vdev.release     = video_device_release_empty;
+	bdisp->vdev.lock        = &bdisp->lock;
+	bdisp->vdev.vfl_dir     = VFL_DIR_M2M;
+	bdisp->vdev.v4l2_dev    = &bdisp->v4l2_dev;
+	snprintf(bdisp->vdev.name, sizeof(bdisp->vdev.name), "%s.%d",
+		 BDISP_NAME, bdisp->id);
+
+	video_set_drvdata(&bdisp->vdev, bdisp);
+
+	bdisp->m2m.vdev = &bdisp->vdev;
+	bdisp->m2m.m2m_dev = v4l2_m2m_init(&bdisp_m2m_ops);
+	if (IS_ERR(bdisp->m2m.m2m_dev)) {
+		dev_err(bdisp->dev, "failed to initialize v4l2-m2m device\n");
+		return PTR_ERR(bdisp->m2m.m2m_dev);
+	}
+
+	ret = video_register_device(&bdisp->vdev, VFL_TYPE_GRABBER, -1);
+	if (ret) {
+		dev_err(bdisp->dev,
+			"%s(): failed to register video device\n", __func__);
+		v4l2_m2m_release(bdisp->m2m.m2m_dev);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void bdisp_unregister_device(struct bdisp_dev *bdisp)
+{
+	if (!bdisp)
+		return;
+
+	if (bdisp->m2m.m2m_dev)
+		v4l2_m2m_release(bdisp->m2m.m2m_dev);
+
+	video_unregister_device(bdisp->m2m.vdev);
+}
+
+static irqreturn_t bdisp_irq_thread(int irq, void *priv)
+{
+	struct bdisp_dev *bdisp = priv;
+	struct bdisp_ctx *ctx;
+
+	spin_lock(&bdisp->slock);
+
+	bdisp_dbg_perf_end(bdisp);
+
+	cancel_delayed_work(&bdisp->timeout_work);
+
+	if (!test_and_clear_bit(ST_M2M_RUNNING, &bdisp->state))
+		goto isr_unlock;
+
+	if (test_and_clear_bit(ST_M2M_SUSPENDING, &bdisp->state)) {
+		set_bit(ST_M2M_SUSPENDED, &bdisp->state);
+		wake_up(&bdisp->irq_queue);
+		goto isr_unlock;
+	}
+
+	ctx = v4l2_m2m_get_curr_priv(bdisp->m2m.m2m_dev);
+	if (!ctx || !ctx->fh.m2m_ctx)
+		goto isr_unlock;
+
+	spin_unlock(&bdisp->slock);
+
+	bdisp_job_finish(ctx, VB2_BUF_STATE_DONE);
+
+	if (bdisp_ctx_state_is_set(BDISP_CTX_STOP_REQ, ctx)) {
+		bdisp_ctx_state_lock_clear(BDISP_CTX_STOP_REQ, ctx);
+		wake_up(&bdisp->irq_queue);
+	}
+
+	return IRQ_HANDLED;
+
+isr_unlock:
+	spin_unlock(&bdisp->slock);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t bdisp_irq_handler(int irq, void *priv)
+{
+	if (bdisp_hw_get_and_clear_irq((struct bdisp_dev *)priv))
+		return IRQ_NONE;
+	else
+		return IRQ_WAKE_THREAD;
+}
+
+static void bdisp_irq_timeout(struct work_struct *ptr)
+{
+	struct delayed_work *twork = to_delayed_work(ptr);
+	struct bdisp_dev *bdisp = container_of(twork, struct bdisp_dev,
+			timeout_work);
+	struct bdisp_ctx *ctx;
+
+	ctx = v4l2_m2m_get_curr_priv(bdisp->m2m.m2m_dev);
+
+	dev_err(ctx->bdisp_dev->dev, "Device work timeout\n");
+
+	spin_lock(&bdisp->slock);
+	clear_bit(ST_M2M_RUNNING, &bdisp->state);
+	spin_unlock(&bdisp->slock);
+
+	bdisp_hw_reset(bdisp);
+
+	bdisp_job_finish(ctx, VB2_BUF_STATE_ERROR);
+}
+
+static int bdisp_m2m_suspend(struct bdisp_dev *bdisp)
+{
+	unsigned long flags;
+	int timeout;
+
+	spin_lock_irqsave(&bdisp->slock, flags);
+	if (!test_bit(ST_M2M_RUNNING, &bdisp->state)) {
+		spin_unlock_irqrestore(&bdisp->slock, flags);
+		return 0;
+	}
+	clear_bit(ST_M2M_SUSPENDED, &bdisp->state);
+	set_bit(ST_M2M_SUSPENDING, &bdisp->state);
+	spin_unlock_irqrestore(&bdisp->slock, flags);
+
+	timeout = wait_event_timeout(bdisp->irq_queue,
+				     test_bit(ST_M2M_SUSPENDED, &bdisp->state),
+				     BDISP_WORK_TIMEOUT);
+
+	clear_bit(ST_M2M_SUSPENDING, &bdisp->state);
+
+	if (!timeout) {
+		dev_err(bdisp->dev, "%s IRQ timeout\n", __func__);
+		return -EAGAIN;
+	}
+
+	return 0;
+}
+
+static int bdisp_m2m_resume(struct bdisp_dev *bdisp)
+{
+	struct bdisp_ctx *ctx;
+	unsigned long flags;
+
+	spin_lock_irqsave(&bdisp->slock, flags);
+	ctx = bdisp->m2m.ctx;
+	bdisp->m2m.ctx = NULL;
+	spin_unlock_irqrestore(&bdisp->slock, flags);
+
+	if (test_and_clear_bit(ST_M2M_SUSPENDED, &bdisp->state))
+		bdisp_job_finish(ctx, VB2_BUF_STATE_ERROR);
+
+	return 0;
+}
+
+static int bdisp_runtime_resume(struct device *dev)
+{
+	struct bdisp_dev *bdisp = dev_get_drvdata(dev);
+	int ret = clk_enable(bdisp->clock);
+
+	if (ret)
+		return ret;
+
+	return bdisp_m2m_resume(bdisp);
+}
+
+static int bdisp_runtime_suspend(struct device *dev)
+{
+	struct bdisp_dev *bdisp = dev_get_drvdata(dev);
+	int ret = bdisp_m2m_suspend(bdisp);
+
+	if (!ret)
+		clk_disable(bdisp->clock);
+
+	return ret;
+}
+
+static int bdisp_resume(struct device *dev)
+{
+	struct bdisp_dev *bdisp = dev_get_drvdata(dev);
+	unsigned long flags;
+	int opened;
+
+	spin_lock_irqsave(&bdisp->slock, flags);
+	opened = test_bit(ST_M2M_OPEN, &bdisp->state);
+	spin_unlock_irqrestore(&bdisp->slock, flags);
+
+	if (!opened)
+		return 0;
+
+	if (!pm_runtime_suspended(dev))
+		return bdisp_runtime_resume(dev);
+
+	return 0;
+}
+
+static int bdisp_suspend(struct device *dev)
+{
+	if (!pm_runtime_suspended(dev))
+		return bdisp_runtime_suspend(dev);
+
+	return 0;
+}
+
+static const struct dev_pm_ops bdisp_pm_ops = {
+	.suspend                = bdisp_suspend,
+	.resume                 = bdisp_resume,
+	.runtime_suspend        = bdisp_runtime_suspend,
+	.runtime_resume         = bdisp_runtime_resume,
+};
+
+static int bdisp_remove(struct platform_device *pdev)
+{
+	struct bdisp_dev *bdisp = platform_get_drvdata(pdev);
+
+	bdisp_unregister_device(bdisp);
+
+	bdisp_hw_free_filters(bdisp->dev);
+
+	vb2_dma_contig_cleanup_ctx(bdisp->alloc_ctx);
+
+	pm_runtime_disable(&pdev->dev);
+
+	bdisp_debugfs_remove(bdisp);
+
+	v4l2_device_unregister(&bdisp->v4l2_dev);
+
+	if (!IS_ERR(bdisp->clock))
+		clk_unprepare(bdisp->clock);
+
+	dev_dbg(&pdev->dev, "%s driver unloaded\n", pdev->name);
+
+	return 0;
+}
+
+static int bdisp_probe(struct platform_device *pdev)
+{
+	struct bdisp_dev *bdisp;
+	struct resource *res;
+	struct device *dev = &pdev->dev;
+	int ret;
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	bdisp = devm_kzalloc(dev, sizeof(struct bdisp_dev), GFP_KERNEL);
+	if (!bdisp)
+		return -ENOMEM;
+
+	bdisp->pdev = pdev;
+	bdisp->dev = dev;
+	platform_set_drvdata(pdev, bdisp);
+
+	if (dev->of_node)
+		bdisp->id = of_alias_get_id(pdev->dev.of_node, BDISP_NAME);
+	else
+		bdisp->id = pdev->id;
+
+	init_waitqueue_head(&bdisp->irq_queue);
+	INIT_DELAYED_WORK(&bdisp->timeout_work, bdisp_irq_timeout);
+	bdisp->work_queue = create_workqueue(BDISP_NAME);
+
+	spin_lock_init(&bdisp->slock);
+	mutex_init(&bdisp->lock);
+
+	/* get resources */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	bdisp->regs = devm_ioremap_resource(dev, res);
+	if (IS_ERR(bdisp->regs)) {
+		dev_err(dev, "failed to get regs\n");
+		return PTR_ERR(bdisp->regs);
+	}
+
+	bdisp->clock = devm_clk_get(dev, BDISP_NAME);
+	if (IS_ERR(bdisp->clock)) {
+		dev_err(dev, "failed to get clock\n");
+		return PTR_ERR(bdisp->clock);
+	}
+
+	ret = clk_prepare(bdisp->clock);
+	if (ret < 0) {
+		dev_err(dev, "clock prepare failed\n");
+		bdisp->clock = ERR_PTR(-EINVAL);
+		return ret;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res) {
+		dev_err(dev, "failed to get IRQ resource\n");
+		goto err_clk;
+	}
+
+	ret = devm_request_threaded_irq(dev, res->start, bdisp_irq_handler,
+					bdisp_irq_thread, IRQF_ONESHOT,
+					pdev->name, bdisp);
+	if (ret) {
+		dev_err(dev, "failed to install irq\n");
+		goto err_clk;
+	}
+
+	/* v4l2 register */
+	ret = v4l2_device_register(dev, &bdisp->v4l2_dev);
+	if (ret) {
+		dev_err(dev, "failed to register\n");
+		goto err_clk;
+	}
+
+	/* Debug */
+	ret = bdisp_debugfs_create(bdisp);
+	if (ret) {
+		dev_err(dev, "failed to create debugfs\n");
+		goto err_v4l2;
+	}
+
+	/* Power management */
+	pm_runtime_enable(dev);
+	ret = pm_runtime_get_sync(dev);
+	if (ret < 0) {
+		dev_err(dev, "failed to set PM\n");
+		goto err_dbg;
+	}
+
+	/* Continuous memory allocator */
+	bdisp->alloc_ctx = vb2_dma_contig_init_ctx(dev);
+	if (IS_ERR(bdisp->alloc_ctx)) {
+		ret = PTR_ERR(bdisp->alloc_ctx);
+		goto err_pm;
+	}
+
+	/* Filters */
+	if (bdisp_hw_alloc_filters(bdisp->dev)) {
+		dev_err(bdisp->dev, "no memory for filters\n");
+		ret = -ENOMEM;
+		goto err_vb2_dma;
+	}
+
+	/* Register */
+	ret = bdisp_register_device(bdisp);
+	if (ret) {
+		dev_err(dev, "failed to register\n");
+		goto err_filter;
+	}
+
+	dev_info(dev, "%s%d registered as /dev/video%d\n", BDISP_NAME,
+		 bdisp->id, bdisp->vdev.num);
+
+	pm_runtime_put(dev);
+
+	return 0;
+
+err_filter:
+	bdisp_hw_free_filters(bdisp->dev);
+err_vb2_dma:
+	vb2_dma_contig_cleanup_ctx(bdisp->alloc_ctx);
+err_pm:
+	pm_runtime_put(dev);
+err_dbg:
+	bdisp_debugfs_remove(bdisp);
+err_v4l2:
+	v4l2_device_unregister(&bdisp->v4l2_dev);
+err_clk:
+	if (!IS_ERR(bdisp->clock))
+		clk_unprepare(bdisp->clock);
+
+	return ret;
+}
+
+static const struct of_device_id bdisp_match_types[] = {
+	{
+		.compatible = "st,stih407-bdisp",
+	},
+	{ /* end node */ }
+};
+
+MODULE_DEVICE_TABLE(of, bdisp_match_types);
+
+static struct platform_driver bdisp_driver = {
+	.probe          = bdisp_probe,
+	.remove         = bdisp_remove,
+	.driver         = {
+		.name           = BDISP_NAME,
+		.of_match_table = bdisp_match_types,
+		.pm             = &bdisp_pm_ops,
+	},
+};
+
+module_platform_driver(bdisp_driver);
+
+MODULE_DESCRIPTION("2D blitter for STMicroelectronics SoC");
+MODULE_AUTHOR("Fabien Dessenne <fabien.dessenne@st.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/sti/bdisp/bdisp.h b/drivers/media/platform/sti/bdisp/bdisp.h
new file mode 100644
index 0000000..0cf9857
--- /dev/null
+++ b/drivers/media/platform/sti/bdisp/bdisp.h
@@ -0,0 +1,216 @@
+/*
+ * Copyright (C) STMicroelectronics SA 2014
+ * Authors: Fabien Dessenne <fabien.dessenne@st.com> for STMicroelectronics.
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+#include <linux/clk.h>
+#include <linux/ktime.h>
+#include <linux/platform_device.h>
+#include <linux/spinlock.h>
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-mem2mem.h>
+
+#include <media/videobuf2-dma-contig.h>
+
+#define BDISP_NAME              "bdisp"
+
+/*
+ *  Max nb of nodes in node-list:
+ *   - 2 nodes to handle wide 4K pictures
+ *   - 2 nodes to handle two planes (Y & CbCr) */
+#define MAX_OUTPUT_PLANES       2
+#define MAX_VERTICAL_STRIDES    2
+#define MAX_NB_NODE             (MAX_OUTPUT_PLANES * MAX_VERTICAL_STRIDES)
+
+/* struct bdisp_ctrls - bdisp control set
+ * @hflip:      horizontal flip
+ * @vflip:      vertical flip
+ */
+struct bdisp_ctrls {
+	struct v4l2_ctrl        *hflip;
+	struct v4l2_ctrl        *vflip;
+};
+
+/**
+ * struct bdisp_fmt - driver's internal color format data
+ * @pixelformat:fourcc code for this format
+ * @nb_planes:  number of planes  (ex: [0]=RGB/Y - [1]=Cb/Cr, ...)
+ * @bpp:        bits per pixel (general)
+ * @bpp_plane0: byte per pixel for the 1st plane
+ * @w_align:    width alignment in pixel (multiple of)
+ * @h_align:    height alignment in pixel (multiple of)
+ */
+struct bdisp_fmt {
+	u32                     pixelformat;
+	u8                      nb_planes;
+	u8                      bpp;
+	u8                      bpp_plane0;
+	u8                      w_align;
+	u8                      h_align;
+};
+
+/**
+ * struct bdisp_frame - frame properties
+ *
+ * @width:      frame width (including padding)
+ * @height:     frame height (including padding)
+ * @fmt:        pointer to frame format descriptor
+ * @field:      frame / field type
+ * @bytesperline: stride of the 1st plane
+ * @sizeimage:  image size in bytes
+ * @colorspace: colorspace
+ * @crop:       crop area
+ * @paddr:      image physical addresses per plane ([0]=RGB/Y - [1]=Cb/Cr, ...)
+ */
+struct bdisp_frame {
+	u32                     width;
+	u32                     height;
+	const struct bdisp_fmt  *fmt;
+	enum v4l2_field         field;
+	u32                     bytesperline;
+	u32                     sizeimage;
+	enum v4l2_colorspace    colorspace;
+	struct v4l2_rect        crop;
+	dma_addr_t              paddr[4];
+};
+
+/**
+ * struct bdisp_request - bdisp request
+ *
+ * @src:        source frame properties
+ * @dst:        destination frame properties
+ * @hflip:      horizontal flip
+ * @vflip:      vertical flip
+ * @nb_req:     number of run request
+ */
+struct bdisp_request {
+	struct bdisp_frame      src;
+	struct bdisp_frame      dst;
+	unsigned int            hflip:1;
+	unsigned int            vflip:1;
+	int                     nb_req;
+};
+
+/**
+ * struct bdisp_ctx - device context data
+ *
+ * @src:        source frame properties
+ * @dst:        destination frame properties
+ * @state:      flags to keep track of user configuration
+ * @hflip:      horizontal flip
+ * @vflip:      vertical flip
+ * @bdisp_dev:  the device this context applies to
+ * @node:       node array
+ * @node_paddr: node physical address array
+ * @fh:         v4l2 file handle
+ * @ctrl_handler: v4l2 controls handler
+ * @bdisp_ctrls: bdisp control set
+ * @ctrls_rdy:  true if the control handler is initialized
+ */
+struct bdisp_ctx {
+	struct bdisp_frame      src;
+	struct bdisp_frame      dst;
+	u32                     state;
+	unsigned int            hflip:1;
+	unsigned int            vflip:1;
+	struct bdisp_dev        *bdisp_dev;
+	struct bdisp_node       *node[MAX_NB_NODE];
+	dma_addr_t              node_paddr[MAX_NB_NODE];
+	struct v4l2_fh          fh;
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct bdisp_ctrls      bdisp_ctrls;
+	bool                    ctrls_rdy;
+};
+
+/**
+ * struct bdisp_m2m_device - v4l2 memory-to-memory device data
+ *
+ * @vdev:       video device node for v4l2 m2m mode
+ * @m2m_dev:    v4l2 m2m device data
+ * @ctx:        hardware context data
+ * @refcnt:     reference counter
+ */
+struct bdisp_m2m_device {
+	struct video_device     *vdev;
+	struct v4l2_m2m_dev     *m2m_dev;
+	struct bdisp_ctx        *ctx;
+	int                     refcnt;
+};
+
+/**
+ * struct bdisp_dbg - debug info
+ *
+ * @debugfs_entry: debugfs
+ * @copy_node:     array of last used nodes
+ * @copy_request:  last bdisp request
+ * @hw_start:      start time of last HW request
+ * @last_duration: last HW processing duration in microsecs
+ * @min_duration:  min HW processing duration in microsecs
+ * @max_duration:  max HW processing duration in microsecs
+ * @tot_duration:  total HW processing duration in microsecs
+ */
+struct bdisp_dbg {
+	struct dentry           *debugfs_entry;
+	struct bdisp_node       *copy_node[MAX_NB_NODE];
+	struct bdisp_request    copy_request;
+	ktime_t                 hw_start;
+	s64                     last_duration;
+	s64                     min_duration;
+	s64                     max_duration;
+	s64                     tot_duration;
+};
+
+/**
+ * struct bdisp_dev - abstraction for bdisp entity
+ *
+ * @v4l2_dev:   v4l2 device
+ * @vdev:       video device
+ * @pdev:       platform device
+ * @dev:        device
+ * @lock:       mutex protecting this data structure
+ * @slock:      spinlock protecting this data structure
+ * @id:         device index
+ * @m2m:        memory-to-memory V4L2 device information
+ * @state:      flags used to synchronize m2m and capture mode operation
+ * @alloc_ctx:  videobuf2 memory allocator context
+ * @clock:      IP clock
+ * @regs:       registers
+ * @irq_queue:  interrupt handler waitqueue
+ * @work_queue: workqueue to handle timeouts
+ * @timeout_work: IRQ timeout structure
+ * @dbg:        debug info
+ */
+struct bdisp_dev {
+	struct v4l2_device      v4l2_dev;
+	struct video_device     vdev;
+	struct platform_device  *pdev;
+	struct device           *dev;
+	spinlock_t              slock;
+	struct mutex            lock;
+	u16                     id;
+	struct bdisp_m2m_device m2m;
+	unsigned long           state;
+	struct vb2_alloc_ctx    *alloc_ctx;
+	struct clk              *clock;
+	void __iomem            *regs;
+	wait_queue_head_t       irq_queue;
+	struct workqueue_struct *work_queue;
+	struct delayed_work     timeout_work;
+	struct bdisp_dbg        dbg;
+};
+
+void bdisp_hw_free_nodes(struct bdisp_ctx *ctx);
+int bdisp_hw_alloc_nodes(struct bdisp_ctx *ctx);
+void bdisp_hw_free_filters(struct device *dev);
+int bdisp_hw_alloc_filters(struct device *dev);
+int bdisp_hw_reset(struct bdisp_dev *bdisp);
+int bdisp_hw_get_and_clear_irq(struct bdisp_dev *bdisp);
+int bdisp_hw_update(struct bdisp_ctx *ctx);
+
+void bdisp_debugfs_remove(struct bdisp_dev *bdisp);
+int bdisp_debugfs_create(struct bdisp_dev *bdisp);
+void bdisp_dbg_perf_begin(struct bdisp_dev *bdisp);
+void bdisp_dbg_perf_end(struct bdisp_dev *bdisp);
diff --git a/drivers/media/platform/sti/c8sectpfe/Kconfig b/drivers/media/platform/sti/c8sectpfe/Kconfig
new file mode 100644
index 0000000..7420a50
--- /dev/null
+++ b/drivers/media/platform/sti/c8sectpfe/Kconfig
@@ -0,0 +1,27 @@
+config DVB_C8SECTPFE
+	tristate "STMicroelectronics C8SECTPFE DVB support"
+	depends on PINCTRL && DVB_CORE && I2C
+	depends on ARCH_STI || ARCH_MULTIPLATFORM || COMPILE_TEST
+	select FW_LOADER
+	select DEBUG_FS
+	select DVB_LNBP21 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_STV090x if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_STB6100 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_STV6110 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_STV0900 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_STV0367 if MEDIA_SUBDRV_AUTOSELECT
+	select MEDIA_TUNER_TDA18212 if MEDIA_SUBDRV_AUTOSELECT
+
+	---help---
+	  This adds support for DVB front-end cards connected
+	  to TS inputs of STiH407/410 SoC.
+
+	  The driver currently supports C8SECTPFE's TS input block,
+	  memdma engine, and HW PID filtering.
+
+	  Supported DVB front-end cards are:
+	  - STMicroelectronics DVB-T B2100A (STV0367 + TDA18212)
+	  - STMicroelectronics DVB-S/S2 STV0903 + STV6110 + LNBP24 board
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called c8sectpfe.
diff --git a/drivers/media/platform/sti/c8sectpfe/Makefile b/drivers/media/platform/sti/c8sectpfe/Makefile
new file mode 100644
index 0000000..b578c7c
--- /dev/null
+++ b/drivers/media/platform/sti/c8sectpfe/Makefile
@@ -0,0 +1,9 @@
+c8sectpfe-y += c8sectpfe-core.o c8sectpfe-common.o c8sectpfe-dvb.o \
+		c8sectpfe-debugfs.o
+
+obj-$(CONFIG_DVB_C8SECTPFE) += c8sectpfe.o
+
+ccflags-y += -Idrivers/media/i2c
+ccflags-y += -Idrivers/media/common
+ccflags-y += -Idrivers/media/dvb-core/ -Idrivers/media/dvb-frontends/ \
+		-Idrivers/media/tuners/
diff --git a/drivers/media/platform/sti/c8sectpfe/c8sectpfe-common.c b/drivers/media/platform/sti/c8sectpfe/c8sectpfe-common.c
new file mode 100644
index 0000000..95223ab
--- /dev/null
+++ b/drivers/media/platform/sti/c8sectpfe/c8sectpfe-common.c
@@ -0,0 +1,265 @@
+/*
+ * c8sectpfe-common.c - C8SECTPFE STi DVB driver
+ *
+ * Copyright (c) STMicroelectronics 2015
+ *
+ *   Author: Peter Griffin <peter.griffin@linaro.org>
+ *
+ *      This program is free software; you can redistribute it and/or
+ *      modify it under the terms of the GNU General Public License as
+ *      published by the Free Software Foundation; either version 2 of
+ *      the License, or (at your option) any later version.
+ */
+#include <linux/completion.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/dvb/dmx.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/ioport.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/time.h>
+#include <linux/wait.h>
+
+#include "dmxdev.h"
+#include "dvbdev.h"
+#include "dvb_demux.h"
+#include "dvb_frontend.h"
+#include "dvb_net.h"
+
+#include "c8sectpfe-common.h"
+#include "c8sectpfe-core.h"
+#include "c8sectpfe-dvb.h"
+
+static int register_dvb(struct stdemux *demux, struct dvb_adapter *adap,
+				void *start_feed, void *stop_feed,
+				struct c8sectpfei *fei)
+{
+	int result;
+
+	demux->dvb_demux.dmx.capabilities = DMX_TS_FILTERING |
+					DMX_SECTION_FILTERING |
+					DMX_MEMORY_BASED_FILTERING;
+
+	demux->dvb_demux.priv = demux;
+	demux->dvb_demux.filternum = C8SECTPFE_MAXCHANNEL;
+	demux->dvb_demux.feednum = C8SECTPFE_MAXCHANNEL;
+
+	demux->dvb_demux.start_feed = start_feed;
+	demux->dvb_demux.stop_feed = stop_feed;
+	demux->dvb_demux.write_to_decoder = NULL;
+
+	result = dvb_dmx_init(&demux->dvb_demux);
+	if (result < 0) {
+		dev_err(fei->dev, "dvb_dmx_init failed (errno = %d)\n",
+			result);
+		goto err_dmx;
+	}
+
+	demux->dmxdev.filternum = demux->dvb_demux.filternum;
+	demux->dmxdev.demux = &demux->dvb_demux.dmx;
+	demux->dmxdev.capabilities = 0;
+
+	result = dvb_dmxdev_init(&demux->dmxdev, adap);
+	if (result < 0) {
+		dev_err(fei->dev, "dvb_dmxdev_init failed (errno = %d)\n",
+			result);
+
+		goto err_dmxdev;
+	}
+
+	demux->hw_frontend.source = DMX_FRONTEND_0 + demux->tsin_index;
+
+	result = demux->dvb_demux.dmx.add_frontend(&demux->dvb_demux.dmx,
+						&demux->hw_frontend);
+	if (result < 0) {
+		dev_err(fei->dev, "add_frontend failed (errno = %d)\n", result);
+		goto err_fe_hw;
+	}
+
+	demux->mem_frontend.source = DMX_MEMORY_FE;
+	result = demux->dvb_demux.dmx.add_frontend(&demux->dvb_demux.dmx,
+						&demux->mem_frontend);
+	if (result < 0) {
+		dev_err(fei->dev, "add_frontend failed (%d)\n", result);
+		goto err_fe_mem;
+	}
+
+	result = demux->dvb_demux.dmx.connect_frontend(&demux->dvb_demux.dmx,
+							&demux->hw_frontend);
+	if (result < 0) {
+		dev_err(fei->dev, "connect_frontend (%d)\n", result);
+		goto err_fe_con;
+	}
+
+	return 0;
+
+err_fe_con:
+	demux->dvb_demux.dmx.remove_frontend(&demux->dvb_demux.dmx,
+						     &demux->mem_frontend);
+err_fe_mem:
+	demux->dvb_demux.dmx.remove_frontend(&demux->dvb_demux.dmx,
+						     &demux->hw_frontend);
+err_fe_hw:
+	dvb_dmxdev_release(&demux->dmxdev);
+err_dmxdev:
+	dvb_dmx_release(&demux->dvb_demux);
+err_dmx:
+	return result;
+
+}
+
+static void unregister_dvb(struct stdemux *demux)
+{
+
+	demux->dvb_demux.dmx.remove_frontend(&demux->dvb_demux.dmx,
+						     &demux->mem_frontend);
+
+	demux->dvb_demux.dmx.remove_frontend(&demux->dvb_demux.dmx,
+						     &demux->hw_frontend);
+
+	dvb_dmxdev_release(&demux->dmxdev);
+
+	dvb_dmx_release(&demux->dvb_demux);
+}
+
+static struct c8sectpfe *c8sectpfe_create(struct c8sectpfei *fei,
+				void *start_feed,
+				void *stop_feed)
+{
+	struct c8sectpfe *c8sectpfe;
+	int result;
+	int i, j;
+
+	short int ids[] = { -1 };
+
+	c8sectpfe = kzalloc(sizeof(struct c8sectpfe), GFP_KERNEL);
+	if (!c8sectpfe)
+		goto err1;
+
+	mutex_init(&c8sectpfe->lock);
+
+	c8sectpfe->device = fei->dev;
+
+	result = dvb_register_adapter(&c8sectpfe->adapter, "STi c8sectpfe",
+					THIS_MODULE, fei->dev, ids);
+	if (result < 0) {
+		dev_err(fei->dev, "dvb_register_adapter failed (errno = %d)\n",
+			result);
+		goto err2;
+	}
+
+	c8sectpfe->adapter.priv = fei;
+
+	for (i = 0; i < fei->tsin_count; i++) {
+
+		c8sectpfe->demux[i].tsin_index = i;
+		c8sectpfe->demux[i].c8sectpfei = fei;
+
+		result = register_dvb(&c8sectpfe->demux[i], &c8sectpfe->adapter,
+				start_feed, stop_feed, fei);
+		if (result < 0) {
+			dev_err(fei->dev,
+				"register_dvb feed=%d failed (errno = %d)\n",
+				result, i);
+
+			/* we take a all or nothing approach */
+			for (j = 0; j < i; j++)
+				unregister_dvb(&c8sectpfe->demux[j]);
+			goto err3;
+		}
+	}
+
+	c8sectpfe->num_feeds = fei->tsin_count;
+
+	return c8sectpfe;
+err3:
+	dvb_unregister_adapter(&c8sectpfe->adapter);
+err2:
+	kfree(c8sectpfe);
+err1:
+	return NULL;
+};
+
+static void c8sectpfe_delete(struct c8sectpfe *c8sectpfe)
+{
+	int i;
+
+	if (!c8sectpfe)
+		return;
+
+	for (i = 0; i < c8sectpfe->num_feeds; i++)
+		unregister_dvb(&c8sectpfe->demux[i]);
+
+	dvb_unregister_adapter(&c8sectpfe->adapter);
+
+	kfree(c8sectpfe);
+};
+
+void c8sectpfe_tuner_unregister_frontend(struct c8sectpfe *c8sectpfe,
+					struct c8sectpfei *fei)
+{
+	int n;
+	struct channel_info *tsin;
+
+	for (n = 0; n < fei->tsin_count; n++) {
+
+		tsin = fei->channel_data[n];
+
+		if (tsin && tsin->frontend) {
+			dvb_unregister_frontend(tsin->frontend);
+			dvb_frontend_detach(tsin->frontend);
+		}
+
+		if (tsin && tsin->i2c_adapter)
+			i2c_put_adapter(tsin->i2c_adapter);
+
+		if (tsin && tsin->i2c_client) {
+			if (tsin->i2c_client->dev.driver->owner)
+				module_put(tsin->i2c_client->dev.driver->owner);
+			i2c_unregister_device(tsin->i2c_client);
+		}
+	}
+
+	c8sectpfe_delete(c8sectpfe);
+};
+
+int c8sectpfe_tuner_register_frontend(struct c8sectpfe **c8sectpfe,
+				struct c8sectpfei *fei,
+				void *start_feed,
+				void *stop_feed)
+{
+	struct channel_info *tsin;
+	struct dvb_frontend *frontend;
+	int n, res;
+
+	*c8sectpfe = c8sectpfe_create(fei, start_feed, stop_feed);
+	if (!*c8sectpfe)
+		return -ENOMEM;
+
+	for (n = 0; n < fei->tsin_count; n++) {
+		tsin = fei->channel_data[n];
+
+		res = c8sectpfe_frontend_attach(&frontend, *c8sectpfe, tsin, n);
+		if (res)
+			goto err;
+
+		res = dvb_register_frontend(&c8sectpfe[0]->adapter, frontend);
+		if (res < 0) {
+			dev_err(fei->dev, "dvb_register_frontend failed (%d)\n",
+				res);
+			goto err;
+		}
+
+		tsin->frontend = frontend;
+	}
+
+	return 0;
+
+err:
+	c8sectpfe_tuner_unregister_frontend(*c8sectpfe, fei);
+	return res;
+}
diff --git a/drivers/media/platform/sti/c8sectpfe/c8sectpfe-common.h b/drivers/media/platform/sti/c8sectpfe/c8sectpfe-common.h
new file mode 100644
index 0000000..da21c0a
--- /dev/null
+++ b/drivers/media/platform/sti/c8sectpfe/c8sectpfe-common.h
@@ -0,0 +1,64 @@
+/*
+ * c8sectpfe-common.h - C8SECTPFE STi DVB driver
+ *
+ * Copyright (c) STMicroelectronics 2015
+ *
+ *   Author: Peter Griffin <peter.griffin@linaro.org>
+ *
+ *      This program is free software; you can redistribute it and/or
+ *      modify it under the terms of the GNU General Public License as
+ *      published by the Free Software Foundation; either version 2 of
+ *      the License, or (at your option) any later version.
+ */
+#ifndef _C8SECTPFE_COMMON_H_
+#define _C8SECTPFE_COMMON_H_
+
+#include <linux/dvb/dmx.h>
+#include <linux/dvb/frontend.h>
+#include <linux/gpio.h>
+#include <linux/version.h>
+
+#include "dmxdev.h"
+#include "dvb_demux.h"
+#include "dvb_frontend.h"
+#include "dvb_net.h"
+
+/* Maximum number of channels */
+#define C8SECTPFE_MAXADAPTER (4)
+#define C8SECTPFE_MAXCHANNEL 64
+#define STPTI_MAXCHANNEL 64
+
+#define MAX_INPUTBLOCKS 7
+
+struct c8sectpfe;
+struct stdemux;
+
+struct stdemux {
+	struct dvb_demux	dvb_demux;
+	struct dmxdev		dmxdev;
+	struct dmx_frontend	hw_frontend;
+	struct dmx_frontend	mem_frontend;
+	int			tsin_index;
+	int			running_feed_count;
+	struct			c8sectpfei *c8sectpfei;
+};
+
+struct c8sectpfe {
+	struct stdemux demux[MAX_INPUTBLOCKS];
+	struct mutex lock;
+	struct dvb_adapter adapter;
+	struct device *device;
+	int mapping;
+	int num_feeds;
+};
+
+/* Channel registration */
+int c8sectpfe_tuner_register_frontend(struct c8sectpfe **c8sectpfe,
+					struct c8sectpfei *fei,
+					void *start_feed,
+					void *stop_feed);
+
+void c8sectpfe_tuner_unregister_frontend(struct c8sectpfe *c8sectpfe,
+						struct c8sectpfei *fei);
+
+#endif
diff --git a/drivers/media/platform/sti/c8sectpfe/c8sectpfe-core.c b/drivers/media/platform/sti/c8sectpfe/c8sectpfe-core.c
new file mode 100644
index 0000000..8490a65
--- /dev/null
+++ b/drivers/media/platform/sti/c8sectpfe/c8sectpfe-core.c
@@ -0,0 +1,1235 @@
+/*
+ * c8sectpfe-core.c - C8SECTPFE STi DVB driver
+ *
+ * Copyright (c) STMicroelectronics 2015
+ *
+ *   Author:Peter Bennett <peter.bennett@st.com>
+ *	    Peter Griffin <peter.griffin@linaro.org>
+ *
+ *	This program is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU General Public License as
+ *	published by the Free Software Foundation; either version 2 of
+ *	the License, or (at your option) any later version.
+ */
+#include <linux/atomic.h>
+#include <linux/clk.h>
+#include <linux/completion.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/dvb/dmx.h>
+#include <linux/dvb/frontend.h>
+#include <linux/errno.h>
+#include <linux/firmware.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of_gpio.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/usb.h>
+#include <linux/slab.h>
+#include <linux/time.h>
+#include <linux/version.h>
+#include <linux/wait.h>
+#include <linux/pinctrl/pinctrl.h>
+
+#include "c8sectpfe-core.h"
+#include "c8sectpfe-common.h"
+#include "c8sectpfe-debugfs.h"
+#include "dmxdev.h"
+#include "dvb_demux.h"
+#include "dvb_frontend.h"
+#include "dvb_net.h"
+
+#define FIRMWARE_MEMDMA "pti_memdma_h407.elf"
+MODULE_FIRMWARE(FIRMWARE_MEMDMA);
+
+#define PID_TABLE_SIZE 1024
+#define POLL_MSECS 50
+
+static int load_c8sectpfe_fw_step1(struct c8sectpfei *fei);
+
+#define TS_PKT_SIZE 188
+#define HEADER_SIZE (4)
+#define PACKET_SIZE (TS_PKT_SIZE+HEADER_SIZE)
+
+#define FEI_ALIGNMENT (32)
+/* hw requires minimum of 8*PACKET_SIZE and padded to 8byte boundary */
+#define FEI_BUFFER_SIZE (8*PACKET_SIZE*340)
+
+#define FIFO_LEN 1024
+
+static void c8sectpfe_timer_interrupt(unsigned long ac8sectpfei)
+{
+	struct c8sectpfei *fei = (struct c8sectpfei *)ac8sectpfei;
+	struct channel_info *channel;
+	int chan_num;
+
+	/* iterate through input block channels */
+	for (chan_num = 0; chan_num < fei->tsin_count; chan_num++) {
+		channel = fei->channel_data[chan_num];
+
+		/* is this descriptor initialised and TP enabled */
+		if (channel->irec && readl(channel->irec + DMA_PRDS_TPENABLE))
+			tasklet_schedule(&channel->tsklet);
+	}
+
+	fei->timer.expires = jiffies +	msecs_to_jiffies(POLL_MSECS);
+	add_timer(&fei->timer);
+}
+
+static void channel_swdemux_tsklet(unsigned long data)
+{
+	struct channel_info *channel = (struct channel_info *)data;
+	struct c8sectpfei *fei = channel->fei;
+	unsigned long wp, rp;
+	int pos, num_packets, n, size;
+	u8 *buf;
+
+	if (unlikely(!channel || !channel->irec))
+		return;
+
+	wp = readl(channel->irec + DMA_PRDS_BUSWP_TP(0));
+	rp = readl(channel->irec + DMA_PRDS_BUSRP_TP(0));
+
+	pos = rp - channel->back_buffer_busaddr;
+
+	/* has it wrapped */
+	if (wp < rp)
+		wp = channel->back_buffer_busaddr + FEI_BUFFER_SIZE;
+
+	size = wp - rp;
+	num_packets = size / PACKET_SIZE;
+
+	/* manage cache so data is visible to CPU */
+	dma_sync_single_for_cpu(fei->dev,
+				rp,
+				size,
+				DMA_FROM_DEVICE);
+
+	buf = (u8 *) channel->back_buffer_aligned;
+
+	dev_dbg(fei->dev,
+		"chan=%d channel=%p num_packets = %d, buf = %p, pos = 0x%x\n\t"
+		"rp=0x%lx, wp=0x%lx\n",
+		channel->tsin_id, channel, num_packets, buf, pos, rp, wp);
+
+	for (n = 0; n < num_packets; n++) {
+		dvb_dmx_swfilter_packets(
+			&fei->c8sectpfe[0]->
+				demux[channel->demux_mapping].dvb_demux,
+			&buf[pos], 1);
+
+		pos += PACKET_SIZE;
+	}
+
+	/* advance the read pointer */
+	if (wp == (channel->back_buffer_busaddr + FEI_BUFFER_SIZE))
+		writel(channel->back_buffer_busaddr, channel->irec +
+			DMA_PRDS_BUSRP_TP(0));
+	else
+		writel(wp, channel->irec + DMA_PRDS_BUSWP_TP(0));
+}
+
+static int c8sectpfe_start_feed(struct dvb_demux_feed *dvbdmxfeed)
+{
+	struct dvb_demux *demux = dvbdmxfeed->demux;
+	struct stdemux *stdemux = (struct stdemux *)demux->priv;
+	struct c8sectpfei *fei = stdemux->c8sectpfei;
+	struct channel_info *channel;
+	u32 tmp;
+	unsigned long *bitmap;
+
+	switch (dvbdmxfeed->type) {
+	case DMX_TYPE_TS:
+		break;
+	case DMX_TYPE_SEC:
+		break;
+	default:
+		dev_err(fei->dev, "%s:%d Error bailing\n"
+			, __func__, __LINE__);
+		return -EINVAL;
+	}
+
+	if (dvbdmxfeed->type == DMX_TYPE_TS) {
+		switch (dvbdmxfeed->pes_type) {
+		case DMX_PES_VIDEO:
+		case DMX_PES_AUDIO:
+		case DMX_PES_TELETEXT:
+		case DMX_PES_PCR:
+		case DMX_PES_OTHER:
+			break;
+		default:
+			dev_err(fei->dev, "%s:%d Error bailing\n"
+				, __func__, __LINE__);
+			return -EINVAL;
+		}
+	}
+
+	if (!atomic_read(&fei->fw_loaded)) {
+		dev_err(fei->dev, "%s: c8sectpfe fw not loaded\n", __func__);
+		return -EINVAL;
+	}
+
+	mutex_lock(&fei->lock);
+
+	channel = fei->channel_data[stdemux->tsin_index];
+
+	bitmap = (unsigned long *) channel->pid_buffer_aligned;
+
+	/* 8192 is a special PID */
+	if (dvbdmxfeed->pid == 8192) {
+		tmp = readl(fei->io + C8SECTPFE_IB_PID_SET(channel->tsin_id));
+		tmp &= ~C8SECTPFE_PID_ENABLE;
+		writel(tmp, fei->io + C8SECTPFE_IB_PID_SET(channel->tsin_id));
+
+	} else {
+		bitmap_set(bitmap, dvbdmxfeed->pid, 1);
+	}
+
+	/* manage cache so PID bitmap is visible to HW */
+	dma_sync_single_for_device(fei->dev,
+					channel->pid_buffer_busaddr,
+					PID_TABLE_SIZE,
+					DMA_TO_DEVICE);
+
+	channel->active = 1;
+
+	if (fei->global_feed_count == 0) {
+		fei->timer.expires = jiffies +
+			msecs_to_jiffies(msecs_to_jiffies(POLL_MSECS));
+
+		add_timer(&fei->timer);
+	}
+
+	if (stdemux->running_feed_count == 0) {
+
+		dev_dbg(fei->dev, "Starting channel=%p\n", channel);
+
+		tasklet_init(&channel->tsklet, channel_swdemux_tsklet,
+			     (unsigned long) channel);
+
+		/* Reset the internal inputblock sram pointers */
+		writel(channel->fifo,
+			fei->io + C8SECTPFE_IB_BUFF_STRT(channel->tsin_id));
+		writel(channel->fifo + FIFO_LEN - 1,
+			fei->io + C8SECTPFE_IB_BUFF_END(channel->tsin_id));
+
+		writel(channel->fifo,
+			fei->io + C8SECTPFE_IB_READ_PNT(channel->tsin_id));
+		writel(channel->fifo,
+			fei->io + C8SECTPFE_IB_WRT_PNT(channel->tsin_id));
+
+
+		/* reset read / write memdma ptrs for this channel */
+		writel(channel->back_buffer_busaddr, channel->irec +
+			DMA_PRDS_BUSBASE_TP(0));
+
+		tmp = channel->back_buffer_busaddr + FEI_BUFFER_SIZE - 1;
+		writel(tmp, channel->irec + DMA_PRDS_BUSTOP_TP(0));
+
+		writel(channel->back_buffer_busaddr, channel->irec +
+			DMA_PRDS_BUSWP_TP(0));
+
+		/* Issue a reset and enable InputBlock */
+		writel(C8SECTPFE_SYS_ENABLE | C8SECTPFE_SYS_RESET
+			, fei->io + C8SECTPFE_IB_SYS(channel->tsin_id));
+
+		/* and enable the tp */
+		writel(0x1, channel->irec + DMA_PRDS_TPENABLE);
+
+		dev_dbg(fei->dev, "%s:%d Starting DMA feed on stdemux=%p\n"
+			, __func__, __LINE__, stdemux);
+	}
+
+	stdemux->running_feed_count++;
+	fei->global_feed_count++;
+
+	mutex_unlock(&fei->lock);
+
+	return 0;
+}
+
+static int c8sectpfe_stop_feed(struct dvb_demux_feed *dvbdmxfeed)
+{
+
+	struct dvb_demux *demux = dvbdmxfeed->demux;
+	struct stdemux *stdemux = (struct stdemux *)demux->priv;
+	struct c8sectpfei *fei = stdemux->c8sectpfei;
+	struct channel_info *channel;
+	int idlereq;
+	u32 tmp;
+	int ret;
+	unsigned long *bitmap;
+
+	if (!atomic_read(&fei->fw_loaded)) {
+		dev_err(fei->dev, "%s: c8sectpfe fw not loaded\n", __func__);
+		return -EINVAL;
+	}
+
+	mutex_lock(&fei->lock);
+
+	channel = fei->channel_data[stdemux->tsin_index];
+
+	bitmap = (unsigned long *) channel->pid_buffer_aligned;
+
+	if (dvbdmxfeed->pid == 8192) {
+		tmp = readl(fei->io + C8SECTPFE_IB_PID_SET(channel->tsin_id));
+		tmp |= C8SECTPFE_PID_ENABLE;
+		writel(tmp, fei->io + C8SECTPFE_IB_PID_SET(channel->tsin_id));
+	} else {
+		bitmap_clear(bitmap, dvbdmxfeed->pid, 1);
+	}
+
+	/* manage cache so data is visible to HW */
+	dma_sync_single_for_device(fei->dev,
+					channel->pid_buffer_busaddr,
+					PID_TABLE_SIZE,
+					DMA_TO_DEVICE);
+
+	if (--stdemux->running_feed_count == 0) {
+
+		channel = fei->channel_data[stdemux->tsin_index];
+
+		/* TP re-configuration on page 168 of functional spec */
+
+		/* disable IB (prevents more TS data going to memdma) */
+		writel(0, fei->io + C8SECTPFE_IB_SYS(channel->tsin_id));
+
+		/* disable this channels descriptor */
+		writel(0,  channel->irec + DMA_PRDS_TPENABLE);
+
+		tasklet_disable(&channel->tsklet);
+
+		/* now request memdma channel goes idle */
+		idlereq = (1 << channel->tsin_id) | IDLEREQ;
+		writel(idlereq, fei->io + DMA_IDLE_REQ);
+
+		/* wait for idle irq handler to signal completion */
+		ret = wait_for_completion_timeout(&channel->idle_completion,
+						msecs_to_jiffies(100));
+
+		if (ret == 0)
+			dev_warn(fei->dev,
+				"Timeout waiting for idle irq on tsin%d\n",
+				channel->tsin_id);
+
+		reinit_completion(&channel->idle_completion);
+
+		/* reset read / write ptrs for this channel */
+
+		writel(channel->back_buffer_busaddr,
+			channel->irec + DMA_PRDS_BUSBASE_TP(0));
+
+		tmp = channel->back_buffer_busaddr + FEI_BUFFER_SIZE - 1;
+		writel(tmp, channel->irec + DMA_PRDS_BUSTOP_TP(0));
+
+		writel(channel->back_buffer_busaddr,
+			channel->irec + DMA_PRDS_BUSWP_TP(0));
+
+		dev_dbg(fei->dev,
+			"%s:%d stopping DMA feed on stdemux=%p channel=%d\n",
+			__func__, __LINE__, stdemux, channel->tsin_id);
+
+		/* turn off all PIDS in the bitmap */
+		memset((void *)channel->pid_buffer_aligned
+			, 0x00, PID_TABLE_SIZE);
+
+		/* manage cache so data is visible to HW */
+		dma_sync_single_for_device(fei->dev,
+					channel->pid_buffer_busaddr,
+					PID_TABLE_SIZE,
+					DMA_TO_DEVICE);
+
+		channel->active = 0;
+	}
+
+	if (--fei->global_feed_count == 0) {
+		dev_dbg(fei->dev, "%s:%d global_feed_count=%d\n"
+			, __func__, __LINE__, fei->global_feed_count);
+
+		del_timer(&fei->timer);
+	}
+
+	mutex_unlock(&fei->lock);
+
+	return 0;
+}
+
+static struct channel_info *find_channel(struct c8sectpfei *fei, int tsin_num)
+{
+	int i;
+
+	for (i = 0; i < C8SECTPFE_MAX_TSIN_CHAN; i++) {
+		if (!fei->channel_data[i])
+			continue;
+
+		if (fei->channel_data[i]->tsin_id == tsin_num)
+			return fei->channel_data[i];
+	}
+
+	return NULL;
+}
+
+static void c8sectpfe_getconfig(struct c8sectpfei *fei)
+{
+	struct c8sectpfe_hw *hw = &fei->hw_stats;
+
+	hw->num_ib = readl(fei->io + SYS_CFG_NUM_IB);
+	hw->num_mib = readl(fei->io + SYS_CFG_NUM_MIB);
+	hw->num_swts = readl(fei->io + SYS_CFG_NUM_SWTS);
+	hw->num_tsout = readl(fei->io + SYS_CFG_NUM_TSOUT);
+	hw->num_ccsc = readl(fei->io + SYS_CFG_NUM_CCSC);
+	hw->num_ram = readl(fei->io + SYS_CFG_NUM_RAM);
+	hw->num_tp = readl(fei->io + SYS_CFG_NUM_TP);
+
+	dev_info(fei->dev, "C8SECTPFE hw supports the following:\n");
+	dev_info(fei->dev, "Input Blocks: %d\n", hw->num_ib);
+	dev_info(fei->dev, "Merged Input Blocks: %d\n", hw->num_mib);
+	dev_info(fei->dev, "Software Transport Stream Inputs: %d\n"
+				, hw->num_swts);
+	dev_info(fei->dev, "Transport Stream Output: %d\n", hw->num_tsout);
+	dev_info(fei->dev, "Cable Card Converter: %d\n", hw->num_ccsc);
+	dev_info(fei->dev, "RAMs supported by C8SECTPFE: %d\n", hw->num_ram);
+	dev_info(fei->dev, "Tango TPs supported by C8SECTPFE: %d\n"
+			, hw->num_tp);
+}
+
+static irqreturn_t c8sectpfe_idle_irq_handler(int irq, void *priv)
+{
+	struct c8sectpfei *fei = priv;
+	struct channel_info *chan;
+	int bit;
+	unsigned long tmp = readl(fei->io + DMA_IDLE_REQ);
+
+	/* page 168 of functional spec: Clear the idle request
+	   by writing 0 to the C8SECTPFE_DMA_IDLE_REQ register. */
+
+	/* signal idle completion */
+	for_each_set_bit(bit, &tmp, fei->hw_stats.num_ib) {
+
+		chan = find_channel(fei, bit);
+
+		if (chan)
+			complete(&chan->idle_completion);
+	}
+
+	writel(0, fei->io + DMA_IDLE_REQ);
+
+	return IRQ_HANDLED;
+}
+
+
+static void free_input_block(struct c8sectpfei *fei, struct channel_info *tsin)
+{
+	if (!fei || !tsin)
+		return;
+
+	if (tsin->back_buffer_busaddr)
+		if (!dma_mapping_error(fei->dev, tsin->back_buffer_busaddr))
+			dma_unmap_single(fei->dev, tsin->back_buffer_busaddr,
+				FEI_BUFFER_SIZE, DMA_BIDIRECTIONAL);
+
+	kfree(tsin->back_buffer_start);
+
+	if (tsin->pid_buffer_busaddr)
+		if (!dma_mapping_error(fei->dev, tsin->pid_buffer_busaddr))
+			dma_unmap_single(fei->dev, tsin->pid_buffer_busaddr,
+				PID_TABLE_SIZE, DMA_BIDIRECTIONAL);
+
+	kfree(tsin->pid_buffer_start);
+}
+
+#define MAX_NAME 20
+
+static int configure_memdma_and_inputblock(struct c8sectpfei *fei,
+				struct channel_info *tsin)
+{
+	int ret;
+	u32 tmp;
+	char tsin_pin_name[MAX_NAME];
+
+	if (!fei || !tsin)
+		return -EINVAL;
+
+	dev_dbg(fei->dev, "%s:%d Configuring channel=%p tsin=%d\n"
+		, __func__, __LINE__, tsin, tsin->tsin_id);
+
+	init_completion(&tsin->idle_completion);
+
+	tsin->back_buffer_start = kzalloc(FEI_BUFFER_SIZE +
+					FEI_ALIGNMENT, GFP_KERNEL);
+
+	if (!tsin->back_buffer_start) {
+		ret = -ENOMEM;
+		goto err_unmap;
+	}
+
+	/* Ensure backbuffer is 32byte aligned */
+	tsin->back_buffer_aligned = tsin->back_buffer_start
+		+ FEI_ALIGNMENT;
+
+	tsin->back_buffer_aligned = (void *)
+		(((uintptr_t) tsin->back_buffer_aligned) & ~0x1F);
+
+	tsin->back_buffer_busaddr = dma_map_single(fei->dev,
+					(void *)tsin->back_buffer_aligned,
+					FEI_BUFFER_SIZE,
+					DMA_BIDIRECTIONAL);
+
+	if (dma_mapping_error(fei->dev, tsin->back_buffer_busaddr)) {
+		dev_err(fei->dev, "failed to map back_buffer\n");
+		ret = -EFAULT;
+		goto err_unmap;
+	}
+
+	/*
+	 * The pid buffer can be configured (in hw) for byte or bit
+	 * per pid. By powers of deduction we conclude stih407 family
+	 * is configured (at SoC design stage) for bit per pid.
+	 */
+	tsin->pid_buffer_start = kzalloc(2048, GFP_KERNEL);
+
+	if (!tsin->pid_buffer_start) {
+		ret = -ENOMEM;
+		goto err_unmap;
+	}
+
+	/*
+	 * PID buffer needs to be aligned to size of the pid table
+	 * which at bit per pid is 1024 bytes (8192 pids / 8).
+	 * PIDF_BASE register enforces this alignment when writing
+	 * the register.
+	 */
+
+	tsin->pid_buffer_aligned = tsin->pid_buffer_start +
+		PID_TABLE_SIZE;
+
+	tsin->pid_buffer_aligned = (void *)
+		(((uintptr_t) tsin->pid_buffer_aligned) & ~0x3ff);
+
+	tsin->pid_buffer_busaddr = dma_map_single(fei->dev,
+						tsin->pid_buffer_aligned,
+						PID_TABLE_SIZE,
+						DMA_BIDIRECTIONAL);
+
+	if (dma_mapping_error(fei->dev, tsin->pid_buffer_busaddr)) {
+		dev_err(fei->dev, "failed to map pid_bitmap\n");
+		ret = -EFAULT;
+		goto err_unmap;
+	}
+
+	/* manage cache so pid bitmap is visible to HW */
+	dma_sync_single_for_device(fei->dev,
+				tsin->pid_buffer_busaddr,
+				PID_TABLE_SIZE,
+				DMA_TO_DEVICE);
+
+	snprintf(tsin_pin_name, MAX_NAME, "tsin%d-%s", tsin->tsin_id,
+		(tsin->serial_not_parallel ? "serial" : "parallel"));
+
+	tsin->pstate = pinctrl_lookup_state(fei->pinctrl, tsin_pin_name);
+	if (IS_ERR(tsin->pstate)) {
+		dev_err(fei->dev, "%s: pinctrl_lookup_state couldn't find %s state\n"
+			, __func__, tsin_pin_name);
+		ret = PTR_ERR(tsin->pstate);
+		goto err_unmap;
+	}
+
+	ret = pinctrl_select_state(fei->pinctrl, tsin->pstate);
+
+	if (ret) {
+		dev_err(fei->dev, "%s: pinctrl_select_state failed\n"
+			, __func__);
+		goto err_unmap;
+	}
+
+	/* Enable this input block */
+	tmp = readl(fei->io + SYS_INPUT_CLKEN);
+	tmp |= BIT(tsin->tsin_id);
+	writel(tmp, fei->io + SYS_INPUT_CLKEN);
+
+	if (tsin->serial_not_parallel)
+		tmp |= C8SECTPFE_SERIAL_NOT_PARALLEL;
+
+	if (tsin->invert_ts_clk)
+		tmp |= C8SECTPFE_INVERT_TSCLK;
+
+	if (tsin->async_not_sync)
+		tmp |= C8SECTPFE_ASYNC_NOT_SYNC;
+
+	tmp |= C8SECTPFE_ALIGN_BYTE_SOP | C8SECTPFE_BYTE_ENDIANNESS_MSB;
+
+	writel(tmp, fei->io + C8SECTPFE_IB_IP_FMT_CFG(tsin->tsin_id));
+
+	writel(C8SECTPFE_SYNC(0x9) |
+		C8SECTPFE_DROP(0x9) |
+		C8SECTPFE_TOKEN(0x47),
+		fei->io + C8SECTPFE_IB_SYNCLCKDRP_CFG(tsin->tsin_id));
+
+	writel(TS_PKT_SIZE, fei->io + C8SECTPFE_IB_PKT_LEN(tsin->tsin_id));
+
+	/* Place the FIFO's at the end of the irec descriptors */
+
+	tsin->fifo = (tsin->tsin_id * FIFO_LEN);
+
+	writel(tsin->fifo, fei->io + C8SECTPFE_IB_BUFF_STRT(tsin->tsin_id));
+	writel(tsin->fifo + FIFO_LEN - 1,
+		fei->io + C8SECTPFE_IB_BUFF_END(tsin->tsin_id));
+
+	writel(tsin->fifo, fei->io + C8SECTPFE_IB_READ_PNT(tsin->tsin_id));
+	writel(tsin->fifo, fei->io + C8SECTPFE_IB_WRT_PNT(tsin->tsin_id));
+
+	writel(tsin->pid_buffer_busaddr,
+		fei->io + PIDF_BASE(tsin->tsin_id));
+
+	dev_info(fei->dev, "chan=%d PIDF_BASE=0x%x pid_bus_addr=%pad\n",
+		tsin->tsin_id, readl(fei->io + PIDF_BASE(tsin->tsin_id)),
+		&tsin->pid_buffer_busaddr);
+
+	/* Configure and enable HW PID filtering */
+
+	/*
+	 * The PID value is created by assembling the first 8 bytes of
+	 * the TS packet into a 64-bit word in big-endian format. A
+	 * slice of that 64-bit word is taken from
+	 * (PID_OFFSET+PID_NUM_BITS-1) to PID_OFFSET.
+	 */
+	tmp = (C8SECTPFE_PID_ENABLE | C8SECTPFE_PID_NUMBITS(13)
+		| C8SECTPFE_PID_OFFSET(40));
+
+	writel(tmp, fei->io + C8SECTPFE_IB_PID_SET(tsin->tsin_id));
+
+	dev_dbg(fei->dev, "chan=%d setting wp: %d, rp: %d, buf: %d-%d\n",
+		tsin->tsin_id,
+		readl(fei->io + C8SECTPFE_IB_WRT_PNT(tsin->tsin_id)),
+		readl(fei->io + C8SECTPFE_IB_READ_PNT(tsin->tsin_id)),
+		readl(fei->io + C8SECTPFE_IB_BUFF_STRT(tsin->tsin_id)),
+		readl(fei->io + C8SECTPFE_IB_BUFF_END(tsin->tsin_id)));
+
+	/* Get base addpress of pointer record block from DMEM */
+	tsin->irec = fei->io + DMA_MEMDMA_OFFSET + DMA_DMEM_OFFSET +
+			readl(fei->io + DMA_PTRREC_BASE);
+
+	/* fill out pointer record data structure */
+
+	/* advance pointer record block to our channel */
+	tsin->irec += (tsin->tsin_id * DMA_PRDS_SIZE);
+
+	writel(tsin->fifo, tsin->irec + DMA_PRDS_MEMBASE);
+
+	writel(tsin->fifo + FIFO_LEN - 1, tsin->irec + DMA_PRDS_MEMTOP);
+
+	writel((188 + 7)&~7, tsin->irec + DMA_PRDS_PKTSIZE);
+
+	writel(0x1, tsin->irec + DMA_PRDS_TPENABLE);
+
+	/* read/write pointers with physical bus address */
+
+	writel(tsin->back_buffer_busaddr, tsin->irec + DMA_PRDS_BUSBASE_TP(0));
+
+	tmp = tsin->back_buffer_busaddr + FEI_BUFFER_SIZE - 1;
+	writel(tmp, tsin->irec + DMA_PRDS_BUSTOP_TP(0));
+
+	writel(tsin->back_buffer_busaddr, tsin->irec + DMA_PRDS_BUSWP_TP(0));
+	writel(tsin->back_buffer_busaddr, tsin->irec + DMA_PRDS_BUSRP_TP(0));
+
+	/* initialize tasklet */
+	tasklet_init(&tsin->tsklet, channel_swdemux_tsklet,
+		(unsigned long) tsin);
+
+	return 0;
+
+err_unmap:
+	free_input_block(fei, tsin);
+	return ret;
+}
+
+static irqreturn_t c8sectpfe_error_irq_handler(int irq, void *priv)
+{
+	struct c8sectpfei *fei = priv;
+
+	dev_err(fei->dev, "%s: error handling not yet implemented\n"
+		, __func__);
+
+	/*
+	 * TODO FIXME we should detect some error conditions here
+	 * and ideally so something about them!
+	 */
+
+	return IRQ_HANDLED;
+}
+
+static int c8sectpfe_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *child, *np = dev->of_node;
+	struct c8sectpfei *fei;
+	struct resource *res;
+	int ret, index = 0;
+	struct channel_info *tsin;
+
+	/* Allocate the c8sectpfei structure */
+	fei = devm_kzalloc(dev, sizeof(struct c8sectpfei), GFP_KERNEL);
+	if (!fei)
+		return -ENOMEM;
+
+	fei->dev = dev;
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "c8sectpfe");
+	fei->io = devm_ioremap_resource(dev, res);
+	if (IS_ERR(fei->io))
+		return PTR_ERR(fei->io);
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
+					"c8sectpfe-ram");
+	fei->sram = devm_ioremap_resource(dev, res);
+	if (IS_ERR(fei->sram))
+		return PTR_ERR(fei->sram);
+
+	fei->sram_size = res->end - res->start;
+
+	fei->idle_irq = platform_get_irq_byname(pdev, "c8sectpfe-idle-irq");
+	if (fei->idle_irq < 0) {
+		dev_err(dev, "Can't get c8sectpfe-idle-irq\n");
+		return fei->idle_irq;
+	}
+
+	fei->error_irq = platform_get_irq_byname(pdev, "c8sectpfe-error-irq");
+	if (fei->error_irq < 0) {
+		dev_err(dev, "Can't get c8sectpfe-error-irq\n");
+		return fei->error_irq;
+	}
+
+	platform_set_drvdata(pdev, fei);
+
+	fei->c8sectpfeclk = devm_clk_get(dev, "c8sectpfe");
+	if (IS_ERR(fei->c8sectpfeclk)) {
+		dev_err(dev, "c8sectpfe clk not found\n");
+		return PTR_ERR(fei->c8sectpfeclk);
+	}
+
+	ret = clk_prepare_enable(fei->c8sectpfeclk);
+	if (ret) {
+		dev_err(dev, "Failed to enable c8sectpfe clock\n");
+		return ret;
+	}
+
+	/* to save power disable all IP's (on by default) */
+	writel(0, fei->io + SYS_INPUT_CLKEN);
+
+	/* Enable memdma clock */
+	writel(MEMDMAENABLE, fei->io + SYS_OTHER_CLKEN);
+
+	/* clear internal sram */
+	memset_io(fei->sram, 0x0, fei->sram_size);
+
+	c8sectpfe_getconfig(fei);
+
+	ret = devm_request_irq(dev, fei->idle_irq, c8sectpfe_idle_irq_handler,
+			0, "c8sectpfe-idle-irq", fei);
+	if (ret) {
+		dev_err(dev, "Can't register c8sectpfe-idle-irq IRQ.\n");
+		goto err_clk_disable;
+	}
+
+	ret = devm_request_irq(dev, fei->error_irq,
+				c8sectpfe_error_irq_handler, 0,
+				"c8sectpfe-error-irq", fei);
+	if (ret) {
+		dev_err(dev, "Can't register c8sectpfe-error-irq IRQ.\n");
+		goto err_clk_disable;
+	}
+
+	fei->tsin_count = of_get_child_count(np);
+
+	if (fei->tsin_count > C8SECTPFE_MAX_TSIN_CHAN ||
+		fei->tsin_count > fei->hw_stats.num_ib) {
+
+		dev_err(dev, "More tsin declared than exist on SoC!\n");
+		ret = -EINVAL;
+		goto err_clk_disable;
+	}
+
+	fei->pinctrl = devm_pinctrl_get(dev);
+
+	if (IS_ERR(fei->pinctrl)) {
+		dev_err(dev, "Error getting tsin pins\n");
+		ret = PTR_ERR(fei->pinctrl);
+		goto err_clk_disable;
+	}
+
+	for_each_child_of_node(np, child) {
+		struct device_node *i2c_bus;
+
+		fei->channel_data[index] = devm_kzalloc(dev,
+						sizeof(struct channel_info),
+						GFP_KERNEL);
+
+		if (!fei->channel_data[index]) {
+			ret = -ENOMEM;
+			goto err_clk_disable;
+		}
+
+		tsin = fei->channel_data[index];
+
+		tsin->fei = fei;
+
+		ret = of_property_read_u32(child, "tsin-num", &tsin->tsin_id);
+		if (ret) {
+			dev_err(&pdev->dev, "No tsin_num found\n");
+			goto err_clk_disable;
+		}
+
+		/* sanity check value */
+		if (tsin->tsin_id > fei->hw_stats.num_ib) {
+			dev_err(&pdev->dev,
+				"tsin-num %d specified greater than number\n\t"
+				"of input block hw in SoC! (%d)",
+				tsin->tsin_id, fei->hw_stats.num_ib);
+			ret = -EINVAL;
+			goto err_clk_disable;
+		}
+
+		tsin->invert_ts_clk = of_property_read_bool(child,
+							"invert-ts-clk");
+
+		tsin->serial_not_parallel = of_property_read_bool(child,
+							"serial-not-parallel");
+
+		tsin->async_not_sync = of_property_read_bool(child,
+							"async-not-sync");
+
+		ret = of_property_read_u32(child, "dvb-card",
+					&tsin->dvb_card);
+		if (ret) {
+			dev_err(&pdev->dev, "No dvb-card found\n");
+			goto err_clk_disable;
+		}
+
+		i2c_bus = of_parse_phandle(child, "i2c-bus", 0);
+		if (!i2c_bus) {
+			dev_err(&pdev->dev, "No i2c-bus found\n");
+			goto err_clk_disable;
+		}
+		tsin->i2c_adapter =
+			of_find_i2c_adapter_by_node(i2c_bus);
+		if (!tsin->i2c_adapter) {
+			dev_err(&pdev->dev, "No i2c adapter found\n");
+			of_node_put(i2c_bus);
+			goto err_clk_disable;
+		}
+		of_node_put(i2c_bus);
+
+		tsin->rst_gpio = of_get_named_gpio(child, "rst-gpio", 0);
+
+		ret = gpio_is_valid(tsin->rst_gpio);
+		if (!ret) {
+			dev_err(dev,
+				"reset gpio for tsin%d not valid (gpio=%d)\n",
+				tsin->tsin_id, tsin->rst_gpio);
+			goto err_clk_disable;
+		}
+
+		ret = devm_gpio_request_one(dev, tsin->rst_gpio,
+					GPIOF_OUT_INIT_LOW, "NIM reset");
+		if (ret && ret != -EBUSY) {
+			dev_err(dev, "Can't request tsin%d reset gpio\n"
+				, fei->channel_data[index]->tsin_id);
+			goto err_clk_disable;
+		}
+
+		if (!ret) {
+			/* toggle reset lines */
+			gpio_direction_output(tsin->rst_gpio, 0);
+			usleep_range(3500, 5000);
+			gpio_direction_output(tsin->rst_gpio, 1);
+			usleep_range(3000, 5000);
+		}
+
+		tsin->demux_mapping = index;
+
+		dev_dbg(fei->dev,
+			"channel=%p n=%d tsin_num=%d, invert-ts-clk=%d\n\t"
+			"serial-not-parallel=%d pkt-clk-valid=%d dvb-card=%d\n",
+			fei->channel_data[index], index,
+			tsin->tsin_id, tsin->invert_ts_clk,
+			tsin->serial_not_parallel, tsin->async_not_sync,
+			tsin->dvb_card);
+
+		index++;
+	}
+
+	/* Setup timer interrupt */
+	init_timer(&fei->timer);
+	fei->timer.function = c8sectpfe_timer_interrupt;
+	fei->timer.data = (unsigned long)fei;
+
+	mutex_init(&fei->lock);
+
+	/* Get the configuration information about the tuners */
+	ret = c8sectpfe_tuner_register_frontend(&fei->c8sectpfe[0],
+					(void *)fei,
+					c8sectpfe_start_feed,
+					c8sectpfe_stop_feed);
+	if (ret) {
+		dev_err(dev, "c8sectpfe_tuner_register_frontend failed (%d)\n",
+			ret);
+		goto err_clk_disable;
+	}
+
+	/* ensure all other init has been done before requesting firmware */
+	ret = load_c8sectpfe_fw_step1(fei);
+	if (ret) {
+		dev_err(dev, "Couldn't load slim core firmware\n");
+		goto err_clk_disable;
+	}
+
+	c8sectpfe_debugfs_init(fei);
+
+	return 0;
+
+err_clk_disable:
+	/* TODO uncomment when upstream has taken a reference on this clk */
+	/*clk_disable_unprepare(fei->c8sectpfeclk);*/
+	return ret;
+}
+
+static int c8sectpfe_remove(struct platform_device *pdev)
+{
+	struct c8sectpfei *fei = platform_get_drvdata(pdev);
+	struct channel_info *channel;
+	int i;
+
+	wait_for_completion(&fei->fw_ack);
+
+	c8sectpfe_tuner_unregister_frontend(fei->c8sectpfe[0], fei);
+
+	/*
+	 * Now loop through and un-configure each of the InputBlock resources
+	 */
+	for (i = 0; i < fei->tsin_count; i++) {
+		channel = fei->channel_data[i];
+		free_input_block(fei, channel);
+	}
+
+	c8sectpfe_debugfs_exit(fei);
+
+	dev_info(fei->dev, "Stopping memdma SLIM core\n");
+	if (readl(fei->io + DMA_CPU_RUN))
+		writel(0x0,  fei->io + DMA_CPU_RUN);
+
+	/* unclock all internal IP's */
+	if (readl(fei->io + SYS_INPUT_CLKEN))
+		writel(0, fei->io + SYS_INPUT_CLKEN);
+
+	if (readl(fei->io + SYS_OTHER_CLKEN))
+		writel(0, fei->io + SYS_OTHER_CLKEN);
+
+	/* TODO uncomment when upstream has taken a reference on this clk */
+	/*
+	if (fei->c8sectpfeclk)
+		clk_disable_unprepare(fei->c8sectpfeclk);
+	*/
+
+	return 0;
+}
+
+
+static int configure_channels(struct c8sectpfei *fei)
+{
+	int index = 0, ret;
+	struct channel_info *tsin;
+	struct device_node *child, *np = fei->dev->of_node;
+
+	/* iterate round each tsin and configure memdma descriptor and IB hw */
+	for_each_child_of_node(np, child) {
+
+		tsin = fei->channel_data[index];
+
+		ret = configure_memdma_and_inputblock(fei,
+						fei->channel_data[index]);
+
+		if (ret) {
+			dev_err(fei->dev,
+				"configure_memdma_and_inputblock failed\n");
+			goto err_unmap;
+		}
+		index++;
+	}
+
+	return 0;
+
+err_unmap:
+	for (index = 0; index < fei->tsin_count; index++) {
+		tsin = fei->channel_data[index];
+		free_input_block(fei, tsin);
+	}
+	return ret;
+}
+
+static int
+c8sectpfe_elf_sanity_check(struct c8sectpfei *fei, const struct firmware *fw)
+{
+	struct elf32_hdr *ehdr;
+	char class;
+
+	if (!fw) {
+		dev_err(fei->dev, "failed to load %s\n", FIRMWARE_MEMDMA);
+		return -EINVAL;
+	}
+
+	if (fw->size < sizeof(struct elf32_hdr)) {
+		dev_err(fei->dev, "Image is too small\n");
+		return -EINVAL;
+	}
+
+	ehdr = (struct elf32_hdr *)fw->data;
+
+	/* We only support ELF32 at this point */
+	class = ehdr->e_ident[EI_CLASS];
+	if (class != ELFCLASS32) {
+		dev_err(fei->dev, "Unsupported class: %d\n", class);
+		return -EINVAL;
+	}
+
+	if (ehdr->e_ident[EI_DATA] != ELFDATA2LSB) {
+		dev_err(fei->dev, "Unsupported firmware endianness\n");
+		return -EINVAL;
+	}
+
+	if (fw->size < ehdr->e_shoff + sizeof(struct elf32_shdr)) {
+		dev_err(fei->dev, "Image is too small\n");
+		return -EINVAL;
+	}
+
+	if (memcmp(ehdr->e_ident, ELFMAG, SELFMAG)) {
+		dev_err(fei->dev, "Image is corrupted (bad magic)\n");
+		return -EINVAL;
+	}
+
+	/* Check ELF magic */
+	ehdr = (Elf32_Ehdr *)fw->data;
+	if (ehdr->e_ident[EI_MAG0] != ELFMAG0 ||
+	    ehdr->e_ident[EI_MAG1] != ELFMAG1 ||
+	    ehdr->e_ident[EI_MAG2] != ELFMAG2 ||
+	    ehdr->e_ident[EI_MAG3] != ELFMAG3) {
+		dev_err(fei->dev, "Invalid ELF magic\n");
+		return -EINVAL;
+	}
+
+	if (ehdr->e_type != ET_EXEC) {
+		dev_err(fei->dev, "Unsupported ELF header type\n");
+		return -EINVAL;
+	}
+
+	if (ehdr->e_phoff > fw->size) {
+		dev_err(fei->dev, "Firmware size is too small\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+
+static void load_imem_segment(struct c8sectpfei *fei, Elf32_Phdr *phdr,
+			const struct firmware *fw, u8 __iomem *dest,
+			int seg_num)
+{
+	const u8 *imem_src = fw->data + phdr->p_offset;
+	int i;
+
+	/*
+	 * For IMEM segments, the segment contains 24-bit
+	 * instructions which must be padded to 32-bit
+	 * instructions before being written. The written
+	 * segment is padded with NOP instructions.
+	 */
+
+	dev_dbg(fei->dev,
+		"Loading IMEM segment %d 0x%08x\n\t"
+		" (0x%x bytes) -> 0x%p (0x%x bytes)\n", seg_num,
+		phdr->p_paddr, phdr->p_filesz,
+		dest, phdr->p_memsz + phdr->p_memsz / 3);
+
+	for (i = 0; i < phdr->p_filesz; i++) {
+
+		writeb(readb((void __iomem *)imem_src), (void __iomem *)dest);
+
+		/* Every 3 bytes, add an additional
+		 * padding zero in destination */
+		if (i % 3 == 2) {
+			dest++;
+			writeb(0x00, (void __iomem *)dest);
+		}
+
+		dest++;
+		imem_src++;
+	}
+}
+
+static void load_dmem_segment(struct c8sectpfei *fei, Elf32_Phdr *phdr,
+			const struct firmware *fw, u8 __iomem *dst, int seg_num)
+{
+	/*
+	 * For DMEM segments copy the segment data from the ELF
+	 * file and pad segment with zeroes
+	 */
+
+	dev_dbg(fei->dev,
+		"Loading DMEM segment %d 0x%08x\n\t"
+		"(0x%x bytes) -> 0x%p (0x%x bytes)\n",
+		seg_num, phdr->p_paddr, phdr->p_filesz,
+		dst, phdr->p_memsz);
+
+	memcpy((void __force *)dst, (void *)fw->data + phdr->p_offset,
+		phdr->p_filesz);
+
+	memset((void __force *)dst + phdr->p_filesz, 0,
+		phdr->p_memsz - phdr->p_filesz);
+}
+
+static int load_slim_core_fw(const struct firmware *fw, void *context)
+{
+	struct c8sectpfei *fei = context;
+	Elf32_Ehdr *ehdr;
+	Elf32_Phdr *phdr;
+	u8 __iomem *dst;
+	int err = 0, i;
+
+	if (!fw || !context)
+		return -EINVAL;
+
+	ehdr = (Elf32_Ehdr *)fw->data;
+	phdr = (Elf32_Phdr *)(fw->data + ehdr->e_phoff);
+
+	/* go through the available ELF segments */
+	for (i = 0; i < ehdr->e_phnum; i++, phdr++) {
+
+		/* Only consider LOAD segments */
+		if (phdr->p_type != PT_LOAD)
+			continue;
+
+		/*
+		 * Check segment is contained within the fw->data buffer
+		 */
+		if (phdr->p_offset + phdr->p_filesz > fw->size) {
+			dev_err(fei->dev,
+				"Segment %d is outside of firmware file\n", i);
+			err = -EINVAL;
+			break;
+		}
+
+		/*
+		 * MEMDMA IMEM has executable flag set, otherwise load
+		 * this segment into DMEM.
+		 *
+		 */
+
+		if (phdr->p_flags & PF_X) {
+			dst = (u8 __iomem *) fei->io + DMA_MEMDMA_IMEM;
+			/*
+			 * The Slim ELF file uses 32-bit word addressing for
+			 * load offsets.
+			 */
+			dst += (phdr->p_paddr & 0xFFFFF) * sizeof(unsigned int);
+			load_imem_segment(fei, phdr, fw, dst, i);
+		} else {
+			dst = (u8 __iomem *) fei->io + DMA_MEMDMA_DMEM;
+			/*
+			 * The Slim ELF file uses 32-bit word addressing for
+			 * load offsets.
+			 */
+			dst += (phdr->p_paddr & 0xFFFFF) * sizeof(unsigned int);
+			load_dmem_segment(fei, phdr, fw, dst, i);
+		}
+	}
+
+	release_firmware(fw);
+	return err;
+}
+
+static void load_c8sectpfe_fw_cb(const struct firmware *fw, void *context)
+{
+	struct c8sectpfei *fei = context;
+	int err;
+
+	err = c8sectpfe_elf_sanity_check(fei, fw);
+	if (err) {
+		dev_err(fei->dev, "c8sectpfe_elf_sanity_check failed err=(%d)\n"
+			, err);
+		goto err;
+	}
+
+	err = load_slim_core_fw(fw, context);
+	if (err) {
+		dev_err(fei->dev, "load_slim_core_fw failed err=(%d)\n", err);
+		goto err;
+	}
+
+	/* now the firmware is loaded configure the input blocks */
+	err = configure_channels(fei);
+	if (err) {
+		dev_err(fei->dev, "configure_channels failed err=(%d)\n", err);
+		goto err;
+	}
+
+	/*
+	 * STBus target port can access IMEM and DMEM ports
+	 * without waiting for CPU
+	 */
+	writel(0x1, fei->io + DMA_PER_STBUS_SYNC);
+
+	dev_info(fei->dev, "Boot the memdma SLIM core\n");
+	writel(0x1,  fei->io + DMA_CPU_RUN);
+
+	atomic_set(&fei->fw_loaded, 1);
+err:
+	complete_all(&fei->fw_ack);
+}
+
+static int load_c8sectpfe_fw_step1(struct c8sectpfei *fei)
+{
+	int err;
+
+	dev_info(fei->dev, "Loading firmware: %s\n", FIRMWARE_MEMDMA);
+
+	init_completion(&fei->fw_ack);
+	atomic_set(&fei->fw_loaded, 0);
+
+	err = request_firmware_nowait(THIS_MODULE, FW_ACTION_HOTPLUG,
+				FIRMWARE_MEMDMA, fei->dev, GFP_KERNEL, fei,
+				load_c8sectpfe_fw_cb);
+
+	if (err) {
+		dev_err(fei->dev, "request_firmware_nowait err: %d.\n", err);
+		complete_all(&fei->fw_ack);
+		return err;
+	}
+
+	return 0;
+}
+
+static const struct of_device_id c8sectpfe_match[] = {
+	{ .compatible = "st,stih407-c8sectpfe" },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, c8sectpfe_match);
+
+static struct platform_driver c8sectpfe_driver = {
+	.driver = {
+		.name = "c8sectpfe",
+		.of_match_table = of_match_ptr(c8sectpfe_match),
+	},
+	.probe	= c8sectpfe_probe,
+	.remove	= c8sectpfe_remove,
+};
+
+module_platform_driver(c8sectpfe_driver);
+
+MODULE_AUTHOR("Peter Bennett <peter.bennett@st.com>");
+MODULE_AUTHOR("Peter Griffin <peter.griffin@linaro.org>");
+MODULE_DESCRIPTION("C8SECTPFE STi DVB Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/sti/c8sectpfe/c8sectpfe-core.h b/drivers/media/platform/sti/c8sectpfe/c8sectpfe-core.h
new file mode 100644
index 0000000..39e7a22
--- /dev/null
+++ b/drivers/media/platform/sti/c8sectpfe/c8sectpfe-core.h
@@ -0,0 +1,288 @@
+/*
+ * c8sectpfe-core.h - C8SECTPFE STi DVB driver
+ *
+ * Copyright (c) STMicroelectronics 2015
+ *
+ *   Author:Peter Bennett <peter.bennett@st.com>
+ *	    Peter Griffin <peter.griffin@linaro.org>
+ *
+ *	This program is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU General Public License as
+ *	published by the Free Software Foundation; either version 2 of
+ *	the License, or (at your option) any later version.
+ */
+#ifndef _C8SECTPFE_CORE_H_
+#define _C8SECTPFE_CORE_H_
+
+#define C8SECTPFEI_MAXCHANNEL 16
+#define C8SECTPFEI_MAXADAPTER 3
+
+#define C8SECTPFE_MAX_TSIN_CHAN 8
+
+struct channel_info {
+
+	int tsin_id;
+	bool invert_ts_clk;
+	bool serial_not_parallel;
+	bool async_not_sync;
+	int i2c;
+	int dvb_card;
+
+	int rst_gpio;
+
+	struct i2c_adapter  *i2c_adapter;
+	struct i2c_adapter  *tuner_i2c;
+	struct i2c_adapter  *lnb_i2c;
+	struct i2c_client   *i2c_client;
+	struct dvb_frontend *frontend;
+
+	struct pinctrl_state *pstate;
+
+	int demux_mapping;
+	int active;
+
+	void *back_buffer_start;
+	void *back_buffer_aligned;
+	dma_addr_t back_buffer_busaddr;
+
+	void *pid_buffer_start;
+	void *pid_buffer_aligned;
+	dma_addr_t pid_buffer_busaddr;
+
+	unsigned long  fifo;
+
+	struct completion idle_completion;
+	struct tasklet_struct tsklet;
+
+	struct c8sectpfei *fei;
+	void __iomem *irec;
+
+};
+
+struct c8sectpfe_hw {
+	int num_ib;
+	int num_mib;
+	int num_swts;
+	int num_tsout;
+	int num_ccsc;
+	int num_ram;
+	int num_tp;
+};
+
+struct c8sectpfei {
+
+	struct device *dev;
+	struct pinctrl *pinctrl;
+
+	struct dentry *root;
+	struct debugfs_regset32	*regset;
+	struct completion fw_ack;
+	atomic_t fw_loaded;
+
+	int tsin_count;
+
+	struct c8sectpfe_hw hw_stats;
+
+	struct c8sectpfe *c8sectpfe[C8SECTPFEI_MAXADAPTER];
+
+	int mapping[C8SECTPFEI_MAXCHANNEL];
+
+	struct mutex lock;
+
+	struct timer_list timer;	/* timer interrupts for outputs */
+
+	void __iomem *io;
+	void __iomem *sram;
+
+	unsigned long sram_size;
+
+	struct channel_info *channel_data[C8SECTPFE_MAX_TSIN_CHAN];
+
+	struct clk *c8sectpfeclk;
+	int nima_rst_gpio;
+	int nimb_rst_gpio;
+
+	int idle_irq;
+	int error_irq;
+
+	int global_feed_count;
+};
+
+/* C8SECTPFE SYS Regs list */
+
+#define SYS_INPUT_ERR_STATUS	0x0
+#define SYS_OTHER_ERR_STATUS	0x8
+#define SYS_INPUT_ERR_MASK	0x10
+#define SYS_OTHER_ERR_MASK	0x18
+#define SYS_DMA_ROUTE		0x20
+#define SYS_INPUT_CLKEN		0x30
+#define IBENABLE_MASK			0x7F
+
+#define SYS_OTHER_CLKEN		0x38
+#define TSDMAENABLE			BIT(1)
+#define MEMDMAENABLE			BIT(0)
+
+#define SYS_CFG_NUM_IB		0x200
+#define SYS_CFG_NUM_MIB		0x204
+#define SYS_CFG_NUM_SWTS	0x208
+#define SYS_CFG_NUM_TSOUT	0x20C
+#define SYS_CFG_NUM_CCSC	0x210
+#define SYS_CFG_NUM_RAM		0x214
+#define SYS_CFG_NUM_TP		0x218
+
+/* Input Block Regs */
+
+#define C8SECTPFE_INPUTBLK_OFFSET	0x1000
+#define C8SECTPFE_CHANNEL_OFFSET(x)	((x*0x40) + C8SECTPFE_INPUTBLK_OFFSET)
+
+#define C8SECTPFE_IB_IP_FMT_CFG(x)      (C8SECTPFE_CHANNEL_OFFSET(x) + 0x00)
+#define C8SECTPFE_IGNORE_ERR_AT_SOP     BIT(7)
+#define C8SECTPFE_IGNORE_ERR_IN_PKT     BIT(6)
+#define C8SECTPFE_IGNORE_ERR_IN_BYTE    BIT(5)
+#define C8SECTPFE_INVERT_TSCLK          BIT(4)
+#define C8SECTPFE_ALIGN_BYTE_SOP        BIT(3)
+#define C8SECTPFE_ASYNC_NOT_SYNC        BIT(2)
+#define C8SECTPFE_BYTE_ENDIANNESS_MSB    BIT(1)
+#define C8SECTPFE_SERIAL_NOT_PARALLEL   BIT(0)
+
+#define C8SECTPFE_IB_SYNCLCKDRP_CFG(x)   (C8SECTPFE_CHANNEL_OFFSET(x) + 0x04)
+#define C8SECTPFE_SYNC(x)                (x & 0xf)
+#define C8SECTPFE_DROP(x)                ((x<<4) & 0xf)
+#define C8SECTPFE_TOKEN(x)               ((x<<8) & 0xff00)
+#define C8SECTPFE_SLDENDIANNESS          BIT(16)
+
+#define C8SECTPFE_IB_TAGBYTES_CFG(x)     (C8SECTPFE_CHANNEL_OFFSET(x) + 0x08)
+#define C8SECTPFE_TAG_HEADER(x)          (x << 16)
+#define C8SECTPFE_TAG_COUNTER(x)         ((x<<1) & 0x7fff)
+#define C8SECTPFE_TAG_ENABLE             BIT(0)
+
+#define C8SECTPFE_IB_PID_SET(x)          (C8SECTPFE_CHANNEL_OFFSET(x) + 0x0C)
+#define C8SECTPFE_PID_OFFSET(x)          (x & 0x3f)
+#define C8SECTPFE_PID_NUMBITS(x)         ((x << 6) & 0xfff)
+#define C8SECTPFE_PID_ENABLE             BIT(31)
+
+#define C8SECTPFE_IB_PKT_LEN(x)          (C8SECTPFE_CHANNEL_OFFSET(x) + 0x10)
+
+#define C8SECTPFE_IB_BUFF_STRT(x)        (C8SECTPFE_CHANNEL_OFFSET(x) + 0x14)
+#define C8SECTPFE_IB_BUFF_END(x)         (C8SECTPFE_CHANNEL_OFFSET(x) + 0x18)
+#define C8SECTPFE_IB_READ_PNT(x)         (C8SECTPFE_CHANNEL_OFFSET(x) + 0x1C)
+#define C8SECTPFE_IB_WRT_PNT(x)          (C8SECTPFE_CHANNEL_OFFSET(x) + 0x20)
+
+#define C8SECTPFE_IB_PRI_THRLD(x)        (C8SECTPFE_CHANNEL_OFFSET(x) + 0x24)
+#define C8SECTPFE_PRI_VALUE(x)           (x & 0x7fffff)
+#define C8SECTPFE_PRI_LOWPRI(x)          ((x & 0xf) << 24)
+#define C8SECTPFE_PRI_HIGHPRI(x)         ((x & 0xf) << 28)
+
+#define C8SECTPFE_IB_STAT(x)             (C8SECTPFE_CHANNEL_OFFSET(x) + 0x28)
+#define C8SECTPFE_STAT_FIFO_OVERFLOW(x)  (x & 0x1)
+#define C8SECTPFE_STAT_BUFFER_OVERFLOW(x) (x & 0x2)
+#define C8SECTPFE_STAT_OUTOFORDERRP(x)   (x & 0x4)
+#define C8SECTPFE_STAT_PID_OVERFLOW(x)   (x & 0x8)
+#define C8SECTPFE_STAT_PKT_OVERFLOW(x)   (x & 0x10)
+#define C8SECTPFE_STAT_ERROR_PACKETS(x)  ((x >> 8) & 0xf)
+#define C8SECTPFE_STAT_SHORT_PACKETS(x)  ((x >> 12) & 0xf)
+
+#define C8SECTPFE_IB_MASK(x)             (C8SECTPFE_CHANNEL_OFFSET(x) + 0x2C)
+#define C8SECTPFE_MASK_FIFO_OVERFLOW     BIT(0)
+#define C8SECTPFE_MASK_BUFFER_OVERFLOW   BIT(1)
+#define C8SECTPFE_MASK_OUTOFORDERRP(x)   BIT(2)
+#define C8SECTPFE_MASK_PID_OVERFLOW(x)   BIT(3)
+#define C8SECTPFE_MASK_PKT_OVERFLOW(x)   BIT(4)
+#define C8SECTPFE_MASK_ERROR_PACKETS(x)  ((x & 0xf) << 8)
+#define C8SECTPFE_MASK_SHORT_PACKETS(x)  ((x & 0xf) >> 12)
+
+#define C8SECTPFE_IB_SYS(x)              (C8SECTPFE_CHANNEL_OFFSET(x) + 0x30)
+#define C8SECTPFE_SYS_RESET              BIT(1)
+#define C8SECTPFE_SYS_ENABLE             BIT(0)
+
+/*
+ * Ponter record data structure required for each input block
+ * see Table 82 on page 167 of functional specification.
+ */
+
+#define DMA_PRDS_MEMBASE	0x0 /* Internal sram base address */
+#define DMA_PRDS_MEMTOP		0x4 /* Internal sram top address */
+
+/*
+ * TS packet size, including tag bytes added by input block,
+ * rounded up to the next multiple of 8 bytes. The packet size,
+ * including any tagging bytes and rounded up to the nearest
+ * multiple of 8 bytes must be less than 255 bytes.
+ */
+#define DMA_PRDS_PKTSIZE	0x8
+#define DMA_PRDS_TPENABLE	0xc
+
+#define TP0_OFFSET		0x10
+#define DMA_PRDS_BUSBASE_TP(x)	((0x10*x) + TP0_OFFSET)
+#define DMA_PRDS_BUSTOP_TP(x)	((0x10*x) + TP0_OFFSET + 0x4)
+#define DMA_PRDS_BUSWP_TP(x)	((0x10*x) + TP0_OFFSET + 0x8)
+#define DMA_PRDS_BUSRP_TP(x)	((0x10*x) + TP0_OFFSET + 0xc)
+
+#define DMA_PRDS_SIZE		(0x20)
+
+#define DMA_MEMDMA_OFFSET	0x4000
+#define DMA_IMEM_OFFSET		0x0
+#define DMA_DMEM_OFFSET		0x4000
+#define DMA_CPU			0x8000
+#define DMA_PER_OFFSET		0xb000
+
+#define DMA_MEMDMA_DMEM (DMA_MEMDMA_OFFSET + DMA_DMEM_OFFSET)
+#define DMA_MEMDMA_IMEM (DMA_MEMDMA_OFFSET + DMA_IMEM_OFFSET)
+
+/* XP70 Slim core regs */
+#define DMA_CPU_ID	(DMA_MEMDMA_OFFSET + DMA_CPU + 0x0)
+#define DMA_CPU_VCR	(DMA_MEMDMA_OFFSET + DMA_CPU + 0x4)
+#define DMA_CPU_RUN	(DMA_MEMDMA_OFFSET + DMA_CPU + 0x8)
+#define DMA_CPU_CLOCKGATE	(DMA_MEMDMA_OFFSET + DMA_CPU + 0xc)
+#define DMA_CPU_PC	(DMA_MEMDMA_OFFSET + DMA_CPU + 0x20)
+
+/* Enable Interrupt for a IB */
+#define DMA_PER_TPn_DREQ_MASK	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xd00)
+/* Ack interrupt by setting corresponding bit */
+#define DMA_PER_TPn_DACK_SET	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xd80)
+#define DMA_PER_TPn_DREQ	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xe00)
+#define DMA_PER_TPn_DACK	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xe80)
+#define DMA_PER_DREQ_MODE	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xf80)
+#define DMA_PER_STBUS_SYNC	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xf88)
+#define DMA_PER_STBUS_ACCESS	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xf8c)
+#define DMA_PER_STBUS_ADDRESS	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xf90)
+#define DMA_PER_IDLE_INT	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xfa8)
+#define DMA_PER_PRIORITY	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xfac)
+#define DMA_PER_MAX_OPCODE	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xfb0)
+#define DMA_PER_MAX_CHUNK	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xfb4)
+#define DMA_PER_PAGE_SIZE	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xfbc)
+#define DMA_PER_MBOX_STATUS	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xfc0)
+#define DMA_PER_MBOX_SET	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xfc8)
+#define DMA_PER_MBOX_CLEAR	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xfd0)
+#define DMA_PER_MBOX_MASK	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xfd8)
+#define DMA_PER_INJECT_PKT_SRC	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xfe0)
+#define DMA_PER_INJECT_PKT_DEST	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xfe4)
+#define DMA_PER_INJECT_PKT_ADDR	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xfe8)
+#define DMA_PER_INJECT_PKT	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xfec)
+#define DMA_PER_PAT_PTR_INIT	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xff0)
+#define DMA_PER_PAT_PTR		(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xff4)
+#define DMA_PER_SLEEP_MASK	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xff8)
+#define DMA_PER_SLEEP_COUNTER	(DMA_MEMDMA_OFFSET + DMA_PER_OFFSET + 0xffc)
+/* #define DMA_RF_CPUREGn	DMA_RFBASEADDR n=0 to 15) slim regsa */
+
+/* The following are from DMA_DMEM_BaseAddress */
+#define DMA_FIRMWARE_VERSION	(DMA_MEMDMA_OFFSET + DMA_DMEM_OFFSET + 0x0)
+#define DMA_PTRREC_BASE		(DMA_MEMDMA_OFFSET + DMA_DMEM_OFFSET + 0x4)
+#define DMA_PTRREC_INPUT_OFFSET	(DMA_MEMDMA_OFFSET + DMA_DMEM_OFFSET + 0x8)
+#define DMA_ERRREC_BASE		(DMA_MEMDMA_OFFSET + DMA_DMEM_OFFSET + 0xc)
+#define DMA_ERROR_RECORD(n)	((n*4) + DMA_ERRREC_BASE + 0x4)
+#define DMA_IDLE_REQ		(DMA_MEMDMA_OFFSET + DMA_DMEM_OFFSET + 0x10)
+#define IDLEREQ			BIT(31)
+
+#define DMA_FIRMWARE_CONFIG	(DMA_MEMDMA_OFFSET + DMA_DMEM_OFFSET + 0x14)
+
+/* Regs for PID Filter */
+
+#define PIDF_OFFSET		0x2800
+#define PIDF_BASE(n)		((n*4) + PIDF_OFFSET)
+#define PIDF_LEAK_ENABLE	(PIDF_OFFSET + 0x100)
+#define PIDF_LEAK_STATUS	(PIDF_OFFSET + 0x108)
+#define PIDF_LEAK_COUNT_RESET	(PIDF_OFFSET + 0x110)
+#define PIDF_LEAK_COUNTER	(PIDF_OFFSET + 0x114)
+
+#endif /* _C8SECTPFE_CORE_H_ */
diff --git a/drivers/media/platform/sti/c8sectpfe/c8sectpfe-debugfs.c b/drivers/media/platform/sti/c8sectpfe/c8sectpfe-debugfs.c
new file mode 100644
index 0000000..e9ba13d
--- /dev/null
+++ b/drivers/media/platform/sti/c8sectpfe/c8sectpfe-debugfs.c
@@ -0,0 +1,271 @@
+/*
+ * c8sectpfe-debugfs.c - C8SECTPFE STi DVB driver
+ *
+ * Copyright (c) STMicroelectronics 2015
+ *
+ * Author: Peter Griffin <peter.griffin@linaro.org>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2  of
+ * the License as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+#include <linux/debugfs.h>
+#include <linux/device.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/seq_file.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+
+#include "c8sectpfe-debugfs.h"
+
+#define dump_register(nm ...)			\
+{						\
+	.name	= #nm,				\
+	.offset	= nm,				\
+}
+
+static const struct debugfs_reg32 fei_sys_regs[] = {
+	dump_register(SYS_INPUT_ERR_STATUS),
+	dump_register(SYS_OTHER_ERR_STATUS),
+	dump_register(SYS_INPUT_ERR_MASK),
+	dump_register(SYS_DMA_ROUTE),
+	dump_register(SYS_INPUT_CLKEN),
+	dump_register(IBENABLE_MASK),
+	dump_register(SYS_OTHER_CLKEN),
+	dump_register(SYS_CFG_NUM_IB),
+	dump_register(SYS_CFG_NUM_MIB),
+	dump_register(SYS_CFG_NUM_SWTS),
+	dump_register(SYS_CFG_NUM_TSOUT),
+	dump_register(SYS_CFG_NUM_CCSC),
+	dump_register(SYS_CFG_NUM_RAM),
+	dump_register(SYS_CFG_NUM_TP),
+
+	dump_register(C8SECTPFE_IB_IP_FMT_CFG(0)),
+	dump_register(C8SECTPFE_IB_TAGBYTES_CFG(0)),
+	dump_register(C8SECTPFE_IB_PID_SET(0)),
+	dump_register(C8SECTPFE_IB_PKT_LEN(0)),
+	dump_register(C8SECTPFE_IB_BUFF_STRT(0)),
+	dump_register(C8SECTPFE_IB_BUFF_END(0)),
+	dump_register(C8SECTPFE_IB_READ_PNT(0)),
+	dump_register(C8SECTPFE_IB_WRT_PNT(0)),
+	dump_register(C8SECTPFE_IB_PRI_THRLD(0)),
+	dump_register(C8SECTPFE_IB_STAT(0)),
+	dump_register(C8SECTPFE_IB_MASK(0)),
+	dump_register(C8SECTPFE_IB_SYS(0)),
+
+	dump_register(C8SECTPFE_IB_IP_FMT_CFG(1)),
+	dump_register(C8SECTPFE_IB_TAGBYTES_CFG(1)),
+	dump_register(C8SECTPFE_IB_PID_SET(1)),
+	dump_register(C8SECTPFE_IB_PKT_LEN(1)),
+	dump_register(C8SECTPFE_IB_BUFF_STRT(1)),
+	dump_register(C8SECTPFE_IB_BUFF_END(1)),
+	dump_register(C8SECTPFE_IB_READ_PNT(1)),
+	dump_register(C8SECTPFE_IB_WRT_PNT(1)),
+	dump_register(C8SECTPFE_IB_PRI_THRLD(1)),
+	dump_register(C8SECTPFE_IB_STAT(1)),
+	dump_register(C8SECTPFE_IB_MASK(1)),
+	dump_register(C8SECTPFE_IB_SYS(1)),
+
+	dump_register(C8SECTPFE_IB_IP_FMT_CFG(2)),
+	dump_register(C8SECTPFE_IB_TAGBYTES_CFG(2)),
+	dump_register(C8SECTPFE_IB_PID_SET(2)),
+	dump_register(C8SECTPFE_IB_PKT_LEN(2)),
+	dump_register(C8SECTPFE_IB_BUFF_STRT(2)),
+	dump_register(C8SECTPFE_IB_BUFF_END(2)),
+	dump_register(C8SECTPFE_IB_READ_PNT(2)),
+	dump_register(C8SECTPFE_IB_WRT_PNT(2)),
+	dump_register(C8SECTPFE_IB_PRI_THRLD(2)),
+	dump_register(C8SECTPFE_IB_STAT(2)),
+	dump_register(C8SECTPFE_IB_MASK(2)),
+	dump_register(C8SECTPFE_IB_SYS(2)),
+
+	dump_register(C8SECTPFE_IB_IP_FMT_CFG(3)),
+	dump_register(C8SECTPFE_IB_TAGBYTES_CFG(3)),
+	dump_register(C8SECTPFE_IB_PID_SET(3)),
+	dump_register(C8SECTPFE_IB_PKT_LEN(3)),
+	dump_register(C8SECTPFE_IB_BUFF_STRT(3)),
+	dump_register(C8SECTPFE_IB_BUFF_END(3)),
+	dump_register(C8SECTPFE_IB_READ_PNT(3)),
+	dump_register(C8SECTPFE_IB_WRT_PNT(3)),
+	dump_register(C8SECTPFE_IB_PRI_THRLD(3)),
+	dump_register(C8SECTPFE_IB_STAT(3)),
+	dump_register(C8SECTPFE_IB_MASK(3)),
+	dump_register(C8SECTPFE_IB_SYS(3)),
+
+	dump_register(C8SECTPFE_IB_IP_FMT_CFG(4)),
+	dump_register(C8SECTPFE_IB_TAGBYTES_CFG(4)),
+	dump_register(C8SECTPFE_IB_PID_SET(4)),
+	dump_register(C8SECTPFE_IB_PKT_LEN(4)),
+	dump_register(C8SECTPFE_IB_BUFF_STRT(4)),
+	dump_register(C8SECTPFE_IB_BUFF_END(4)),
+	dump_register(C8SECTPFE_IB_READ_PNT(4)),
+	dump_register(C8SECTPFE_IB_WRT_PNT(4)),
+	dump_register(C8SECTPFE_IB_PRI_THRLD(4)),
+	dump_register(C8SECTPFE_IB_STAT(4)),
+	dump_register(C8SECTPFE_IB_MASK(4)),
+	dump_register(C8SECTPFE_IB_SYS(4)),
+
+	dump_register(C8SECTPFE_IB_IP_FMT_CFG(5)),
+	dump_register(C8SECTPFE_IB_TAGBYTES_CFG(5)),
+	dump_register(C8SECTPFE_IB_PID_SET(5)),
+	dump_register(C8SECTPFE_IB_PKT_LEN(5)),
+	dump_register(C8SECTPFE_IB_BUFF_STRT(5)),
+	dump_register(C8SECTPFE_IB_BUFF_END(5)),
+	dump_register(C8SECTPFE_IB_READ_PNT(5)),
+	dump_register(C8SECTPFE_IB_WRT_PNT(5)),
+	dump_register(C8SECTPFE_IB_PRI_THRLD(5)),
+	dump_register(C8SECTPFE_IB_STAT(5)),
+	dump_register(C8SECTPFE_IB_MASK(5)),
+	dump_register(C8SECTPFE_IB_SYS(5)),
+
+	dump_register(C8SECTPFE_IB_IP_FMT_CFG(6)),
+	dump_register(C8SECTPFE_IB_TAGBYTES_CFG(6)),
+	dump_register(C8SECTPFE_IB_PID_SET(6)),
+	dump_register(C8SECTPFE_IB_PKT_LEN(6)),
+	dump_register(C8SECTPFE_IB_BUFF_STRT(6)),
+	dump_register(C8SECTPFE_IB_BUFF_END(6)),
+	dump_register(C8SECTPFE_IB_READ_PNT(6)),
+	dump_register(C8SECTPFE_IB_WRT_PNT(6)),
+	dump_register(C8SECTPFE_IB_PRI_THRLD(6)),
+	dump_register(C8SECTPFE_IB_STAT(6)),
+	dump_register(C8SECTPFE_IB_MASK(6)),
+	dump_register(C8SECTPFE_IB_SYS(6)),
+
+	dump_register(DMA_CPU_ID),
+	dump_register(DMA_CPU_VCR),
+	dump_register(DMA_CPU_RUN),
+	dump_register(DMA_CPU_PC),
+
+	dump_register(DMA_PER_TPn_DREQ_MASK),
+	dump_register(DMA_PER_TPn_DACK_SET),
+	dump_register(DMA_PER_TPn_DREQ),
+	dump_register(DMA_PER_TPn_DACK),
+	dump_register(DMA_PER_DREQ_MODE),
+	dump_register(DMA_PER_STBUS_SYNC),
+	dump_register(DMA_PER_STBUS_ACCESS),
+	dump_register(DMA_PER_STBUS_ADDRESS),
+	dump_register(DMA_PER_IDLE_INT),
+	dump_register(DMA_PER_PRIORITY),
+	dump_register(DMA_PER_MAX_OPCODE),
+	dump_register(DMA_PER_MAX_CHUNK),
+	dump_register(DMA_PER_PAGE_SIZE),
+	dump_register(DMA_PER_MBOX_STATUS),
+	dump_register(DMA_PER_MBOX_SET),
+	dump_register(DMA_PER_MBOX_CLEAR),
+	dump_register(DMA_PER_MBOX_MASK),
+	dump_register(DMA_PER_INJECT_PKT_SRC),
+	dump_register(DMA_PER_INJECT_PKT_DEST),
+	dump_register(DMA_PER_INJECT_PKT_ADDR),
+	dump_register(DMA_PER_INJECT_PKT),
+	dump_register(DMA_PER_PAT_PTR_INIT),
+	dump_register(DMA_PER_PAT_PTR),
+	dump_register(DMA_PER_SLEEP_MASK),
+	dump_register(DMA_PER_SLEEP_COUNTER),
+
+	dump_register(DMA_FIRMWARE_VERSION),
+	dump_register(DMA_PTRREC_BASE),
+	dump_register(DMA_PTRREC_INPUT_OFFSET),
+	dump_register(DMA_ERRREC_BASE),
+
+	dump_register(DMA_ERROR_RECORD(0)),
+	dump_register(DMA_ERROR_RECORD(1)),
+	dump_register(DMA_ERROR_RECORD(2)),
+	dump_register(DMA_ERROR_RECORD(3)),
+	dump_register(DMA_ERROR_RECORD(4)),
+	dump_register(DMA_ERROR_RECORD(5)),
+	dump_register(DMA_ERROR_RECORD(6)),
+	dump_register(DMA_ERROR_RECORD(7)),
+	dump_register(DMA_ERROR_RECORD(8)),
+	dump_register(DMA_ERROR_RECORD(9)),
+	dump_register(DMA_ERROR_RECORD(10)),
+	dump_register(DMA_ERROR_RECORD(11)),
+	dump_register(DMA_ERROR_RECORD(12)),
+	dump_register(DMA_ERROR_RECORD(13)),
+	dump_register(DMA_ERROR_RECORD(14)),
+	dump_register(DMA_ERROR_RECORD(15)),
+	dump_register(DMA_ERROR_RECORD(16)),
+	dump_register(DMA_ERROR_RECORD(17)),
+	dump_register(DMA_ERROR_RECORD(18)),
+	dump_register(DMA_ERROR_RECORD(19)),
+	dump_register(DMA_ERROR_RECORD(20)),
+	dump_register(DMA_ERROR_RECORD(21)),
+	dump_register(DMA_ERROR_RECORD(22)),
+
+	dump_register(DMA_IDLE_REQ),
+	dump_register(DMA_FIRMWARE_CONFIG),
+
+	dump_register(PIDF_BASE(0)),
+	dump_register(PIDF_BASE(1)),
+	dump_register(PIDF_BASE(2)),
+	dump_register(PIDF_BASE(3)),
+	dump_register(PIDF_BASE(4)),
+	dump_register(PIDF_BASE(5)),
+	dump_register(PIDF_BASE(6)),
+	dump_register(PIDF_BASE(7)),
+	dump_register(PIDF_BASE(8)),
+	dump_register(PIDF_BASE(9)),
+	dump_register(PIDF_BASE(10)),
+	dump_register(PIDF_BASE(11)),
+	dump_register(PIDF_BASE(12)),
+	dump_register(PIDF_BASE(13)),
+	dump_register(PIDF_BASE(14)),
+	dump_register(PIDF_BASE(15)),
+	dump_register(PIDF_BASE(16)),
+	dump_register(PIDF_BASE(17)),
+	dump_register(PIDF_BASE(18)),
+	dump_register(PIDF_BASE(19)),
+	dump_register(PIDF_BASE(20)),
+	dump_register(PIDF_BASE(21)),
+	dump_register(PIDF_BASE(22)),
+	dump_register(PIDF_LEAK_ENABLE),
+	dump_register(PIDF_LEAK_STATUS),
+	dump_register(PIDF_LEAK_COUNT_RESET),
+	dump_register(PIDF_LEAK_COUNTER),
+};
+
+void c8sectpfe_debugfs_init(struct c8sectpfei *fei)
+{
+	struct dentry		*root;
+	struct dentry		*file;
+
+	root = debugfs_create_dir("c8sectpfe", NULL);
+	if (!root)
+		goto err;
+
+	fei->root = root;
+
+	fei->regset =  devm_kzalloc(fei->dev, sizeof(*fei->regset), GFP_KERNEL);
+	if (!fei->regset)
+		goto err;
+
+	fei->regset->regs = fei_sys_regs;
+	fei->regset->nregs = ARRAY_SIZE(fei_sys_regs);
+	fei->regset->base = fei->io;
+
+	file = debugfs_create_regset32("registers", S_IRUGO, root,
+				fei->regset);
+	if (!file) {
+		dev_err(fei->dev,
+			"%s not able to create 'registers' debugfs\n"
+			, __func__);
+		goto err;
+	}
+
+	return;
+
+err:
+	debugfs_remove_recursive(root);
+}
+
+void c8sectpfe_debugfs_exit(struct c8sectpfei *fei)
+{
+	debugfs_remove_recursive(fei->root);
+	fei->root = NULL;
+}
diff --git a/drivers/media/platform/sti/c8sectpfe/c8sectpfe-debugfs.h b/drivers/media/platform/sti/c8sectpfe/c8sectpfe-debugfs.h
new file mode 100644
index 0000000..8af1ac1
--- /dev/null
+++ b/drivers/media/platform/sti/c8sectpfe/c8sectpfe-debugfs.h
@@ -0,0 +1,26 @@
+/**
+ * c8sectpfe-debugfs.h - C8SECTPFE STi DVB driver debugfs header
+ *
+ * Copyright (c) STMicroelectronics 2015
+ *
+ * Authors: Peter Griffin <peter.griffin@linaro.org>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2  of
+ * the License as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef __C8SECTPFE_DEBUG_H
+#define __C8SECTPFE_DEBUG_H
+
+#include "c8sectpfe-core.h"
+
+void c8sectpfe_debugfs_init(struct c8sectpfei *);
+void c8sectpfe_debugfs_exit(struct c8sectpfei *);
+
+#endif /* __C8SECTPFE_DEBUG_H */
diff --git a/drivers/media/platform/sti/c8sectpfe/c8sectpfe-dvb.c b/drivers/media/platform/sti/c8sectpfe/c8sectpfe-dvb.c
new file mode 100644
index 0000000..69d7fe4
--- /dev/null
+++ b/drivers/media/platform/sti/c8sectpfe/c8sectpfe-dvb.c
@@ -0,0 +1,244 @@
+/*
+ *  c8sectpfe-dvb.c - C8SECTPFE STi DVB driver
+ *
+ * Copyright (c) STMicroelectronics 2015
+ *
+ *  Author Peter Griffin <peter.griffin@linaro.org>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *
+ *  GNU General Public License for more details.
+ */
+#include <linux/completion.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/version.h>
+
+#include <dt-bindings/media/c8sectpfe.h>
+
+#include "c8sectpfe-common.h"
+#include "c8sectpfe-core.h"
+#include "c8sectpfe-dvb.h"
+
+#include "dvb-pll.h"
+#include "lnbh24.h"
+#include "stv0367.h"
+#include "stv0367_priv.h"
+#include "stv6110x.h"
+#include "stv090x.h"
+#include "tda18212.h"
+
+static inline const char *dvb_card_str(unsigned int c)
+{
+	switch (c) {
+	case STV0367_TDA18212_NIMA_1:	return "STV0367_TDA18212_NIMA_1";
+	case STV0367_TDA18212_NIMA_2:	return "STV0367_TDA18212_NIMA_2";
+	case STV0367_TDA18212_NIMB_1:	return "STV0367_TDA18212_NIMB_1";
+	case STV0367_TDA18212_NIMB_2:	return "STV0367_TDA18212_NIMB_2";
+	case STV0903_6110_LNB24_NIMA:	return "STV0903_6110_LNB24_NIMA";
+	case STV0903_6110_LNB24_NIMB:	return "STV0903_6110_LNB24_NIMB";
+	default:			return "unknown dvb frontend card";
+	}
+}
+
+static struct stv090x_config stv090x_config = {
+	.device                 = STV0903,
+	.demod_mode             = STV090x_SINGLE,
+	.clk_mode               = STV090x_CLK_EXT,
+	.xtal                   = 16000000,
+	.address                = 0x69,
+
+	.ts1_mode               = STV090x_TSMODE_SERIAL_CONTINUOUS,
+	.ts2_mode               = STV090x_TSMODE_SERIAL_CONTINUOUS,
+
+	.repeater_level         = STV090x_RPTLEVEL_64,
+
+	.tuner_init             = NULL,
+	.tuner_set_mode         = NULL,
+	.tuner_set_frequency    = NULL,
+	.tuner_get_frequency    = NULL,
+	.tuner_set_bandwidth    = NULL,
+	.tuner_get_bandwidth    = NULL,
+	.tuner_set_bbgain       = NULL,
+	.tuner_get_bbgain       = NULL,
+	.tuner_set_refclk       = NULL,
+	.tuner_get_status       = NULL,
+};
+
+static struct stv6110x_config stv6110x_config = {
+	.addr                   = 0x60,
+	.refclk                 = 16000000,
+};
+
+#define NIMA 0
+#define NIMB 1
+
+static struct stv0367_config stv0367_tda18212_config[] = {
+	{
+		.demod_address = 0x1c,
+		.xtal = 16000000,
+		.if_khz = 4500,
+		.if_iq_mode = FE_TER_NORMAL_IF_TUNER,
+		.ts_mode = STV0367_SERIAL_PUNCT_CLOCK,
+		.clk_pol = STV0367_CLOCKPOLARITY_DEFAULT,
+	}, {
+		.demod_address = 0x1d,
+		.xtal = 16000000,
+		.if_khz = 4500,
+		.if_iq_mode = FE_TER_NORMAL_IF_TUNER,
+		.ts_mode = STV0367_SERIAL_PUNCT_CLOCK,
+		.clk_pol = STV0367_CLOCKPOLARITY_DEFAULT,
+	}, {
+		.demod_address = 0x1e,
+		.xtal = 16000000,
+		.if_khz = 4500,
+		.if_iq_mode = FE_TER_NORMAL_IF_TUNER,
+		.ts_mode = STV0367_SERIAL_PUNCT_CLOCK,
+		.clk_pol = STV0367_CLOCKPOLARITY_DEFAULT,
+	},
+};
+
+static struct tda18212_config tda18212_conf = {
+	.if_dvbt_6 = 4150,
+	.if_dvbt_7 = 4150,
+	.if_dvbt_8 = 4500,
+	.if_dvbc = 5000,
+};
+
+int c8sectpfe_frontend_attach(struct dvb_frontend **fe,
+		struct c8sectpfe *c8sectpfe,
+		struct channel_info *tsin, int chan_num)
+{
+	struct tda18212_config *tda18212;
+	struct stv6110x_devctl *fe2;
+	struct i2c_client *client;
+	struct i2c_board_info tda18212_info = {
+		.type = "tda18212",
+		.addr = 0x60,
+	};
+
+	if (!tsin)
+		return -EINVAL;
+
+	switch (tsin->dvb_card) {
+
+	case STV0367_TDA18212_NIMA_1:
+	case STV0367_TDA18212_NIMA_2:
+	case STV0367_TDA18212_NIMB_1:
+	case STV0367_TDA18212_NIMB_2:
+		if (tsin->dvb_card == STV0367_TDA18212_NIMA_1)
+			*fe = dvb_attach(stv0367ter_attach,
+				 &stv0367_tda18212_config[0],
+					tsin->i2c_adapter);
+		else if (tsin->dvb_card == STV0367_TDA18212_NIMB_1)
+			*fe = dvb_attach(stv0367ter_attach,
+				 &stv0367_tda18212_config[1],
+					tsin->i2c_adapter);
+		else
+			*fe = dvb_attach(stv0367ter_attach,
+				 &stv0367_tda18212_config[2],
+					tsin->i2c_adapter);
+
+		if (!*fe) {
+			dev_err(c8sectpfe->device,
+				"%s: stv0367ter_attach failed for NIM card %s\n"
+				, __func__, dvb_card_str(tsin->dvb_card));
+			return -ENODEV;
+		};
+
+		/*
+		 * init the demod so that i2c gate_ctrl
+		 * to the tuner works correctly
+		 */
+		(*fe)->ops.init(*fe);
+
+		/* Allocate the tda18212 structure */
+		tda18212 = devm_kzalloc(c8sectpfe->device,
+					sizeof(struct tda18212_config),
+					GFP_KERNEL);
+		if (!tda18212) {
+			dev_err(c8sectpfe->device,
+				"%s: devm_kzalloc failed\n", __func__);
+			return -ENOMEM;
+		}
+
+		memcpy(tda18212, &tda18212_conf,
+			sizeof(struct tda18212_config));
+
+		tda18212->fe = (*fe);
+
+		tda18212_info.platform_data = tda18212;
+
+		/* attach tuner */
+		request_module("tda18212");
+		client = i2c_new_device(tsin->i2c_adapter, &tda18212_info);
+		if (!client || !client->dev.driver) {
+			dvb_frontend_detach(*fe);
+			return -ENODEV;
+		}
+
+		if (!try_module_get(client->dev.driver->owner)) {
+			i2c_unregister_device(client);
+			dvb_frontend_detach(*fe);
+			return -ENODEV;
+		}
+
+		tsin->i2c_client = client;
+
+		break;
+
+	case STV0903_6110_LNB24_NIMA:
+		*fe = dvb_attach(stv090x_attach,	&stv090x_config,
+				tsin->i2c_adapter, STV090x_DEMODULATOR_0);
+		if (!*fe) {
+			dev_err(c8sectpfe->device, "%s: stv090x_attach failed\n"
+				"\tfor NIM card %s\n",
+				__func__, dvb_card_str(tsin->dvb_card));
+			return -ENODEV;
+		}
+
+		fe2 = dvb_attach(stv6110x_attach, *fe,
+					&stv6110x_config, tsin->i2c_adapter);
+		if (!fe2) {
+			dev_err(c8sectpfe->device,
+				"%s: stv6110x_attach failed for NIM card %s\n"
+				, __func__, dvb_card_str(tsin->dvb_card));
+			return -ENODEV;
+		};
+
+		stv090x_config.tuner_init = fe2->tuner_init;
+		stv090x_config.tuner_set_mode = fe2->tuner_set_mode;
+		stv090x_config.tuner_set_frequency = fe2->tuner_set_frequency;
+		stv090x_config.tuner_get_frequency = fe2->tuner_get_frequency;
+		stv090x_config.tuner_set_bandwidth = fe2->tuner_set_bandwidth;
+		stv090x_config.tuner_get_bandwidth = fe2->tuner_get_bandwidth;
+		stv090x_config.tuner_set_bbgain = fe2->tuner_set_bbgain;
+		stv090x_config.tuner_get_bbgain = fe2->tuner_get_bbgain;
+		stv090x_config.tuner_set_refclk = fe2->tuner_set_refclk;
+		stv090x_config.tuner_get_status = fe2->tuner_get_status;
+
+		dvb_attach(lnbh24_attach, *fe, tsin->i2c_adapter, 0, 0, 0x9);
+		break;
+
+	default:
+		dev_err(c8sectpfe->device,
+			"%s: DVB frontend card %s not yet supported\n",
+			__func__, dvb_card_str(tsin->dvb_card));
+		return -ENODEV;
+	}
+
+	(*fe)->id = chan_num;
+
+	dev_info(c8sectpfe->device,
+			"DVB frontend card %s successfully attached",
+			dvb_card_str(tsin->dvb_card));
+	return 0;
+}
diff --git a/drivers/media/platform/sti/c8sectpfe/c8sectpfe-dvb.h b/drivers/media/platform/sti/c8sectpfe/c8sectpfe-dvb.h
new file mode 100644
index 0000000..bd366db
--- /dev/null
+++ b/drivers/media/platform/sti/c8sectpfe/c8sectpfe-dvb.h
@@ -0,0 +1,20 @@
+/*
+ * c8sectpfe-common.h - C8SECTPFE STi DVB driver
+ *
+ * Copyright (c) STMicroelectronics 2015
+ *
+ *   Author: Peter Griffin <peter.griffin@linaro.org>
+ *
+ *      This program is free software; you can redistribute it and/or
+ *      modify it under the terms of the GNU General Public License as
+ *      published by the Free Software Foundation; either version 2 of
+ *      the License, or (at your option) any later version.
+ */
+#ifndef _C8SECTPFE_DVB_H_
+#define _C8SECTPFE_DVB_H_
+
+int c8sectpfe_frontend_attach(struct dvb_frontend **fe,
+			struct c8sectpfe *c8sectpfe, struct channel_info *tsin,
+			int chan_num);
+
+#endif
diff --git a/drivers/media/platform/ti-vpe/Makefile b/drivers/media/platform/ti-vpe/Makefile
new file mode 100644
index 0000000..be680f8
--- /dev/null
+++ b/drivers/media/platform/ti-vpe/Makefile
@@ -0,0 +1,5 @@
+obj-$(CONFIG_VIDEO_TI_VPE) += ti-vpe.o
+
+ti-vpe-y := vpe.o sc.o csc.o vpdma.o
+
+ccflags-$(CONFIG_VIDEO_TI_VPE_DEBUG) += -DDEBUG
diff --git a/drivers/media/platform/ti-vpe/csc.c b/drivers/media/platform/ti-vpe/csc.c
new file mode 100644
index 0000000..bec6749
--- /dev/null
+++ b/drivers/media/platform/ti-vpe/csc.c
@@ -0,0 +1,192 @@
+/*
+ * Color space converter library
+ *
+ * Copyright (c) 2013 Texas Instruments Inc.
+ *
+ * David Griego, <dagriego@biglakesoftware.com>
+ * Dale Farnsworth, <dale@farnsworth.org>
+ * Archit Taneja, <archit@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ */
+
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+
+#include "csc.h"
+
+/*
+ * 16 coefficients in the order:
+ * a0, b0, c0, a1, b1, c1, a2, b2, c2, d0, d1, d2
+ * (we may need to pass non-default values from user space later on, we might
+ * need to make the coefficient struct more easy to populate)
+ */
+struct colorspace_coeffs {
+	u16	sd[12];
+	u16	hd[12];
+};
+
+/* VIDEO_RANGE: limited range, GRAPHICS_RANGE: full range */
+#define	CSC_COEFFS_VIDEO_RANGE_Y2R	0
+#define	CSC_COEFFS_GRAPHICS_RANGE_Y2R	1
+#define	CSC_COEFFS_VIDEO_RANGE_R2Y	2
+#define	CSC_COEFFS_GRAPHICS_RANGE_R2Y	3
+
+/* default colorspace coefficients */
+static struct colorspace_coeffs colorspace_coeffs[4] = {
+	[CSC_COEFFS_VIDEO_RANGE_Y2R] = {
+		{
+			/* SDTV */
+			0x0400, 0x0000, 0x057D, 0x0400, 0x1EA7, 0x1D35,
+			0x0400, 0x06EF, 0x1FFE, 0x0D40, 0x0210, 0x0C88,
+		},
+		{
+			/* HDTV */
+			0x0400, 0x0000, 0x0629, 0x0400, 0x1F45, 0x1E2B,
+			0x0400, 0x0742, 0x0000, 0x0CEC, 0x0148, 0x0C60,
+		},
+	},
+	[CSC_COEFFS_GRAPHICS_RANGE_Y2R] = {
+		{
+			/* SDTV */
+			0x04A8, 0x1FFE, 0x0662, 0x04A8, 0x1E6F, 0x1CBF,
+			0x04A8, 0x0812, 0x1FFF, 0x0C84, 0x0220, 0x0BAC,
+		},
+		{
+			/* HDTV */
+			0x04A8, 0x0000, 0x072C, 0x04A8, 0x1F26, 0x1DDE,
+			0x04A8, 0x0873, 0x0000, 0x0C20, 0x0134, 0x0B7C,
+		},
+	},
+	[CSC_COEFFS_VIDEO_RANGE_R2Y] = {
+		{
+			/* SDTV */
+			0x0132, 0x0259, 0x0075, 0x1F50, 0x1EA5, 0x020B,
+			0x020B, 0x1E4A, 0x1FAB, 0x0000, 0x0200, 0x0200,
+		},
+		{
+			/* HDTV */
+			0x00DA, 0x02DC, 0x004A, 0x1F88, 0x1E6C, 0x020C,
+			0x020C, 0x1E24, 0x1FD0, 0x0000, 0x0200, 0x0200,
+		},
+	},
+	[CSC_COEFFS_GRAPHICS_RANGE_R2Y] = {
+		{
+			/* SDTV */
+			0x0107, 0x0204, 0x0064, 0x1F68, 0x1ED6, 0x01C2,
+			0x01C2, 0x1E87, 0x1FB7, 0x0040, 0x0200, 0x0200,
+		},
+		{
+			/* HDTV */
+			0x04A8, 0x0000, 0x072C, 0x04A8, 0x1F26, 0x1DDE,
+			0x04A8, 0x0873, 0x0000, 0x0C20, 0x0134, 0x0B7C,
+		},
+	},
+};
+
+void csc_dump_regs(struct csc_data *csc)
+{
+	struct device *dev = &csc->pdev->dev;
+
+#define DUMPREG(r) dev_dbg(dev, "%-35s %08x\n", #r, \
+	ioread32(csc->base + CSC_##r))
+
+	DUMPREG(CSC00);
+	DUMPREG(CSC01);
+	DUMPREG(CSC02);
+	DUMPREG(CSC03);
+	DUMPREG(CSC04);
+	DUMPREG(CSC05);
+
+#undef DUMPREG
+}
+
+void csc_set_coeff_bypass(struct csc_data *csc, u32 *csc_reg5)
+{
+	*csc_reg5 |= CSC_BYPASS;
+}
+
+/*
+ * set the color space converter coefficient shadow register values
+ */
+void csc_set_coeff(struct csc_data *csc, u32 *csc_reg0,
+		enum v4l2_colorspace src_colorspace,
+		enum v4l2_colorspace dst_colorspace)
+{
+	u32 *csc_reg5 = csc_reg0 + 5;
+	u32 *shadow_csc = csc_reg0;
+	struct colorspace_coeffs *sd_hd_coeffs;
+	u16 *coeff, *end_coeff;
+	enum v4l2_colorspace yuv_colorspace;
+	int sel = 0;
+
+	/*
+	 * support only graphics data range(full range) for now, a control ioctl
+	 * would be nice here
+	 */
+	/* Y2R */
+	if (dst_colorspace == V4L2_COLORSPACE_SRGB &&
+			(src_colorspace == V4L2_COLORSPACE_SMPTE170M ||
+			src_colorspace == V4L2_COLORSPACE_REC709)) {
+		/* Y2R */
+		sel = 1;
+		yuv_colorspace = src_colorspace;
+	} else if ((dst_colorspace == V4L2_COLORSPACE_SMPTE170M ||
+			dst_colorspace == V4L2_COLORSPACE_REC709) &&
+			src_colorspace == V4L2_COLORSPACE_SRGB) {
+		/* R2Y */
+		sel = 3;
+		yuv_colorspace = dst_colorspace;
+	} else {
+		*csc_reg5 |= CSC_BYPASS;
+		return;
+	}
+
+	sd_hd_coeffs = &colorspace_coeffs[sel];
+
+	/* select between SD or HD coefficients */
+	if (yuv_colorspace == V4L2_COLORSPACE_SMPTE170M)
+		coeff = sd_hd_coeffs->sd;
+	else
+		coeff = sd_hd_coeffs->hd;
+
+	end_coeff = coeff + 12;
+
+	for (; coeff < end_coeff; coeff += 2)
+		*shadow_csc++ = (*(coeff + 1) << 16) | *coeff;
+}
+
+struct csc_data *csc_create(struct platform_device *pdev)
+{
+	struct csc_data *csc;
+
+	dev_dbg(&pdev->dev, "csc_create\n");
+
+	csc = devm_kzalloc(&pdev->dev, sizeof(*csc), GFP_KERNEL);
+	if (!csc) {
+		dev_err(&pdev->dev, "couldn't alloc csc_data\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	csc->pdev = pdev;
+
+	csc->res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
+			"csc");
+	if (csc->res == NULL) {
+		dev_err(&pdev->dev, "missing platform resources data\n");
+		return ERR_PTR(-ENODEV);
+	}
+
+	csc->base = devm_ioremap_resource(&pdev->dev, csc->res);
+	if (IS_ERR(csc->base)) {
+		dev_err(&pdev->dev, "failed to ioremap\n");
+		return ERR_CAST(csc->base);
+	}
+
+	return csc;
+}
diff --git a/drivers/media/platform/ti-vpe/csc.h b/drivers/media/platform/ti-vpe/csc.h
new file mode 100644
index 0000000..1ad2b6d
--- /dev/null
+++ b/drivers/media/platform/ti-vpe/csc.h
@@ -0,0 +1,68 @@
+/*
+ * Copyright (c) 2013 Texas Instruments Inc.
+ *
+ * David Griego, <dagriego@biglakesoftware.com>
+ * Dale Farnsworth, <dale@farnsworth.org>
+ * Archit Taneja, <archit@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ */
+#ifndef TI_CSC_H
+#define TI_CSC_H
+
+/* VPE color space converter regs */
+#define CSC_CSC00		0x00
+#define CSC_A0_MASK		0x1fff
+#define CSC_A0_SHIFT		0
+#define CSC_B0_MASK		0x1fff
+#define CSC_B0_SHIFT		16
+
+#define CSC_CSC01		0x04
+#define CSC_C0_MASK		0x1fff
+#define CSC_C0_SHIFT		0
+#define CSC_A1_MASK		0x1fff
+#define CSC_A1_SHIFT		16
+
+#define CSC_CSC02		0x08
+#define CSC_B1_MASK		0x1fff
+#define CSC_B1_SHIFT		0
+#define CSC_C1_MASK		0x1fff
+#define CSC_C1_SHIFT		16
+
+#define CSC_CSC03		0x0c
+#define CSC_A2_MASK		0x1fff
+#define CSC_A2_SHIFT		0
+#define CSC_B2_MASK		0x1fff
+#define CSC_B2_SHIFT		16
+
+#define CSC_CSC04		0x10
+#define CSC_C2_MASK		0x1fff
+#define CSC_C2_SHIFT		0
+#define CSC_D0_MASK		0x0fff
+#define CSC_D0_SHIFT		16
+
+#define CSC_CSC05		0x14
+#define CSC_D1_MASK		0x0fff
+#define CSC_D1_SHIFT		0
+#define CSC_D2_MASK		0x0fff
+#define CSC_D2_SHIFT		16
+
+#define CSC_BYPASS		(1 << 28)
+
+struct csc_data {
+	void __iomem		*base;
+	struct resource		*res;
+
+	struct platform_device	*pdev;
+};
+
+void csc_dump_regs(struct csc_data *csc);
+void csc_set_coeff_bypass(struct csc_data *csc, u32 *csc_reg5);
+void csc_set_coeff(struct csc_data *csc, u32 *csc_reg0,
+		enum v4l2_colorspace src_colorspace,
+		enum v4l2_colorspace dst_colorspace);
+struct csc_data *csc_create(struct platform_device *pdev);
+
+#endif
diff --git a/drivers/media/platform/ti-vpe/sc.c b/drivers/media/platform/ti-vpe/sc.c
new file mode 100644
index 0000000..f82d1c7
--- /dev/null
+++ b/drivers/media/platform/ti-vpe/sc.c
@@ -0,0 +1,307 @@
+/*
+ * Scaler library
+ *
+ * Copyright (c) 2013 Texas Instruments Inc.
+ *
+ * David Griego, <dagriego@biglakesoftware.com>
+ * Dale Farnsworth, <dale@farnsworth.org>
+ * Archit Taneja, <archit@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ */
+
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#include "sc.h"
+#include "sc_coeff.h"
+
+void sc_dump_regs(struct sc_data *sc)
+{
+	struct device *dev = &sc->pdev->dev;
+
+#define DUMPREG(r) dev_dbg(dev, "%-35s %08x\n", #r, \
+	ioread32(sc->base + CFG_##r))
+
+	DUMPREG(SC0);
+	DUMPREG(SC1);
+	DUMPREG(SC2);
+	DUMPREG(SC3);
+	DUMPREG(SC4);
+	DUMPREG(SC5);
+	DUMPREG(SC6);
+	DUMPREG(SC8);
+	DUMPREG(SC9);
+	DUMPREG(SC10);
+	DUMPREG(SC11);
+	DUMPREG(SC12);
+	DUMPREG(SC13);
+	DUMPREG(SC17);
+	DUMPREG(SC18);
+	DUMPREG(SC19);
+	DUMPREG(SC20);
+	DUMPREG(SC21);
+	DUMPREG(SC22);
+	DUMPREG(SC23);
+	DUMPREG(SC24);
+	DUMPREG(SC25);
+
+#undef DUMPREG
+}
+
+/*
+ * set the horizontal scaler coefficients according to the ratio of output to
+ * input widths, after accounting for up to two levels of decimation
+ */
+void sc_set_hs_coeffs(struct sc_data *sc, void *addr, unsigned int src_w,
+		unsigned int dst_w)
+{
+	int sixteenths;
+	int idx;
+	int i, j;
+	u16 *coeff_h = addr;
+	const u16 *cp;
+
+	if (dst_w > src_w) {
+		idx = HS_UP_SCALE;
+	} else {
+		if ((dst_w << 1) < src_w)
+			dst_w <<= 1;	/* first level decimation */
+		if ((dst_w << 1) < src_w)
+			dst_w <<= 1;	/* second level decimation */
+
+		if (dst_w == src_w) {
+			idx = HS_LE_16_16_SCALE;
+		} else {
+			sixteenths = (dst_w << 4) / src_w;
+			if (sixteenths < 8)
+				sixteenths = 8;
+			idx = HS_LT_9_16_SCALE + sixteenths - 8;
+		}
+	}
+
+	if (idx == sc->hs_index)
+		return;
+
+	cp = scaler_hs_coeffs[idx];
+
+	for (i = 0; i < SC_NUM_PHASES * 2; i++) {
+		for (j = 0; j < SC_H_NUM_TAPS; j++)
+			*coeff_h++ = *cp++;
+		/*
+		 * for each phase, the scaler expects space for 8 coefficients
+		 * in it's memory. For the horizontal scaler, we copy the first
+		 * 7 coefficients and skip the last slot to move to the next
+		 * row to hold coefficients for the next phase
+		 */
+		coeff_h += SC_NUM_TAPS_MEM_ALIGN - SC_H_NUM_TAPS;
+	}
+
+	sc->hs_index = idx;
+
+	sc->load_coeff_h = true;
+}
+
+/*
+ * set the vertical scaler coefficients according to the ratio of output to
+ * input heights
+ */
+void sc_set_vs_coeffs(struct sc_data *sc, void *addr, unsigned int src_h,
+		unsigned int dst_h)
+{
+	int sixteenths;
+	int idx;
+	int i, j;
+	u16 *coeff_v = addr;
+	const u16 *cp;
+
+	if (dst_h > src_h) {
+		idx = VS_UP_SCALE;
+	} else if (dst_h == src_h) {
+		idx = VS_1_TO_1_SCALE;
+	} else {
+		sixteenths = (dst_h << 4) / src_h;
+		if (sixteenths < 8)
+			sixteenths = 8;
+		idx = VS_LT_9_16_SCALE + sixteenths - 8;
+	}
+
+	if (idx == sc->vs_index)
+		return;
+
+	cp = scaler_vs_coeffs[idx];
+
+	for (i = 0; i < SC_NUM_PHASES * 2; i++) {
+		for (j = 0; j < SC_V_NUM_TAPS; j++)
+			*coeff_v++ = *cp++;
+		/*
+		 * for the vertical scaler, we copy the first 5 coefficients and
+		 * skip the last 3 slots to move to the next row to hold
+		 * coefficients for the next phase
+		 */
+		coeff_v += SC_NUM_TAPS_MEM_ALIGN - SC_V_NUM_TAPS;
+	}
+
+	sc->vs_index = idx;
+	sc->load_coeff_v = true;
+}
+
+void sc_config_scaler(struct sc_data *sc, u32 *sc_reg0, u32 *sc_reg8,
+		u32 *sc_reg17, unsigned int src_w, unsigned int src_h,
+		unsigned int dst_w, unsigned int dst_h)
+{
+	struct device *dev = &sc->pdev->dev;
+	u32 val;
+	int dcm_x, dcm_shift;
+	bool use_rav;
+	unsigned long lltmp;
+	u32 lin_acc_inc, lin_acc_inc_u;
+	u32 col_acc_offset;
+	u16 factor = 0;
+	int row_acc_init_rav = 0, row_acc_init_rav_b = 0;
+	u32 row_acc_inc = 0, row_acc_offset = 0, row_acc_offset_b = 0;
+	/*
+	 * location of SC register in payload memory with respect to the first
+	 * register in the mmr address data block
+	 */
+	u32 *sc_reg9 = sc_reg8 + 1;
+	u32 *sc_reg12 = sc_reg8 + 4;
+	u32 *sc_reg13 = sc_reg8 + 5;
+	u32 *sc_reg24 = sc_reg17 + 7;
+
+	val = sc_reg0[0];
+
+	/* clear all the features(they may get enabled elsewhere later) */
+	val &= ~(CFG_SELFGEN_FID | CFG_TRIM | CFG_ENABLE_SIN2_VER_INTP |
+		CFG_INTERLACE_I | CFG_DCM_4X | CFG_DCM_2X | CFG_AUTO_HS |
+		CFG_ENABLE_EV | CFG_USE_RAV | CFG_INVT_FID | CFG_SC_BYPASS |
+		CFG_INTERLACE_O | CFG_Y_PK_EN | CFG_HP_BYPASS | CFG_LINEAR);
+
+	if (src_w == dst_w && src_h == dst_h) {
+		val |= CFG_SC_BYPASS;
+		sc_reg0[0] = val;
+		return;
+	}
+
+	/* we only support linear scaling for now */
+	val |= CFG_LINEAR;
+
+	/* configure horizontal scaler */
+
+	/* enable 2X or 4X decimation */
+	dcm_x = src_w / dst_w;
+	if (dcm_x > 4) {
+		val |= CFG_DCM_4X;
+		dcm_shift = 2;
+	} else if (dcm_x > 2) {
+		val |= CFG_DCM_2X;
+		dcm_shift = 1;
+	} else {
+		dcm_shift = 0;
+	}
+
+	lltmp = dst_w - 1;
+	lin_acc_inc = div64_u64(((u64)(src_w >> dcm_shift) - 1) << 24, lltmp);
+	lin_acc_inc_u = 0;
+	col_acc_offset = 0;
+
+	dev_dbg(dev, "hs config: src_w = %d, dst_w = %d, decimation = %s, lin_acc_inc = %08x\n",
+		src_w, dst_w, dcm_shift == 2 ? "4x" :
+		(dcm_shift == 1 ? "2x" : "none"), lin_acc_inc);
+
+	/* configure vertical scaler */
+
+	/* use RAV for vertical scaler if vertical downscaling is > 4x */
+	if (dst_h < (src_h >> 2)) {
+		use_rav = true;
+		val |= CFG_USE_RAV;
+	} else {
+		use_rav = false;
+	}
+
+	if (use_rav) {
+		/* use RAV */
+		factor = (u16) ((dst_h << 10) / src_h);
+
+		row_acc_init_rav = factor + ((1 + factor) >> 1);
+		if (row_acc_init_rav >= 1024)
+			row_acc_init_rav -= 1024;
+
+		row_acc_init_rav_b = row_acc_init_rav +
+				(1 + (row_acc_init_rav >> 1)) -
+				(1024 >> 1);
+
+		if (row_acc_init_rav_b < 0) {
+			row_acc_init_rav_b += row_acc_init_rav;
+			row_acc_init_rav *= 2;
+		}
+
+		dev_dbg(dev, "vs config(RAV): src_h = %d, dst_h = %d, factor = %d, acc_init = %08x, acc_init_b = %08x\n",
+			src_h, dst_h, factor, row_acc_init_rav,
+			row_acc_init_rav_b);
+	} else {
+		/* use polyphase */
+		row_acc_inc = ((src_h - 1) << 16) / (dst_h - 1);
+		row_acc_offset = 0;
+		row_acc_offset_b = 0;
+
+		dev_dbg(dev, "vs config(POLY): src_h = %d, dst_h = %d,row_acc_inc = %08x\n",
+			src_h, dst_h, row_acc_inc);
+	}
+
+
+	sc_reg0[0] = val;
+	sc_reg0[1] = row_acc_inc;
+	sc_reg0[2] = row_acc_offset;
+	sc_reg0[3] = row_acc_offset_b;
+
+	sc_reg0[4] = ((lin_acc_inc_u & CFG_LIN_ACC_INC_U_MASK) <<
+			CFG_LIN_ACC_INC_U_SHIFT) | (dst_w << CFG_TAR_W_SHIFT) |
+			(dst_h << CFG_TAR_H_SHIFT);
+
+	sc_reg0[5] = (src_w << CFG_SRC_W_SHIFT) | (src_h << CFG_SRC_H_SHIFT);
+
+	sc_reg0[6] = (row_acc_init_rav_b << CFG_ROW_ACC_INIT_RAV_B_SHIFT) |
+		(row_acc_init_rav << CFG_ROW_ACC_INIT_RAV_SHIFT);
+
+	*sc_reg9 = lin_acc_inc;
+
+	*sc_reg12 = col_acc_offset << CFG_COL_ACC_OFFSET_SHIFT;
+
+	*sc_reg13 = factor;
+
+	*sc_reg24 = (src_w << CFG_ORG_W_SHIFT) | (src_h << CFG_ORG_H_SHIFT);
+}
+
+struct sc_data *sc_create(struct platform_device *pdev)
+{
+	struct sc_data *sc;
+
+	dev_dbg(&pdev->dev, "sc_create\n");
+
+	sc = devm_kzalloc(&pdev->dev, sizeof(*sc), GFP_KERNEL);
+	if (!sc) {
+		dev_err(&pdev->dev, "couldn't alloc sc_data\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	sc->pdev = pdev;
+
+	sc->res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "sc");
+	if (!sc->res) {
+		dev_err(&pdev->dev, "missing platform resources data\n");
+		return ERR_PTR(-ENODEV);
+	}
+
+	sc->base = devm_ioremap_resource(&pdev->dev, sc->res);
+	if (IS_ERR(sc->base)) {
+		dev_err(&pdev->dev, "failed to ioremap\n");
+		return ERR_CAST(sc->base);
+	}
+
+	return sc;
+}
diff --git a/drivers/media/platform/ti-vpe/sc.h b/drivers/media/platform/ti-vpe/sc.h
new file mode 100644
index 0000000..60e411e
--- /dev/null
+++ b/drivers/media/platform/ti-vpe/sc.h
@@ -0,0 +1,208 @@
+/*
+ * Copyright (c) 2013 Texas Instruments Inc.
+ *
+ * David Griego, <dagriego@biglakesoftware.com>
+ * Dale Farnsworth, <dale@farnsworth.org>
+ * Archit Taneja, <archit@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ */
+#ifndef TI_SC_H
+#define TI_SC_H
+
+/* Scaler regs */
+#define CFG_SC0				0x0
+#define CFG_INTERLACE_O			(1 << 0)
+#define CFG_LINEAR			(1 << 1)
+#define CFG_SC_BYPASS			(1 << 2)
+#define CFG_INVT_FID			(1 << 3)
+#define CFG_USE_RAV			(1 << 4)
+#define CFG_ENABLE_EV			(1 << 5)
+#define CFG_AUTO_HS			(1 << 6)
+#define CFG_DCM_2X			(1 << 7)
+#define CFG_DCM_4X			(1 << 8)
+#define CFG_HP_BYPASS			(1 << 9)
+#define CFG_INTERLACE_I			(1 << 10)
+#define CFG_ENABLE_SIN2_VER_INTP	(1 << 11)
+#define CFG_Y_PK_EN			(1 << 14)
+#define CFG_TRIM			(1 << 15)
+#define CFG_SELFGEN_FID			(1 << 16)
+
+#define CFG_SC1				0x4
+#define CFG_ROW_ACC_INC_MASK		0x07ffffff
+#define CFG_ROW_ACC_INC_SHIFT		0
+
+#define CFG_SC2				0x08
+#define CFG_ROW_ACC_OFFSET_MASK		0x0fffffff
+#define CFG_ROW_ACC_OFFSET_SHIFT	0
+
+#define CFG_SC3				0x0c
+#define CFG_ROW_ACC_OFFSET_B_MASK	0x0fffffff
+#define CFG_ROW_ACC_OFFSET_B_SHIFT	0
+
+#define CFG_SC4				0x10
+#define CFG_TAR_H_MASK			0x07ff
+#define CFG_TAR_H_SHIFT			0
+#define CFG_TAR_W_MASK			0x07ff
+#define CFG_TAR_W_SHIFT			12
+#define CFG_LIN_ACC_INC_U_MASK		0x07
+#define CFG_LIN_ACC_INC_U_SHIFT		24
+#define CFG_NLIN_ACC_INIT_U_MASK	0x07
+#define CFG_NLIN_ACC_INIT_U_SHIFT	28
+
+#define CFG_SC5				0x14
+#define CFG_SRC_H_MASK			0x07ff
+#define CFG_SRC_H_SHIFT			0
+#define CFG_SRC_W_MASK			0x07ff
+#define CFG_SRC_W_SHIFT			12
+#define CFG_NLIN_ACC_INC_U_MASK		0x07
+#define CFG_NLIN_ACC_INC_U_SHIFT	24
+
+#define CFG_SC6				0x18
+#define CFG_ROW_ACC_INIT_RAV_MASK	0x03ff
+#define CFG_ROW_ACC_INIT_RAV_SHIFT	0
+#define CFG_ROW_ACC_INIT_RAV_B_MASK	0x03ff
+#define CFG_ROW_ACC_INIT_RAV_B_SHIFT	10
+
+#define CFG_SC8				0x20
+#define CFG_NLIN_LEFT_MASK		0x07ff
+#define CFG_NLIN_LEFT_SHIFT		0
+#define CFG_NLIN_RIGHT_MASK		0x07ff
+#define CFG_NLIN_RIGHT_SHIFT		12
+
+#define CFG_SC9				0x24
+#define CFG_LIN_ACC_INC			CFG_SC9
+
+#define CFG_SC10			0x28
+#define CFG_NLIN_ACC_INIT		CFG_SC10
+
+#define CFG_SC11			0x2c
+#define CFG_NLIN_ACC_INC		CFG_SC11
+
+#define CFG_SC12			0x30
+#define CFG_COL_ACC_OFFSET_MASK		0x01ffffff
+#define CFG_COL_ACC_OFFSET_SHIFT	0
+
+#define CFG_SC13			0x34
+#define CFG_SC_FACTOR_RAV_MASK		0xff
+#define CFG_SC_FACTOR_RAV_SHIFT		0
+#define CFG_CHROMA_INTP_THR_MASK	0x03ff
+#define CFG_CHROMA_INTP_THR_SHIFT	12
+#define CFG_DELTA_CHROMA_THR_MASK	0x0f
+#define CFG_DELTA_CHROMA_THR_SHIFT	24
+
+#define CFG_SC17			0x44
+#define CFG_EV_THR_MASK			0x03ff
+#define CFG_EV_THR_SHIFT		12
+#define CFG_DELTA_LUMA_THR_MASK		0x0f
+#define CFG_DELTA_LUMA_THR_SHIFT	24
+#define CFG_DELTA_EV_THR_MASK		0x0f
+#define CFG_DELTA_EV_THR_SHIFT		28
+
+#define CFG_SC18			0x48
+#define CFG_HS_FACTOR_MASK		0x03ff
+#define CFG_HS_FACTOR_SHIFT		0
+#define CFG_CONF_DEFAULT_MASK		0x01ff
+#define CFG_CONF_DEFAULT_SHIFT		16
+
+#define CFG_SC19			0x4c
+#define CFG_HPF_COEFF0_MASK		0xff
+#define CFG_HPF_COEFF0_SHIFT		0
+#define CFG_HPF_COEFF1_MASK		0xff
+#define CFG_HPF_COEFF1_SHIFT		8
+#define CFG_HPF_COEFF2_MASK		0xff
+#define CFG_HPF_COEFF2_SHIFT		16
+#define CFG_HPF_COEFF3_MASK		0xff
+#define CFG_HPF_COEFF3_SHIFT		23
+
+#define CFG_SC20			0x50
+#define CFG_HPF_COEFF4_MASK		0xff
+#define CFG_HPF_COEFF4_SHIFT		0
+#define CFG_HPF_COEFF5_MASK		0xff
+#define CFG_HPF_COEFF5_SHIFT		8
+#define CFG_HPF_NORM_SHIFT_MASK		0x07
+#define CFG_HPF_NORM_SHIFT_SHIFT	16
+#define CFG_NL_LIMIT_MASK		0x1ff
+#define CFG_NL_LIMIT_SHIFT		20
+
+#define CFG_SC21			0x54
+#define CFG_NL_LO_THR_MASK		0x01ff
+#define CFG_NL_LO_THR_SHIFT		0
+#define CFG_NL_LO_SLOPE_MASK		0xff
+#define CFG_NL_LO_SLOPE_SHIFT		16
+
+#define CFG_SC22			0x58
+#define CFG_NL_HI_THR_MASK		0x01ff
+#define CFG_NL_HI_THR_SHIFT		0
+#define CFG_NL_HI_SLOPE_SH_MASK		0x07
+#define CFG_NL_HI_SLOPE_SH_SHIFT	16
+
+#define CFG_SC23			0x5c
+#define CFG_GRADIENT_THR_MASK		0x07ff
+#define CFG_GRADIENT_THR_SHIFT		0
+#define CFG_GRADIENT_THR_RANGE_MASK	0x0f
+#define CFG_GRADIENT_THR_RANGE_SHIFT	12
+#define CFG_MIN_GY_THR_MASK		0xff
+#define CFG_MIN_GY_THR_SHIFT		16
+#define CFG_MIN_GY_THR_RANGE_MASK	0x0f
+#define CFG_MIN_GY_THR_RANGE_SHIFT	28
+
+#define CFG_SC24			0x60
+#define CFG_ORG_H_MASK			0x07ff
+#define CFG_ORG_H_SHIFT			0
+#define CFG_ORG_W_MASK			0x07ff
+#define CFG_ORG_W_SHIFT			16
+
+#define CFG_SC25			0x64
+#define CFG_OFF_H_MASK			0x07ff
+#define CFG_OFF_H_SHIFT			0
+#define CFG_OFF_W_MASK			0x07ff
+#define CFG_OFF_W_SHIFT			16
+
+/* number of phases supported by the polyphase scalers */
+#define SC_NUM_PHASES			32
+
+/* number of taps used by horizontal polyphase scaler */
+#define SC_H_NUM_TAPS			7
+
+/* number of taps used by vertical polyphase scaler */
+#define SC_V_NUM_TAPS			5
+
+/* number of taps expected by the scaler in it's coefficient memory */
+#define SC_NUM_TAPS_MEM_ALIGN		8
+
+/*
+ * coefficient memory size in bytes:
+ * num phases x num sets(luma and chroma) x num taps(aligned) x coeff size
+ */
+#define SC_COEF_SRAM_SIZE	(SC_NUM_PHASES * 2 * SC_NUM_TAPS_MEM_ALIGN * 2)
+
+struct sc_data {
+	void __iomem		*base;
+	struct resource		*res;
+
+	dma_addr_t		loaded_coeff_h; /* loaded h coeffs in SC */
+	dma_addr_t		loaded_coeff_v; /* loaded v coeffs in SC */
+
+	bool			load_coeff_h;	/* have new h SC coeffs */
+	bool			load_coeff_v;	/* have new v SC coeffs */
+
+	unsigned int		hs_index;	/* h SC coeffs selector */
+	unsigned int		vs_index;	/* v SC coeffs selector */
+
+	struct platform_device *pdev;
+};
+
+void sc_dump_regs(struct sc_data *sc);
+void sc_set_hs_coeffs(struct sc_data *sc, void *addr, unsigned int src_w,
+		unsigned int dst_w);
+void sc_set_vs_coeffs(struct sc_data *sc, void *addr, unsigned int src_h,
+		unsigned int dst_h);
+void sc_config_scaler(struct sc_data *sc, u32 *sc_reg0, u32 *sc_reg8,
+		u32 *sc_reg17, unsigned int src_w, unsigned int src_h,
+		unsigned int dst_w, unsigned int dst_h);
+struct sc_data *sc_create(struct platform_device *pdev);
+
+#endif
diff --git a/drivers/media/platform/ti-vpe/sc_coeff.h b/drivers/media/platform/ti-vpe/sc_coeff.h
new file mode 100644
index 0000000..5bfa5c0
--- /dev/null
+++ b/drivers/media/platform/ti-vpe/sc_coeff.h
@@ -0,0 +1,1342 @@
+/*
+ * VPE SC coefs
+ *
+ * Copyright (c) 2013 Texas Instruments Inc.
+ *
+ * David Griego, <dagriego@biglakesoftware.com>
+ * Dale Farnsworth, <dale@farnsworth.org>
+ * Archit Taneja, <archit@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ */
+
+#ifndef __TI_SC_COEFF_H
+#define __TI_SC_COEFF_H
+
+/* horizontal scaler coefficients */
+enum {
+	HS_UP_SCALE = 0,
+	HS_LT_9_16_SCALE,
+	HS_LT_10_16_SCALE,
+	HS_LT_11_16_SCALE,
+	HS_LT_12_16_SCALE,
+	HS_LT_13_16_SCALE,
+	HS_LT_14_16_SCALE,
+	HS_LT_15_16_SCALE,
+	HS_LE_16_16_SCALE,
+};
+
+static const u16 scaler_hs_coeffs[13][SC_NUM_PHASES * 2 * SC_H_NUM_TAPS] = {
+	[HS_UP_SCALE] = {
+		/* Luma */
+		0x001F, 0x1F90, 0x00D2, 0x06FE, 0x00D2, 0x1F90, 0x001F,
+		0x001C, 0x1F9E, 0x009F, 0x06FB, 0x0108, 0x1F82, 0x0022,
+		0x0019, 0x1FAC, 0x006F, 0x06F3, 0x0140, 0x1F74, 0x0025,
+		0x0016, 0x1FB9, 0x0041, 0x06E7, 0x017B, 0x1F66, 0x0028,
+		0x0013, 0x1FC6, 0x0017, 0x06D6, 0x01B7, 0x1F58, 0x002B,
+		0x0010, 0x1FD3, 0x1FEF, 0x06C0, 0x01F6, 0x1F4B, 0x002D,
+		0x000E, 0x1FDF, 0x1FCB, 0x06A5, 0x0235, 0x1F3F, 0x002F,
+		0x000B, 0x1FEA, 0x1FAA, 0x0686, 0x0277, 0x1F33, 0x0031,
+		0x0009, 0x1FF5, 0x1F8C, 0x0663, 0x02B8, 0x1F28, 0x0033,
+		0x0007, 0x1FFF, 0x1F72, 0x063A, 0x02FB, 0x1F1F, 0x0034,
+		0x0005, 0x0008, 0x1F5A, 0x060F, 0x033E, 0x1F17, 0x0035,
+		0x0003, 0x0010, 0x1F46, 0x05E0, 0x0382, 0x1F10, 0x0035,
+		0x0002, 0x0017, 0x1F34, 0x05AF, 0x03C5, 0x1F0B, 0x0034,
+		0x0001, 0x001E, 0x1F26, 0x0579, 0x0407, 0x1F08, 0x0033,
+		0x0000, 0x0023, 0x1F1A, 0x0541, 0x0449, 0x1F07, 0x0032,
+		0x1FFF, 0x0028, 0x1F12, 0x0506, 0x048A, 0x1F08, 0x002F,
+		0x002C, 0x1F0C, 0x04C8, 0x04C8, 0x1F0C, 0x002C, 0x0000,
+		0x002F, 0x1F08, 0x048A, 0x0506, 0x1F12, 0x0028, 0x1FFF,
+		0x0032, 0x1F07, 0x0449, 0x0541, 0x1F1A, 0x0023, 0x0000,
+		0x0033, 0x1F08, 0x0407, 0x0579, 0x1F26, 0x001E, 0x0001,
+		0x0034, 0x1F0B, 0x03C5, 0x05AF, 0x1F34, 0x0017, 0x0002,
+		0x0035, 0x1F10, 0x0382, 0x05E0, 0x1F46, 0x0010, 0x0003,
+		0x0035, 0x1F17, 0x033E, 0x060F, 0x1F5A, 0x0008, 0x0005,
+		0x0034, 0x1F1F, 0x02FB, 0x063A, 0x1F72, 0x1FFF, 0x0007,
+		0x0033, 0x1F28, 0x02B8, 0x0663, 0x1F8C, 0x1FF5, 0x0009,
+		0x0031, 0x1F33, 0x0277, 0x0686, 0x1FAA, 0x1FEA, 0x000B,
+		0x002F, 0x1F3F, 0x0235, 0x06A5, 0x1FCB, 0x1FDF, 0x000E,
+		0x002D, 0x1F4B, 0x01F6, 0x06C0, 0x1FEF, 0x1FD3, 0x0010,
+		0x002B, 0x1F58, 0x01B7, 0x06D6, 0x0017, 0x1FC6, 0x0013,
+		0x0028, 0x1F66, 0x017B, 0x06E7, 0x0041, 0x1FB9, 0x0016,
+		0x0025, 0x1F74, 0x0140, 0x06F3, 0x006F, 0x1FAC, 0x0019,
+		0x0022, 0x1F82, 0x0108, 0x06FB, 0x009F, 0x1F9E, 0x001C,
+		/* Chroma */
+		0x001F, 0x1F90, 0x00D2, 0x06FE, 0x00D2, 0x1F90, 0x001F,
+		0x001C, 0x1F9E, 0x009F, 0x06FB, 0x0108, 0x1F82, 0x0022,
+		0x0019, 0x1FAC, 0x006F, 0x06F3, 0x0140, 0x1F74, 0x0025,
+		0x0016, 0x1FB9, 0x0041, 0x06E7, 0x017B, 0x1F66, 0x0028,
+		0x0013, 0x1FC6, 0x0017, 0x06D6, 0x01B7, 0x1F58, 0x002B,
+		0x0010, 0x1FD3, 0x1FEF, 0x06C0, 0x01F6, 0x1F4B, 0x002D,
+		0x000E, 0x1FDF, 0x1FCB, 0x06A5, 0x0235, 0x1F3F, 0x002F,
+		0x000B, 0x1FEA, 0x1FAA, 0x0686, 0x0277, 0x1F33, 0x0031,
+		0x0009, 0x1FF5, 0x1F8C, 0x0663, 0x02B8, 0x1F28, 0x0033,
+		0x0007, 0x1FFF, 0x1F72, 0x063A, 0x02FB, 0x1F1F, 0x0034,
+		0x0005, 0x0008, 0x1F5A, 0x060F, 0x033E, 0x1F17, 0x0035,
+		0x0003, 0x0010, 0x1F46, 0x05E0, 0x0382, 0x1F10, 0x0035,
+		0x0002, 0x0017, 0x1F34, 0x05AF, 0x03C5, 0x1F0B, 0x0034,
+		0x0001, 0x001E, 0x1F26, 0x0579, 0x0407, 0x1F08, 0x0033,
+		0x0000, 0x0023, 0x1F1A, 0x0541, 0x0449, 0x1F07, 0x0032,
+		0x1FFF, 0x0028, 0x1F12, 0x0506, 0x048A, 0x1F08, 0x002F,
+		0x002C, 0x1F0C, 0x04C8, 0x04C8, 0x1F0C, 0x002C, 0x0000,
+		0x002F, 0x1F08, 0x048A, 0x0506, 0x1F12, 0x0028, 0x1FFF,
+		0x0032, 0x1F07, 0x0449, 0x0541, 0x1F1A, 0x0023, 0x0000,
+		0x0033, 0x1F08, 0x0407, 0x0579, 0x1F26, 0x001E, 0x0001,
+		0x0034, 0x1F0B, 0x03C5, 0x05AF, 0x1F34, 0x0017, 0x0002,
+		0x0035, 0x1F10, 0x0382, 0x05E0, 0x1F46, 0x0010, 0x0003,
+		0x0035, 0x1F17, 0x033E, 0x060F, 0x1F5A, 0x0008, 0x0005,
+		0x0034, 0x1F1F, 0x02FB, 0x063A, 0x1F72, 0x1FFF, 0x0007,
+		0x0033, 0x1F28, 0x02B8, 0x0663, 0x1F8C, 0x1FF5, 0x0009,
+		0x0031, 0x1F33, 0x0277, 0x0686, 0x1FAA, 0x1FEA, 0x000B,
+		0x002F, 0x1F3F, 0x0235, 0x06A5, 0x1FCB, 0x1FDF, 0x000E,
+		0x002D, 0x1F4B, 0x01F6, 0x06C0, 0x1FEF, 0x1FD3, 0x0010,
+		0x002B, 0x1F58, 0x01B7, 0x06D6, 0x0017, 0x1FC6, 0x0013,
+		0x0028, 0x1F66, 0x017B, 0x06E7, 0x0041, 0x1FB9, 0x0016,
+		0x0025, 0x1F74, 0x0140, 0x06F3, 0x006F, 0x1FAC, 0x0019,
+		0x0022, 0x1F82, 0x0108, 0x06FB, 0x009F, 0x1F9E, 0x001C,
+	},
+	[HS_LT_9_16_SCALE] = {
+		/* Luma */
+		0x1FA3, 0x005E, 0x024A, 0x036A, 0x024A, 0x005E, 0x1FA3,
+		0x1FA3, 0x0052, 0x023A, 0x036A, 0x0259, 0x006A, 0x1FA4,
+		0x1FA3, 0x0046, 0x022A, 0x036A, 0x0269, 0x0076, 0x1FA4,
+		0x1FA3, 0x003B, 0x021A, 0x0368, 0x0278, 0x0083, 0x1FA5,
+		0x1FA4, 0x0031, 0x020A, 0x0365, 0x0286, 0x0090, 0x1FA6,
+		0x1FA5, 0x0026, 0x01F9, 0x0362, 0x0294, 0x009E, 0x1FA8,
+		0x1FA6, 0x001C, 0x01E8, 0x035E, 0x02A3, 0x00AB, 0x1FAA,
+		0x1FA7, 0x0013, 0x01D7, 0x035A, 0x02B0, 0x00B9, 0x1FAC,
+		0x1FA9, 0x000A, 0x01C6, 0x0354, 0x02BD, 0x00C7, 0x1FAF,
+		0x1FAA, 0x0001, 0x01B6, 0x034E, 0x02C9, 0x00D6, 0x1FB2,
+		0x1FAC, 0x1FF9, 0x01A5, 0x0347, 0x02D5, 0x00E5, 0x1FB5,
+		0x1FAE, 0x1FF1, 0x0194, 0x0340, 0x02E1, 0x00F3, 0x1FB9,
+		0x1FB0, 0x1FEA, 0x0183, 0x0338, 0x02EC, 0x0102, 0x1FBD,
+		0x1FB2, 0x1FE3, 0x0172, 0x0330, 0x02F6, 0x0112, 0x1FC1,
+		0x1FB4, 0x1FDC, 0x0161, 0x0327, 0x0301, 0x0121, 0x1FC6,
+		0x1FB7, 0x1FD6, 0x0151, 0x031D, 0x030A, 0x0130, 0x1FCB,
+		0x1FD2, 0x0136, 0x02F8, 0x02F8, 0x0136, 0x1FD2, 0x0000,
+		0x1FCB, 0x0130, 0x030A, 0x031D, 0x0151, 0x1FD6, 0x1FB7,
+		0x1FC6, 0x0121, 0x0301, 0x0327, 0x0161, 0x1FDC, 0x1FB4,
+		0x1FC1, 0x0112, 0x02F6, 0x0330, 0x0172, 0x1FE3, 0x1FB2,
+		0x1FBD, 0x0102, 0x02EC, 0x0338, 0x0183, 0x1FEA, 0x1FB0,
+		0x1FB9, 0x00F3, 0x02E1, 0x0340, 0x0194, 0x1FF1, 0x1FAE,
+		0x1FB5, 0x00E5, 0x02D5, 0x0347, 0x01A5, 0x1FF9, 0x1FAC,
+		0x1FB2, 0x00D6, 0x02C9, 0x034E, 0x01B6, 0x0001, 0x1FAA,
+		0x1FAF, 0x00C7, 0x02BD, 0x0354, 0x01C6, 0x000A, 0x1FA9,
+		0x1FAC, 0x00B9, 0x02B0, 0x035A, 0x01D7, 0x0013, 0x1FA7,
+		0x1FAA, 0x00AB, 0x02A3, 0x035E, 0x01E8, 0x001C, 0x1FA6,
+		0x1FA8, 0x009E, 0x0294, 0x0362, 0x01F9, 0x0026, 0x1FA5,
+		0x1FA6, 0x0090, 0x0286, 0x0365, 0x020A, 0x0031, 0x1FA4,
+		0x1FA5, 0x0083, 0x0278, 0x0368, 0x021A, 0x003B, 0x1FA3,
+		0x1FA4, 0x0076, 0x0269, 0x036A, 0x022A, 0x0046, 0x1FA3,
+		0x1FA4, 0x006A, 0x0259, 0x036A, 0x023A, 0x0052, 0x1FA3,
+		/* Chroma */
+		0x1FA3, 0x005E, 0x024A, 0x036A, 0x024A, 0x005E, 0x1FA3,
+		0x1FA3, 0x0052, 0x023A, 0x036A, 0x0259, 0x006A, 0x1FA4,
+		0x1FA3, 0x0046, 0x022A, 0x036A, 0x0269, 0x0076, 0x1FA4,
+		0x1FA3, 0x003B, 0x021A, 0x0368, 0x0278, 0x0083, 0x1FA5,
+		0x1FA4, 0x0031, 0x020A, 0x0365, 0x0286, 0x0090, 0x1FA6,
+		0x1FA5, 0x0026, 0x01F9, 0x0362, 0x0294, 0x009E, 0x1FA8,
+		0x1FA6, 0x001C, 0x01E8, 0x035E, 0x02A3, 0x00AB, 0x1FAA,
+		0x1FA7, 0x0013, 0x01D7, 0x035A, 0x02B0, 0x00B9, 0x1FAC,
+		0x1FA9, 0x000A, 0x01C6, 0x0354, 0x02BD, 0x00C7, 0x1FAF,
+		0x1FAA, 0x0001, 0x01B6, 0x034E, 0x02C9, 0x00D6, 0x1FB2,
+		0x1FAC, 0x1FF9, 0x01A5, 0x0347, 0x02D5, 0x00E5, 0x1FB5,
+		0x1FAE, 0x1FF1, 0x0194, 0x0340, 0x02E1, 0x00F3, 0x1FB9,
+		0x1FB0, 0x1FEA, 0x0183, 0x0338, 0x02EC, 0x0102, 0x1FBD,
+		0x1FB2, 0x1FE3, 0x0172, 0x0330, 0x02F6, 0x0112, 0x1FC1,
+		0x1FB4, 0x1FDC, 0x0161, 0x0327, 0x0301, 0x0121, 0x1FC6,
+		0x1FB7, 0x1FD6, 0x0151, 0x031D, 0x030A, 0x0130, 0x1FCB,
+		0x1FD2, 0x0136, 0x02F8, 0x02F8, 0x0136, 0x1FD2, 0x0000,
+		0x1FCB, 0x0130, 0x030A, 0x031D, 0x0151, 0x1FD6, 0x1FB7,
+		0x1FC6, 0x0121, 0x0301, 0x0327, 0x0161, 0x1FDC, 0x1FB4,
+		0x1FC1, 0x0112, 0x02F6, 0x0330, 0x0172, 0x1FE3, 0x1FB2,
+		0x1FBD, 0x0102, 0x02EC, 0x0338, 0x0183, 0x1FEA, 0x1FB0,
+		0x1FB9, 0x00F3, 0x02E1, 0x0340, 0x0194, 0x1FF1, 0x1FAE,
+		0x1FB5, 0x00E5, 0x02D5, 0x0347, 0x01A5, 0x1FF9, 0x1FAC,
+		0x1FB2, 0x00D6, 0x02C9, 0x034E, 0x01B6, 0x0001, 0x1FAA,
+		0x1FAF, 0x00C7, 0x02BD, 0x0354, 0x01C6, 0x000A, 0x1FA9,
+		0x1FAC, 0x00B9, 0x02B0, 0x035A, 0x01D7, 0x0013, 0x1FA7,
+		0x1FAA, 0x00AB, 0x02A3, 0x035E, 0x01E8, 0x001C, 0x1FA6,
+		0x1FA8, 0x009E, 0x0294, 0x0362, 0x01F9, 0x0026, 0x1FA5,
+		0x1FA6, 0x0090, 0x0286, 0x0365, 0x020A, 0x0031, 0x1FA4,
+		0x1FA5, 0x0083, 0x0278, 0x0368, 0x021A, 0x003B, 0x1FA3,
+		0x1FA4, 0x0076, 0x0269, 0x036A, 0x022A, 0x0046, 0x1FA3,
+		0x1FA4, 0x006A, 0x0259, 0x036A, 0x023A, 0x0052, 0x1FA3,
+	},
+	[HS_LT_10_16_SCALE] = {
+		/* Luma */
+		0x1F8D, 0x000C, 0x026A, 0x03FA, 0x026A, 0x000C, 0x1F8D,
+		0x1F8F, 0x0000, 0x0255, 0x03FA, 0x027F, 0x0019, 0x1F8A,
+		0x1F92, 0x1FF5, 0x023F, 0x03F8, 0x0293, 0x0027, 0x1F88,
+		0x1F95, 0x1FEA, 0x022A, 0x03F6, 0x02A7, 0x0034, 0x1F86,
+		0x1F99, 0x1FDF, 0x0213, 0x03F2, 0x02BB, 0x0043, 0x1F85,
+		0x1F9C, 0x1FD5, 0x01FE, 0x03ED, 0x02CF, 0x0052, 0x1F83,
+		0x1FA0, 0x1FCC, 0x01E8, 0x03E7, 0x02E1, 0x0061, 0x1F83,
+		0x1FA4, 0x1FC3, 0x01D2, 0x03E0, 0x02F4, 0x0071, 0x1F82,
+		0x1FA7, 0x1FBB, 0x01BC, 0x03D9, 0x0306, 0x0081, 0x1F82,
+		0x1FAB, 0x1FB4, 0x01A6, 0x03D0, 0x0317, 0x0092, 0x1F82,
+		0x1FAF, 0x1FAD, 0x0190, 0x03C7, 0x0327, 0x00A3, 0x1F83,
+		0x1FB3, 0x1FA7, 0x017A, 0x03BC, 0x0337, 0x00B5, 0x1F84,
+		0x1FB8, 0x1FA1, 0x0165, 0x03B0, 0x0346, 0x00C7, 0x1F85,
+		0x1FBC, 0x1F9C, 0x0150, 0x03A4, 0x0354, 0x00D9, 0x1F87,
+		0x1FC0, 0x1F98, 0x013A, 0x0397, 0x0361, 0x00EC, 0x1F8A,
+		0x1FC4, 0x1F93, 0x0126, 0x0389, 0x036F, 0x00FE, 0x1F8D,
+		0x1F93, 0x010A, 0x0363, 0x0363, 0x010A, 0x1F93, 0x0000,
+		0x1F8D, 0x00FE, 0x036F, 0x0389, 0x0126, 0x1F93, 0x1FC4,
+		0x1F8A, 0x00EC, 0x0361, 0x0397, 0x013A, 0x1F98, 0x1FC0,
+		0x1F87, 0x00D9, 0x0354, 0x03A4, 0x0150, 0x1F9C, 0x1FBC,
+		0x1F85, 0x00C7, 0x0346, 0x03B0, 0x0165, 0x1FA1, 0x1FB8,
+		0x1F84, 0x00B5, 0x0337, 0x03BC, 0x017A, 0x1FA7, 0x1FB3,
+		0x1F83, 0x00A3, 0x0327, 0x03C7, 0x0190, 0x1FAD, 0x1FAF,
+		0x1F82, 0x0092, 0x0317, 0x03D0, 0x01A6, 0x1FB4, 0x1FAB,
+		0x1F82, 0x0081, 0x0306, 0x03D9, 0x01BC, 0x1FBB, 0x1FA7,
+		0x1F82, 0x0071, 0x02F4, 0x03E0, 0x01D2, 0x1FC3, 0x1FA4,
+		0x1F83, 0x0061, 0x02E1, 0x03E7, 0x01E8, 0x1FCC, 0x1FA0,
+		0x1F83, 0x0052, 0x02CF, 0x03ED, 0x01FE, 0x1FD5, 0x1F9C,
+		0x1F85, 0x0043, 0x02BB, 0x03F2, 0x0213, 0x1FDF, 0x1F99,
+		0x1F86, 0x0034, 0x02A7, 0x03F6, 0x022A, 0x1FEA, 0x1F95,
+		0x1F88, 0x0027, 0x0293, 0x03F8, 0x023F, 0x1FF5, 0x1F92,
+		0x1F8A, 0x0019, 0x027F, 0x03FA, 0x0255, 0x0000, 0x1F8F,
+		/* Chroma */
+		0x1F8D, 0x000C, 0x026A, 0x03FA, 0x026A, 0x000C, 0x1F8D,
+		0x1F8F, 0x0000, 0x0255, 0x03FA, 0x027F, 0x0019, 0x1F8A,
+		0x1F92, 0x1FF5, 0x023F, 0x03F8, 0x0293, 0x0027, 0x1F88,
+		0x1F95, 0x1FEA, 0x022A, 0x03F6, 0x02A7, 0x0034, 0x1F86,
+		0x1F99, 0x1FDF, 0x0213, 0x03F2, 0x02BB, 0x0043, 0x1F85,
+		0x1F9C, 0x1FD5, 0x01FE, 0x03ED, 0x02CF, 0x0052, 0x1F83,
+		0x1FA0, 0x1FCC, 0x01E8, 0x03E7, 0x02E1, 0x0061, 0x1F83,
+		0x1FA4, 0x1FC3, 0x01D2, 0x03E0, 0x02F4, 0x0071, 0x1F82,
+		0x1FA7, 0x1FBB, 0x01BC, 0x03D9, 0x0306, 0x0081, 0x1F82,
+		0x1FAB, 0x1FB4, 0x01A6, 0x03D0, 0x0317, 0x0092, 0x1F82,
+		0x1FAF, 0x1FAD, 0x0190, 0x03C7, 0x0327, 0x00A3, 0x1F83,
+		0x1FB3, 0x1FA7, 0x017A, 0x03BC, 0x0337, 0x00B5, 0x1F84,
+		0x1FB8, 0x1FA1, 0x0165, 0x03B0, 0x0346, 0x00C7, 0x1F85,
+		0x1FBC, 0x1F9C, 0x0150, 0x03A4, 0x0354, 0x00D9, 0x1F87,
+		0x1FC0, 0x1F98, 0x013A, 0x0397, 0x0361, 0x00EC, 0x1F8A,
+		0x1FC4, 0x1F93, 0x0126, 0x0389, 0x036F, 0x00FE, 0x1F8D,
+		0x1F93, 0x010A, 0x0363, 0x0363, 0x010A, 0x1F93, 0x0000,
+		0x1F8D, 0x00FE, 0x036F, 0x0389, 0x0126, 0x1F93, 0x1FC4,
+		0x1F8A, 0x00EC, 0x0361, 0x0397, 0x013A, 0x1F98, 0x1FC0,
+		0x1F87, 0x00D9, 0x0354, 0x03A4, 0x0150, 0x1F9C, 0x1FBC,
+		0x1F85, 0x00C7, 0x0346, 0x03B0, 0x0165, 0x1FA1, 0x1FB8,
+		0x1F84, 0x00B5, 0x0337, 0x03BC, 0x017A, 0x1FA7, 0x1FB3,
+		0x1F83, 0x00A3, 0x0327, 0x03C7, 0x0190, 0x1FAD, 0x1FAF,
+		0x1F82, 0x0092, 0x0317, 0x03D0, 0x01A6, 0x1FB4, 0x1FAB,
+		0x1F82, 0x0081, 0x0306, 0x03D9, 0x01BC, 0x1FBB, 0x1FA7,
+		0x1F82, 0x0071, 0x02F4, 0x03E0, 0x01D2, 0x1FC3, 0x1FA4,
+		0x1F83, 0x0061, 0x02E1, 0x03E7, 0x01E8, 0x1FCC, 0x1FA0,
+		0x1F83, 0x0052, 0x02CF, 0x03ED, 0x01FE, 0x1FD5, 0x1F9C,
+		0x1F85, 0x0043, 0x02BB, 0x03F2, 0x0213, 0x1FDF, 0x1F99,
+		0x1F86, 0x0034, 0x02A7, 0x03F6, 0x022A, 0x1FEA, 0x1F95,
+		0x1F88, 0x0027, 0x0293, 0x03F8, 0x023F, 0x1FF5, 0x1F92,
+		0x1F8A, 0x0019, 0x027F, 0x03FA, 0x0255, 0x0000, 0x1F8F,
+	},
+	[HS_LT_11_16_SCALE] = {
+		/* Luma */
+		0x1F95, 0x1FB5, 0x0272, 0x0488, 0x0272, 0x1FB5, 0x1F95,
+		0x1F9B, 0x1FAA, 0x0257, 0x0486, 0x028D, 0x1FC1, 0x1F90,
+		0x1FA0, 0x1FA0, 0x023C, 0x0485, 0x02A8, 0x1FCD, 0x1F8A,
+		0x1FA6, 0x1F96, 0x0221, 0x0481, 0x02C2, 0x1FDB, 0x1F85,
+		0x1FAC, 0x1F8E, 0x0205, 0x047C, 0x02DC, 0x1FE9, 0x1F80,
+		0x1FB1, 0x1F86, 0x01E9, 0x0476, 0x02F6, 0x1FF8, 0x1F7C,
+		0x1FB7, 0x1F7F, 0x01CE, 0x046E, 0x030F, 0x0008, 0x1F77,
+		0x1FBD, 0x1F79, 0x01B3, 0x0465, 0x0326, 0x0019, 0x1F73,
+		0x1FC3, 0x1F73, 0x0197, 0x045B, 0x033E, 0x002A, 0x1F70,
+		0x1FC8, 0x1F6F, 0x017D, 0x044E, 0x0355, 0x003C, 0x1F6D,
+		0x1FCE, 0x1F6B, 0x0162, 0x0441, 0x036B, 0x004F, 0x1F6A,
+		0x1FD3, 0x1F68, 0x0148, 0x0433, 0x0380, 0x0063, 0x1F67,
+		0x1FD8, 0x1F65, 0x012E, 0x0424, 0x0395, 0x0077, 0x1F65,
+		0x1FDE, 0x1F63, 0x0115, 0x0413, 0x03A8, 0x008B, 0x1F64,
+		0x1FE3, 0x1F62, 0x00FC, 0x0403, 0x03BA, 0x00A0, 0x1F62,
+		0x1FE7, 0x1F62, 0x00E4, 0x03EF, 0x03CC, 0x00B6, 0x1F62,
+		0x1F63, 0x00CA, 0x03D3, 0x03D3, 0x00CA, 0x1F63, 0x0000,
+		0x1F62, 0x00B6, 0x03CC, 0x03EF, 0x00E4, 0x1F62, 0x1FE7,
+		0x1F62, 0x00A0, 0x03BA, 0x0403, 0x00FC, 0x1F62, 0x1FE3,
+		0x1F64, 0x008B, 0x03A8, 0x0413, 0x0115, 0x1F63, 0x1FDE,
+		0x1F65, 0x0077, 0x0395, 0x0424, 0x012E, 0x1F65, 0x1FD8,
+		0x1F67, 0x0063, 0x0380, 0x0433, 0x0148, 0x1F68, 0x1FD3,
+		0x1F6A, 0x004F, 0x036B, 0x0441, 0x0162, 0x1F6B, 0x1FCE,
+		0x1F6D, 0x003C, 0x0355, 0x044E, 0x017D, 0x1F6F, 0x1FC8,
+		0x1F70, 0x002A, 0x033E, 0x045B, 0x0197, 0x1F73, 0x1FC3,
+		0x1F73, 0x0019, 0x0326, 0x0465, 0x01B3, 0x1F79, 0x1FBD,
+		0x1F77, 0x0008, 0x030F, 0x046E, 0x01CE, 0x1F7F, 0x1FB7,
+		0x1F7C, 0x1FF8, 0x02F6, 0x0476, 0x01E9, 0x1F86, 0x1FB1,
+		0x1F80, 0x1FE9, 0x02DC, 0x047C, 0x0205, 0x1F8E, 0x1FAC,
+		0x1F85, 0x1FDB, 0x02C2, 0x0481, 0x0221, 0x1F96, 0x1FA6,
+		0x1F8A, 0x1FCD, 0x02A8, 0x0485, 0x023C, 0x1FA0, 0x1FA0,
+		0x1F90, 0x1FC1, 0x028D, 0x0486, 0x0257, 0x1FAA, 0x1F9B,
+		/* Chroma */
+		0x1F95, 0x1FB5, 0x0272, 0x0488, 0x0272, 0x1FB5, 0x1F95,
+		0x1F9B, 0x1FAA, 0x0257, 0x0486, 0x028D, 0x1FC1, 0x1F90,
+		0x1FA0, 0x1FA0, 0x023C, 0x0485, 0x02A8, 0x1FCD, 0x1F8A,
+		0x1FA6, 0x1F96, 0x0221, 0x0481, 0x02C2, 0x1FDB, 0x1F85,
+		0x1FAC, 0x1F8E, 0x0205, 0x047C, 0x02DC, 0x1FE9, 0x1F80,
+		0x1FB1, 0x1F86, 0x01E9, 0x0476, 0x02F6, 0x1FF8, 0x1F7C,
+		0x1FB7, 0x1F7F, 0x01CE, 0x046E, 0x030F, 0x0008, 0x1F77,
+		0x1FBD, 0x1F79, 0x01B3, 0x0465, 0x0326, 0x0019, 0x1F73,
+		0x1FC3, 0x1F73, 0x0197, 0x045B, 0x033E, 0x002A, 0x1F70,
+		0x1FC8, 0x1F6F, 0x017D, 0x044E, 0x0355, 0x003C, 0x1F6D,
+		0x1FCE, 0x1F6B, 0x0162, 0x0441, 0x036B, 0x004F, 0x1F6A,
+		0x1FD3, 0x1F68, 0x0148, 0x0433, 0x0380, 0x0063, 0x1F67,
+		0x1FD8, 0x1F65, 0x012E, 0x0424, 0x0395, 0x0077, 0x1F65,
+		0x1FDE, 0x1F63, 0x0115, 0x0413, 0x03A8, 0x008B, 0x1F64,
+		0x1FE3, 0x1F62, 0x00FC, 0x0403, 0x03BA, 0x00A0, 0x1F62,
+		0x1FE7, 0x1F62, 0x00E4, 0x03EF, 0x03CC, 0x00B6, 0x1F62,
+		0x1F63, 0x00CA, 0x03D3, 0x03D3, 0x00CA, 0x1F63, 0x0000,
+		0x1F62, 0x00B6, 0x03CC, 0x03EF, 0x00E4, 0x1F62, 0x1FE7,
+		0x1F62, 0x00A0, 0x03BA, 0x0403, 0x00FC, 0x1F62, 0x1FE3,
+		0x1F64, 0x008B, 0x03A8, 0x0413, 0x0115, 0x1F63, 0x1FDE,
+		0x1F65, 0x0077, 0x0395, 0x0424, 0x012E, 0x1F65, 0x1FD8,
+		0x1F67, 0x0063, 0x0380, 0x0433, 0x0148, 0x1F68, 0x1FD3,
+		0x1F6A, 0x004F, 0x036B, 0x0441, 0x0162, 0x1F6B, 0x1FCE,
+		0x1F6D, 0x003C, 0x0355, 0x044E, 0x017D, 0x1F6F, 0x1FC8,
+		0x1F70, 0x002A, 0x033E, 0x045B, 0x0197, 0x1F73, 0x1FC3,
+		0x1F73, 0x0019, 0x0326, 0x0465, 0x01B3, 0x1F79, 0x1FBD,
+		0x1F77, 0x0008, 0x030F, 0x046E, 0x01CE, 0x1F7F, 0x1FB7,
+		0x1F7C, 0x1FF8, 0x02F6, 0x0476, 0x01E9, 0x1F86, 0x1FB1,
+		0x1F80, 0x1FE9, 0x02DC, 0x047C, 0x0205, 0x1F8E, 0x1FAC,
+		0x1F85, 0x1FDB, 0x02C2, 0x0481, 0x0221, 0x1F96, 0x1FA6,
+		0x1F8A, 0x1FCD, 0x02A8, 0x0485, 0x023C, 0x1FA0, 0x1FA0,
+		0x1F90, 0x1FC1, 0x028D, 0x0486, 0x0257, 0x1FAA, 0x1F9B,
+	},
+	[HS_LT_12_16_SCALE] = {
+		/* Luma */
+		0x1FBB, 0x1F65, 0x025E, 0x0504, 0x025E, 0x1F65, 0x1FBB,
+		0x1FC3, 0x1F5D, 0x023C, 0x0503, 0x027F, 0x1F6E, 0x1FB4,
+		0x1FCA, 0x1F56, 0x021B, 0x0501, 0x02A0, 0x1F78, 0x1FAC,
+		0x1FD1, 0x1F50, 0x01FA, 0x04FD, 0x02C0, 0x1F83, 0x1FA5,
+		0x1FD8, 0x1F4B, 0x01D9, 0x04F6, 0x02E1, 0x1F90, 0x1F9D,
+		0x1FDF, 0x1F47, 0x01B8, 0x04EF, 0x0301, 0x1F9D, 0x1F95,
+		0x1FE6, 0x1F43, 0x0198, 0x04E5, 0x0321, 0x1FAB, 0x1F8E,
+		0x1FEC, 0x1F41, 0x0178, 0x04DA, 0x0340, 0x1FBB, 0x1F86,
+		0x1FF2, 0x1F40, 0x0159, 0x04CC, 0x035E, 0x1FCC, 0x1F7F,
+		0x1FF8, 0x1F40, 0x013A, 0x04BE, 0x037B, 0x1FDD, 0x1F78,
+		0x1FFE, 0x1F40, 0x011B, 0x04AD, 0x0398, 0x1FF0, 0x1F72,
+		0x0003, 0x1F41, 0x00FD, 0x049C, 0x03B4, 0x0004, 0x1F6B,
+		0x0008, 0x1F43, 0x00E0, 0x0489, 0x03CE, 0x0019, 0x1F65,
+		0x000D, 0x1F46, 0x00C4, 0x0474, 0x03E8, 0x002E, 0x1F5F,
+		0x0011, 0x1F49, 0x00A9, 0x045E, 0x0400, 0x0045, 0x1F5A,
+		0x0015, 0x1F4D, 0x008E, 0x0447, 0x0418, 0x005C, 0x1F55,
+		0x1F4F, 0x0076, 0x043B, 0x043B, 0x0076, 0x1F4F, 0x0000,
+		0x1F55, 0x005C, 0x0418, 0x0447, 0x008E, 0x1F4D, 0x0015,
+		0x1F5A, 0x0045, 0x0400, 0x045E, 0x00A9, 0x1F49, 0x0011,
+		0x1F5F, 0x002E, 0x03E8, 0x0474, 0x00C4, 0x1F46, 0x000D,
+		0x1F65, 0x0019, 0x03CE, 0x0489, 0x00E0, 0x1F43, 0x0008,
+		0x1F6B, 0x0004, 0x03B4, 0x049C, 0x00FD, 0x1F41, 0x0003,
+		0x1F72, 0x1FF0, 0x0398, 0x04AD, 0x011B, 0x1F40, 0x1FFE,
+		0x1F78, 0x1FDD, 0x037B, 0x04BE, 0x013A, 0x1F40, 0x1FF8,
+		0x1F7F, 0x1FCC, 0x035E, 0x04CC, 0x0159, 0x1F40, 0x1FF2,
+		0x1F86, 0x1FBB, 0x0340, 0x04DA, 0x0178, 0x1F41, 0x1FEC,
+		0x1F8E, 0x1FAB, 0x0321, 0x04E5, 0x0198, 0x1F43, 0x1FE6,
+		0x1F95, 0x1F9D, 0x0301, 0x04EF, 0x01B8, 0x1F47, 0x1FDF,
+		0x1F9D, 0x1F90, 0x02E1, 0x04F6, 0x01D9, 0x1F4B, 0x1FD8,
+		0x1FA5, 0x1F83, 0x02C0, 0x04FD, 0x01FA, 0x1F50, 0x1FD1,
+		0x1FAC, 0x1F78, 0x02A0, 0x0501, 0x021B, 0x1F56, 0x1FCA,
+		0x1FB4, 0x1F6E, 0x027F, 0x0503, 0x023C, 0x1F5D, 0x1FC3,
+		/* Chroma */
+		0x1FBB, 0x1F65, 0x025E, 0x0504, 0x025E, 0x1F65, 0x1FBB,
+		0x1FC3, 0x1F5D, 0x023C, 0x0503, 0x027F, 0x1F6E, 0x1FB4,
+		0x1FCA, 0x1F56, 0x021B, 0x0501, 0x02A0, 0x1F78, 0x1FAC,
+		0x1FD1, 0x1F50, 0x01FA, 0x04FD, 0x02C0, 0x1F83, 0x1FA5,
+		0x1FD8, 0x1F4B, 0x01D9, 0x04F6, 0x02E1, 0x1F90, 0x1F9D,
+		0x1FDF, 0x1F47, 0x01B8, 0x04EF, 0x0301, 0x1F9D, 0x1F95,
+		0x1FE6, 0x1F43, 0x0198, 0x04E5, 0x0321, 0x1FAB, 0x1F8E,
+		0x1FEC, 0x1F41, 0x0178, 0x04DA, 0x0340, 0x1FBB, 0x1F86,
+		0x1FF2, 0x1F40, 0x0159, 0x04CC, 0x035E, 0x1FCC, 0x1F7F,
+		0x1FF8, 0x1F40, 0x013A, 0x04BE, 0x037B, 0x1FDD, 0x1F78,
+		0x1FFE, 0x1F40, 0x011B, 0x04AD, 0x0398, 0x1FF0, 0x1F72,
+		0x0003, 0x1F41, 0x00FD, 0x049C, 0x03B4, 0x0004, 0x1F6B,
+		0x0008, 0x1F43, 0x00E0, 0x0489, 0x03CE, 0x0019, 0x1F65,
+		0x000D, 0x1F46, 0x00C4, 0x0474, 0x03E8, 0x002E, 0x1F5F,
+		0x0011, 0x1F49, 0x00A9, 0x045E, 0x0400, 0x0045, 0x1F5A,
+		0x0015, 0x1F4D, 0x008E, 0x0447, 0x0418, 0x005C, 0x1F55,
+		0x1F4F, 0x0076, 0x043B, 0x043B, 0x0076, 0x1F4F, 0x0000,
+		0x1F55, 0x005C, 0x0418, 0x0447, 0x008E, 0x1F4D, 0x0015,
+		0x1F5A, 0x0045, 0x0400, 0x045E, 0x00A9, 0x1F49, 0x0011,
+		0x1F5F, 0x002E, 0x03E8, 0x0474, 0x00C4, 0x1F46, 0x000D,
+		0x1F65, 0x0019, 0x03CE, 0x0489, 0x00E0, 0x1F43, 0x0008,
+		0x1F6B, 0x0004, 0x03B4, 0x049C, 0x00FD, 0x1F41, 0x0003,
+		0x1F72, 0x1FF0, 0x0398, 0x04AD, 0x011B, 0x1F40, 0x1FFE,
+		0x1F78, 0x1FDD, 0x037B, 0x04BE, 0x013A, 0x1F40, 0x1FF8,
+		0x1F7F, 0x1FCC, 0x035E, 0x04CC, 0x0159, 0x1F40, 0x1FF2,
+		0x1F86, 0x1FBB, 0x0340, 0x04DA, 0x0178, 0x1F41, 0x1FEC,
+		0x1F8E, 0x1FAB, 0x0321, 0x04E5, 0x0198, 0x1F43, 0x1FE6,
+		0x1F95, 0x1F9D, 0x0301, 0x04EF, 0x01B8, 0x1F47, 0x1FDF,
+		0x1F9D, 0x1F90, 0x02E1, 0x04F6, 0x01D9, 0x1F4B, 0x1FD8,
+		0x1FA5, 0x1F83, 0x02C0, 0x04FD, 0x01FA, 0x1F50, 0x1FD1,
+		0x1FAC, 0x1F78, 0x02A0, 0x0501, 0x021B, 0x1F56, 0x1FCA,
+		0x1FB4, 0x1F6E, 0x027F, 0x0503, 0x023C, 0x1F5D, 0x1FC3,
+	},
+	[HS_LT_13_16_SCALE] = {
+		/* Luma */
+		0x1FF4, 0x1F29, 0x022D, 0x056C, 0x022D, 0x1F29, 0x1FF4,
+		0x1FFC, 0x1F26, 0x0206, 0x056A, 0x0254, 0x1F2E, 0x1FEC,
+		0x0003, 0x1F24, 0x01E0, 0x0567, 0x027A, 0x1F34, 0x1FE4,
+		0x000A, 0x1F23, 0x01BA, 0x0561, 0x02A2, 0x1F3B, 0x1FDB,
+		0x0011, 0x1F22, 0x0194, 0x055B, 0x02C9, 0x1F43, 0x1FD2,
+		0x0017, 0x1F23, 0x016F, 0x0551, 0x02F0, 0x1F4D, 0x1FC9,
+		0x001D, 0x1F25, 0x014B, 0x0545, 0x0316, 0x1F58, 0x1FC0,
+		0x0022, 0x1F28, 0x0127, 0x0538, 0x033C, 0x1F65, 0x1FB6,
+		0x0027, 0x1F2C, 0x0104, 0x0528, 0x0361, 0x1F73, 0x1FAD,
+		0x002B, 0x1F30, 0x00E2, 0x0518, 0x0386, 0x1F82, 0x1FA3,
+		0x002F, 0x1F36, 0x00C2, 0x0504, 0x03AA, 0x1F92, 0x1F99,
+		0x0032, 0x1F3C, 0x00A2, 0x04EF, 0x03CD, 0x1FA4, 0x1F90,
+		0x0035, 0x1F42, 0x0083, 0x04D9, 0x03EF, 0x1FB8, 0x1F86,
+		0x0038, 0x1F49, 0x0065, 0x04C0, 0x0410, 0x1FCD, 0x1F7D,
+		0x003A, 0x1F51, 0x0048, 0x04A6, 0x0431, 0x1FE3, 0x1F73,
+		0x003C, 0x1F59, 0x002D, 0x048A, 0x0450, 0x1FFA, 0x1F6A,
+		0x1F5D, 0x0014, 0x048F, 0x048F, 0x0014, 0x1F5D, 0x0000,
+		0x1F6A, 0x1FFA, 0x0450, 0x048A, 0x002D, 0x1F59, 0x003C,
+		0x1F73, 0x1FE3, 0x0431, 0x04A6, 0x0048, 0x1F51, 0x003A,
+		0x1F7D, 0x1FCD, 0x0410, 0x04C0, 0x0065, 0x1F49, 0x0038,
+		0x1F86, 0x1FB8, 0x03EF, 0x04D9, 0x0083, 0x1F42, 0x0035,
+		0x1F90, 0x1FA4, 0x03CD, 0x04EF, 0x00A2, 0x1F3C, 0x0032,
+		0x1F99, 0x1F92, 0x03AA, 0x0504, 0x00C2, 0x1F36, 0x002F,
+		0x1FA3, 0x1F82, 0x0386, 0x0518, 0x00E2, 0x1F30, 0x002B,
+		0x1FAD, 0x1F73, 0x0361, 0x0528, 0x0104, 0x1F2C, 0x0027,
+		0x1FB6, 0x1F65, 0x033C, 0x0538, 0x0127, 0x1F28, 0x0022,
+		0x1FC0, 0x1F58, 0x0316, 0x0545, 0x014B, 0x1F25, 0x001D,
+		0x1FC9, 0x1F4D, 0x02F0, 0x0551, 0x016F, 0x1F23, 0x0017,
+		0x1FD2, 0x1F43, 0x02C9, 0x055B, 0x0194, 0x1F22, 0x0011,
+		0x1FDB, 0x1F3B, 0x02A2, 0x0561, 0x01BA, 0x1F23, 0x000A,
+		0x1FE4, 0x1F34, 0x027A, 0x0567, 0x01E0, 0x1F24, 0x0003,
+		0x1FEC, 0x1F2E, 0x0254, 0x056A, 0x0206, 0x1F26, 0x1FFC,
+		/* Chroma */
+		0x1FF4, 0x1F29, 0x022D, 0x056C, 0x022D, 0x1F29, 0x1FF4,
+		0x1FFC, 0x1F26, 0x0206, 0x056A, 0x0254, 0x1F2E, 0x1FEC,
+		0x0003, 0x1F24, 0x01E0, 0x0567, 0x027A, 0x1F34, 0x1FE4,
+		0x000A, 0x1F23, 0x01BA, 0x0561, 0x02A2, 0x1F3B, 0x1FDB,
+		0x0011, 0x1F22, 0x0194, 0x055B, 0x02C9, 0x1F43, 0x1FD2,
+		0x0017, 0x1F23, 0x016F, 0x0551, 0x02F0, 0x1F4D, 0x1FC9,
+		0x001D, 0x1F25, 0x014B, 0x0545, 0x0316, 0x1F58, 0x1FC0,
+		0x0022, 0x1F28, 0x0127, 0x0538, 0x033C, 0x1F65, 0x1FB6,
+		0x0027, 0x1F2C, 0x0104, 0x0528, 0x0361, 0x1F73, 0x1FAD,
+		0x002B, 0x1F30, 0x00E2, 0x0518, 0x0386, 0x1F82, 0x1FA3,
+		0x002F, 0x1F36, 0x00C2, 0x0504, 0x03AA, 0x1F92, 0x1F99,
+		0x0032, 0x1F3C, 0x00A2, 0x04EF, 0x03CD, 0x1FA4, 0x1F90,
+		0x0035, 0x1F42, 0x0083, 0x04D9, 0x03EF, 0x1FB8, 0x1F86,
+		0x0038, 0x1F49, 0x0065, 0x04C0, 0x0410, 0x1FCD, 0x1F7D,
+		0x003A, 0x1F51, 0x0048, 0x04A6, 0x0431, 0x1FE3, 0x1F73,
+		0x003C, 0x1F59, 0x002D, 0x048A, 0x0450, 0x1FFA, 0x1F6A,
+		0x1F5D, 0x0014, 0x048F, 0x048F, 0x0014, 0x1F5D, 0x0000,
+		0x1F6A, 0x1FFA, 0x0450, 0x048A, 0x002D, 0x1F59, 0x003C,
+		0x1F73, 0x1FE3, 0x0431, 0x04A6, 0x0048, 0x1F51, 0x003A,
+		0x1F7D, 0x1FCD, 0x0410, 0x04C0, 0x0065, 0x1F49, 0x0038,
+		0x1F86, 0x1FB8, 0x03EF, 0x04D9, 0x0083, 0x1F42, 0x0035,
+		0x1F90, 0x1FA4, 0x03CD, 0x04EF, 0x00A2, 0x1F3C, 0x0032,
+		0x1F99, 0x1F92, 0x03AA, 0x0504, 0x00C2, 0x1F36, 0x002F,
+		0x1FA3, 0x1F82, 0x0386, 0x0518, 0x00E2, 0x1F30, 0x002B,
+		0x1FAD, 0x1F73, 0x0361, 0x0528, 0x0104, 0x1F2C, 0x0027,
+		0x1FB6, 0x1F65, 0x033C, 0x0538, 0x0127, 0x1F28, 0x0022,
+		0x1FC0, 0x1F58, 0x0316, 0x0545, 0x014B, 0x1F25, 0x001D,
+		0x1FC9, 0x1F4D, 0x02F0, 0x0551, 0x016F, 0x1F23, 0x0017,
+		0x1FD2, 0x1F43, 0x02C9, 0x055B, 0x0194, 0x1F22, 0x0011,
+		0x1FDB, 0x1F3B, 0x02A2, 0x0561, 0x01BA, 0x1F23, 0x000A,
+		0x1FE4, 0x1F34, 0x027A, 0x0567, 0x01E0, 0x1F24, 0x0003,
+		0x1FEC, 0x1F2E, 0x0254, 0x056A, 0x0206, 0x1F26, 0x1FFC,
+	},
+	[HS_LT_14_16_SCALE] = {
+		/* Luma */
+		0x002F, 0x1F0B, 0x01E7, 0x05BE, 0x01E7, 0x1F0B, 0x002F,
+		0x0035, 0x1F0D, 0x01BC, 0x05BD, 0x0213, 0x1F0A, 0x0028,
+		0x003A, 0x1F11, 0x0191, 0x05BA, 0x023F, 0x1F0A, 0x0021,
+		0x003F, 0x1F15, 0x0167, 0x05B3, 0x026C, 0x1F0C, 0x001A,
+		0x0043, 0x1F1B, 0x013E, 0x05AA, 0x0299, 0x1F0F, 0x0012,
+		0x0046, 0x1F21, 0x0116, 0x05A1, 0x02C6, 0x1F13, 0x0009,
+		0x0049, 0x1F28, 0x00EF, 0x0593, 0x02F4, 0x1F19, 0x0000,
+		0x004C, 0x1F30, 0x00C9, 0x0584, 0x0321, 0x1F20, 0x1FF6,
+		0x004E, 0x1F39, 0x00A4, 0x0572, 0x034D, 0x1F2A, 0x1FEC,
+		0x004F, 0x1F43, 0x0080, 0x055E, 0x037A, 0x1F34, 0x1FE2,
+		0x0050, 0x1F4D, 0x005E, 0x0548, 0x03A5, 0x1F41, 0x1FD7,
+		0x0050, 0x1F57, 0x003D, 0x0531, 0x03D1, 0x1F4F, 0x1FCB,
+		0x0050, 0x1F62, 0x001E, 0x0516, 0x03FB, 0x1F5F, 0x1FC0,
+		0x004F, 0x1F6D, 0x0000, 0x04FA, 0x0425, 0x1F71, 0x1FB4,
+		0x004E, 0x1F79, 0x1FE4, 0x04DC, 0x044D, 0x1F84, 0x1FA8,
+		0x004D, 0x1F84, 0x1FCA, 0x04BC, 0x0474, 0x1F99, 0x1F9C,
+		0x1F8C, 0x1FAE, 0x04C6, 0x04C6, 0x1FAE, 0x1F8C, 0x0000,
+		0x1F9C, 0x1F99, 0x0474, 0x04BC, 0x1FCA, 0x1F84, 0x004D,
+		0x1FA8, 0x1F84, 0x044D, 0x04DC, 0x1FE4, 0x1F79, 0x004E,
+		0x1FB4, 0x1F71, 0x0425, 0x04FA, 0x0000, 0x1F6D, 0x004F,
+		0x1FC0, 0x1F5F, 0x03FB, 0x0516, 0x001E, 0x1F62, 0x0050,
+		0x1FCB, 0x1F4F, 0x03D1, 0x0531, 0x003D, 0x1F57, 0x0050,
+		0x1FD7, 0x1F41, 0x03A5, 0x0548, 0x005E, 0x1F4D, 0x0050,
+		0x1FE2, 0x1F34, 0x037A, 0x055E, 0x0080, 0x1F43, 0x004F,
+		0x1FEC, 0x1F2A, 0x034D, 0x0572, 0x00A4, 0x1F39, 0x004E,
+		0x1FF6, 0x1F20, 0x0321, 0x0584, 0x00C9, 0x1F30, 0x004C,
+		0x0000, 0x1F19, 0x02F4, 0x0593, 0x00EF, 0x1F28, 0x0049,
+		0x0009, 0x1F13, 0x02C6, 0x05A1, 0x0116, 0x1F21, 0x0046,
+		0x0012, 0x1F0F, 0x0299, 0x05AA, 0x013E, 0x1F1B, 0x0043,
+		0x001A, 0x1F0C, 0x026C, 0x05B3, 0x0167, 0x1F15, 0x003F,
+		0x0021, 0x1F0A, 0x023F, 0x05BA, 0x0191, 0x1F11, 0x003A,
+		0x0028, 0x1F0A, 0x0213, 0x05BD, 0x01BC, 0x1F0D, 0x0035,
+		/* Chroma */
+		0x002F, 0x1F0B, 0x01E7, 0x05BE, 0x01E7, 0x1F0B, 0x002F,
+		0x0035, 0x1F0D, 0x01BC, 0x05BD, 0x0213, 0x1F0A, 0x0028,
+		0x003A, 0x1F11, 0x0191, 0x05BA, 0x023F, 0x1F0A, 0x0021,
+		0x003F, 0x1F15, 0x0167, 0x05B3, 0x026C, 0x1F0C, 0x001A,
+		0x0043, 0x1F1B, 0x013E, 0x05AA, 0x0299, 0x1F0F, 0x0012,
+		0x0046, 0x1F21, 0x0116, 0x05A1, 0x02C6, 0x1F13, 0x0009,
+		0x0049, 0x1F28, 0x00EF, 0x0593, 0x02F4, 0x1F19, 0x0000,
+		0x004C, 0x1F30, 0x00C9, 0x0584, 0x0321, 0x1F20, 0x1FF6,
+		0x004E, 0x1F39, 0x00A4, 0x0572, 0x034D, 0x1F2A, 0x1FEC,
+		0x004F, 0x1F43, 0x0080, 0x055E, 0x037A, 0x1F34, 0x1FE2,
+		0x0050, 0x1F4D, 0x005E, 0x0548, 0x03A5, 0x1F41, 0x1FD7,
+		0x0050, 0x1F57, 0x003D, 0x0531, 0x03D1, 0x1F4F, 0x1FCB,
+		0x0050, 0x1F62, 0x001E, 0x0516, 0x03FB, 0x1F5F, 0x1FC0,
+		0x004F, 0x1F6D, 0x0000, 0x04FA, 0x0425, 0x1F71, 0x1FB4,
+		0x004E, 0x1F79, 0x1FE4, 0x04DC, 0x044D, 0x1F84, 0x1FA8,
+		0x004D, 0x1F84, 0x1FCA, 0x04BC, 0x0474, 0x1F99, 0x1F9C,
+		0x1F8C, 0x1FAE, 0x04C6, 0x04C6, 0x1FAE, 0x1F8C, 0x0000,
+		0x1F9C, 0x1F99, 0x0474, 0x04BC, 0x1FCA, 0x1F84, 0x004D,
+		0x1FA8, 0x1F84, 0x044D, 0x04DC, 0x1FE4, 0x1F79, 0x004E,
+		0x1FB4, 0x1F71, 0x0425, 0x04FA, 0x0000, 0x1F6D, 0x004F,
+		0x1FC0, 0x1F5F, 0x03FB, 0x0516, 0x001E, 0x1F62, 0x0050,
+		0x1FCB, 0x1F4F, 0x03D1, 0x0531, 0x003D, 0x1F57, 0x0050,
+		0x1FD7, 0x1F41, 0x03A5, 0x0548, 0x005E, 0x1F4D, 0x0050,
+		0x1FE2, 0x1F34, 0x037A, 0x055E, 0x0080, 0x1F43, 0x004F,
+		0x1FEC, 0x1F2A, 0x034D, 0x0572, 0x00A4, 0x1F39, 0x004E,
+		0x1FF6, 0x1F20, 0x0321, 0x0584, 0x00C9, 0x1F30, 0x004C,
+		0x0000, 0x1F19, 0x02F4, 0x0593, 0x00EF, 0x1F28, 0x0049,
+		0x0009, 0x1F13, 0x02C6, 0x05A1, 0x0116, 0x1F21, 0x0046,
+		0x0012, 0x1F0F, 0x0299, 0x05AA, 0x013E, 0x1F1B, 0x0043,
+		0x001A, 0x1F0C, 0x026C, 0x05B3, 0x0167, 0x1F15, 0x003F,
+		0x0021, 0x1F0A, 0x023F, 0x05BA, 0x0191, 0x1F11, 0x003A,
+		0x0028, 0x1F0A, 0x0213, 0x05BD, 0x01BC, 0x1F0D, 0x0035,
+	},
+	[HS_LT_15_16_SCALE] = {
+		/* Luma */
+		0x005B, 0x1F0A, 0x0195, 0x060C, 0x0195, 0x1F0A, 0x005B,
+		0x005D, 0x1F13, 0x0166, 0x0609, 0x01C6, 0x1F03, 0x0058,
+		0x005F, 0x1F1C, 0x0138, 0x0605, 0x01F7, 0x1EFD, 0x0054,
+		0x0060, 0x1F26, 0x010B, 0x05FF, 0x0229, 0x1EF8, 0x004F,
+		0x0060, 0x1F31, 0x00DF, 0x05F5, 0x025C, 0x1EF5, 0x004A,
+		0x0060, 0x1F3D, 0x00B5, 0x05E8, 0x028F, 0x1EF3, 0x0044,
+		0x005F, 0x1F49, 0x008C, 0x05DA, 0x02C3, 0x1EF2, 0x003D,
+		0x005E, 0x1F56, 0x0065, 0x05C7, 0x02F6, 0x1EF4, 0x0036,
+		0x005C, 0x1F63, 0x003F, 0x05B3, 0x032B, 0x1EF7, 0x002D,
+		0x0059, 0x1F71, 0x001B, 0x059D, 0x035F, 0x1EFB, 0x0024,
+		0x0057, 0x1F7F, 0x1FF9, 0x0583, 0x0392, 0x1F02, 0x001A,
+		0x0053, 0x1F8D, 0x1FD9, 0x0567, 0x03C5, 0x1F0B, 0x0010,
+		0x0050, 0x1F9B, 0x1FBB, 0x0548, 0x03F8, 0x1F15, 0x0005,
+		0x004C, 0x1FA9, 0x1F9E, 0x0528, 0x042A, 0x1F22, 0x1FF9,
+		0x0048, 0x1FB7, 0x1F84, 0x0505, 0x045A, 0x1F31, 0x1FED,
+		0x0043, 0x1FC5, 0x1F6C, 0x04E0, 0x048A, 0x1F42, 0x1FE0,
+		0x1FD1, 0x1F50, 0x04DF, 0x04DF, 0x1F50, 0x1FD1, 0x0000,
+		0x1FE0, 0x1F42, 0x048A, 0x04E0, 0x1F6C, 0x1FC5, 0x0043,
+		0x1FED, 0x1F31, 0x045A, 0x0505, 0x1F84, 0x1FB7, 0x0048,
+		0x1FF9, 0x1F22, 0x042A, 0x0528, 0x1F9E, 0x1FA9, 0x004C,
+		0x0005, 0x1F15, 0x03F8, 0x0548, 0x1FBB, 0x1F9B, 0x0050,
+		0x0010, 0x1F0B, 0x03C5, 0x0567, 0x1FD9, 0x1F8D, 0x0053,
+		0x001A, 0x1F02, 0x0392, 0x0583, 0x1FF9, 0x1F7F, 0x0057,
+		0x0024, 0x1EFB, 0x035F, 0x059D, 0x001B, 0x1F71, 0x0059,
+		0x002D, 0x1EF7, 0x032B, 0x05B3, 0x003F, 0x1F63, 0x005C,
+		0x0036, 0x1EF4, 0x02F6, 0x05C7, 0x0065, 0x1F56, 0x005E,
+		0x003D, 0x1EF2, 0x02C3, 0x05DA, 0x008C, 0x1F49, 0x005F,
+		0x0044, 0x1EF3, 0x028F, 0x05E8, 0x00B5, 0x1F3D, 0x0060,
+		0x004A, 0x1EF5, 0x025C, 0x05F5, 0x00DF, 0x1F31, 0x0060,
+		0x004F, 0x1EF8, 0x0229, 0x05FF, 0x010B, 0x1F26, 0x0060,
+		0x0054, 0x1EFD, 0x01F7, 0x0605, 0x0138, 0x1F1C, 0x005F,
+		0x0058, 0x1F03, 0x01C6, 0x0609, 0x0166, 0x1F13, 0x005D,
+		/* Chroma */
+		0x005B, 0x1F0A, 0x0195, 0x060C, 0x0195, 0x1F0A, 0x005B,
+		0x005D, 0x1F13, 0x0166, 0x0609, 0x01C6, 0x1F03, 0x0058,
+		0x005F, 0x1F1C, 0x0138, 0x0605, 0x01F7, 0x1EFD, 0x0054,
+		0x0060, 0x1F26, 0x010B, 0x05FF, 0x0229, 0x1EF8, 0x004F,
+		0x0060, 0x1F31, 0x00DF, 0x05F5, 0x025C, 0x1EF5, 0x004A,
+		0x0060, 0x1F3D, 0x00B5, 0x05E8, 0x028F, 0x1EF3, 0x0044,
+		0x005F, 0x1F49, 0x008C, 0x05DA, 0x02C3, 0x1EF2, 0x003D,
+		0x005E, 0x1F56, 0x0065, 0x05C7, 0x02F6, 0x1EF4, 0x0036,
+		0x005C, 0x1F63, 0x003F, 0x05B3, 0x032B, 0x1EF7, 0x002D,
+		0x0059, 0x1F71, 0x001B, 0x059D, 0x035F, 0x1EFB, 0x0024,
+		0x0057, 0x1F7F, 0x1FF9, 0x0583, 0x0392, 0x1F02, 0x001A,
+		0x0053, 0x1F8D, 0x1FD9, 0x0567, 0x03C5, 0x1F0B, 0x0010,
+		0x0050, 0x1F9B, 0x1FBB, 0x0548, 0x03F8, 0x1F15, 0x0005,
+		0x004C, 0x1FA9, 0x1F9E, 0x0528, 0x042A, 0x1F22, 0x1FF9,
+		0x0048, 0x1FB7, 0x1F84, 0x0505, 0x045A, 0x1F31, 0x1FED,
+		0x0043, 0x1FC5, 0x1F6C, 0x04E0, 0x048A, 0x1F42, 0x1FE0,
+		0x1FD1, 0x1F50, 0x04DF, 0x04DF, 0x1F50, 0x1FD1, 0x0000,
+		0x1FE0, 0x1F42, 0x048A, 0x04E0, 0x1F6C, 0x1FC5, 0x0043,
+		0x1FED, 0x1F31, 0x045A, 0x0505, 0x1F84, 0x1FB7, 0x0048,
+		0x1FF9, 0x1F22, 0x042A, 0x0528, 0x1F9E, 0x1FA9, 0x004C,
+		0x0005, 0x1F15, 0x03F8, 0x0548, 0x1FBB, 0x1F9B, 0x0050,
+		0x0010, 0x1F0B, 0x03C5, 0x0567, 0x1FD9, 0x1F8D, 0x0053,
+		0x001A, 0x1F02, 0x0392, 0x0583, 0x1FF9, 0x1F7F, 0x0057,
+		0x0024, 0x1EFB, 0x035F, 0x059D, 0x001B, 0x1F71, 0x0059,
+		0x002D, 0x1EF7, 0x032B, 0x05B3, 0x003F, 0x1F63, 0x005C,
+		0x0036, 0x1EF4, 0x02F6, 0x05C7, 0x0065, 0x1F56, 0x005E,
+		0x003D, 0x1EF2, 0x02C3, 0x05DA, 0x008C, 0x1F49, 0x005F,
+		0x0044, 0x1EF3, 0x028F, 0x05E8, 0x00B5, 0x1F3D, 0x0060,
+		0x004A, 0x1EF5, 0x025C, 0x05F5, 0x00DF, 0x1F31, 0x0060,
+		0x004F, 0x1EF8, 0x0229, 0x05FF, 0x010B, 0x1F26, 0x0060,
+		0x0054, 0x1EFD, 0x01F7, 0x0605, 0x0138, 0x1F1C, 0x005F,
+		0x0058, 0x1F03, 0x01C6, 0x0609, 0x0166, 0x1F13, 0x005D,
+	},
+	[HS_LE_16_16_SCALE] = {
+		/* Luma */
+		0x006E, 0x1F24, 0x013E, 0x0660, 0x013E, 0x1F24, 0x006E,
+		0x006C, 0x1F33, 0x010B, 0x065D, 0x0172, 0x1F17, 0x0070,
+		0x0069, 0x1F41, 0x00DA, 0x0659, 0x01A8, 0x1F0B, 0x0070,
+		0x0066, 0x1F51, 0x00AA, 0x0650, 0x01DF, 0x1F00, 0x0070,
+		0x0062, 0x1F61, 0x007D, 0x0644, 0x0217, 0x1EF6, 0x006F,
+		0x005E, 0x1F71, 0x0051, 0x0636, 0x0250, 0x1EED, 0x006D,
+		0x0059, 0x1F81, 0x0028, 0x0624, 0x028A, 0x1EE5, 0x006B,
+		0x0054, 0x1F91, 0x0000, 0x060F, 0x02C5, 0x1EE0, 0x0067,
+		0x004E, 0x1FA2, 0x1FDB, 0x05F6, 0x0300, 0x1EDC, 0x0063,
+		0x0049, 0x1FB2, 0x1FB8, 0x05DB, 0x033B, 0x1EDA, 0x005D,
+		0x0043, 0x1FC3, 0x1F98, 0x05BC, 0x0376, 0x1ED9, 0x0057,
+		0x003D, 0x1FD3, 0x1F7A, 0x059B, 0x03B1, 0x1EDB, 0x004F,
+		0x0036, 0x1FE2, 0x1F5E, 0x0578, 0x03EC, 0x1EDF, 0x0047,
+		0x0030, 0x1FF1, 0x1F45, 0x0551, 0x0426, 0x1EE6, 0x003D,
+		0x002A, 0x0000, 0x1F2E, 0x0528, 0x045F, 0x1EEE, 0x0033,
+		0x0023, 0x000E, 0x1F19, 0x04FD, 0x0498, 0x1EFA, 0x0027,
+		0x001B, 0x1F04, 0x04E1, 0x04E1, 0x1F04, 0x001B, 0x0000,
+		0x0027, 0x1EFA, 0x0498, 0x04FD, 0x1F19, 0x000E, 0x0023,
+		0x0033, 0x1EEE, 0x045F, 0x0528, 0x1F2E, 0x0000, 0x002A,
+		0x003D, 0x1EE6, 0x0426, 0x0551, 0x1F45, 0x1FF1, 0x0030,
+		0x0047, 0x1EDF, 0x03EC, 0x0578, 0x1F5E, 0x1FE2, 0x0036,
+		0x004F, 0x1EDB, 0x03B1, 0x059B, 0x1F7A, 0x1FD3, 0x003D,
+		0x0057, 0x1ED9, 0x0376, 0x05BC, 0x1F98, 0x1FC3, 0x0043,
+		0x005D, 0x1EDA, 0x033B, 0x05DB, 0x1FB8, 0x1FB2, 0x0049,
+		0x0063, 0x1EDC, 0x0300, 0x05F6, 0x1FDB, 0x1FA2, 0x004E,
+		0x0067, 0x1EE0, 0x02C5, 0x060F, 0x0000, 0x1F91, 0x0054,
+		0x006B, 0x1EE5, 0x028A, 0x0624, 0x0028, 0x1F81, 0x0059,
+		0x006D, 0x1EED, 0x0250, 0x0636, 0x0051, 0x1F71, 0x005E,
+		0x006F, 0x1EF6, 0x0217, 0x0644, 0x007D, 0x1F61, 0x0062,
+		0x0070, 0x1F00, 0x01DF, 0x0650, 0x00AA, 0x1F51, 0x0066,
+		0x0070, 0x1F0B, 0x01A8, 0x0659, 0x00DA, 0x1F41, 0x0069,
+		0x0070, 0x1F17, 0x0172, 0x065D, 0x010B, 0x1F33, 0x006C,
+		/* Chroma */
+		0x006E, 0x1F24, 0x013E, 0x0660, 0x013E, 0x1F24, 0x006E,
+		0x006C, 0x1F33, 0x010B, 0x065D, 0x0172, 0x1F17, 0x0070,
+		0x0069, 0x1F41, 0x00DA, 0x0659, 0x01A8, 0x1F0B, 0x0070,
+		0x0066, 0x1F51, 0x00AA, 0x0650, 0x01DF, 0x1F00, 0x0070,
+		0x0062, 0x1F61, 0x007D, 0x0644, 0x0217, 0x1EF6, 0x006F,
+		0x005E, 0x1F71, 0x0051, 0x0636, 0x0250, 0x1EED, 0x006D,
+		0x0059, 0x1F81, 0x0028, 0x0624, 0x028A, 0x1EE5, 0x006B,
+		0x0054, 0x1F91, 0x0000, 0x060F, 0x02C5, 0x1EE0, 0x0067,
+		0x004E, 0x1FA2, 0x1FDB, 0x05F6, 0x0300, 0x1EDC, 0x0063,
+		0x0049, 0x1FB2, 0x1FB8, 0x05DB, 0x033B, 0x1EDA, 0x005D,
+		0x0043, 0x1FC3, 0x1F98, 0x05BC, 0x0376, 0x1ED9, 0x0057,
+		0x003D, 0x1FD3, 0x1F7A, 0x059B, 0x03B1, 0x1EDB, 0x004F,
+		0x0036, 0x1FE2, 0x1F5E, 0x0578, 0x03EC, 0x1EDF, 0x0047,
+		0x0030, 0x1FF1, 0x1F45, 0x0551, 0x0426, 0x1EE6, 0x003D,
+		0x002A, 0x0000, 0x1F2E, 0x0528, 0x045F, 0x1EEE, 0x0033,
+		0x0023, 0x000E, 0x1F19, 0x04FD, 0x0498, 0x1EFA, 0x0027,
+		0x001B, 0x1F04, 0x04E1, 0x04E1, 0x1F04, 0x001B, 0x0000,
+		0x0027, 0x1EFA, 0x0498, 0x04FD, 0x1F19, 0x000E, 0x0023,
+		0x0033, 0x1EEE, 0x045F, 0x0528, 0x1F2E, 0x0000, 0x002A,
+		0x003D, 0x1EE6, 0x0426, 0x0551, 0x1F45, 0x1FF1, 0x0030,
+		0x0047, 0x1EDF, 0x03EC, 0x0578, 0x1F5E, 0x1FE2, 0x0036,
+		0x004F, 0x1EDB, 0x03B1, 0x059B, 0x1F7A, 0x1FD3, 0x003D,
+		0x0057, 0x1ED9, 0x0376, 0x05BC, 0x1F98, 0x1FC3, 0x0043,
+		0x005D, 0x1EDA, 0x033B, 0x05DB, 0x1FB8, 0x1FB2, 0x0049,
+		0x0063, 0x1EDC, 0x0300, 0x05F6, 0x1FDB, 0x1FA2, 0x004E,
+		0x0067, 0x1EE0, 0x02C5, 0x060F, 0x0000, 0x1F91, 0x0054,
+		0x006B, 0x1EE5, 0x028A, 0x0624, 0x0028, 0x1F81, 0x0059,
+		0x006D, 0x1EED, 0x0250, 0x0636, 0x0051, 0x1F71, 0x005E,
+		0x006F, 0x1EF6, 0x0217, 0x0644, 0x007D, 0x1F61, 0x0062,
+		0x0070, 0x1F00, 0x01DF, 0x0650, 0x00AA, 0x1F51, 0x0066,
+		0x0070, 0x1F0B, 0x01A8, 0x0659, 0x00DA, 0x1F41, 0x0069,
+		0x0070, 0x1F17, 0x0172, 0x065D, 0x010B, 0x1F33, 0x006C,
+	},
+};
+
+/* vertical scaler coefficients */
+enum {
+	VS_UP_SCALE = 0,
+	VS_LT_9_16_SCALE,
+	VS_LT_10_16_SCALE,
+	VS_LT_11_16_SCALE,
+	VS_LT_12_16_SCALE,
+	VS_LT_13_16_SCALE,
+	VS_LT_14_16_SCALE,
+	VS_LT_15_16_SCALE,
+	VS_LT_16_16_SCALE,
+	VS_1_TO_1_SCALE,
+};
+
+static const u16 scaler_vs_coeffs[15][SC_NUM_PHASES * 2 * SC_V_NUM_TAPS] = {
+	[VS_UP_SCALE] = {
+		/* Luma */
+		0x1FD1, 0x00B1, 0x06FC, 0x00B1, 0x1FD1,
+		0x1FD8, 0x0085, 0x06F9, 0x00E1, 0x1FC9,
+		0x1FDF, 0x005B, 0x06F2, 0x0114, 0x1FC0,
+		0x1FE5, 0x0035, 0x06E5, 0x014A, 0x1FB7,
+		0x1FEB, 0x0012, 0x06D3, 0x0182, 0x1FAE,
+		0x1FF1, 0x1FF3, 0x06BA, 0x01BD, 0x1FA5,
+		0x1FF5, 0x1FD7, 0x069D, 0x01FB, 0x1F9C,
+		0x1FF9, 0x1FBE, 0x067C, 0x023A, 0x1F93,
+		0x1FFD, 0x1FA8, 0x0656, 0x027B, 0x1F8A,
+		0x0000, 0x1F95, 0x062B, 0x02BF, 0x1F81,
+		0x0002, 0x1F86, 0x05FC, 0x0303, 0x1F79,
+		0x0004, 0x1F79, 0x05CA, 0x0347, 0x1F72,
+		0x0005, 0x1F6F, 0x0594, 0x038D, 0x1F6B,
+		0x0006, 0x1F67, 0x055B, 0x03D2, 0x1F66,
+		0x0007, 0x1F62, 0x051E, 0x0417, 0x1F62,
+		0x0007, 0x1F5F, 0x04DF, 0x045C, 0x1F5F,
+		0x1F5E, 0x04A2, 0x04A2, 0x1F5E, 0x0000,
+		0x1F5F, 0x045C, 0x04DF, 0x1F5F, 0x0007,
+		0x1F62, 0x0417, 0x051E, 0x1F62, 0x0007,
+		0x1F66, 0x03D2, 0x055B, 0x1F67, 0x0006,
+		0x1F6B, 0x038D, 0x0594, 0x1F6F, 0x0005,
+		0x1F72, 0x0347, 0x05CA, 0x1F79, 0x0004,
+		0x1F79, 0x0303, 0x05FC, 0x1F86, 0x0002,
+		0x1F81, 0x02BF, 0x062B, 0x1F95, 0x0000,
+		0x1F8A, 0x027B, 0x0656, 0x1FA8, 0x1FFD,
+		0x1F93, 0x023A, 0x067C, 0x1FBE, 0x1FF9,
+		0x1F9C, 0x01FB, 0x069D, 0x1FD7, 0x1FF5,
+		0x1FA5, 0x01BD, 0x06BA, 0x1FF3, 0x1FF1,
+		0x1FAE, 0x0182, 0x06D3, 0x0012, 0x1FEB,
+		0x1FB7, 0x014A, 0x06E5, 0x0035, 0x1FE5,
+		0x1FC0, 0x0114, 0x06F2, 0x005B, 0x1FDF,
+		0x1FC9, 0x00E1, 0x06F9, 0x0085, 0x1FD8,
+		/* Chroma */
+		0x1FD1, 0x00B1, 0x06FC, 0x00B1, 0x1FD1,
+		0x1FD8, 0x0085, 0x06F9, 0x00E1, 0x1FC9,
+		0x1FDF, 0x005B, 0x06F2, 0x0114, 0x1FC0,
+		0x1FE5, 0x0035, 0x06E5, 0x014A, 0x1FB7,
+		0x1FEB, 0x0012, 0x06D3, 0x0182, 0x1FAE,
+		0x1FF1, 0x1FF3, 0x06BA, 0x01BD, 0x1FA5,
+		0x1FF5, 0x1FD7, 0x069D, 0x01FB, 0x1F9C,
+		0x1FF9, 0x1FBE, 0x067C, 0x023A, 0x1F93,
+		0x1FFD, 0x1FA8, 0x0656, 0x027B, 0x1F8A,
+		0x0000, 0x1F95, 0x062B, 0x02BF, 0x1F81,
+		0x0002, 0x1F86, 0x05FC, 0x0303, 0x1F79,
+		0x0004, 0x1F79, 0x05CA, 0x0347, 0x1F72,
+		0x0005, 0x1F6F, 0x0594, 0x038D, 0x1F6B,
+		0x0006, 0x1F67, 0x055B, 0x03D2, 0x1F66,
+		0x0007, 0x1F62, 0x051E, 0x0417, 0x1F62,
+		0x0007, 0x1F5F, 0x04DF, 0x045C, 0x1F5F,
+		0x1F5E, 0x04A2, 0x04A2, 0x1F5E, 0x0000,
+		0x1F5F, 0x045C, 0x04DF, 0x1F5F, 0x0007,
+		0x1F62, 0x0417, 0x051E, 0x1F62, 0x0007,
+		0x1F66, 0x03D2, 0x055B, 0x1F67, 0x0006,
+		0x1F6B, 0x038D, 0x0594, 0x1F6F, 0x0005,
+		0x1F72, 0x0347, 0x05CA, 0x1F79, 0x0004,
+		0x1F79, 0x0303, 0x05FC, 0x1F86, 0x0002,
+		0x1F81, 0x02BF, 0x062B, 0x1F95, 0x0000,
+		0x1F8A, 0x027B, 0x0656, 0x1FA8, 0x1FFD,
+		0x1F93, 0x023A, 0x067C, 0x1FBE, 0x1FF9,
+		0x1F9C, 0x01FB, 0x069D, 0x1FD7, 0x1FF5,
+		0x1FA5, 0x01BD, 0x06BA, 0x1FF3, 0x1FF1,
+		0x1FAE, 0x0182, 0x06D3, 0x0012, 0x1FEB,
+		0x1FB7, 0x014A, 0x06E5, 0x0035, 0x1FE5,
+		0x1FC0, 0x0114, 0x06F2, 0x005B, 0x1FDF,
+		0x1FC9, 0x00E1, 0x06F9, 0x0085, 0x1FD8,
+	},
+	[VS_LT_9_16_SCALE] = {
+		/* Luma */
+		0x001C, 0x01F6, 0x03DC, 0x01F6, 0x001C,
+		0x0018, 0x01DF, 0x03DB, 0x020C, 0x0022,
+		0x0013, 0x01C9, 0x03D9, 0x0223, 0x0028,
+		0x000F, 0x01B3, 0x03D6, 0x023A, 0x002E,
+		0x000C, 0x019D, 0x03D2, 0x0250, 0x0035,
+		0x0009, 0x0188, 0x03CC, 0x0266, 0x003D,
+		0x0006, 0x0173, 0x03C5, 0x027D, 0x0045,
+		0x0004, 0x015E, 0x03BD, 0x0293, 0x004E,
+		0x0002, 0x014A, 0x03B4, 0x02A8, 0x0058,
+		0x0000, 0x0136, 0x03AA, 0x02BE, 0x0062,
+		0x1FFF, 0x0123, 0x039E, 0x02D3, 0x006D,
+		0x1FFE, 0x0110, 0x0392, 0x02E8, 0x0078,
+		0x1FFD, 0x00FE, 0x0384, 0x02FC, 0x0085,
+		0x1FFD, 0x00ED, 0x0376, 0x030F, 0x0091,
+		0x1FFC, 0x00DC, 0x0367, 0x0322, 0x009F,
+		0x1FFC, 0x00CC, 0x0357, 0x0334, 0x00AD,
+		0x00BC, 0x0344, 0x0344, 0x00BC, 0x0000,
+		0x00AD, 0x0334, 0x0357, 0x00CC, 0x1FFC,
+		0x009F, 0x0322, 0x0367, 0x00DC, 0x1FFC,
+		0x0091, 0x030F, 0x0376, 0x00ED, 0x1FFD,
+		0x0085, 0x02FC, 0x0384, 0x00FE, 0x1FFD,
+		0x0078, 0x02E8, 0x0392, 0x0110, 0x1FFE,
+		0x006D, 0x02D3, 0x039E, 0x0123, 0x1FFF,
+		0x0062, 0x02BE, 0x03AA, 0x0136, 0x0000,
+		0x0058, 0x02A8, 0x03B4, 0x014A, 0x0002,
+		0x004E, 0x0293, 0x03BD, 0x015E, 0x0004,
+		0x0045, 0x027D, 0x03C5, 0x0173, 0x0006,
+		0x003D, 0x0266, 0x03CC, 0x0188, 0x0009,
+		0x0035, 0x0250, 0x03D2, 0x019D, 0x000C,
+		0x002E, 0x023A, 0x03D6, 0x01B3, 0x000F,
+		0x0028, 0x0223, 0x03D9, 0x01C9, 0x0013,
+		0x0022, 0x020C, 0x03DB, 0x01DF, 0x0018,
+		/* Chroma */
+		0x001C, 0x01F6, 0x03DC, 0x01F6, 0x001C,
+		0x0018, 0x01DF, 0x03DB, 0x020C, 0x0022,
+		0x0013, 0x01C9, 0x03D9, 0x0223, 0x0028,
+		0x000F, 0x01B3, 0x03D6, 0x023A, 0x002E,
+		0x000C, 0x019D, 0x03D2, 0x0250, 0x0035,
+		0x0009, 0x0188, 0x03CC, 0x0266, 0x003D,
+		0x0006, 0x0173, 0x03C5, 0x027D, 0x0045,
+		0x0004, 0x015E, 0x03BD, 0x0293, 0x004E,
+		0x0002, 0x014A, 0x03B4, 0x02A8, 0x0058,
+		0x0000, 0x0136, 0x03AA, 0x02BE, 0x0062,
+		0x1FFF, 0x0123, 0x039E, 0x02D3, 0x006D,
+		0x1FFE, 0x0110, 0x0392, 0x02E8, 0x0078,
+		0x1FFD, 0x00FE, 0x0384, 0x02FC, 0x0085,
+		0x1FFD, 0x00ED, 0x0376, 0x030F, 0x0091,
+		0x1FFC, 0x00DC, 0x0367, 0x0322, 0x009F,
+		0x1FFC, 0x00CC, 0x0357, 0x0334, 0x00AD,
+		0x00BC, 0x0344, 0x0344, 0x00BC, 0x0000,
+		0x00AD, 0x0334, 0x0357, 0x00CC, 0x1FFC,
+		0x009F, 0x0322, 0x0367, 0x00DC, 0x1FFC,
+		0x0091, 0x030F, 0x0376, 0x00ED, 0x1FFD,
+		0x0085, 0x02FC, 0x0384, 0x00FE, 0x1FFD,
+		0x0078, 0x02E8, 0x0392, 0x0110, 0x1FFE,
+		0x006D, 0x02D3, 0x039E, 0x0123, 0x1FFF,
+		0x0062, 0x02BE, 0x03AA, 0x0136, 0x0000,
+		0x0058, 0x02A8, 0x03B4, 0x014A, 0x0002,
+		0x004E, 0x0293, 0x03BD, 0x015E, 0x0004,
+		0x0045, 0x027D, 0x03C5, 0x0173, 0x0006,
+		0x003D, 0x0266, 0x03CC, 0x0188, 0x0009,
+		0x0035, 0x0250, 0x03D2, 0x019D, 0x000C,
+		0x002E, 0x023A, 0x03D6, 0x01B3, 0x000F,
+		0x0028, 0x0223, 0x03D9, 0x01C9, 0x0013,
+		0x0022, 0x020C, 0x03DB, 0x01DF, 0x0018,
+	},
+	[VS_LT_10_16_SCALE] = {
+		/* Luma */
+		0x0003, 0x01E9, 0x0428, 0x01E9, 0x0003,
+		0x0000, 0x01D0, 0x0426, 0x0203, 0x0007,
+		0x1FFD, 0x01B7, 0x0424, 0x021C, 0x000C,
+		0x1FFB, 0x019E, 0x0420, 0x0236, 0x0011,
+		0x1FF9, 0x0186, 0x041A, 0x0250, 0x0017,
+		0x1FF7, 0x016E, 0x0414, 0x026A, 0x001D,
+		0x1FF6, 0x0157, 0x040B, 0x0284, 0x0024,
+		0x1FF5, 0x0140, 0x0401, 0x029E, 0x002C,
+		0x1FF4, 0x012A, 0x03F6, 0x02B7, 0x0035,
+		0x1FF4, 0x0115, 0x03E9, 0x02D0, 0x003E,
+		0x1FF4, 0x0100, 0x03DB, 0x02E9, 0x0048,
+		0x1FF4, 0x00EC, 0x03CC, 0x0301, 0x0053,
+		0x1FF4, 0x00D9, 0x03BC, 0x0318, 0x005F,
+		0x1FF5, 0x00C7, 0x03AA, 0x032F, 0x006B,
+		0x1FF6, 0x00B5, 0x0398, 0x0345, 0x0078,
+		0x1FF6, 0x00A5, 0x0384, 0x035B, 0x0086,
+		0x0094, 0x036C, 0x036C, 0x0094, 0x0000,
+		0x0086, 0x035B, 0x0384, 0x00A5, 0x1FF6,
+		0x0078, 0x0345, 0x0398, 0x00B5, 0x1FF6,
+		0x006B, 0x032F, 0x03AA, 0x00C7, 0x1FF5,
+		0x005F, 0x0318, 0x03BC, 0x00D9, 0x1FF4,
+		0x0053, 0x0301, 0x03CC, 0x00EC, 0x1FF4,
+		0x0048, 0x02E9, 0x03DB, 0x0100, 0x1FF4,
+		0x003E, 0x02D0, 0x03E9, 0x0115, 0x1FF4,
+		0x0035, 0x02B7, 0x03F6, 0x012A, 0x1FF4,
+		0x002C, 0x029E, 0x0401, 0x0140, 0x1FF5,
+		0x0024, 0x0284, 0x040B, 0x0157, 0x1FF6,
+		0x001D, 0x026A, 0x0414, 0x016E, 0x1FF7,
+		0x0017, 0x0250, 0x041A, 0x0186, 0x1FF9,
+		0x0011, 0x0236, 0x0420, 0x019E, 0x1FFB,
+		0x000C, 0x021C, 0x0424, 0x01B7, 0x1FFD,
+		0x0007, 0x0203, 0x0426, 0x01D0, 0x0000,
+		/* Chroma */
+		0x0003, 0x01E9, 0x0428, 0x01E9, 0x0003,
+		0x0000, 0x01D0, 0x0426, 0x0203, 0x0007,
+		0x1FFD, 0x01B7, 0x0424, 0x021C, 0x000C,
+		0x1FFB, 0x019E, 0x0420, 0x0236, 0x0011,
+		0x1FF9, 0x0186, 0x041A, 0x0250, 0x0017,
+		0x1FF7, 0x016E, 0x0414, 0x026A, 0x001D,
+		0x1FF6, 0x0157, 0x040B, 0x0284, 0x0024,
+		0x1FF5, 0x0140, 0x0401, 0x029E, 0x002C,
+		0x1FF4, 0x012A, 0x03F6, 0x02B7, 0x0035,
+		0x1FF4, 0x0115, 0x03E9, 0x02D0, 0x003E,
+		0x1FF4, 0x0100, 0x03DB, 0x02E9, 0x0048,
+		0x1FF4, 0x00EC, 0x03CC, 0x0301, 0x0053,
+		0x1FF4, 0x00D9, 0x03BC, 0x0318, 0x005F,
+		0x1FF5, 0x00C7, 0x03AA, 0x032F, 0x006B,
+		0x1FF6, 0x00B5, 0x0398, 0x0345, 0x0078,
+		0x1FF6, 0x00A5, 0x0384, 0x035B, 0x0086,
+		0x0094, 0x036C, 0x036C, 0x0094, 0x0000,
+		0x0086, 0x035B, 0x0384, 0x00A5, 0x1FF6,
+		0x0078, 0x0345, 0x0398, 0x00B5, 0x1FF6,
+		0x006B, 0x032F, 0x03AA, 0x00C7, 0x1FF5,
+		0x005F, 0x0318, 0x03BC, 0x00D9, 0x1FF4,
+		0x0053, 0x0301, 0x03CC, 0x00EC, 0x1FF4,
+		0x0048, 0x02E9, 0x03DB, 0x0100, 0x1FF4,
+		0x003E, 0x02D0, 0x03E9, 0x0115, 0x1FF4,
+		0x0035, 0x02B7, 0x03F6, 0x012A, 0x1FF4,
+		0x002C, 0x029E, 0x0401, 0x0140, 0x1FF5,
+		0x0024, 0x0284, 0x040B, 0x0157, 0x1FF6,
+		0x001D, 0x026A, 0x0414, 0x016E, 0x1FF7,
+		0x0017, 0x0250, 0x041A, 0x0186, 0x1FF9,
+		0x0011, 0x0236, 0x0420, 0x019E, 0x1FFB,
+		0x000C, 0x021C, 0x0424, 0x01B7, 0x1FFD,
+		0x0007, 0x0203, 0x0426, 0x01D0, 0x0000,
+	},
+	[VS_LT_11_16_SCALE] = {
+		/* Luma */
+		0x1FEC, 0x01D6, 0x047C, 0x01D6, 0x1FEC,
+		0x1FEA, 0x01BA, 0x047B, 0x01F3, 0x1FEE,
+		0x1FE9, 0x019D, 0x0478, 0x0211, 0x1FF1,
+		0x1FE8, 0x0182, 0x0473, 0x022E, 0x1FF5,
+		0x1FE8, 0x0167, 0x046C, 0x024C, 0x1FF9,
+		0x1FE8, 0x014D, 0x0464, 0x026A, 0x1FFD,
+		0x1FE8, 0x0134, 0x0459, 0x0288, 0x0003,
+		0x1FE9, 0x011B, 0x044D, 0x02A6, 0x0009,
+		0x1FE9, 0x0104, 0x0440, 0x02C3, 0x0010,
+		0x1FEA, 0x00ED, 0x0430, 0x02E1, 0x0018,
+		0x1FEB, 0x00D7, 0x0420, 0x02FD, 0x0021,
+		0x1FED, 0x00C2, 0x040D, 0x0319, 0x002B,
+		0x1FEE, 0x00AE, 0x03F9, 0x0336, 0x0035,
+		0x1FF0, 0x009C, 0x03E3, 0x0350, 0x0041,
+		0x1FF1, 0x008A, 0x03CD, 0x036B, 0x004D,
+		0x1FF3, 0x0079, 0x03B5, 0x0384, 0x005B,
+		0x0069, 0x0397, 0x0397, 0x0069, 0x0000,
+		0x005B, 0x0384, 0x03B5, 0x0079, 0x1FF3,
+		0x004D, 0x036B, 0x03CD, 0x008A, 0x1FF1,
+		0x0041, 0x0350, 0x03E3, 0x009C, 0x1FF0,
+		0x0035, 0x0336, 0x03F9, 0x00AE, 0x1FEE,
+		0x002B, 0x0319, 0x040D, 0x00C2, 0x1FED,
+		0x0021, 0x02FD, 0x0420, 0x00D7, 0x1FEB,
+		0x0018, 0x02E1, 0x0430, 0x00ED, 0x1FEA,
+		0x0010, 0x02C3, 0x0440, 0x0104, 0x1FE9,
+		0x0009, 0x02A6, 0x044D, 0x011B, 0x1FE9,
+		0x0003, 0x0288, 0x0459, 0x0134, 0x1FE8,
+		0x1FFD, 0x026A, 0x0464, 0x014D, 0x1FE8,
+		0x1FF9, 0x024C, 0x046C, 0x0167, 0x1FE8,
+		0x1FF5, 0x022E, 0x0473, 0x0182, 0x1FE8,
+		0x1FF1, 0x0211, 0x0478, 0x019D, 0x1FE9,
+		0x1FEE, 0x01F3, 0x047B, 0x01BA, 0x1FEA,
+		/* Chroma */
+		0x1FEC, 0x01D6, 0x047C, 0x01D6, 0x1FEC,
+		0x1FEA, 0x01BA, 0x047B, 0x01F3, 0x1FEE,
+		0x1FE9, 0x019D, 0x0478, 0x0211, 0x1FF1,
+		0x1FE8, 0x0182, 0x0473, 0x022E, 0x1FF5,
+		0x1FE8, 0x0167, 0x046C, 0x024C, 0x1FF9,
+		0x1FE8, 0x014D, 0x0464, 0x026A, 0x1FFD,
+		0x1FE8, 0x0134, 0x0459, 0x0288, 0x0003,
+		0x1FE9, 0x011B, 0x044D, 0x02A6, 0x0009,
+		0x1FE9, 0x0104, 0x0440, 0x02C3, 0x0010,
+		0x1FEA, 0x00ED, 0x0430, 0x02E1, 0x0018,
+		0x1FEB, 0x00D7, 0x0420, 0x02FD, 0x0021,
+		0x1FED, 0x00C2, 0x040D, 0x0319, 0x002B,
+		0x1FEE, 0x00AE, 0x03F9, 0x0336, 0x0035,
+		0x1FF0, 0x009C, 0x03E3, 0x0350, 0x0041,
+		0x1FF1, 0x008A, 0x03CD, 0x036B, 0x004D,
+		0x1FF3, 0x0079, 0x03B5, 0x0384, 0x005B,
+		0x0069, 0x0397, 0x0397, 0x0069, 0x0000,
+		0x005B, 0x0384, 0x03B5, 0x0079, 0x1FF3,
+		0x004D, 0x036B, 0x03CD, 0x008A, 0x1FF1,
+		0x0041, 0x0350, 0x03E3, 0x009C, 0x1FF0,
+		0x0035, 0x0336, 0x03F9, 0x00AE, 0x1FEE,
+		0x002B, 0x0319, 0x040D, 0x00C2, 0x1FED,
+		0x0021, 0x02FD, 0x0420, 0x00D7, 0x1FEB,
+		0x0018, 0x02E1, 0x0430, 0x00ED, 0x1FEA,
+		0x0010, 0x02C3, 0x0440, 0x0104, 0x1FE9,
+		0x0009, 0x02A6, 0x044D, 0x011B, 0x1FE9,
+		0x0003, 0x0288, 0x0459, 0x0134, 0x1FE8,
+		0x1FFD, 0x026A, 0x0464, 0x014D, 0x1FE8,
+		0x1FF9, 0x024C, 0x046C, 0x0167, 0x1FE8,
+		0x1FF5, 0x022E, 0x0473, 0x0182, 0x1FE8,
+		0x1FF1, 0x0211, 0x0478, 0x019D, 0x1FE9,
+		0x1FEE, 0x01F3, 0x047B, 0x01BA, 0x1FEA,
+	},
+	[VS_LT_12_16_SCALE] = {
+		/* Luma */
+		0x1FD8, 0x01BC, 0x04D8, 0x01BC, 0x1FD8,
+		0x1FD8, 0x019C, 0x04D8, 0x01DC, 0x1FD8,
+		0x1FD8, 0x017D, 0x04D4, 0x01FE, 0x1FD9,
+		0x1FD9, 0x015E, 0x04CF, 0x0220, 0x1FDA,
+		0x1FDB, 0x0141, 0x04C7, 0x0241, 0x1FDC,
+		0x1FDC, 0x0125, 0x04BC, 0x0264, 0x1FDF,
+		0x1FDE, 0x0109, 0x04B0, 0x0286, 0x1FE3,
+		0x1FE0, 0x00EF, 0x04A1, 0x02A9, 0x1FE7,
+		0x1FE2, 0x00D6, 0x0491, 0x02CB, 0x1FEC,
+		0x1FE4, 0x00BE, 0x047E, 0x02EE, 0x1FF2,
+		0x1FE6, 0x00A7, 0x046A, 0x030F, 0x1FFA,
+		0x1FE9, 0x0092, 0x0453, 0x0330, 0x0002,
+		0x1FEB, 0x007E, 0x043B, 0x0351, 0x000B,
+		0x1FED, 0x006B, 0x0421, 0x0372, 0x0015,
+		0x1FEF, 0x005A, 0x0406, 0x0391, 0x0020,
+		0x1FF1, 0x0049, 0x03EA, 0x03AF, 0x002D,
+		0x003A, 0x03C6, 0x03C6, 0x003A, 0x0000,
+		0x002D, 0x03AF, 0x03EA, 0x0049, 0x1FF1,
+		0x0020, 0x0391, 0x0406, 0x005A, 0x1FEF,
+		0x0015, 0x0372, 0x0421, 0x006B, 0x1FED,
+		0x000B, 0x0351, 0x043B, 0x007E, 0x1FEB,
+		0x0002, 0x0330, 0x0453, 0x0092, 0x1FE9,
+		0x1FFA, 0x030F, 0x046A, 0x00A7, 0x1FE6,
+		0x1FF2, 0x02EE, 0x047E, 0x00BE, 0x1FE4,
+		0x1FEC, 0x02CB, 0x0491, 0x00D6, 0x1FE2,
+		0x1FE7, 0x02A9, 0x04A1, 0x00EF, 0x1FE0,
+		0x1FE3, 0x0286, 0x04B0, 0x0109, 0x1FDE,
+		0x1FDF, 0x0264, 0x04BC, 0x0125, 0x1FDC,
+		0x1FDC, 0x0241, 0x04C7, 0x0141, 0x1FDB,
+		0x1FDA, 0x0220, 0x04CF, 0x015E, 0x1FD9,
+		0x1FD9, 0x01FE, 0x04D4, 0x017D, 0x1FD8,
+		0x1FD8, 0x01DC, 0x04D8, 0x019C, 0x1FD8,
+		/* Chroma */
+		0x1FD8, 0x01BC, 0x04D8, 0x01BC, 0x1FD8,
+		0x1FD8, 0x019C, 0x04D8, 0x01DC, 0x1FD8,
+		0x1FD8, 0x017D, 0x04D4, 0x01FE, 0x1FD9,
+		0x1FD9, 0x015E, 0x04CF, 0x0220, 0x1FDA,
+		0x1FDB, 0x0141, 0x04C7, 0x0241, 0x1FDC,
+		0x1FDC, 0x0125, 0x04BC, 0x0264, 0x1FDF,
+		0x1FDE, 0x0109, 0x04B0, 0x0286, 0x1FE3,
+		0x1FE0, 0x00EF, 0x04A1, 0x02A9, 0x1FE7,
+		0x1FE2, 0x00D6, 0x0491, 0x02CB, 0x1FEC,
+		0x1FE4, 0x00BE, 0x047E, 0x02EE, 0x1FF2,
+		0x1FE6, 0x00A7, 0x046A, 0x030F, 0x1FFA,
+		0x1FE9, 0x0092, 0x0453, 0x0330, 0x0002,
+		0x1FEB, 0x007E, 0x043B, 0x0351, 0x000B,
+		0x1FED, 0x006B, 0x0421, 0x0372, 0x0015,
+		0x1FEF, 0x005A, 0x0406, 0x0391, 0x0020,
+		0x1FF1, 0x0049, 0x03EA, 0x03AF, 0x002D,
+		0x003A, 0x03C6, 0x03C6, 0x003A, 0x0000,
+		0x002D, 0x03AF, 0x03EA, 0x0049, 0x1FF1,
+		0x0020, 0x0391, 0x0406, 0x005A, 0x1FEF,
+		0x0015, 0x0372, 0x0421, 0x006B, 0x1FED,
+		0x000B, 0x0351, 0x043B, 0x007E, 0x1FEB,
+		0x0002, 0x0330, 0x0453, 0x0092, 0x1FE9,
+		0x1FFA, 0x030F, 0x046A, 0x00A7, 0x1FE6,
+		0x1FF2, 0x02EE, 0x047E, 0x00BE, 0x1FE4,
+		0x1FEC, 0x02CB, 0x0491, 0x00D6, 0x1FE2,
+		0x1FE7, 0x02A9, 0x04A1, 0x00EF, 0x1FE0,
+		0x1FE3, 0x0286, 0x04B0, 0x0109, 0x1FDE,
+		0x1FDF, 0x0264, 0x04BC, 0x0125, 0x1FDC,
+		0x1FDC, 0x0241, 0x04C7, 0x0141, 0x1FDB,
+		0x1FDA, 0x0220, 0x04CF, 0x015E, 0x1FD9,
+		0x1FD9, 0x01FE, 0x04D4, 0x017D, 0x1FD8,
+		0x1FD8, 0x01DC, 0x04D8, 0x019C, 0x1FD8,
+	},
+	[VS_LT_13_16_SCALE] = {
+		/* Luma */
+		0x1FC8, 0x0199, 0x053E, 0x0199, 0x1FC8,
+		0x1FCA, 0x0175, 0x053E, 0x01BD, 0x1FC6,
+		0x1FCD, 0x0153, 0x0539, 0x01E2, 0x1FC5,
+		0x1FCF, 0x0132, 0x0532, 0x0209, 0x1FC4,
+		0x1FD2, 0x0112, 0x0529, 0x022F, 0x1FC4,
+		0x1FD5, 0x00F4, 0x051C, 0x0256, 0x1FC5,
+		0x1FD8, 0x00D7, 0x050D, 0x027E, 0x1FC6,
+		0x1FDC, 0x00BB, 0x04FB, 0x02A6, 0x1FC8,
+		0x1FDF, 0x00A1, 0x04E7, 0x02CE, 0x1FCB,
+		0x1FE2, 0x0089, 0x04D1, 0x02F5, 0x1FCF,
+		0x1FE5, 0x0072, 0x04B8, 0x031D, 0x1FD4,
+		0x1FE8, 0x005D, 0x049E, 0x0344, 0x1FD9,
+		0x1FEB, 0x0049, 0x0480, 0x036B, 0x1FE1,
+		0x1FEE, 0x0037, 0x0462, 0x0390, 0x1FE9,
+		0x1FF0, 0x0026, 0x0442, 0x03B6, 0x1FF2,
+		0x1FF2, 0x0017, 0x0420, 0x03DA, 0x1FFD,
+		0x0009, 0x03F7, 0x03F7, 0x0009, 0x0000,
+		0x1FFD, 0x03DA, 0x0420, 0x0017, 0x1FF2,
+		0x1FF2, 0x03B6, 0x0442, 0x0026, 0x1FF0,
+		0x1FE9, 0x0390, 0x0462, 0x0037, 0x1FEE,
+		0x1FE1, 0x036B, 0x0480, 0x0049, 0x1FEB,
+		0x1FD9, 0x0344, 0x049E, 0x005D, 0x1FE8,
+		0x1FD4, 0x031D, 0x04B8, 0x0072, 0x1FE5,
+		0x1FCF, 0x02F5, 0x04D1, 0x0089, 0x1FE2,
+		0x1FCB, 0x02CE, 0x04E7, 0x00A1, 0x1FDF,
+		0x1FC8, 0x02A6, 0x04FB, 0x00BB, 0x1FDC,
+		0x1FC6, 0x027E, 0x050D, 0x00D7, 0x1FD8,
+		0x1FC5, 0x0256, 0x051C, 0x00F4, 0x1FD5,
+		0x1FC4, 0x022F, 0x0529, 0x0112, 0x1FD2,
+		0x1FC4, 0x0209, 0x0532, 0x0132, 0x1FCF,
+		0x1FC5, 0x01E2, 0x0539, 0x0153, 0x1FCD,
+		0x1FC6, 0x01BD, 0x053E, 0x0175, 0x1FCA,
+		/* Chroma */
+		0x1FC8, 0x0199, 0x053E, 0x0199, 0x1FC8,
+		0x1FCA, 0x0175, 0x053E, 0x01BD, 0x1FC6,
+		0x1FCD, 0x0153, 0x0539, 0x01E2, 0x1FC5,
+		0x1FCF, 0x0132, 0x0532, 0x0209, 0x1FC4,
+		0x1FD2, 0x0112, 0x0529, 0x022F, 0x1FC4,
+		0x1FD5, 0x00F4, 0x051C, 0x0256, 0x1FC5,
+		0x1FD8, 0x00D7, 0x050D, 0x027E, 0x1FC6,
+		0x1FDC, 0x00BB, 0x04FB, 0x02A6, 0x1FC8,
+		0x1FDF, 0x00A1, 0x04E7, 0x02CE, 0x1FCB,
+		0x1FE2, 0x0089, 0x04D1, 0x02F5, 0x1FCF,
+		0x1FE5, 0x0072, 0x04B8, 0x031D, 0x1FD4,
+		0x1FE8, 0x005D, 0x049E, 0x0344, 0x1FD9,
+		0x1FEB, 0x0049, 0x0480, 0x036B, 0x1FE1,
+		0x1FEE, 0x0037, 0x0462, 0x0390, 0x1FE9,
+		0x1FF0, 0x0026, 0x0442, 0x03B6, 0x1FF2,
+		0x1FF2, 0x0017, 0x0420, 0x03DA, 0x1FFD,
+		0x0009, 0x03F7, 0x03F7, 0x0009, 0x0000,
+		0x1FFD, 0x03DA, 0x0420, 0x0017, 0x1FF2,
+		0x1FF2, 0x03B6, 0x0442, 0x0026, 0x1FF0,
+		0x1FE9, 0x0390, 0x0462, 0x0037, 0x1FEE,
+		0x1FE1, 0x036B, 0x0480, 0x0049, 0x1FEB,
+		0x1FD9, 0x0344, 0x049E, 0x005D, 0x1FE8,
+		0x1FD4, 0x031D, 0x04B8, 0x0072, 0x1FE5,
+		0x1FCF, 0x02F5, 0x04D1, 0x0089, 0x1FE2,
+		0x1FCB, 0x02CE, 0x04E7, 0x00A1, 0x1FDF,
+		0x1FC8, 0x02A6, 0x04FB, 0x00BB, 0x1FDC,
+		0x1FC6, 0x027E, 0x050D, 0x00D7, 0x1FD8,
+		0x1FC5, 0x0256, 0x051C, 0x00F4, 0x1FD5,
+		0x1FC4, 0x022F, 0x0529, 0x0112, 0x1FD2,
+		0x1FC4, 0x0209, 0x0532, 0x0132, 0x1FCF,
+		0x1FC5, 0x01E2, 0x0539, 0x0153, 0x1FCD,
+		0x1FC6, 0x01BD, 0x053E, 0x0175, 0x1FCA,
+	},
+	[VS_LT_14_16_SCALE] = {
+		/* Luma */
+		0x1FBF, 0x016C, 0x05AA, 0x016C, 0x1FBF,
+		0x1FC3, 0x0146, 0x05A8, 0x0194, 0x1FBB,
+		0x1FC7, 0x0121, 0x05A3, 0x01BD, 0x1FB8,
+		0x1FCB, 0x00FD, 0x059B, 0x01E8, 0x1FB5,
+		0x1FD0, 0x00DC, 0x058F, 0x0213, 0x1FB2,
+		0x1FD4, 0x00BC, 0x0580, 0x0240, 0x1FB0,
+		0x1FD8, 0x009E, 0x056E, 0x026D, 0x1FAF,
+		0x1FDC, 0x0082, 0x055A, 0x029A, 0x1FAE,
+		0x1FE0, 0x0067, 0x0542, 0x02C9, 0x1FAE,
+		0x1FE4, 0x004F, 0x0528, 0x02F6, 0x1FAF,
+		0x1FE8, 0x0038, 0x050A, 0x0325, 0x1FB1,
+		0x1FEB, 0x0024, 0x04EB, 0x0352, 0x1FB4,
+		0x1FEE, 0x0011, 0x04C8, 0x0380, 0x1FB9,
+		0x1FF1, 0x0000, 0x04A4, 0x03AC, 0x1FBF,
+		0x1FF4, 0x1FF1, 0x047D, 0x03D8, 0x1FC6,
+		0x1FF6, 0x1FE4, 0x0455, 0x0403, 0x1FCE,
+		0x1FD8, 0x0428, 0x0428, 0x1FD8, 0x0000,
+		0x1FCE, 0x0403, 0x0455, 0x1FE4, 0x1FF6,
+		0x1FC6, 0x03D8, 0x047D, 0x1FF1, 0x1FF4,
+		0x1FBF, 0x03AC, 0x04A4, 0x0000, 0x1FF1,
+		0x1FB9, 0x0380, 0x04C8, 0x0011, 0x1FEE,
+		0x1FB4, 0x0352, 0x04EB, 0x0024, 0x1FEB,
+		0x1FB1, 0x0325, 0x050A, 0x0038, 0x1FE8,
+		0x1FAF, 0x02F6, 0x0528, 0x004F, 0x1FE4,
+		0x1FAE, 0x02C9, 0x0542, 0x0067, 0x1FE0,
+		0x1FAE, 0x029A, 0x055A, 0x0082, 0x1FDC,
+		0x1FAF, 0x026D, 0x056E, 0x009E, 0x1FD8,
+		0x1FB0, 0x0240, 0x0580, 0x00BC, 0x1FD4,
+		0x1FB2, 0x0213, 0x058F, 0x00DC, 0x1FD0,
+		0x1FB5, 0x01E8, 0x059B, 0x00FD, 0x1FCB,
+		0x1FB8, 0x01BD, 0x05A3, 0x0121, 0x1FC7,
+		0x1FBB, 0x0194, 0x05A8, 0x0146, 0x1FC3,
+		/* Chroma */
+		0x1FBF, 0x016C, 0x05AA, 0x016C, 0x1FBF,
+		0x1FC3, 0x0146, 0x05A8, 0x0194, 0x1FBB,
+		0x1FC7, 0x0121, 0x05A3, 0x01BD, 0x1FB8,
+		0x1FCB, 0x00FD, 0x059B, 0x01E8, 0x1FB5,
+		0x1FD0, 0x00DC, 0x058F, 0x0213, 0x1FB2,
+		0x1FD4, 0x00BC, 0x0580, 0x0240, 0x1FB0,
+		0x1FD8, 0x009E, 0x056E, 0x026D, 0x1FAF,
+		0x1FDC, 0x0082, 0x055A, 0x029A, 0x1FAE,
+		0x1FE0, 0x0067, 0x0542, 0x02C9, 0x1FAE,
+		0x1FE4, 0x004F, 0x0528, 0x02F6, 0x1FAF,
+		0x1FE8, 0x0038, 0x050A, 0x0325, 0x1FB1,
+		0x1FEB, 0x0024, 0x04EB, 0x0352, 0x1FB4,
+		0x1FEE, 0x0011, 0x04C8, 0x0380, 0x1FB9,
+		0x1FF1, 0x0000, 0x04A4, 0x03AC, 0x1FBF,
+		0x1FF4, 0x1FF1, 0x047D, 0x03D8, 0x1FC6,
+		0x1FF6, 0x1FE4, 0x0455, 0x0403, 0x1FCE,
+		0x1FD8, 0x0428, 0x0428, 0x1FD8, 0x0000,
+		0x1FCE, 0x0403, 0x0455, 0x1FE4, 0x1FF6,
+		0x1FC6, 0x03D8, 0x047D, 0x1FF1, 0x1FF4,
+		0x1FBF, 0x03AC, 0x04A4, 0x0000, 0x1FF1,
+		0x1FB9, 0x0380, 0x04C8, 0x0011, 0x1FEE,
+		0x1FB4, 0x0352, 0x04EB, 0x0024, 0x1FEB,
+		0x1FB1, 0x0325, 0x050A, 0x0038, 0x1FE8,
+		0x1FAF, 0x02F6, 0x0528, 0x004F, 0x1FE4,
+		0x1FAE, 0x02C9, 0x0542, 0x0067, 0x1FE0,
+		0x1FAE, 0x029A, 0x055A, 0x0082, 0x1FDC,
+		0x1FAF, 0x026D, 0x056E, 0x009E, 0x1FD8,
+		0x1FB0, 0x0240, 0x0580, 0x00BC, 0x1FD4,
+		0x1FB2, 0x0213, 0x058F, 0x00DC, 0x1FD0,
+		0x1FB5, 0x01E8, 0x059B, 0x00FD, 0x1FCB,
+		0x1FB8, 0x01BD, 0x05A3, 0x0121, 0x1FC7,
+		0x1FBB, 0x0194, 0x05A8, 0x0146, 0x1FC3,
+	},
+	[VS_LT_15_16_SCALE] = {
+		/* Luma */
+		0x1FBD, 0x0136, 0x061A, 0x0136, 0x1FBD,
+		0x1FC3, 0x010D, 0x0617, 0x0161, 0x1FB8,
+		0x1FC9, 0x00E6, 0x0611, 0x018E, 0x1FB2,
+		0x1FCE, 0x00C1, 0x0607, 0x01BD, 0x1FAD,
+		0x1FD4, 0x009E, 0x05F9, 0x01ED, 0x1FA8,
+		0x1FD9, 0x007D, 0x05E8, 0x021F, 0x1FA3,
+		0x1FDE, 0x005E, 0x05D3, 0x0252, 0x1F9F,
+		0x1FE2, 0x0042, 0x05BC, 0x0285, 0x1F9B,
+		0x1FE7, 0x0029, 0x059F, 0x02B9, 0x1F98,
+		0x1FEA, 0x0011, 0x0580, 0x02EF, 0x1F96,
+		0x1FEE, 0x1FFC, 0x055D, 0x0324, 0x1F95,
+		0x1FF1, 0x1FE9, 0x0538, 0x0359, 0x1F95,
+		0x1FF4, 0x1FD8, 0x0510, 0x038E, 0x1F96,
+		0x1FF7, 0x1FC9, 0x04E5, 0x03C2, 0x1F99,
+		0x1FF9, 0x1FBD, 0x04B8, 0x03F5, 0x1F9D,
+		0x1FFB, 0x1FB2, 0x0489, 0x0428, 0x1FA2,
+		0x1FAA, 0x0456, 0x0456, 0x1FAA, 0x0000,
+		0x1FA2, 0x0428, 0x0489, 0x1FB2, 0x1FFB,
+		0x1F9D, 0x03F5, 0x04B8, 0x1FBD, 0x1FF9,
+		0x1F99, 0x03C2, 0x04E5, 0x1FC9, 0x1FF7,
+		0x1F96, 0x038E, 0x0510, 0x1FD8, 0x1FF4,
+		0x1F95, 0x0359, 0x0538, 0x1FE9, 0x1FF1,
+		0x1F95, 0x0324, 0x055D, 0x1FFC, 0x1FEE,
+		0x1F96, 0x02EF, 0x0580, 0x0011, 0x1FEA,
+		0x1F98, 0x02B9, 0x059F, 0x0029, 0x1FE7,
+		0x1F9B, 0x0285, 0x05BC, 0x0042, 0x1FE2,
+		0x1F9F, 0x0252, 0x05D3, 0x005E, 0x1FDE,
+		0x1FA3, 0x021F, 0x05E8, 0x007D, 0x1FD9,
+		0x1FA8, 0x01ED, 0x05F9, 0x009E, 0x1FD4,
+		0x1FAD, 0x01BD, 0x0607, 0x00C1, 0x1FCE,
+		0x1FB2, 0x018E, 0x0611, 0x00E6, 0x1FC9,
+		0x1FB8, 0x0161, 0x0617, 0x010D, 0x1FC3,
+		/* Chroma */
+		0x1FBD, 0x0136, 0x061A, 0x0136, 0x1FBD,
+		0x1FC3, 0x010D, 0x0617, 0x0161, 0x1FB8,
+		0x1FC9, 0x00E6, 0x0611, 0x018E, 0x1FB2,
+		0x1FCE, 0x00C1, 0x0607, 0x01BD, 0x1FAD,
+		0x1FD4, 0x009E, 0x05F9, 0x01ED, 0x1FA8,
+		0x1FD9, 0x007D, 0x05E8, 0x021F, 0x1FA3,
+		0x1FDE, 0x005E, 0x05D3, 0x0252, 0x1F9F,
+		0x1FE2, 0x0042, 0x05BC, 0x0285, 0x1F9B,
+		0x1FE7, 0x0029, 0x059F, 0x02B9, 0x1F98,
+		0x1FEA, 0x0011, 0x0580, 0x02EF, 0x1F96,
+		0x1FEE, 0x1FFC, 0x055D, 0x0324, 0x1F95,
+		0x1FF1, 0x1FE9, 0x0538, 0x0359, 0x1F95,
+		0x1FF4, 0x1FD8, 0x0510, 0x038E, 0x1F96,
+		0x1FF7, 0x1FC9, 0x04E5, 0x03C2, 0x1F99,
+		0x1FF9, 0x1FBD, 0x04B8, 0x03F5, 0x1F9D,
+		0x1FFB, 0x1FB2, 0x0489, 0x0428, 0x1FA2,
+		0x1FAA, 0x0456, 0x0456, 0x1FAA, 0x0000,
+		0x1FA2, 0x0428, 0x0489, 0x1FB2, 0x1FFB,
+		0x1F9D, 0x03F5, 0x04B8, 0x1FBD, 0x1FF9,
+		0x1F99, 0x03C2, 0x04E5, 0x1FC9, 0x1FF7,
+		0x1F96, 0x038E, 0x0510, 0x1FD8, 0x1FF4,
+		0x1F95, 0x0359, 0x0538, 0x1FE9, 0x1FF1,
+		0x1F95, 0x0324, 0x055D, 0x1FFC, 0x1FEE,
+		0x1F96, 0x02EF, 0x0580, 0x0011, 0x1FEA,
+		0x1F98, 0x02B9, 0x059F, 0x0029, 0x1FE7,
+		0x1F9B, 0x0285, 0x05BC, 0x0042, 0x1FE2,
+		0x1F9F, 0x0252, 0x05D3, 0x005E, 0x1FDE,
+		0x1FA3, 0x021F, 0x05E8, 0x007D, 0x1FD9,
+		0x1FA8, 0x01ED, 0x05F9, 0x009E, 0x1FD4,
+		0x1FAD, 0x01BD, 0x0607, 0x00C1, 0x1FCE,
+		0x1FB2, 0x018E, 0x0611, 0x00E6, 0x1FC9,
+		0x1FB8, 0x0161, 0x0617, 0x010D, 0x1FC3,
+	},
+	[VS_LT_16_16_SCALE] = {
+		/* Luma */
+		0x1FC3, 0x00F8, 0x068A, 0x00F8, 0x1FC3,
+		0x1FCA, 0x00CC, 0x0689, 0x0125, 0x1FBC,
+		0x1FD1, 0x00A3, 0x0681, 0x0156, 0x1FB5,
+		0x1FD7, 0x007D, 0x0676, 0x0188, 0x1FAE,
+		0x1FDD, 0x005A, 0x0666, 0x01BD, 0x1FA6,
+		0x1FE3, 0x0039, 0x0652, 0x01F3, 0x1F9F,
+		0x1FE8, 0x001B, 0x0639, 0x022C, 0x1F98,
+		0x1FEC, 0x0000, 0x061D, 0x0265, 0x1F92,
+		0x1FF0, 0x1FE8, 0x05FC, 0x02A0, 0x1F8C,
+		0x1FF4, 0x1FD2, 0x05D7, 0x02DC, 0x1F87,
+		0x1FF7, 0x1FBF, 0x05AF, 0x0319, 0x1F82,
+		0x1FFA, 0x1FAF, 0x0583, 0x0356, 0x1F7E,
+		0x1FFC, 0x1FA1, 0x0554, 0x0393, 0x1F7C,
+		0x1FFE, 0x1F95, 0x0523, 0x03CF, 0x1F7B,
+		0x0000, 0x1F8C, 0x04EE, 0x040B, 0x1F7B,
+		0x0001, 0x1F85, 0x04B8, 0x0446, 0x1F7C,
+		0x1F80, 0x0480, 0x0480, 0x1F80, 0x0000,
+		0x1F7C, 0x0446, 0x04B8, 0x1F85, 0x0001,
+		0x1F7B, 0x040B, 0x04EE, 0x1F8C, 0x0000,
+		0x1F7B, 0x03CF, 0x0523, 0x1F95, 0x1FFE,
+		0x1F7C, 0x0393, 0x0554, 0x1FA1, 0x1FFC,
+		0x1F7E, 0x0356, 0x0583, 0x1FAF, 0x1FFA,
+		0x1F82, 0x0319, 0x05AF, 0x1FBF, 0x1FF7,
+		0x1F87, 0x02DC, 0x05D7, 0x1FD2, 0x1FF4,
+		0x1F8C, 0x02A0, 0x05FC, 0x1FE8, 0x1FF0,
+		0x1F92, 0x0265, 0x061D, 0x0000, 0x1FEC,
+		0x1F98, 0x022C, 0x0639, 0x001B, 0x1FE8,
+		0x1F9F, 0x01F3, 0x0652, 0x0039, 0x1FE3,
+		0x1FA6, 0x01BD, 0x0666, 0x005A, 0x1FDD,
+		0x1FAE, 0x0188, 0x0676, 0x007D, 0x1FD7,
+		0x1FB5, 0x0156, 0x0681, 0x00A3, 0x1FD1,
+		0x1FBC, 0x0125, 0x0689, 0x00CC, 0x1FCA,
+		/* Chroma */
+		0x1FC3, 0x00F8, 0x068A, 0x00F8, 0x1FC3,
+		0x1FCA, 0x00CC, 0x0689, 0x0125, 0x1FBC,
+		0x1FD1, 0x00A3, 0x0681, 0x0156, 0x1FB5,
+		0x1FD7, 0x007D, 0x0676, 0x0188, 0x1FAE,
+		0x1FDD, 0x005A, 0x0666, 0x01BD, 0x1FA6,
+		0x1FE3, 0x0039, 0x0652, 0x01F3, 0x1F9F,
+		0x1FE8, 0x001B, 0x0639, 0x022C, 0x1F98,
+		0x1FEC, 0x0000, 0x061D, 0x0265, 0x1F92,
+		0x1FF0, 0x1FE8, 0x05FC, 0x02A0, 0x1F8C,
+		0x1FF4, 0x1FD2, 0x05D7, 0x02DC, 0x1F87,
+		0x1FF7, 0x1FBF, 0x05AF, 0x0319, 0x1F82,
+		0x1FFA, 0x1FAF, 0x0583, 0x0356, 0x1F7E,
+		0x1FFC, 0x1FA1, 0x0554, 0x0393, 0x1F7C,
+		0x1FFE, 0x1F95, 0x0523, 0x03CF, 0x1F7B,
+		0x0000, 0x1F8C, 0x04EE, 0x040B, 0x1F7B,
+		0x0001, 0x1F85, 0x04B8, 0x0446, 0x1F7C,
+		0x1F80, 0x0480, 0x0480, 0x1F80, 0x0000,
+		0x1F7C, 0x0446, 0x04B8, 0x1F85, 0x0001,
+		0x1F7B, 0x040B, 0x04EE, 0x1F8C, 0x0000,
+		0x1F7B, 0x03CF, 0x0523, 0x1F95, 0x1FFE,
+		0x1F7C, 0x0393, 0x0554, 0x1FA1, 0x1FFC,
+		0x1F7E, 0x0356, 0x0583, 0x1FAF, 0x1FFA,
+		0x1F82, 0x0319, 0x05AF, 0x1FBF, 0x1FF7,
+		0x1F87, 0x02DC, 0x05D7, 0x1FD2, 0x1FF4,
+		0x1F8C, 0x02A0, 0x05FC, 0x1FE8, 0x1FF0,
+		0x1F92, 0x0265, 0x061D, 0x0000, 0x1FEC,
+		0x1F98, 0x022C, 0x0639, 0x001B, 0x1FE8,
+		0x1F9F, 0x01F3, 0x0652, 0x0039, 0x1FE3,
+		0x1FA6, 0x01BD, 0x0666, 0x005A, 0x1FDD,
+		0x1FAE, 0x0188, 0x0676, 0x007D, 0x1FD7,
+		0x1FB5, 0x0156, 0x0681, 0x00A3, 0x1FD1,
+		0x1FBC, 0x0125, 0x0689, 0x00CC, 0x1FCA,
+	},
+	[VS_1_TO_1_SCALE] = {
+		/* Luma */
+		0x0000, 0x0000, 0x0800, 0x0000, 0x0000,
+		0x1FD8, 0x0085, 0x06F9, 0x00E1, 0x1FC9,
+		0x1FDF, 0x005B, 0x06F2, 0x0114, 0x1FC0,
+		0x1FE5, 0x0035, 0x06E5, 0x014A, 0x1FB7,
+		0x1FEB, 0x0012, 0x06D3, 0x0182, 0x1FAE,
+		0x1FF1, 0x1FF3, 0x06BA, 0x01BD, 0x1FA5,
+		0x1FF5, 0x1FD7, 0x069D, 0x01FB, 0x1F9C,
+		0x1FF9, 0x1FBE, 0x067C, 0x023A, 0x1F93,
+		0x1FFD, 0x1FA8, 0x0656, 0x027B, 0x1F8A,
+		0x0000, 0x1F95, 0x062B, 0x02BF, 0x1F81,
+		0x0002, 0x1F86, 0x05FC, 0x0303, 0x1F79,
+		0x0004, 0x1F79, 0x05CA, 0x0347, 0x1F72,
+		0x0005, 0x1F6F, 0x0594, 0x038D, 0x1F6B,
+		0x0006, 0x1F67, 0x055B, 0x03D2, 0x1F66,
+		0x0007, 0x1F62, 0x051E, 0x0417, 0x1F62,
+		0x0007, 0x1F5F, 0x04DF, 0x045C, 0x1F5F,
+		0x1F5E, 0x04A2, 0x04A2, 0x1F5E, 0x0000,
+		0x1F5F, 0x045C, 0x04DF, 0x1F5F, 0x0007,
+		0x1F62, 0x0417, 0x051E, 0x1F62, 0x0007,
+		0x1F66, 0x03D2, 0x055B, 0x1F67, 0x0006,
+		0x1F6B, 0x038D, 0x0594, 0x1F6F, 0x0005,
+		0x1F72, 0x0347, 0x05CA, 0x1F79, 0x0004,
+		0x1F79, 0x0303, 0x05FC, 0x1F86, 0x0002,
+		0x1F81, 0x02BF, 0x062B, 0x1F95, 0x0000,
+		0x1F8A, 0x027B, 0x0656, 0x1FA8, 0x1FFD,
+		0x1F93, 0x023A, 0x067C, 0x1FBE, 0x1FF9,
+		0x1F9C, 0x01FB, 0x069D, 0x1FD7, 0x1FF5,
+		0x1FA5, 0x01BD, 0x06BA, 0x1FF3, 0x1FF1,
+		0x1FAE, 0x0182, 0x06D3, 0x0012, 0x1FEB,
+		0x1FB7, 0x014A, 0x06E5, 0x0035, 0x1FE5,
+		0x1FC0, 0x0114, 0x06F2, 0x005B, 0x1FDF,
+		0x1FC9, 0x00E1, 0x06F9, 0x0085, 0x1FD8,
+		/* Chroma */
+		0x0000, 0x0000, 0x0800, 0x0000, 0x0000,
+		0x1FD8, 0x0085, 0x06F9, 0x00E1, 0x1FC9,
+		0x1FDF, 0x005B, 0x06F2, 0x0114, 0x1FC0,
+		0x1FE5, 0x0035, 0x06E5, 0x014A, 0x1FB7,
+		0x1FEB, 0x0012, 0x06D3, 0x0182, 0x1FAE,
+		0x1FF1, 0x1FF3, 0x06BA, 0x01BD, 0x1FA5,
+		0x1FF5, 0x1FD7, 0x069D, 0x01FB, 0x1F9C,
+		0x1FF9, 0x1FBE, 0x067C, 0x023A, 0x1F93,
+		0x1FFD, 0x1FA8, 0x0656, 0x027B, 0x1F8A,
+		0x0000, 0x1F95, 0x062B, 0x02BF, 0x1F81,
+		0x0002, 0x1F86, 0x05FC, 0x0303, 0x1F79,
+		0x0004, 0x1F79, 0x05CA, 0x0347, 0x1F72,
+		0x0005, 0x1F6F, 0x0594, 0x038D, 0x1F6B,
+		0x0006, 0x1F67, 0x055B, 0x03D2, 0x1F66,
+		0x0007, 0x1F62, 0x051E, 0x0417, 0x1F62,
+		0x0007, 0x1F5F, 0x04DF, 0x045C, 0x1F5F,
+		0x1F5E, 0x04A2, 0x04A2, 0x1F5E, 0x0000,
+		0x1F5F, 0x045C, 0x04DF, 0x1F5F, 0x0007,
+		0x1F62, 0x0417, 0x051E, 0x1F62, 0x0007,
+		0x1F66, 0x03D2, 0x055B, 0x1F67, 0x0006,
+		0x1F6B, 0x038D, 0x0594, 0x1F6F, 0x0005,
+		0x1F72, 0x0347, 0x05CA, 0x1F79, 0x0004,
+		0x1F79, 0x0303, 0x05FC, 0x1F86, 0x0002,
+		0x1F81, 0x02BF, 0x062B, 0x1F95, 0x0000,
+		0x1F8A, 0x027B, 0x0656, 0x1FA8, 0x1FFD,
+		0x1F93, 0x023A, 0x067C, 0x1FBE, 0x1FF9,
+		0x1F9C, 0x01FB, 0x069D, 0x1FD7, 0x1FF5,
+		0x1FA5, 0x01BD, 0x06BA, 0x1FF3, 0x1FF1,
+		0x1FAE, 0x0182, 0x06D3, 0x0012, 0x1FEB,
+		0x1FB7, 0x014A, 0x06E5, 0x0035, 0x1FE5,
+		0x1FC0, 0x0114, 0x06F2, 0x005B, 0x1FDF,
+		0x1FC9, 0x00E1, 0x06F9, 0x0085, 0x1FD8,
+	},
+};
+#endif
diff --git a/drivers/media/platform/ti-vpe/vpdma.c b/drivers/media/platform/ti-vpe/vpdma.c
new file mode 100644
index 0000000..3e2e3a3
--- /dev/null
+++ b/drivers/media/platform/ti-vpe/vpdma.c
@@ -0,0 +1,912 @@
+/*
+ * VPDMA helper library
+ *
+ * Copyright (c) 2013 Texas Instruments Inc.
+ *
+ * David Griego, <dagriego@biglakesoftware.com>
+ * Dale Farnsworth, <dale@farnsworth.org>
+ * Archit Taneja, <archit@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ */
+
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/err.h>
+#include <linux/firmware.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+
+#include "vpdma.h"
+#include "vpdma_priv.h"
+
+#define VPDMA_FIRMWARE	"vpdma-1b8.bin"
+
+const struct vpdma_data_format vpdma_yuv_fmts[] = {
+	[VPDMA_DATA_FMT_Y444] = {
+		.type		= VPDMA_DATA_FMT_TYPE_YUV,
+		.data_type	= DATA_TYPE_Y444,
+		.depth		= 8,
+	},
+	[VPDMA_DATA_FMT_Y422] = {
+		.type		= VPDMA_DATA_FMT_TYPE_YUV,
+		.data_type	= DATA_TYPE_Y422,
+		.depth		= 8,
+	},
+	[VPDMA_DATA_FMT_Y420] = {
+		.type		= VPDMA_DATA_FMT_TYPE_YUV,
+		.data_type	= DATA_TYPE_Y420,
+		.depth		= 8,
+	},
+	[VPDMA_DATA_FMT_C444] = {
+		.type		= VPDMA_DATA_FMT_TYPE_YUV,
+		.data_type	= DATA_TYPE_C444,
+		.depth		= 8,
+	},
+	[VPDMA_DATA_FMT_C422] = {
+		.type		= VPDMA_DATA_FMT_TYPE_YUV,
+		.data_type	= DATA_TYPE_C422,
+		.depth		= 8,
+	},
+	[VPDMA_DATA_FMT_C420] = {
+		.type		= VPDMA_DATA_FMT_TYPE_YUV,
+		.data_type	= DATA_TYPE_C420,
+		.depth		= 4,
+	},
+	[VPDMA_DATA_FMT_YC422] = {
+		.type		= VPDMA_DATA_FMT_TYPE_YUV,
+		.data_type	= DATA_TYPE_YC422,
+		.depth		= 16,
+	},
+	[VPDMA_DATA_FMT_YC444] = {
+		.type		= VPDMA_DATA_FMT_TYPE_YUV,
+		.data_type	= DATA_TYPE_YC444,
+		.depth		= 24,
+	},
+	[VPDMA_DATA_FMT_CY422] = {
+		.type		= VPDMA_DATA_FMT_TYPE_YUV,
+		.data_type	= DATA_TYPE_CY422,
+		.depth		= 16,
+	},
+};
+
+const struct vpdma_data_format vpdma_rgb_fmts[] = {
+	[VPDMA_DATA_FMT_RGB565] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
+		.data_type	= DATA_TYPE_RGB16_565,
+		.depth		= 16,
+	},
+	[VPDMA_DATA_FMT_ARGB16_1555] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
+		.data_type	= DATA_TYPE_ARGB_1555,
+		.depth		= 16,
+	},
+	[VPDMA_DATA_FMT_ARGB16] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
+		.data_type	= DATA_TYPE_ARGB_4444,
+		.depth		= 16,
+	},
+	[VPDMA_DATA_FMT_RGBA16_5551] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
+		.data_type	= DATA_TYPE_RGBA_5551,
+		.depth		= 16,
+	},
+	[VPDMA_DATA_FMT_RGBA16] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
+		.data_type	= DATA_TYPE_RGBA_4444,
+		.depth		= 16,
+	},
+	[VPDMA_DATA_FMT_ARGB24] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
+		.data_type	= DATA_TYPE_ARGB24_6666,
+		.depth		= 24,
+	},
+	[VPDMA_DATA_FMT_RGB24] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
+		.data_type	= DATA_TYPE_RGB24_888,
+		.depth		= 24,
+	},
+	[VPDMA_DATA_FMT_ARGB32] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
+		.data_type	= DATA_TYPE_ARGB32_8888,
+		.depth		= 32,
+	},
+	[VPDMA_DATA_FMT_RGBA24] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
+		.data_type	= DATA_TYPE_RGBA24_6666,
+		.depth		= 24,
+	},
+	[VPDMA_DATA_FMT_RGBA32] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
+		.data_type	= DATA_TYPE_RGBA32_8888,
+		.depth		= 32,
+	},
+	[VPDMA_DATA_FMT_BGR565] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
+		.data_type	= DATA_TYPE_BGR16_565,
+		.depth		= 16,
+	},
+	[VPDMA_DATA_FMT_ABGR16_1555] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
+		.data_type	= DATA_TYPE_ABGR_1555,
+		.depth		= 16,
+	},
+	[VPDMA_DATA_FMT_ABGR16] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
+		.data_type	= DATA_TYPE_ABGR_4444,
+		.depth		= 16,
+	},
+	[VPDMA_DATA_FMT_BGRA16_5551] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
+		.data_type	= DATA_TYPE_BGRA_5551,
+		.depth		= 16,
+	},
+	[VPDMA_DATA_FMT_BGRA16] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
+		.data_type	= DATA_TYPE_BGRA_4444,
+		.depth		= 16,
+	},
+	[VPDMA_DATA_FMT_ABGR24] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
+		.data_type	= DATA_TYPE_ABGR24_6666,
+		.depth		= 24,
+	},
+	[VPDMA_DATA_FMT_BGR24] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
+		.data_type	= DATA_TYPE_BGR24_888,
+		.depth		= 24,
+	},
+	[VPDMA_DATA_FMT_ABGR32] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
+		.data_type	= DATA_TYPE_ABGR32_8888,
+		.depth		= 32,
+	},
+	[VPDMA_DATA_FMT_BGRA24] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
+		.data_type	= DATA_TYPE_BGRA24_6666,
+		.depth		= 24,
+	},
+	[VPDMA_DATA_FMT_BGRA32] = {
+		.type		= VPDMA_DATA_FMT_TYPE_RGB,
+		.data_type	= DATA_TYPE_BGRA32_8888,
+		.depth		= 32,
+	},
+};
+
+const struct vpdma_data_format vpdma_misc_fmts[] = {
+	[VPDMA_DATA_FMT_MV] = {
+		.type		= VPDMA_DATA_FMT_TYPE_MISC,
+		.data_type	= DATA_TYPE_MV,
+		.depth		= 4,
+	},
+};
+
+struct vpdma_channel_info {
+	int num;		/* VPDMA channel number */
+	int cstat_offset;	/* client CSTAT register offset */
+};
+
+static const struct vpdma_channel_info chan_info[] = {
+	[VPE_CHAN_LUMA1_IN] = {
+		.num		= VPE_CHAN_NUM_LUMA1_IN,
+		.cstat_offset	= VPDMA_DEI_LUMA1_CSTAT,
+	},
+	[VPE_CHAN_CHROMA1_IN] = {
+		.num		= VPE_CHAN_NUM_CHROMA1_IN,
+		.cstat_offset	= VPDMA_DEI_CHROMA1_CSTAT,
+	},
+	[VPE_CHAN_LUMA2_IN] = {
+		.num		= VPE_CHAN_NUM_LUMA2_IN,
+		.cstat_offset	= VPDMA_DEI_LUMA2_CSTAT,
+	},
+	[VPE_CHAN_CHROMA2_IN] = {
+		.num		= VPE_CHAN_NUM_CHROMA2_IN,
+		.cstat_offset	= VPDMA_DEI_CHROMA2_CSTAT,
+	},
+	[VPE_CHAN_LUMA3_IN] = {
+		.num		= VPE_CHAN_NUM_LUMA3_IN,
+		.cstat_offset	= VPDMA_DEI_LUMA3_CSTAT,
+	},
+	[VPE_CHAN_CHROMA3_IN] = {
+		.num		= VPE_CHAN_NUM_CHROMA3_IN,
+		.cstat_offset	= VPDMA_DEI_CHROMA3_CSTAT,
+	},
+	[VPE_CHAN_MV_IN] = {
+		.num		= VPE_CHAN_NUM_MV_IN,
+		.cstat_offset	= VPDMA_DEI_MV_IN_CSTAT,
+	},
+	[VPE_CHAN_MV_OUT] = {
+		.num		= VPE_CHAN_NUM_MV_OUT,
+		.cstat_offset	= VPDMA_DEI_MV_OUT_CSTAT,
+	},
+	[VPE_CHAN_LUMA_OUT] = {
+		.num		= VPE_CHAN_NUM_LUMA_OUT,
+		.cstat_offset	= VPDMA_VIP_UP_Y_CSTAT,
+	},
+	[VPE_CHAN_CHROMA_OUT] = {
+		.num		= VPE_CHAN_NUM_CHROMA_OUT,
+		.cstat_offset	= VPDMA_VIP_UP_UV_CSTAT,
+	},
+	[VPE_CHAN_RGB_OUT] = {
+		.num		= VPE_CHAN_NUM_RGB_OUT,
+		.cstat_offset	= VPDMA_VIP_UP_Y_CSTAT,
+	},
+};
+
+static u32 read_reg(struct vpdma_data *vpdma, int offset)
+{
+	return ioread32(vpdma->base + offset);
+}
+
+static void write_reg(struct vpdma_data *vpdma, int offset, u32 value)
+{
+	iowrite32(value, vpdma->base + offset);
+}
+
+static int read_field_reg(struct vpdma_data *vpdma, int offset,
+		u32 mask, int shift)
+{
+	return (read_reg(vpdma, offset) & (mask << shift)) >> shift;
+}
+
+static void write_field_reg(struct vpdma_data *vpdma, int offset, u32 field,
+		u32 mask, int shift)
+{
+	u32 val = read_reg(vpdma, offset);
+
+	val &= ~(mask << shift);
+	val |= (field & mask) << shift;
+
+	write_reg(vpdma, offset, val);
+}
+
+void vpdma_dump_regs(struct vpdma_data *vpdma)
+{
+	struct device *dev = &vpdma->pdev->dev;
+
+#define DUMPREG(r) dev_dbg(dev, "%-35s %08x\n", #r, read_reg(vpdma, VPDMA_##r))
+
+	dev_dbg(dev, "VPDMA Registers:\n");
+
+	DUMPREG(PID);
+	DUMPREG(LIST_ADDR);
+	DUMPREG(LIST_ATTR);
+	DUMPREG(LIST_STAT_SYNC);
+	DUMPREG(BG_RGB);
+	DUMPREG(BG_YUV);
+	DUMPREG(SETUP);
+	DUMPREG(MAX_SIZE1);
+	DUMPREG(MAX_SIZE2);
+	DUMPREG(MAX_SIZE3);
+
+	/*
+	 * dumping registers of only group0 and group3, because VPE channels
+	 * lie within group0 and group3 registers
+	 */
+	DUMPREG(INT_CHAN_STAT(0));
+	DUMPREG(INT_CHAN_MASK(0));
+	DUMPREG(INT_CHAN_STAT(3));
+	DUMPREG(INT_CHAN_MASK(3));
+	DUMPREG(INT_CLIENT0_STAT);
+	DUMPREG(INT_CLIENT0_MASK);
+	DUMPREG(INT_CLIENT1_STAT);
+	DUMPREG(INT_CLIENT1_MASK);
+	DUMPREG(INT_LIST0_STAT);
+	DUMPREG(INT_LIST0_MASK);
+
+	/*
+	 * these are registers specific to VPE clients, we can make this
+	 * function dump client registers specific to VPE or VIP based on
+	 * who is using it
+	 */
+	DUMPREG(DEI_CHROMA1_CSTAT);
+	DUMPREG(DEI_LUMA1_CSTAT);
+	DUMPREG(DEI_CHROMA2_CSTAT);
+	DUMPREG(DEI_LUMA2_CSTAT);
+	DUMPREG(DEI_CHROMA3_CSTAT);
+	DUMPREG(DEI_LUMA3_CSTAT);
+	DUMPREG(DEI_MV_IN_CSTAT);
+	DUMPREG(DEI_MV_OUT_CSTAT);
+	DUMPREG(VIP_UP_Y_CSTAT);
+	DUMPREG(VIP_UP_UV_CSTAT);
+	DUMPREG(VPI_CTL_CSTAT);
+}
+
+/*
+ * Allocate a DMA buffer
+ */
+int vpdma_alloc_desc_buf(struct vpdma_buf *buf, size_t size)
+{
+	buf->size = size;
+	buf->mapped = false;
+	buf->addr = kzalloc(size, GFP_KERNEL);
+	if (!buf->addr)
+		return -ENOMEM;
+
+	WARN_ON(((unsigned long)buf->addr & VPDMA_DESC_ALIGN) != 0);
+
+	return 0;
+}
+
+void vpdma_free_desc_buf(struct vpdma_buf *buf)
+{
+	WARN_ON(buf->mapped);
+	kfree(buf->addr);
+	buf->addr = NULL;
+	buf->size = 0;
+}
+
+/*
+ * map descriptor/payload DMA buffer, enabling DMA access
+ */
+int vpdma_map_desc_buf(struct vpdma_data *vpdma, struct vpdma_buf *buf)
+{
+	struct device *dev = &vpdma->pdev->dev;
+
+	WARN_ON(buf->mapped);
+	buf->dma_addr = dma_map_single(dev, buf->addr, buf->size,
+				DMA_TO_DEVICE);
+	if (dma_mapping_error(dev, buf->dma_addr)) {
+		dev_err(dev, "failed to map buffer\n");
+		return -EINVAL;
+	}
+
+	buf->mapped = true;
+
+	return 0;
+}
+
+/*
+ * unmap descriptor/payload DMA buffer, disabling DMA access and
+ * allowing the main processor to acces the data
+ */
+void vpdma_unmap_desc_buf(struct vpdma_data *vpdma, struct vpdma_buf *buf)
+{
+	struct device *dev = &vpdma->pdev->dev;
+
+	if (buf->mapped)
+		dma_unmap_single(dev, buf->dma_addr, buf->size, DMA_TO_DEVICE);
+
+	buf->mapped = false;
+}
+
+/*
+ * create a descriptor list, the user of this list will append configuration,
+ * control and data descriptors to this list, this list will be submitted to
+ * VPDMA. VPDMA's list parser will go through each descriptor and perform the
+ * required DMA operations
+ */
+int vpdma_create_desc_list(struct vpdma_desc_list *list, size_t size, int type)
+{
+	int r;
+
+	r = vpdma_alloc_desc_buf(&list->buf, size);
+	if (r)
+		return r;
+
+	list->next = list->buf.addr;
+
+	list->type = type;
+
+	return 0;
+}
+
+/*
+ * once a descriptor list is parsed by VPDMA, we reset the list by emptying it,
+ * to allow new descriptors to be added to the list.
+ */
+void vpdma_reset_desc_list(struct vpdma_desc_list *list)
+{
+	list->next = list->buf.addr;
+}
+
+/*
+ * free the buffer allocated fot the VPDMA descriptor list, this should be
+ * called when the user doesn't want to use VPDMA any more.
+ */
+void vpdma_free_desc_list(struct vpdma_desc_list *list)
+{
+	vpdma_free_desc_buf(&list->buf);
+
+	list->next = NULL;
+}
+
+static bool vpdma_list_busy(struct vpdma_data *vpdma, int list_num)
+{
+	return read_reg(vpdma, VPDMA_LIST_STAT_SYNC) & BIT(list_num + 16);
+}
+
+/*
+ * submit a list of DMA descriptors to the VPE VPDMA, do not wait for completion
+ */
+int vpdma_submit_descs(struct vpdma_data *vpdma, struct vpdma_desc_list *list)
+{
+	/* we always use the first list */
+	int list_num = 0;
+	int list_size;
+
+	if (vpdma_list_busy(vpdma, list_num))
+		return -EBUSY;
+
+	/* 16-byte granularity */
+	list_size = (list->next - list->buf.addr) >> 4;
+
+	write_reg(vpdma, VPDMA_LIST_ADDR, (u32) list->buf.dma_addr);
+
+	write_reg(vpdma, VPDMA_LIST_ATTR,
+			(list_num << VPDMA_LIST_NUM_SHFT) |
+			(list->type << VPDMA_LIST_TYPE_SHFT) |
+			list_size);
+
+	return 0;
+}
+
+static void dump_cfd(struct vpdma_cfd *cfd)
+{
+	int class;
+
+	class = cfd_get_class(cfd);
+
+	pr_debug("config descriptor of payload class: %s\n",
+		class == CFD_CLS_BLOCK ? "simple block" :
+		"address data block");
+
+	if (class == CFD_CLS_BLOCK)
+		pr_debug("word0: dst_addr_offset = 0x%08x\n",
+			cfd->dest_addr_offset);
+
+	if (class == CFD_CLS_BLOCK)
+		pr_debug("word1: num_data_wrds = %d\n", cfd->block_len);
+
+	pr_debug("word2: payload_addr = 0x%08x\n", cfd->payload_addr);
+
+	pr_debug("word3: pkt_type = %d, direct = %d, class = %d, dest = %d, "
+		"payload_len = %d\n", cfd_get_pkt_type(cfd),
+		cfd_get_direct(cfd), class, cfd_get_dest(cfd),
+		cfd_get_payload_len(cfd));
+}
+
+/*
+ * append a configuration descriptor to the given descriptor list, where the
+ * payload is in the form of a simple data block specified in the descriptor
+ * header, this is used to upload scaler coefficients to the scaler module
+ */
+void vpdma_add_cfd_block(struct vpdma_desc_list *list, int client,
+		struct vpdma_buf *blk, u32 dest_offset)
+{
+	struct vpdma_cfd *cfd;
+	int len = blk->size;
+
+	WARN_ON(blk->dma_addr & VPDMA_DESC_ALIGN);
+
+	cfd = list->next;
+	WARN_ON((void *)(cfd + 1) > (list->buf.addr + list->buf.size));
+
+	cfd->dest_addr_offset = dest_offset;
+	cfd->block_len = len;
+	cfd->payload_addr = (u32) blk->dma_addr;
+	cfd->ctl_payload_len = cfd_pkt_payload_len(CFD_INDIRECT, CFD_CLS_BLOCK,
+				client, len >> 4);
+
+	list->next = cfd + 1;
+
+	dump_cfd(cfd);
+}
+
+/*
+ * append a configuration descriptor to the given descriptor list, where the
+ * payload is in the address data block format, this is used to a configure a
+ * discontiguous set of MMRs
+ */
+void vpdma_add_cfd_adb(struct vpdma_desc_list *list, int client,
+		struct vpdma_buf *adb)
+{
+	struct vpdma_cfd *cfd;
+	unsigned int len = adb->size;
+
+	WARN_ON(len & VPDMA_ADB_SIZE_ALIGN);
+	WARN_ON(adb->dma_addr & VPDMA_DESC_ALIGN);
+
+	cfd = list->next;
+	BUG_ON((void *)(cfd + 1) > (list->buf.addr + list->buf.size));
+
+	cfd->w0 = 0;
+	cfd->w1 = 0;
+	cfd->payload_addr = (u32) adb->dma_addr;
+	cfd->ctl_payload_len = cfd_pkt_payload_len(CFD_INDIRECT, CFD_CLS_ADB,
+				client, len >> 4);
+
+	list->next = cfd + 1;
+
+	dump_cfd(cfd);
+};
+
+/*
+ * control descriptor format change based on what type of control descriptor it
+ * is, we only use 'sync on channel' control descriptors for now, so assume it's
+ * that
+ */
+static void dump_ctd(struct vpdma_ctd *ctd)
+{
+	pr_debug("control descriptor\n");
+
+	pr_debug("word3: pkt_type = %d, source = %d, ctl_type = %d\n",
+		ctd_get_pkt_type(ctd), ctd_get_source(ctd), ctd_get_ctl(ctd));
+}
+
+/*
+ * append a 'sync on channel' type control descriptor to the given descriptor
+ * list, this descriptor stalls the VPDMA list till the time DMA is completed
+ * on the specified channel
+ */
+void vpdma_add_sync_on_channel_ctd(struct vpdma_desc_list *list,
+		enum vpdma_channel chan)
+{
+	struct vpdma_ctd *ctd;
+
+	ctd = list->next;
+	WARN_ON((void *)(ctd + 1) > (list->buf.addr + list->buf.size));
+
+	ctd->w0 = 0;
+	ctd->w1 = 0;
+	ctd->w2 = 0;
+	ctd->type_source_ctl = ctd_type_source_ctl(chan_info[chan].num,
+				CTD_TYPE_SYNC_ON_CHANNEL);
+
+	list->next = ctd + 1;
+
+	dump_ctd(ctd);
+}
+
+static void dump_dtd(struct vpdma_dtd *dtd)
+{
+	int dir, chan;
+
+	dir = dtd_get_dir(dtd);
+	chan = dtd_get_chan(dtd);
+
+	pr_debug("%s data transfer descriptor for channel %d\n",
+		dir == DTD_DIR_OUT ? "outbound" : "inbound", chan);
+
+	pr_debug("word0: data_type = %d, notify = %d, field = %d, 1D = %d, "
+		"even_ln_skp = %d, odd_ln_skp = %d, line_stride = %d\n",
+		dtd_get_data_type(dtd), dtd_get_notify(dtd), dtd_get_field(dtd),
+		dtd_get_1d(dtd), dtd_get_even_line_skip(dtd),
+		dtd_get_odd_line_skip(dtd), dtd_get_line_stride(dtd));
+
+	if (dir == DTD_DIR_IN)
+		pr_debug("word1: line_length = %d, xfer_height = %d\n",
+			dtd_get_line_length(dtd), dtd_get_xfer_height(dtd));
+
+	pr_debug("word2: start_addr = %pad\n", &dtd->start_addr);
+
+	pr_debug("word3: pkt_type = %d, mode = %d, dir = %d, chan = %d, "
+		"pri = %d, next_chan = %d\n", dtd_get_pkt_type(dtd),
+		dtd_get_mode(dtd), dir, chan, dtd_get_priority(dtd),
+		dtd_get_next_chan(dtd));
+
+	if (dir == DTD_DIR_IN)
+		pr_debug("word4: frame_width = %d, frame_height = %d\n",
+			dtd_get_frame_width(dtd), dtd_get_frame_height(dtd));
+	else
+		pr_debug("word4: desc_write_addr = 0x%08x, write_desc = %d, "
+			"drp_data = %d, use_desc_reg = %d\n",
+			dtd_get_desc_write_addr(dtd), dtd_get_write_desc(dtd),
+			dtd_get_drop_data(dtd), dtd_get_use_desc(dtd));
+
+	if (dir == DTD_DIR_IN)
+		pr_debug("word5: hor_start = %d, ver_start = %d\n",
+			dtd_get_h_start(dtd), dtd_get_v_start(dtd));
+	else
+		pr_debug("word5: max_width %d, max_height %d\n",
+			dtd_get_max_width(dtd), dtd_get_max_height(dtd));
+
+	pr_debug("word6: client specific attr0 = 0x%08x\n", dtd->client_attr0);
+	pr_debug("word7: client specific attr1 = 0x%08x\n", dtd->client_attr1);
+}
+
+/*
+ * append an outbound data transfer descriptor to the given descriptor list,
+ * this sets up a 'client to memory' VPDMA transfer for the given VPDMA channel
+ *
+ * @list: vpdma desc list to which we add this decriptor
+ * @width: width of the image in pixels in memory
+ * @c_rect: compose params of output image
+ * @fmt: vpdma data format of the buffer
+ * dma_addr: dma address as seen by VPDMA
+ * chan: VPDMA channel
+ * flags: VPDMA flags to configure some descriptor fileds
+ */
+void vpdma_add_out_dtd(struct vpdma_desc_list *list, int width,
+		const struct v4l2_rect *c_rect,
+		const struct vpdma_data_format *fmt, dma_addr_t dma_addr,
+		enum vpdma_channel chan, u32 flags)
+{
+	int priority = 0;
+	int field = 0;
+	int notify = 1;
+	int channel, next_chan;
+	struct v4l2_rect rect = *c_rect;
+	int depth = fmt->depth;
+	int stride;
+	struct vpdma_dtd *dtd;
+
+	channel = next_chan = chan_info[chan].num;
+
+	if (fmt->type == VPDMA_DATA_FMT_TYPE_YUV &&
+			fmt->data_type == DATA_TYPE_C420) {
+		rect.height >>= 1;
+		rect.top >>= 1;
+		depth = 8;
+	}
+
+	stride = ALIGN((depth * width) >> 3, VPDMA_STRIDE_ALIGN);
+
+	dma_addr += rect.top * stride + (rect.left * depth >> 3);
+
+	dtd = list->next;
+	WARN_ON((void *)(dtd + 1) > (list->buf.addr + list->buf.size));
+
+	dtd->type_ctl_stride = dtd_type_ctl_stride(fmt->data_type,
+					notify,
+					field,
+					!!(flags & VPDMA_DATA_FRAME_1D),
+					!!(flags & VPDMA_DATA_EVEN_LINE_SKIP),
+					!!(flags & VPDMA_DATA_ODD_LINE_SKIP),
+					stride);
+	dtd->w1 = 0;
+	dtd->start_addr = (u32) dma_addr;
+	dtd->pkt_ctl = dtd_pkt_ctl(!!(flags & VPDMA_DATA_MODE_TILED),
+				DTD_DIR_OUT, channel, priority, next_chan);
+	dtd->desc_write_addr = dtd_desc_write_addr(0, 0, 0, 0);
+	dtd->max_width_height = dtd_max_width_height(MAX_OUT_WIDTH_1920,
+					MAX_OUT_HEIGHT_1080);
+	dtd->client_attr0 = 0;
+	dtd->client_attr1 = 0;
+
+	list->next = dtd + 1;
+
+	dump_dtd(dtd);
+}
+
+/*
+ * append an inbound data transfer descriptor to the given descriptor list,
+ * this sets up a 'memory to client' VPDMA transfer for the given VPDMA channel
+ *
+ * @list: vpdma desc list to which we add this decriptor
+ * @width: width of the image in pixels in memory(not the cropped width)
+ * @c_rect: crop params of input image
+ * @fmt: vpdma data format of the buffer
+ * dma_addr: dma address as seen by VPDMA
+ * chan: VPDMA channel
+ * field: top or bottom field info of the input image
+ * flags: VPDMA flags to configure some descriptor fileds
+ * frame_width/height: the complete width/height of the image presented to the
+ *			client (this makes sense when multiple channels are
+ *			connected to the same client, forming a larger frame)
+ * start_h, start_v: position where the given channel starts providing pixel
+ *			data to the client (makes sense when multiple channels
+ *			contribute to the client)
+ */
+void vpdma_add_in_dtd(struct vpdma_desc_list *list, int width,
+		const struct v4l2_rect *c_rect,
+		const struct vpdma_data_format *fmt, dma_addr_t dma_addr,
+		enum vpdma_channel chan, int field, u32 flags, int frame_width,
+		int frame_height, int start_h, int start_v)
+{
+	int priority = 0;
+	int notify = 1;
+	int depth = fmt->depth;
+	int channel, next_chan;
+	struct v4l2_rect rect = *c_rect;
+	int stride;
+	struct vpdma_dtd *dtd;
+
+	channel = next_chan = chan_info[chan].num;
+
+	if (fmt->type == VPDMA_DATA_FMT_TYPE_YUV &&
+			fmt->data_type == DATA_TYPE_C420) {
+		rect.height >>= 1;
+		rect.top >>= 1;
+		depth = 8;
+	}
+
+	stride = ALIGN((depth * width) >> 3, VPDMA_STRIDE_ALIGN);
+
+	dma_addr += rect.top * stride + (rect.left * depth >> 3);
+
+	dtd = list->next;
+	WARN_ON((void *)(dtd + 1) > (list->buf.addr + list->buf.size));
+
+	dtd->type_ctl_stride = dtd_type_ctl_stride(fmt->data_type,
+					notify,
+					field,
+					!!(flags & VPDMA_DATA_FRAME_1D),
+					!!(flags & VPDMA_DATA_EVEN_LINE_SKIP),
+					!!(flags & VPDMA_DATA_ODD_LINE_SKIP),
+					stride);
+
+	dtd->xfer_length_height = dtd_xfer_length_height(rect.width,
+					rect.height);
+	dtd->start_addr = (u32) dma_addr;
+	dtd->pkt_ctl = dtd_pkt_ctl(!!(flags & VPDMA_DATA_MODE_TILED),
+				DTD_DIR_IN, channel, priority, next_chan);
+	dtd->frame_width_height = dtd_frame_width_height(frame_width,
+					frame_height);
+	dtd->start_h_v = dtd_start_h_v(start_h, start_v);
+	dtd->client_attr0 = 0;
+	dtd->client_attr1 = 0;
+
+	list->next = dtd + 1;
+
+	dump_dtd(dtd);
+}
+
+/* set or clear the mask for list complete interrupt */
+void vpdma_enable_list_complete_irq(struct vpdma_data *vpdma, int list_num,
+		bool enable)
+{
+	u32 val;
+
+	val = read_reg(vpdma, VPDMA_INT_LIST0_MASK);
+	if (enable)
+		val |= (1 << (list_num * 2));
+	else
+		val &= ~(1 << (list_num * 2));
+	write_reg(vpdma, VPDMA_INT_LIST0_MASK, val);
+}
+
+/* clear previosuly occured list intterupts in the LIST_STAT register */
+void vpdma_clear_list_stat(struct vpdma_data *vpdma)
+{
+	write_reg(vpdma, VPDMA_INT_LIST0_STAT,
+		read_reg(vpdma, VPDMA_INT_LIST0_STAT));
+}
+
+/*
+ * configures the output mode of the line buffer for the given client, the
+ * line buffer content can either be mirrored(each line repeated twice) or
+ * passed to the client as is
+ */
+void vpdma_set_line_mode(struct vpdma_data *vpdma, int line_mode,
+		enum vpdma_channel chan)
+{
+	int client_cstat = chan_info[chan].cstat_offset;
+
+	write_field_reg(vpdma, client_cstat, line_mode,
+		VPDMA_CSTAT_LINE_MODE_MASK, VPDMA_CSTAT_LINE_MODE_SHIFT);
+}
+
+/*
+ * configures the event which should trigger VPDMA transfer for the given
+ * client
+ */
+void vpdma_set_frame_start_event(struct vpdma_data *vpdma,
+		enum vpdma_frame_start_event fs_event,
+		enum vpdma_channel chan)
+{
+	int client_cstat = chan_info[chan].cstat_offset;
+
+	write_field_reg(vpdma, client_cstat, fs_event,
+		VPDMA_CSTAT_FRAME_START_MASK, VPDMA_CSTAT_FRAME_START_SHIFT);
+}
+
+static void vpdma_firmware_cb(const struct firmware *f, void *context)
+{
+	struct vpdma_data *vpdma = context;
+	struct vpdma_buf fw_dma_buf;
+	int i, r;
+
+	dev_dbg(&vpdma->pdev->dev, "firmware callback\n");
+
+	if (!f || !f->data) {
+		dev_err(&vpdma->pdev->dev, "couldn't get firmware\n");
+		return;
+	}
+
+	/* already initialized */
+	if (read_field_reg(vpdma, VPDMA_LIST_ATTR, VPDMA_LIST_RDY_MASK,
+			VPDMA_LIST_RDY_SHFT)) {
+		vpdma->cb(vpdma->pdev);
+		return;
+	}
+
+	r = vpdma_alloc_desc_buf(&fw_dma_buf, f->size);
+	if (r) {
+		dev_err(&vpdma->pdev->dev,
+			"failed to allocate dma buffer for firmware\n");
+		goto rel_fw;
+	}
+
+	memcpy(fw_dma_buf.addr, f->data, f->size);
+
+	vpdma_map_desc_buf(vpdma, &fw_dma_buf);
+
+	write_reg(vpdma, VPDMA_LIST_ADDR, (u32) fw_dma_buf.dma_addr);
+
+	for (i = 0; i < 100; i++) {		/* max 1 second */
+		msleep_interruptible(10);
+
+		if (read_field_reg(vpdma, VPDMA_LIST_ATTR, VPDMA_LIST_RDY_MASK,
+				VPDMA_LIST_RDY_SHFT))
+			break;
+	}
+
+	if (i == 100) {
+		dev_err(&vpdma->pdev->dev, "firmware upload failed\n");
+		goto free_buf;
+	}
+
+	vpdma->cb(vpdma->pdev);
+
+free_buf:
+	vpdma_unmap_desc_buf(vpdma, &fw_dma_buf);
+
+	vpdma_free_desc_buf(&fw_dma_buf);
+rel_fw:
+	release_firmware(f);
+}
+
+static int vpdma_load_firmware(struct vpdma_data *vpdma)
+{
+	int r;
+	struct device *dev = &vpdma->pdev->dev;
+
+	r = request_firmware_nowait(THIS_MODULE, 1,
+		(const char *) VPDMA_FIRMWARE, dev, GFP_KERNEL, vpdma,
+		vpdma_firmware_cb);
+	if (r) {
+		dev_err(dev, "firmware not available %s\n", VPDMA_FIRMWARE);
+		return r;
+	} else {
+		dev_info(dev, "loading firmware %s\n", VPDMA_FIRMWARE);
+	}
+
+	return 0;
+}
+
+struct vpdma_data *vpdma_create(struct platform_device *pdev,
+		void (*cb)(struct platform_device *pdev))
+{
+	struct resource *res;
+	struct vpdma_data *vpdma;
+	int r;
+
+	dev_dbg(&pdev->dev, "vpdma_create\n");
+
+	vpdma = devm_kzalloc(&pdev->dev, sizeof(*vpdma), GFP_KERNEL);
+	if (!vpdma) {
+		dev_err(&pdev->dev, "couldn't alloc vpdma_dev\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	vpdma->pdev = pdev;
+	vpdma->cb = cb;
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "vpdma");
+	if (res == NULL) {
+		dev_err(&pdev->dev, "missing platform resources data\n");
+		return ERR_PTR(-ENODEV);
+	}
+
+	vpdma->base = devm_ioremap(&pdev->dev, res->start, resource_size(res));
+	if (!vpdma->base) {
+		dev_err(&pdev->dev, "failed to ioremap\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	r = vpdma_load_firmware(vpdma);
+	if (r) {
+		pr_err("failed to load firmware %s\n", VPDMA_FIRMWARE);
+		return ERR_PTR(r);
+	}
+
+	return vpdma;
+}
+MODULE_FIRMWARE(VPDMA_FIRMWARE);
diff --git a/drivers/media/platform/ti-vpe/vpdma.h b/drivers/media/platform/ti-vpe/vpdma.h
new file mode 100644
index 0000000..2bd8fb0
--- /dev/null
+++ b/drivers/media/platform/ti-vpe/vpdma.h
@@ -0,0 +1,216 @@
+/*
+ * Copyright (c) 2013 Texas Instruments Inc.
+ *
+ * David Griego, <dagriego@biglakesoftware.com>
+ * Dale Farnsworth, <dale@farnsworth.org>
+ * Archit Taneja, <archit@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ */
+
+#ifndef __TI_VPDMA_H_
+#define __TI_VPDMA_H_
+
+/*
+ * A vpdma_buf tracks the size, DMA address and mapping status of each
+ * driver DMA area.
+ */
+struct vpdma_buf {
+	void			*addr;
+	dma_addr_t		dma_addr;
+	size_t			size;
+	bool			mapped;
+};
+
+struct vpdma_desc_list {
+	struct vpdma_buf buf;
+	void *next;
+	int type;
+};
+
+struct vpdma_data {
+	void __iomem		*base;
+
+	struct platform_device	*pdev;
+
+	/* callback to VPE driver when the firmware is loaded */
+	void (*cb)(struct platform_device *pdev);
+};
+
+enum vpdma_data_format_type {
+	VPDMA_DATA_FMT_TYPE_YUV,
+	VPDMA_DATA_FMT_TYPE_RGB,
+	VPDMA_DATA_FMT_TYPE_MISC,
+};
+
+struct vpdma_data_format {
+	enum vpdma_data_format_type type;
+	int data_type;
+	u8 depth;
+};
+
+#define VPDMA_DESC_ALIGN		16	/* 16-byte descriptor alignment */
+#define VPDMA_STRIDE_ALIGN		16	/*
+						 * line stride of source and dest
+						 * buffers should be 16 byte aligned
+						 */
+#define VPDMA_DTD_DESC_SIZE		32	/* 8 words */
+#define VPDMA_CFD_CTD_DESC_SIZE		16	/* 4 words */
+
+#define VPDMA_LIST_TYPE_NORMAL		0
+#define VPDMA_LIST_TYPE_SELF_MODIFYING	1
+#define VPDMA_LIST_TYPE_DOORBELL	2
+
+enum vpdma_yuv_formats {
+	VPDMA_DATA_FMT_Y444 = 0,
+	VPDMA_DATA_FMT_Y422,
+	VPDMA_DATA_FMT_Y420,
+	VPDMA_DATA_FMT_C444,
+	VPDMA_DATA_FMT_C422,
+	VPDMA_DATA_FMT_C420,
+	VPDMA_DATA_FMT_YC422,
+	VPDMA_DATA_FMT_YC444,
+	VPDMA_DATA_FMT_CY422,
+};
+
+enum vpdma_rgb_formats {
+	VPDMA_DATA_FMT_RGB565 = 0,
+	VPDMA_DATA_FMT_ARGB16_1555,
+	VPDMA_DATA_FMT_ARGB16,
+	VPDMA_DATA_FMT_RGBA16_5551,
+	VPDMA_DATA_FMT_RGBA16,
+	VPDMA_DATA_FMT_ARGB24,
+	VPDMA_DATA_FMT_RGB24,
+	VPDMA_DATA_FMT_ARGB32,
+	VPDMA_DATA_FMT_RGBA24,
+	VPDMA_DATA_FMT_RGBA32,
+	VPDMA_DATA_FMT_BGR565,
+	VPDMA_DATA_FMT_ABGR16_1555,
+	VPDMA_DATA_FMT_ABGR16,
+	VPDMA_DATA_FMT_BGRA16_5551,
+	VPDMA_DATA_FMT_BGRA16,
+	VPDMA_DATA_FMT_ABGR24,
+	VPDMA_DATA_FMT_BGR24,
+	VPDMA_DATA_FMT_ABGR32,
+	VPDMA_DATA_FMT_BGRA24,
+	VPDMA_DATA_FMT_BGRA32,
+};
+
+enum vpdma_misc_formats {
+	VPDMA_DATA_FMT_MV = 0,
+};
+
+extern const struct vpdma_data_format vpdma_yuv_fmts[];
+extern const struct vpdma_data_format vpdma_rgb_fmts[];
+extern const struct vpdma_data_format vpdma_misc_fmts[];
+
+enum vpdma_frame_start_event {
+	VPDMA_FSEVENT_HDMI_FID = 0,
+	VPDMA_FSEVENT_DVO2_FID,
+	VPDMA_FSEVENT_HDCOMP_FID,
+	VPDMA_FSEVENT_SD_FID,
+	VPDMA_FSEVENT_LM_FID0,
+	VPDMA_FSEVENT_LM_FID1,
+	VPDMA_FSEVENT_LM_FID2,
+	VPDMA_FSEVENT_CHANNEL_ACTIVE,
+};
+
+/*
+ * VPDMA channel numbers
+ */
+enum vpdma_channel {
+	VPE_CHAN_LUMA1_IN,
+	VPE_CHAN_CHROMA1_IN,
+	VPE_CHAN_LUMA2_IN,
+	VPE_CHAN_CHROMA2_IN,
+	VPE_CHAN_LUMA3_IN,
+	VPE_CHAN_CHROMA3_IN,
+	VPE_CHAN_MV_IN,
+	VPE_CHAN_MV_OUT,
+	VPE_CHAN_LUMA_OUT,
+	VPE_CHAN_CHROMA_OUT,
+	VPE_CHAN_RGB_OUT,
+};
+
+/* flags for VPDMA data descriptors */
+#define VPDMA_DATA_ODD_LINE_SKIP	(1 << 0)
+#define VPDMA_DATA_EVEN_LINE_SKIP	(1 << 1)
+#define VPDMA_DATA_FRAME_1D		(1 << 2)
+#define VPDMA_DATA_MODE_TILED		(1 << 3)
+
+/*
+ * client identifiers used for configuration descriptors
+ */
+#define CFD_MMR_CLIENT		0
+#define CFD_SC_CLIENT		4
+
+/* Address data block header format */
+struct vpdma_adb_hdr {
+	u32			offset;
+	u32			nwords;
+	u32			reserved0;
+	u32			reserved1;
+};
+
+/* helpers for creating ADB headers for config descriptors MMRs as client */
+#define ADB_ADDR(dma_buf, str, fld)	((dma_buf)->addr + offsetof(str, fld))
+#define MMR_ADB_ADDR(buf, str, fld)	ADB_ADDR(&(buf), struct str, fld)
+
+#define VPDMA_SET_MMR_ADB_HDR(buf, str, hdr, regs, offset_a)	\
+	do {							\
+		struct vpdma_adb_hdr *h;			\
+		struct str *adb = NULL;				\
+		h = MMR_ADB_ADDR(buf, str, hdr);		\
+		h->offset = (offset_a);				\
+		h->nwords = sizeof(adb->regs) >> 2;		\
+	} while (0)
+
+/* vpdma descriptor buffer allocation and management */
+int vpdma_alloc_desc_buf(struct vpdma_buf *buf, size_t size);
+void vpdma_free_desc_buf(struct vpdma_buf *buf);
+int vpdma_map_desc_buf(struct vpdma_data *vpdma, struct vpdma_buf *buf);
+void vpdma_unmap_desc_buf(struct vpdma_data *vpdma, struct vpdma_buf *buf);
+
+/* vpdma descriptor list funcs */
+int vpdma_create_desc_list(struct vpdma_desc_list *list, size_t size, int type);
+void vpdma_reset_desc_list(struct vpdma_desc_list *list);
+void vpdma_free_desc_list(struct vpdma_desc_list *list);
+int vpdma_submit_descs(struct vpdma_data *vpdma, struct vpdma_desc_list *list);
+
+/* helpers for creating vpdma descriptors */
+void vpdma_add_cfd_block(struct vpdma_desc_list *list, int client,
+		struct vpdma_buf *blk, u32 dest_offset);
+void vpdma_add_cfd_adb(struct vpdma_desc_list *list, int client,
+		struct vpdma_buf *adb);
+void vpdma_add_sync_on_channel_ctd(struct vpdma_desc_list *list,
+		enum vpdma_channel chan);
+void vpdma_add_out_dtd(struct vpdma_desc_list *list, int width,
+		const struct v4l2_rect *c_rect,
+		const struct vpdma_data_format *fmt, dma_addr_t dma_addr,
+		enum vpdma_channel chan, u32 flags);
+void vpdma_add_in_dtd(struct vpdma_desc_list *list, int width,
+		const struct v4l2_rect *c_rect,
+		const struct vpdma_data_format *fmt, dma_addr_t dma_addr,
+		enum vpdma_channel chan, int field, u32 flags, int frame_width,
+		int frame_height, int start_h, int start_v);
+
+/* vpdma list interrupt management */
+void vpdma_enable_list_complete_irq(struct vpdma_data *vpdma, int list_num,
+		bool enable);
+void vpdma_clear_list_stat(struct vpdma_data *vpdma);
+
+/* vpdma client configuration */
+void vpdma_set_line_mode(struct vpdma_data *vpdma, int line_mode,
+		enum vpdma_channel chan);
+void vpdma_set_frame_start_event(struct vpdma_data *vpdma,
+		enum vpdma_frame_start_event fs_event, enum vpdma_channel chan);
+
+void vpdma_dump_regs(struct vpdma_data *vpdma);
+
+/* initialize vpdma, passed with VPE's platform device pointer */
+struct vpdma_data *vpdma_create(struct platform_device *pdev,
+		void (*cb)(struct platform_device *pdev));
+
+#endif
diff --git a/drivers/media/platform/ti-vpe/vpdma_priv.h b/drivers/media/platform/ti-vpe/vpdma_priv.h
new file mode 100644
index 0000000..c1a6ce1
--- /dev/null
+++ b/drivers/media/platform/ti-vpe/vpdma_priv.h
@@ -0,0 +1,641 @@
+/*
+ * Copyright (c) 2013 Texas Instruments Inc.
+ *
+ * David Griego, <dagriego@biglakesoftware.com>
+ * Dale Farnsworth, <dale@farnsworth.org>
+ * Archit Taneja, <archit@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ */
+
+#ifndef _TI_VPDMA_PRIV_H_
+#define _TI_VPDMA_PRIV_H_
+
+/*
+ * VPDMA Register offsets
+ */
+
+/* Top level */
+#define VPDMA_PID		0x00
+#define VPDMA_LIST_ADDR		0x04
+#define VPDMA_LIST_ATTR		0x08
+#define VPDMA_LIST_STAT_SYNC	0x0c
+#define VPDMA_BG_RGB		0x18
+#define VPDMA_BG_YUV		0x1c
+#define VPDMA_SETUP		0x30
+#define VPDMA_MAX_SIZE1		0x34
+#define VPDMA_MAX_SIZE2		0x38
+#define VPDMA_MAX_SIZE3		0x3c
+
+/* Interrupts */
+#define VPDMA_INT_CHAN_STAT(grp)	(0x40 + grp * 8)
+#define VPDMA_INT_CHAN_MASK(grp)	(VPDMA_INT_CHAN_STAT(grp) + 4)
+#define VPDMA_INT_CLIENT0_STAT		0x78
+#define VPDMA_INT_CLIENT0_MASK		0x7c
+#define VPDMA_INT_CLIENT1_STAT		0x80
+#define VPDMA_INT_CLIENT1_MASK		0x84
+#define VPDMA_INT_LIST0_STAT		0x88
+#define VPDMA_INT_LIST0_MASK		0x8c
+
+#define VPDMA_PERFMON(i)		(0x200 + i * 4)
+
+/* VPE specific client registers */
+#define VPDMA_DEI_CHROMA1_CSTAT		0x0300
+#define VPDMA_DEI_LUMA1_CSTAT		0x0304
+#define VPDMA_DEI_LUMA2_CSTAT		0x0308
+#define VPDMA_DEI_CHROMA2_CSTAT		0x030c
+#define VPDMA_DEI_LUMA3_CSTAT		0x0310
+#define VPDMA_DEI_CHROMA3_CSTAT		0x0314
+#define VPDMA_DEI_MV_IN_CSTAT		0x0330
+#define VPDMA_DEI_MV_OUT_CSTAT		0x033c
+#define VPDMA_VIP_UP_Y_CSTAT		0x0390
+#define VPDMA_VIP_UP_UV_CSTAT		0x0394
+#define VPDMA_VPI_CTL_CSTAT		0x03d0
+
+/* Reg field info for VPDMA_CLIENT_CSTAT registers */
+#define VPDMA_CSTAT_LINE_MODE_MASK	0x03
+#define VPDMA_CSTAT_LINE_MODE_SHIFT	8
+#define VPDMA_CSTAT_FRAME_START_MASK	0xf
+#define VPDMA_CSTAT_FRAME_START_SHIFT	10
+
+#define VPDMA_LIST_NUM_MASK		0x07
+#define VPDMA_LIST_NUM_SHFT		24
+#define VPDMA_LIST_STOP_SHFT		20
+#define VPDMA_LIST_RDY_MASK		0x01
+#define VPDMA_LIST_RDY_SHFT		19
+#define VPDMA_LIST_TYPE_MASK		0x03
+#define VPDMA_LIST_TYPE_SHFT		16
+#define VPDMA_LIST_SIZE_MASK		0xffff
+
+/* VPDMA data type values for data formats */
+#define DATA_TYPE_Y444				0x0
+#define DATA_TYPE_Y422				0x1
+#define DATA_TYPE_Y420				0x2
+#define DATA_TYPE_C444				0x4
+#define DATA_TYPE_C422				0x5
+#define DATA_TYPE_C420				0x6
+#define DATA_TYPE_YC422				0x7
+#define DATA_TYPE_YC444				0x8
+#define DATA_TYPE_CY422				0x27
+
+#define DATA_TYPE_RGB16_565			0x0
+#define DATA_TYPE_ARGB_1555			0x1
+#define DATA_TYPE_ARGB_4444			0x2
+#define DATA_TYPE_RGBA_5551			0x3
+#define DATA_TYPE_RGBA_4444			0x4
+#define DATA_TYPE_ARGB24_6666			0x5
+#define DATA_TYPE_RGB24_888			0x6
+#define DATA_TYPE_ARGB32_8888			0x7
+#define DATA_TYPE_RGBA24_6666			0x8
+#define DATA_TYPE_RGBA32_8888			0x9
+#define DATA_TYPE_BGR16_565			0x10
+#define DATA_TYPE_ABGR_1555			0x11
+#define DATA_TYPE_ABGR_4444			0x12
+#define DATA_TYPE_BGRA_5551			0x13
+#define DATA_TYPE_BGRA_4444			0x14
+#define DATA_TYPE_ABGR24_6666			0x15
+#define DATA_TYPE_BGR24_888			0x16
+#define DATA_TYPE_ABGR32_8888			0x17
+#define DATA_TYPE_BGRA24_6666			0x18
+#define DATA_TYPE_BGRA32_8888			0x19
+
+#define DATA_TYPE_MV				0x3
+
+/* VPDMA channel numbers(only VPE channels for now) */
+#define	VPE_CHAN_NUM_LUMA1_IN		0
+#define	VPE_CHAN_NUM_CHROMA1_IN		1
+#define	VPE_CHAN_NUM_LUMA2_IN		2
+#define	VPE_CHAN_NUM_CHROMA2_IN		3
+#define	VPE_CHAN_NUM_LUMA3_IN		4
+#define	VPE_CHAN_NUM_CHROMA3_IN		5
+#define	VPE_CHAN_NUM_MV_IN		12
+#define	VPE_CHAN_NUM_MV_OUT		15
+#define	VPE_CHAN_NUM_LUMA_OUT		102
+#define	VPE_CHAN_NUM_CHROMA_OUT		103
+#define	VPE_CHAN_NUM_RGB_OUT		106
+
+/*
+ * a VPDMA address data block payload for a configuration descriptor needs to
+ * have each sub block length as a multiple of 16 bytes. Therefore, the overall
+ * size of the payload also needs to be a multiple of 16 bytes. The sub block
+ * lengths should be ensured to be aligned by the VPDMA user.
+ */
+#define VPDMA_ADB_SIZE_ALIGN		0x0f
+
+/*
+ * data transfer descriptor
+ */
+struct vpdma_dtd {
+	u32			type_ctl_stride;
+	union {
+		u32		xfer_length_height;
+		u32		w1;
+	};
+	dma_addr_t		start_addr;
+	u32			pkt_ctl;
+	union {
+		u32		frame_width_height;	/* inbound */
+		dma_addr_t	desc_write_addr;	/* outbound */
+	};
+	union {
+		u32		start_h_v;		/* inbound */
+		u32		max_width_height;	/* outbound */
+	};
+	u32			client_attr0;
+	u32			client_attr1;
+};
+
+/* Data Transfer Descriptor specifics */
+#define DTD_NO_NOTIFY		0
+#define DTD_NOTIFY		1
+
+#define DTD_PKT_TYPE		0xa
+#define DTD_DIR_IN		0
+#define DTD_DIR_OUT		1
+
+/* type_ctl_stride */
+#define DTD_DATA_TYPE_MASK	0x3f
+#define DTD_DATA_TYPE_SHFT	26
+#define DTD_NOTIFY_MASK		0x01
+#define DTD_NOTIFY_SHFT		25
+#define DTD_FIELD_MASK		0x01
+#define DTD_FIELD_SHFT		24
+#define DTD_1D_MASK		0x01
+#define DTD_1D_SHFT		23
+#define DTD_EVEN_LINE_SKIP_MASK	0x01
+#define DTD_EVEN_LINE_SKIP_SHFT	20
+#define DTD_ODD_LINE_SKIP_MASK	0x01
+#define DTD_ODD_LINE_SKIP_SHFT	16
+#define DTD_LINE_STRIDE_MASK	0xffff
+#define DTD_LINE_STRIDE_SHFT	0
+
+/* xfer_length_height */
+#define DTD_LINE_LENGTH_MASK	0xffff
+#define DTD_LINE_LENGTH_SHFT	16
+#define DTD_XFER_HEIGHT_MASK	0xffff
+#define DTD_XFER_HEIGHT_SHFT	0
+
+/* pkt_ctl */
+#define DTD_PKT_TYPE_MASK	0x1f
+#define DTD_PKT_TYPE_SHFT	27
+#define DTD_MODE_MASK		0x01
+#define DTD_MODE_SHFT		26
+#define DTD_DIR_MASK		0x01
+#define DTD_DIR_SHFT		25
+#define DTD_CHAN_MASK		0x01ff
+#define DTD_CHAN_SHFT		16
+#define DTD_PRI_MASK		0x0f
+#define DTD_PRI_SHFT		9
+#define DTD_NEXT_CHAN_MASK	0x01ff
+#define DTD_NEXT_CHAN_SHFT	0
+
+/* frame_width_height */
+#define DTD_FRAME_WIDTH_MASK	0xffff
+#define DTD_FRAME_WIDTH_SHFT	16
+#define DTD_FRAME_HEIGHT_MASK	0xffff
+#define DTD_FRAME_HEIGHT_SHFT	0
+
+/* start_h_v */
+#define DTD_H_START_MASK	0xffff
+#define DTD_H_START_SHFT	16
+#define DTD_V_START_MASK	0xffff
+#define DTD_V_START_SHFT	0
+
+#define DTD_DESC_START_SHIFT	5
+#define DTD_WRITE_DESC_MASK	0x01
+#define DTD_WRITE_DESC_SHIFT	2
+#define DTD_DROP_DATA_MASK	0x01
+#define DTD_DROP_DATA_SHIFT	1
+#define DTD_USE_DESC_MASK	0x01
+#define DTD_USE_DESC_SHIFT	0
+
+/* max_width_height */
+#define DTD_MAX_WIDTH_MASK	0x07
+#define DTD_MAX_WIDTH_SHFT	4
+#define DTD_MAX_HEIGHT_MASK	0x07
+#define DTD_MAX_HEIGHT_SHFT	0
+
+/* max width configurations */
+ /* unlimited width */
+#define	MAX_OUT_WIDTH_UNLIMITED		0
+/* as specified in max_size1 reg */
+#define MAX_OUT_WIDTH_REG1		1
+/* as specified in max_size2 reg */
+#define MAX_OUT_WIDTH_REG2		2
+/* as specified in max_size3 reg */
+#define	MAX_OUT_WIDTH_REG3		3
+/* maximum of 352 pixels as width */
+#define MAX_OUT_WIDTH_352		4
+/* maximum of 768 pixels as width */
+#define	MAX_OUT_WIDTH_768		5
+/* maximum of 1280 pixels width */
+#define	MAX_OUT_WIDTH_1280		6
+/* maximum of 1920 pixels as width */
+#define	MAX_OUT_WIDTH_1920		7
+
+/* max height configurations */
+ /* unlimited height */
+#define	MAX_OUT_HEIGHT_UNLIMITED	0
+/* as specified in max_size1 reg */
+#define MAX_OUT_HEIGHT_REG1		1
+/* as specified in max_size2 reg */
+#define MAX_OUT_HEIGHT_REG2		2
+/* as specified in max_size3 reg */
+#define	MAX_OUT_HEIGHT_REG3		3
+/* maximum of 288 lines as height */
+#define MAX_OUT_HEIGHT_288		4
+/* maximum of 576 lines as height */
+#define	MAX_OUT_HEIGHT_576		5
+/* maximum of 720 lines as height */
+#define	MAX_OUT_HEIGHT_720		6
+/* maximum of 1080 lines as height */
+#define	MAX_OUT_HEIGHT_1080		7
+
+static inline u32 dtd_type_ctl_stride(int type, bool notify, int field,
+			bool one_d, bool even_line_skip, bool odd_line_skip,
+			int line_stride)
+{
+	return (type << DTD_DATA_TYPE_SHFT) | (notify << DTD_NOTIFY_SHFT) |
+		(field << DTD_FIELD_SHFT) | (one_d << DTD_1D_SHFT) |
+		(even_line_skip << DTD_EVEN_LINE_SKIP_SHFT) |
+		(odd_line_skip << DTD_ODD_LINE_SKIP_SHFT) |
+		line_stride;
+}
+
+static inline u32 dtd_xfer_length_height(int line_length, int xfer_height)
+{
+	return (line_length << DTD_LINE_LENGTH_SHFT) | xfer_height;
+}
+
+static inline u32 dtd_pkt_ctl(bool mode, bool dir, int chan, int pri,
+			int next_chan)
+{
+	return (DTD_PKT_TYPE << DTD_PKT_TYPE_SHFT) | (mode << DTD_MODE_SHFT) |
+		(dir << DTD_DIR_SHFT) | (chan << DTD_CHAN_SHFT) |
+		(pri << DTD_PRI_SHFT) | next_chan;
+}
+
+static inline u32 dtd_frame_width_height(int width, int height)
+{
+	return (width << DTD_FRAME_WIDTH_SHFT) | height;
+}
+
+static inline u32 dtd_desc_write_addr(unsigned int addr, bool write_desc,
+			bool drop_data, bool use_desc)
+{
+	return (addr << DTD_DESC_START_SHIFT) |
+		(write_desc << DTD_WRITE_DESC_SHIFT) |
+		(drop_data << DTD_DROP_DATA_SHIFT) |
+		use_desc;
+}
+
+static inline u32 dtd_start_h_v(int h_start, int v_start)
+{
+	return (h_start << DTD_H_START_SHFT) | v_start;
+}
+
+static inline u32 dtd_max_width_height(int max_width, int max_height)
+{
+	return (max_width << DTD_MAX_WIDTH_SHFT) | max_height;
+}
+
+static inline int dtd_get_data_type(struct vpdma_dtd *dtd)
+{
+	return dtd->type_ctl_stride >> DTD_DATA_TYPE_SHFT;
+}
+
+static inline bool dtd_get_notify(struct vpdma_dtd *dtd)
+{
+	return (dtd->type_ctl_stride >> DTD_NOTIFY_SHFT) & DTD_NOTIFY_MASK;
+}
+
+static inline int dtd_get_field(struct vpdma_dtd *dtd)
+{
+	return (dtd->type_ctl_stride >> DTD_FIELD_SHFT) & DTD_FIELD_MASK;
+}
+
+static inline bool dtd_get_1d(struct vpdma_dtd *dtd)
+{
+	return (dtd->type_ctl_stride >> DTD_1D_SHFT) & DTD_1D_MASK;
+}
+
+static inline bool dtd_get_even_line_skip(struct vpdma_dtd *dtd)
+{
+	return (dtd->type_ctl_stride >> DTD_EVEN_LINE_SKIP_SHFT)
+		& DTD_EVEN_LINE_SKIP_MASK;
+}
+
+static inline bool dtd_get_odd_line_skip(struct vpdma_dtd *dtd)
+{
+	return (dtd->type_ctl_stride >> DTD_ODD_LINE_SKIP_SHFT)
+		& DTD_ODD_LINE_SKIP_MASK;
+}
+
+static inline int dtd_get_line_stride(struct vpdma_dtd *dtd)
+{
+	return dtd->type_ctl_stride & DTD_LINE_STRIDE_MASK;
+}
+
+static inline int dtd_get_line_length(struct vpdma_dtd *dtd)
+{
+	return dtd->xfer_length_height >> DTD_LINE_LENGTH_SHFT;
+}
+
+static inline int dtd_get_xfer_height(struct vpdma_dtd *dtd)
+{
+	return dtd->xfer_length_height & DTD_XFER_HEIGHT_MASK;
+}
+
+static inline int dtd_get_pkt_type(struct vpdma_dtd *dtd)
+{
+	return dtd->pkt_ctl >> DTD_PKT_TYPE_SHFT;
+}
+
+static inline bool dtd_get_mode(struct vpdma_dtd *dtd)
+{
+	return (dtd->pkt_ctl >> DTD_MODE_SHFT) & DTD_MODE_MASK;
+}
+
+static inline bool dtd_get_dir(struct vpdma_dtd *dtd)
+{
+	return (dtd->pkt_ctl >> DTD_DIR_SHFT) & DTD_DIR_MASK;
+}
+
+static inline int dtd_get_chan(struct vpdma_dtd *dtd)
+{
+	return (dtd->pkt_ctl >> DTD_CHAN_SHFT) & DTD_CHAN_MASK;
+}
+
+static inline int dtd_get_priority(struct vpdma_dtd *dtd)
+{
+	return (dtd->pkt_ctl >> DTD_PRI_SHFT) & DTD_PRI_MASK;
+}
+
+static inline int dtd_get_next_chan(struct vpdma_dtd *dtd)
+{
+	return (dtd->pkt_ctl >> DTD_NEXT_CHAN_SHFT) & DTD_NEXT_CHAN_MASK;
+}
+
+static inline int dtd_get_frame_width(struct vpdma_dtd *dtd)
+{
+	return dtd->frame_width_height >> DTD_FRAME_WIDTH_SHFT;
+}
+
+static inline int dtd_get_frame_height(struct vpdma_dtd *dtd)
+{
+	return dtd->frame_width_height & DTD_FRAME_HEIGHT_MASK;
+}
+
+static inline int dtd_get_desc_write_addr(struct vpdma_dtd *dtd)
+{
+	return dtd->desc_write_addr >> DTD_DESC_START_SHIFT;
+}
+
+static inline bool dtd_get_write_desc(struct vpdma_dtd *dtd)
+{
+	return (dtd->desc_write_addr >> DTD_WRITE_DESC_SHIFT) &
+							DTD_WRITE_DESC_MASK;
+}
+
+static inline bool dtd_get_drop_data(struct vpdma_dtd *dtd)
+{
+	return (dtd->desc_write_addr >> DTD_DROP_DATA_SHIFT) &
+							DTD_DROP_DATA_MASK;
+}
+
+static inline bool dtd_get_use_desc(struct vpdma_dtd *dtd)
+{
+	return dtd->desc_write_addr & DTD_USE_DESC_MASK;
+}
+
+static inline int dtd_get_h_start(struct vpdma_dtd *dtd)
+{
+	return dtd->start_h_v >> DTD_H_START_SHFT;
+}
+
+static inline int dtd_get_v_start(struct vpdma_dtd *dtd)
+{
+	return dtd->start_h_v & DTD_V_START_MASK;
+}
+
+static inline int dtd_get_max_width(struct vpdma_dtd *dtd)
+{
+	return (dtd->max_width_height >> DTD_MAX_WIDTH_SHFT) &
+							DTD_MAX_WIDTH_MASK;
+}
+
+static inline int dtd_get_max_height(struct vpdma_dtd *dtd)
+{
+	return (dtd->max_width_height >> DTD_MAX_HEIGHT_SHFT) &
+							DTD_MAX_HEIGHT_MASK;
+}
+
+/*
+ * configuration descriptor
+ */
+struct vpdma_cfd {
+	union {
+		u32	dest_addr_offset;
+		u32	w0;
+	};
+	union {
+		u32	block_len;		/* in words */
+		u32	w1;
+	};
+	u32		payload_addr;
+	u32		ctl_payload_len;	/* in words */
+};
+
+/* Configuration descriptor specifics */
+
+#define CFD_PKT_TYPE		0xb
+
+#define CFD_DIRECT		1
+#define CFD_INDIRECT		0
+#define CFD_CLS_ADB		0
+#define CFD_CLS_BLOCK		1
+
+/* block_len */
+#define CFD__BLOCK_LEN_MASK	0xffff
+#define CFD__BLOCK_LEN_SHFT	0
+
+/* ctl_payload_len */
+#define CFD_PKT_TYPE_MASK	0x1f
+#define CFD_PKT_TYPE_SHFT	27
+#define CFD_DIRECT_MASK		0x01
+#define CFD_DIRECT_SHFT		26
+#define CFD_CLASS_MASK		0x03
+#define CFD_CLASS_SHFT		24
+#define CFD_DEST_MASK		0xff
+#define CFD_DEST_SHFT		16
+#define CFD_PAYLOAD_LEN_MASK	0xffff
+#define CFD_PAYLOAD_LEN_SHFT	0
+
+static inline u32 cfd_pkt_payload_len(bool direct, int cls, int dest,
+		int payload_len)
+{
+	return (CFD_PKT_TYPE << CFD_PKT_TYPE_SHFT) |
+		(direct << CFD_DIRECT_SHFT) |
+		(cls << CFD_CLASS_SHFT) |
+		(dest << CFD_DEST_SHFT) |
+		payload_len;
+}
+
+static inline int cfd_get_pkt_type(struct vpdma_cfd *cfd)
+{
+	return cfd->ctl_payload_len >> CFD_PKT_TYPE_SHFT;
+}
+
+static inline bool cfd_get_direct(struct vpdma_cfd *cfd)
+{
+	return (cfd->ctl_payload_len >> CFD_DIRECT_SHFT) & CFD_DIRECT_MASK;
+}
+
+static inline bool cfd_get_class(struct vpdma_cfd *cfd)
+{
+	return (cfd->ctl_payload_len >> CFD_CLASS_SHFT) & CFD_CLASS_MASK;
+}
+
+static inline int cfd_get_dest(struct vpdma_cfd *cfd)
+{
+	return (cfd->ctl_payload_len >> CFD_DEST_SHFT) & CFD_DEST_MASK;
+}
+
+static inline int cfd_get_payload_len(struct vpdma_cfd *cfd)
+{
+	return cfd->ctl_payload_len & CFD_PAYLOAD_LEN_MASK;
+}
+
+/*
+ * control descriptor
+ */
+struct vpdma_ctd {
+	union {
+		u32	timer_value;
+		u32	list_addr;
+		u32	w0;
+	};
+	union {
+		u32	pixel_line_count;
+		u32	list_size;
+		u32	w1;
+	};
+	union {
+		u32	event;
+		u32	fid_ctl;
+		u32	w2;
+	};
+	u32		type_source_ctl;
+};
+
+/* control descriptor types */
+#define CTD_TYPE_SYNC_ON_CLIENT		0
+#define CTD_TYPE_SYNC_ON_LIST		1
+#define CTD_TYPE_SYNC_ON_EXT		2
+#define CTD_TYPE_SYNC_ON_LM_TIMER	3
+#define CTD_TYPE_SYNC_ON_CHANNEL	4
+#define CTD_TYPE_CHNG_CLIENT_IRQ	5
+#define CTD_TYPE_SEND_IRQ		6
+#define CTD_TYPE_RELOAD_LIST		7
+#define CTD_TYPE_ABORT_CHANNEL		8
+
+#define CTD_PKT_TYPE		0xc
+
+/* timer_value */
+#define CTD_TIMER_VALUE_MASK	0xffff
+#define CTD_TIMER_VALUE_SHFT	0
+
+/* pixel_line_count */
+#define CTD_PIXEL_COUNT_MASK	0xffff
+#define CTD_PIXEL_COUNT_SHFT	16
+#define CTD_LINE_COUNT_MASK	0xffff
+#define CTD_LINE_COUNT_SHFT	0
+
+/* list_size */
+#define CTD_LIST_SIZE_MASK	0xffff
+#define CTD_LIST_SIZE_SHFT	0
+
+/* event */
+#define CTD_EVENT_MASK		0x0f
+#define CTD_EVENT_SHFT		0
+
+/* fid_ctl */
+#define CTD_FID2_MASK		0x03
+#define CTD_FID2_SHFT		4
+#define CTD_FID1_MASK		0x03
+#define CTD_FID1_SHFT		2
+#define CTD_FID0_MASK		0x03
+#define CTD_FID0_SHFT		0
+
+/* type_source_ctl */
+#define CTD_PKT_TYPE_MASK	0x1f
+#define CTD_PKT_TYPE_SHFT	27
+#define CTD_SOURCE_MASK		0xff
+#define CTD_SOURCE_SHFT		16
+#define CTD_CONTROL_MASK	0x0f
+#define CTD_CONTROL_SHFT	0
+
+static inline u32 ctd_pixel_line_count(int pixel_count, int line_count)
+{
+	return (pixel_count << CTD_PIXEL_COUNT_SHFT) | line_count;
+}
+
+static inline u32 ctd_set_fid_ctl(int fid0, int fid1, int fid2)
+{
+	return (fid2 << CTD_FID2_SHFT) | (fid1 << CTD_FID1_SHFT) | fid0;
+}
+
+static inline u32 ctd_type_source_ctl(int source, int control)
+{
+	return (CTD_PKT_TYPE << CTD_PKT_TYPE_SHFT) |
+		(source << CTD_SOURCE_SHFT) | control;
+}
+
+static inline u32 ctd_get_pixel_count(struct vpdma_ctd *ctd)
+{
+	return ctd->pixel_line_count >> CTD_PIXEL_COUNT_SHFT;
+}
+
+static inline int ctd_get_line_count(struct vpdma_ctd *ctd)
+{
+	return ctd->pixel_line_count & CTD_LINE_COUNT_MASK;
+}
+
+static inline int ctd_get_event(struct vpdma_ctd *ctd)
+{
+	return ctd->event & CTD_EVENT_MASK;
+}
+
+static inline int ctd_get_fid2_ctl(struct vpdma_ctd *ctd)
+{
+	return (ctd->fid_ctl >> CTD_FID2_SHFT) & CTD_FID2_MASK;
+}
+
+static inline int ctd_get_fid1_ctl(struct vpdma_ctd *ctd)
+{
+	return (ctd->fid_ctl >> CTD_FID1_SHFT) & CTD_FID1_MASK;
+}
+
+static inline int ctd_get_fid0_ctl(struct vpdma_ctd *ctd)
+{
+	return ctd->fid_ctl & CTD_FID2_MASK;
+}
+
+static inline int ctd_get_pkt_type(struct vpdma_ctd *ctd)
+{
+	return ctd->type_source_ctl >> CTD_PKT_TYPE_SHFT;
+}
+
+static inline int ctd_get_source(struct vpdma_ctd *ctd)
+{
+	return (ctd->type_source_ctl >> CTD_SOURCE_SHFT) & CTD_SOURCE_MASK;
+}
+
+static inline int ctd_get_ctl(struct vpdma_ctd *ctd)
+{
+	return ctd->type_source_ctl & CTD_CONTROL_MASK;
+}
+
+#endif
diff --git a/drivers/media/platform/ti-vpe/vpe.c b/drivers/media/platform/ti-vpe/vpe.c
new file mode 100644
index 0000000..de24eff
--- /dev/null
+++ b/drivers/media/platform/ti-vpe/vpe.c
@@ -0,0 +1,2321 @@
+/*
+ * TI VPE mem2mem driver, based on the virtual v4l2-mem2mem example driver
+ *
+ * Copyright (c) 2013 Texas Instruments Inc.
+ * David Griego, <dagriego@biglakesoftware.com>
+ * Dale Farnsworth, <dale@farnsworth.org>
+ * Archit Taneja, <archit@ti.com>
+ *
+ * Copyright (c) 2009-2010 Samsung Electronics Co., Ltd.
+ * Pawel Osciak, <pawel@osciak.com>
+ * Marek Szyprowski, <m.szyprowski@samsung.com>
+ *
+ * Based on the virtual v4l2-mem2mem example device
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation
+ */
+
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/err.h>
+#include <linux/fs.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/ioctl.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <linux/log2.h>
+#include <linux/sizes.h>
+
+#include <media/v4l2-common.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "vpdma.h"
+#include "vpe_regs.h"
+#include "sc.h"
+#include "csc.h"
+
+#define VPE_MODULE_NAME "vpe"
+
+/* minimum and maximum frame sizes */
+#define MIN_W		32
+#define MIN_H		32
+#define MAX_W		1920
+#define MAX_H		1080
+
+/* required alignments */
+#define S_ALIGN		0	/* multiple of 1 */
+#define H_ALIGN		1	/* multiple of 2 */
+
+/* flags that indicate a format can be used for capture/output */
+#define VPE_FMT_TYPE_CAPTURE	(1 << 0)
+#define VPE_FMT_TYPE_OUTPUT	(1 << 1)
+
+/* used as plane indices */
+#define VPE_MAX_PLANES	2
+#define VPE_LUMA	0
+#define VPE_CHROMA	1
+
+/* per m2m context info */
+#define VPE_MAX_SRC_BUFS	3	/* need 3 src fields to de-interlace */
+
+#define VPE_DEF_BUFS_PER_JOB	1	/* default one buffer per batch job */
+
+/*
+ * each VPE context can need up to 3 config descriptors, 7 input descriptors,
+ * 3 output descriptors, and 10 control descriptors
+ */
+#define VPE_DESC_LIST_SIZE	(10 * VPDMA_DTD_DESC_SIZE +	\
+					13 * VPDMA_CFD_CTD_DESC_SIZE)
+
+#define vpe_dbg(vpedev, fmt, arg...)	\
+		dev_dbg((vpedev)->v4l2_dev.dev, fmt, ##arg)
+#define vpe_err(vpedev, fmt, arg...)	\
+		dev_err((vpedev)->v4l2_dev.dev, fmt, ##arg)
+
+struct vpe_us_coeffs {
+	unsigned short	anchor_fid0_c0;
+	unsigned short	anchor_fid0_c1;
+	unsigned short	anchor_fid0_c2;
+	unsigned short	anchor_fid0_c3;
+	unsigned short	interp_fid0_c0;
+	unsigned short	interp_fid0_c1;
+	unsigned short	interp_fid0_c2;
+	unsigned short	interp_fid0_c3;
+	unsigned short	anchor_fid1_c0;
+	unsigned short	anchor_fid1_c1;
+	unsigned short	anchor_fid1_c2;
+	unsigned short	anchor_fid1_c3;
+	unsigned short	interp_fid1_c0;
+	unsigned short	interp_fid1_c1;
+	unsigned short	interp_fid1_c2;
+	unsigned short	interp_fid1_c3;
+};
+
+/*
+ * Default upsampler coefficients
+ */
+static const struct vpe_us_coeffs us_coeffs[] = {
+	{
+		/* Coefficients for progressive input */
+		0x00C8, 0x0348, 0x0018, 0x3FD8, 0x3FB8, 0x0378, 0x00E8, 0x3FE8,
+		0x00C8, 0x0348, 0x0018, 0x3FD8, 0x3FB8, 0x0378, 0x00E8, 0x3FE8,
+	},
+	{
+		/* Coefficients for Top Field Interlaced input */
+		0x0051, 0x03D5, 0x3FE3, 0x3FF7, 0x3FB5, 0x02E9, 0x018F, 0x3FD3,
+		/* Coefficients for Bottom Field Interlaced input */
+		0x016B, 0x0247, 0x00B1, 0x3F9D, 0x3FCF, 0x03DB, 0x005D, 0x3FF9,
+	},
+};
+
+/*
+ * the following registers are for configuring some of the parameters of the
+ * motion and edge detection blocks inside DEI, these generally remain the same,
+ * these could be passed later via userspace if some one needs to tweak these.
+ */
+struct vpe_dei_regs {
+	unsigned long mdt_spacial_freq_thr_reg;		/* VPE_DEI_REG2 */
+	unsigned long edi_config_reg;			/* VPE_DEI_REG3 */
+	unsigned long edi_lut_reg0;			/* VPE_DEI_REG4 */
+	unsigned long edi_lut_reg1;			/* VPE_DEI_REG5 */
+	unsigned long edi_lut_reg2;			/* VPE_DEI_REG6 */
+	unsigned long edi_lut_reg3;			/* VPE_DEI_REG7 */
+};
+
+/*
+ * default expert DEI register values, unlikely to be modified.
+ */
+static const struct vpe_dei_regs dei_regs = {
+	.mdt_spacial_freq_thr_reg = 0x020C0804u,
+	.edi_config_reg = 0x0118100Fu,
+	.edi_lut_reg0 = 0x08040200u,
+	.edi_lut_reg1 = 0x1010100Cu,
+	.edi_lut_reg2 = 0x10101010u,
+	.edi_lut_reg3 = 0x10101010u,
+};
+
+/*
+ * The port_data structure contains per-port data.
+ */
+struct vpe_port_data {
+	enum vpdma_channel channel;	/* VPDMA channel */
+	u8	vb_index;		/* input frame f, f-1, f-2 index */
+	u8	vb_part;		/* plane index for co-panar formats */
+};
+
+/*
+ * Define indices into the port_data tables
+ */
+#define VPE_PORT_LUMA1_IN	0
+#define VPE_PORT_CHROMA1_IN	1
+#define VPE_PORT_LUMA2_IN	2
+#define VPE_PORT_CHROMA2_IN	3
+#define VPE_PORT_LUMA3_IN	4
+#define VPE_PORT_CHROMA3_IN	5
+#define VPE_PORT_MV_IN		6
+#define VPE_PORT_MV_OUT		7
+#define VPE_PORT_LUMA_OUT	8
+#define VPE_PORT_CHROMA_OUT	9
+#define VPE_PORT_RGB_OUT	10
+
+static const struct vpe_port_data port_data[11] = {
+	[VPE_PORT_LUMA1_IN] = {
+		.channel	= VPE_CHAN_LUMA1_IN,
+		.vb_index	= 0,
+		.vb_part	= VPE_LUMA,
+	},
+	[VPE_PORT_CHROMA1_IN] = {
+		.channel	= VPE_CHAN_CHROMA1_IN,
+		.vb_index	= 0,
+		.vb_part	= VPE_CHROMA,
+	},
+	[VPE_PORT_LUMA2_IN] = {
+		.channel	= VPE_CHAN_LUMA2_IN,
+		.vb_index	= 1,
+		.vb_part	= VPE_LUMA,
+	},
+	[VPE_PORT_CHROMA2_IN] = {
+		.channel	= VPE_CHAN_CHROMA2_IN,
+		.vb_index	= 1,
+		.vb_part	= VPE_CHROMA,
+	},
+	[VPE_PORT_LUMA3_IN] = {
+		.channel	= VPE_CHAN_LUMA3_IN,
+		.vb_index	= 2,
+		.vb_part	= VPE_LUMA,
+	},
+	[VPE_PORT_CHROMA3_IN] = {
+		.channel	= VPE_CHAN_CHROMA3_IN,
+		.vb_index	= 2,
+		.vb_part	= VPE_CHROMA,
+	},
+	[VPE_PORT_MV_IN] = {
+		.channel	= VPE_CHAN_MV_IN,
+	},
+	[VPE_PORT_MV_OUT] = {
+		.channel	= VPE_CHAN_MV_OUT,
+	},
+	[VPE_PORT_LUMA_OUT] = {
+		.channel	= VPE_CHAN_LUMA_OUT,
+		.vb_part	= VPE_LUMA,
+	},
+	[VPE_PORT_CHROMA_OUT] = {
+		.channel	= VPE_CHAN_CHROMA_OUT,
+		.vb_part	= VPE_CHROMA,
+	},
+	[VPE_PORT_RGB_OUT] = {
+		.channel	= VPE_CHAN_RGB_OUT,
+		.vb_part	= VPE_LUMA,
+	},
+};
+
+
+/* driver info for each of the supported video formats */
+struct vpe_fmt {
+	char	*name;			/* human-readable name */
+	u32	fourcc;			/* standard format identifier */
+	u8	types;			/* CAPTURE and/or OUTPUT */
+	u8	coplanar;		/* set for unpacked Luma and Chroma */
+	/* vpdma format info for each plane */
+	struct vpdma_data_format const *vpdma_fmt[VPE_MAX_PLANES];
+};
+
+static struct vpe_fmt vpe_formats[] = {
+	{
+		.name		= "YUV 422 co-planar",
+		.fourcc		= V4L2_PIX_FMT_NV16,
+		.types		= VPE_FMT_TYPE_CAPTURE | VPE_FMT_TYPE_OUTPUT,
+		.coplanar	= 1,
+		.vpdma_fmt	= { &vpdma_yuv_fmts[VPDMA_DATA_FMT_Y444],
+				    &vpdma_yuv_fmts[VPDMA_DATA_FMT_C444],
+				  },
+	},
+	{
+		.name		= "YUV 420 co-planar",
+		.fourcc		= V4L2_PIX_FMT_NV12,
+		.types		= VPE_FMT_TYPE_CAPTURE | VPE_FMT_TYPE_OUTPUT,
+		.coplanar	= 1,
+		.vpdma_fmt	= { &vpdma_yuv_fmts[VPDMA_DATA_FMT_Y420],
+				    &vpdma_yuv_fmts[VPDMA_DATA_FMT_C420],
+				  },
+	},
+	{
+		.name		= "YUYV 422 packed",
+		.fourcc		= V4L2_PIX_FMT_YUYV,
+		.types		= VPE_FMT_TYPE_CAPTURE | VPE_FMT_TYPE_OUTPUT,
+		.coplanar	= 0,
+		.vpdma_fmt	= { &vpdma_yuv_fmts[VPDMA_DATA_FMT_YC422],
+				  },
+	},
+	{
+		.name		= "UYVY 422 packed",
+		.fourcc		= V4L2_PIX_FMT_UYVY,
+		.types		= VPE_FMT_TYPE_CAPTURE | VPE_FMT_TYPE_OUTPUT,
+		.coplanar	= 0,
+		.vpdma_fmt	= { &vpdma_yuv_fmts[VPDMA_DATA_FMT_CY422],
+				  },
+	},
+	{
+		.name		= "RGB888 packed",
+		.fourcc		= V4L2_PIX_FMT_RGB24,
+		.types		= VPE_FMT_TYPE_CAPTURE,
+		.coplanar	= 0,
+		.vpdma_fmt	= { &vpdma_rgb_fmts[VPDMA_DATA_FMT_RGB24],
+				  },
+	},
+	{
+		.name		= "ARGB32",
+		.fourcc		= V4L2_PIX_FMT_RGB32,
+		.types		= VPE_FMT_TYPE_CAPTURE,
+		.coplanar	= 0,
+		.vpdma_fmt	= { &vpdma_rgb_fmts[VPDMA_DATA_FMT_ARGB32],
+				  },
+	},
+	{
+		.name		= "BGR888 packed",
+		.fourcc		= V4L2_PIX_FMT_BGR24,
+		.types		= VPE_FMT_TYPE_CAPTURE,
+		.coplanar	= 0,
+		.vpdma_fmt	= { &vpdma_rgb_fmts[VPDMA_DATA_FMT_BGR24],
+				  },
+	},
+	{
+		.name		= "ABGR32",
+		.fourcc		= V4L2_PIX_FMT_BGR32,
+		.types		= VPE_FMT_TYPE_CAPTURE,
+		.coplanar	= 0,
+		.vpdma_fmt	= { &vpdma_rgb_fmts[VPDMA_DATA_FMT_ABGR32],
+				  },
+	},
+};
+
+/*
+ * per-queue, driver-specific private data.
+ * there is one source queue and one destination queue for each m2m context.
+ */
+struct vpe_q_data {
+	unsigned int		width;				/* frame width */
+	unsigned int		height;				/* frame height */
+	unsigned int		bytesperline[VPE_MAX_PLANES];	/* bytes per line in memory */
+	enum v4l2_colorspace	colorspace;
+	enum v4l2_field		field;				/* supported field value */
+	unsigned int		flags;
+	unsigned int		sizeimage[VPE_MAX_PLANES];	/* image size in memory */
+	struct v4l2_rect	c_rect;				/* crop/compose rectangle */
+	struct vpe_fmt		*fmt;				/* format info */
+};
+
+/* vpe_q_data flag bits */
+#define	Q_DATA_FRAME_1D		(1 << 0)
+#define	Q_DATA_MODE_TILED	(1 << 1)
+#define	Q_DATA_INTERLACED	(1 << 2)
+
+enum {
+	Q_DATA_SRC = 0,
+	Q_DATA_DST = 1,
+};
+
+/* find our format description corresponding to the passed v4l2_format */
+static struct vpe_fmt *find_format(struct v4l2_format *f)
+{
+	struct vpe_fmt *fmt;
+	unsigned int k;
+
+	for (k = 0; k < ARRAY_SIZE(vpe_formats); k++) {
+		fmt = &vpe_formats[k];
+		if (fmt->fourcc == f->fmt.pix.pixelformat)
+			return fmt;
+	}
+
+	return NULL;
+}
+
+/*
+ * there is one vpe_dev structure in the driver, it is shared by
+ * all instances.
+ */
+struct vpe_dev {
+	struct v4l2_device	v4l2_dev;
+	struct video_device	vfd;
+	struct v4l2_m2m_dev	*m2m_dev;
+
+	atomic_t		num_instances;	/* count of driver instances */
+	dma_addr_t		loaded_mmrs;	/* shadow mmrs in device */
+	struct mutex		dev_mutex;
+	spinlock_t		lock;
+
+	int			irq;
+	void __iomem		*base;
+	struct resource		*res;
+
+	struct vb2_alloc_ctx	*alloc_ctx;
+	struct vpdma_data	*vpdma;		/* vpdma data handle */
+	struct sc_data		*sc;		/* scaler data handle */
+	struct csc_data		*csc;		/* csc data handle */
+};
+
+/*
+ * There is one vpe_ctx structure for each m2m context.
+ */
+struct vpe_ctx {
+	struct v4l2_fh		fh;
+	struct vpe_dev		*dev;
+	struct v4l2_ctrl_handler hdl;
+
+	unsigned int		field;			/* current field */
+	unsigned int		sequence;		/* current frame/field seq */
+	unsigned int		aborting;		/* abort after next irq */
+
+	unsigned int		bufs_per_job;		/* input buffers per batch */
+	unsigned int		bufs_completed;		/* bufs done in this batch */
+
+	struct vpe_q_data	q_data[2];		/* src & dst queue data */
+	struct vb2_v4l2_buffer	*src_vbs[VPE_MAX_SRC_BUFS];
+	struct vb2_v4l2_buffer	*dst_vb;
+
+	dma_addr_t		mv_buf_dma[2];		/* dma addrs of motion vector in/out bufs */
+	void			*mv_buf[2];		/* virtual addrs of motion vector bufs */
+	size_t			mv_buf_size;		/* current motion vector buffer size */
+	struct vpdma_buf	mmr_adb;		/* shadow reg addr/data block */
+	struct vpdma_buf	sc_coeff_h;		/* h coeff buffer */
+	struct vpdma_buf	sc_coeff_v;		/* v coeff buffer */
+	struct vpdma_desc_list	desc_list;		/* DMA descriptor list */
+
+	bool			deinterlacing;		/* using de-interlacer */
+	bool			load_mmrs;		/* have new shadow reg values */
+
+	unsigned int		src_mv_buf_selector;
+};
+
+
+/*
+ * M2M devices get 2 queues.
+ * Return the queue given the type.
+ */
+static struct vpe_q_data *get_q_data(struct vpe_ctx *ctx,
+				     enum v4l2_buf_type type)
+{
+	switch (type) {
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+		return &ctx->q_data[Q_DATA_SRC];
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		return &ctx->q_data[Q_DATA_DST];
+	default:
+		BUG();
+	}
+	return NULL;
+}
+
+static u32 read_reg(struct vpe_dev *dev, int offset)
+{
+	return ioread32(dev->base + offset);
+}
+
+static void write_reg(struct vpe_dev *dev, int offset, u32 value)
+{
+	iowrite32(value, dev->base + offset);
+}
+
+/* register field read/write helpers */
+static int get_field(u32 value, u32 mask, int shift)
+{
+	return (value & (mask << shift)) >> shift;
+}
+
+static int read_field_reg(struct vpe_dev *dev, int offset, u32 mask, int shift)
+{
+	return get_field(read_reg(dev, offset), mask, shift);
+}
+
+static void write_field(u32 *valp, u32 field, u32 mask, int shift)
+{
+	u32 val = *valp;
+
+	val &= ~(mask << shift);
+	val |= (field & mask) << shift;
+	*valp = val;
+}
+
+static void write_field_reg(struct vpe_dev *dev, int offset, u32 field,
+		u32 mask, int shift)
+{
+	u32 val = read_reg(dev, offset);
+
+	write_field(&val, field, mask, shift);
+
+	write_reg(dev, offset, val);
+}
+
+/*
+ * DMA address/data block for the shadow registers
+ */
+struct vpe_mmr_adb {
+	struct vpdma_adb_hdr	out_fmt_hdr;
+	u32			out_fmt_reg[1];
+	u32			out_fmt_pad[3];
+	struct vpdma_adb_hdr	us1_hdr;
+	u32			us1_regs[8];
+	struct vpdma_adb_hdr	us2_hdr;
+	u32			us2_regs[8];
+	struct vpdma_adb_hdr	us3_hdr;
+	u32			us3_regs[8];
+	struct vpdma_adb_hdr	dei_hdr;
+	u32			dei_regs[8];
+	struct vpdma_adb_hdr	sc_hdr0;
+	u32			sc_regs0[7];
+	u32			sc_pad0[1];
+	struct vpdma_adb_hdr	sc_hdr8;
+	u32			sc_regs8[6];
+	u32			sc_pad8[2];
+	struct vpdma_adb_hdr	sc_hdr17;
+	u32			sc_regs17[9];
+	u32			sc_pad17[3];
+	struct vpdma_adb_hdr	csc_hdr;
+	u32			csc_regs[6];
+	u32			csc_pad[2];
+};
+
+#define GET_OFFSET_TOP(ctx, obj, reg)	\
+	((obj)->res->start - ctx->dev->res->start + reg)
+
+#define VPE_SET_MMR_ADB_HDR(ctx, hdr, regs, offset_a)	\
+	VPDMA_SET_MMR_ADB_HDR(ctx->mmr_adb, vpe_mmr_adb, hdr, regs, offset_a)
+/*
+ * Set the headers for all of the address/data block structures.
+ */
+static void init_adb_hdrs(struct vpe_ctx *ctx)
+{
+	VPE_SET_MMR_ADB_HDR(ctx, out_fmt_hdr, out_fmt_reg, VPE_CLK_FORMAT_SELECT);
+	VPE_SET_MMR_ADB_HDR(ctx, us1_hdr, us1_regs, VPE_US1_R0);
+	VPE_SET_MMR_ADB_HDR(ctx, us2_hdr, us2_regs, VPE_US2_R0);
+	VPE_SET_MMR_ADB_HDR(ctx, us3_hdr, us3_regs, VPE_US3_R0);
+	VPE_SET_MMR_ADB_HDR(ctx, dei_hdr, dei_regs, VPE_DEI_FRAME_SIZE);
+	VPE_SET_MMR_ADB_HDR(ctx, sc_hdr0, sc_regs0,
+		GET_OFFSET_TOP(ctx, ctx->dev->sc, CFG_SC0));
+	VPE_SET_MMR_ADB_HDR(ctx, sc_hdr8, sc_regs8,
+		GET_OFFSET_TOP(ctx, ctx->dev->sc, CFG_SC8));
+	VPE_SET_MMR_ADB_HDR(ctx, sc_hdr17, sc_regs17,
+		GET_OFFSET_TOP(ctx, ctx->dev->sc, CFG_SC17));
+	VPE_SET_MMR_ADB_HDR(ctx, csc_hdr, csc_regs,
+		GET_OFFSET_TOP(ctx, ctx->dev->csc, CSC_CSC00));
+};
+
+/*
+ * Allocate or re-allocate the motion vector DMA buffers
+ * There are two buffers, one for input and one for output.
+ * However, the roles are reversed after each field is processed.
+ * In other words, after each field is processed, the previous
+ * output (dst) MV buffer becomes the new input (src) MV buffer.
+ */
+static int realloc_mv_buffers(struct vpe_ctx *ctx, size_t size)
+{
+	struct device *dev = ctx->dev->v4l2_dev.dev;
+
+	if (ctx->mv_buf_size == size)
+		return 0;
+
+	if (ctx->mv_buf[0])
+		dma_free_coherent(dev, ctx->mv_buf_size, ctx->mv_buf[0],
+			ctx->mv_buf_dma[0]);
+
+	if (ctx->mv_buf[1])
+		dma_free_coherent(dev, ctx->mv_buf_size, ctx->mv_buf[1],
+			ctx->mv_buf_dma[1]);
+
+	if (size == 0)
+		return 0;
+
+	ctx->mv_buf[0] = dma_alloc_coherent(dev, size, &ctx->mv_buf_dma[0],
+				GFP_KERNEL);
+	if (!ctx->mv_buf[0]) {
+		vpe_err(ctx->dev, "failed to allocate motion vector buffer\n");
+		return -ENOMEM;
+	}
+
+	ctx->mv_buf[1] = dma_alloc_coherent(dev, size, &ctx->mv_buf_dma[1],
+				GFP_KERNEL);
+	if (!ctx->mv_buf[1]) {
+		vpe_err(ctx->dev, "failed to allocate motion vector buffer\n");
+		dma_free_coherent(dev, size, ctx->mv_buf[0],
+			ctx->mv_buf_dma[0]);
+
+		return -ENOMEM;
+	}
+
+	ctx->mv_buf_size = size;
+	ctx->src_mv_buf_selector = 0;
+
+	return 0;
+}
+
+static void free_mv_buffers(struct vpe_ctx *ctx)
+{
+	realloc_mv_buffers(ctx, 0);
+}
+
+/*
+ * While de-interlacing, we keep the two most recent input buffers
+ * around.  This function frees those two buffers when we have
+ * finished processing the current stream.
+ */
+static void free_vbs(struct vpe_ctx *ctx)
+{
+	struct vpe_dev *dev = ctx->dev;
+	unsigned long flags;
+
+	if (ctx->src_vbs[2] == NULL)
+		return;
+
+	spin_lock_irqsave(&dev->lock, flags);
+	if (ctx->src_vbs[2]) {
+		v4l2_m2m_buf_done(ctx->src_vbs[2], VB2_BUF_STATE_DONE);
+		v4l2_m2m_buf_done(ctx->src_vbs[1], VB2_BUF_STATE_DONE);
+	}
+	spin_unlock_irqrestore(&dev->lock, flags);
+}
+
+/*
+ * Enable or disable the VPE clocks
+ */
+static void vpe_set_clock_enable(struct vpe_dev *dev, bool on)
+{
+	u32 val = 0;
+
+	if (on)
+		val = VPE_DATA_PATH_CLK_ENABLE | VPE_VPEDMA_CLK_ENABLE;
+	write_reg(dev, VPE_CLK_ENABLE, val);
+}
+
+static void vpe_top_reset(struct vpe_dev *dev)
+{
+
+	write_field_reg(dev, VPE_CLK_RESET, 1, VPE_DATA_PATH_CLK_RESET_MASK,
+		VPE_DATA_PATH_CLK_RESET_SHIFT);
+
+	usleep_range(100, 150);
+
+	write_field_reg(dev, VPE_CLK_RESET, 0, VPE_DATA_PATH_CLK_RESET_MASK,
+		VPE_DATA_PATH_CLK_RESET_SHIFT);
+}
+
+static void vpe_top_vpdma_reset(struct vpe_dev *dev)
+{
+	write_field_reg(dev, VPE_CLK_RESET, 1, VPE_VPDMA_CLK_RESET_MASK,
+		VPE_VPDMA_CLK_RESET_SHIFT);
+
+	usleep_range(100, 150);
+
+	write_field_reg(dev, VPE_CLK_RESET, 0, VPE_VPDMA_CLK_RESET_MASK,
+		VPE_VPDMA_CLK_RESET_SHIFT);
+}
+
+/*
+ * Load the correct of upsampler coefficients into the shadow MMRs
+ */
+static void set_us_coefficients(struct vpe_ctx *ctx)
+{
+	struct vpe_mmr_adb *mmr_adb = ctx->mmr_adb.addr;
+	struct vpe_q_data *s_q_data = &ctx->q_data[Q_DATA_SRC];
+	u32 *us1_reg = &mmr_adb->us1_regs[0];
+	u32 *us2_reg = &mmr_adb->us2_regs[0];
+	u32 *us3_reg = &mmr_adb->us3_regs[0];
+	const unsigned short *cp, *end_cp;
+
+	cp = &us_coeffs[0].anchor_fid0_c0;
+
+	if (s_q_data->flags & Q_DATA_INTERLACED)	/* interlaced */
+		cp += sizeof(us_coeffs[0]) / sizeof(*cp);
+
+	end_cp = cp + sizeof(us_coeffs[0]) / sizeof(*cp);
+
+	while (cp < end_cp) {
+		write_field(us1_reg, *cp++, VPE_US_C0_MASK, VPE_US_C0_SHIFT);
+		write_field(us1_reg, *cp++, VPE_US_C1_MASK, VPE_US_C1_SHIFT);
+		*us2_reg++ = *us1_reg;
+		*us3_reg++ = *us1_reg++;
+	}
+	ctx->load_mmrs = true;
+}
+
+/*
+ * Set the upsampler config mode and the VPDMA line mode in the shadow MMRs.
+ */
+static void set_cfg_and_line_modes(struct vpe_ctx *ctx)
+{
+	struct vpe_fmt *fmt = ctx->q_data[Q_DATA_SRC].fmt;
+	struct vpe_mmr_adb *mmr_adb = ctx->mmr_adb.addr;
+	u32 *us1_reg0 = &mmr_adb->us1_regs[0];
+	u32 *us2_reg0 = &mmr_adb->us2_regs[0];
+	u32 *us3_reg0 = &mmr_adb->us3_regs[0];
+	int line_mode = 1;
+	int cfg_mode = 1;
+
+	/*
+	 * Cfg Mode 0: YUV420 source, enable upsampler, DEI is de-interlacing.
+	 * Cfg Mode 1: YUV422 source, disable upsampler, DEI is de-interlacing.
+	 */
+
+	if (fmt->fourcc == V4L2_PIX_FMT_NV12) {
+		cfg_mode = 0;
+		line_mode = 0;		/* double lines to line buffer */
+	}
+
+	write_field(us1_reg0, cfg_mode, VPE_US_MODE_MASK, VPE_US_MODE_SHIFT);
+	write_field(us2_reg0, cfg_mode, VPE_US_MODE_MASK, VPE_US_MODE_SHIFT);
+	write_field(us3_reg0, cfg_mode, VPE_US_MODE_MASK, VPE_US_MODE_SHIFT);
+
+	/* regs for now */
+	vpdma_set_line_mode(ctx->dev->vpdma, line_mode, VPE_CHAN_CHROMA1_IN);
+	vpdma_set_line_mode(ctx->dev->vpdma, line_mode, VPE_CHAN_CHROMA2_IN);
+	vpdma_set_line_mode(ctx->dev->vpdma, line_mode, VPE_CHAN_CHROMA3_IN);
+
+	/* frame start for input luma */
+	vpdma_set_frame_start_event(ctx->dev->vpdma, VPDMA_FSEVENT_CHANNEL_ACTIVE,
+		VPE_CHAN_LUMA1_IN);
+	vpdma_set_frame_start_event(ctx->dev->vpdma, VPDMA_FSEVENT_CHANNEL_ACTIVE,
+		VPE_CHAN_LUMA2_IN);
+	vpdma_set_frame_start_event(ctx->dev->vpdma, VPDMA_FSEVENT_CHANNEL_ACTIVE,
+		VPE_CHAN_LUMA3_IN);
+
+	/* frame start for input chroma */
+	vpdma_set_frame_start_event(ctx->dev->vpdma, VPDMA_FSEVENT_CHANNEL_ACTIVE,
+		VPE_CHAN_CHROMA1_IN);
+	vpdma_set_frame_start_event(ctx->dev->vpdma, VPDMA_FSEVENT_CHANNEL_ACTIVE,
+		VPE_CHAN_CHROMA2_IN);
+	vpdma_set_frame_start_event(ctx->dev->vpdma, VPDMA_FSEVENT_CHANNEL_ACTIVE,
+		VPE_CHAN_CHROMA3_IN);
+
+	/* frame start for MV in client */
+	vpdma_set_frame_start_event(ctx->dev->vpdma, VPDMA_FSEVENT_CHANNEL_ACTIVE,
+		VPE_CHAN_MV_IN);
+
+	ctx->load_mmrs = true;
+}
+
+/*
+ * Set the shadow registers that are modified when the source
+ * format changes.
+ */
+static void set_src_registers(struct vpe_ctx *ctx)
+{
+	set_us_coefficients(ctx);
+}
+
+/*
+ * Set the shadow registers that are modified when the destination
+ * format changes.
+ */
+static void set_dst_registers(struct vpe_ctx *ctx)
+{
+	struct vpe_mmr_adb *mmr_adb = ctx->mmr_adb.addr;
+	enum v4l2_colorspace clrspc = ctx->q_data[Q_DATA_DST].colorspace;
+	struct vpe_fmt *fmt = ctx->q_data[Q_DATA_DST].fmt;
+	u32 val = 0;
+
+	if (clrspc == V4L2_COLORSPACE_SRGB)
+		val |= VPE_RGB_OUT_SELECT;
+	else if (fmt->fourcc == V4L2_PIX_FMT_NV16)
+		val |= VPE_COLOR_SEPARATE_422;
+
+	/*
+	 * the source of CHR_DS and CSC is always the scaler, irrespective of
+	 * whether it's used or not
+	 */
+	val |= VPE_DS_SRC_DEI_SCALER | VPE_CSC_SRC_DEI_SCALER;
+
+	if (fmt->fourcc != V4L2_PIX_FMT_NV12)
+		val |= VPE_DS_BYPASS;
+
+	mmr_adb->out_fmt_reg[0] = val;
+
+	ctx->load_mmrs = true;
+}
+
+/*
+ * Set the de-interlacer shadow register values
+ */
+static void set_dei_regs(struct vpe_ctx *ctx)
+{
+	struct vpe_mmr_adb *mmr_adb = ctx->mmr_adb.addr;
+	struct vpe_q_data *s_q_data = &ctx->q_data[Q_DATA_SRC];
+	unsigned int src_h = s_q_data->c_rect.height;
+	unsigned int src_w = s_q_data->c_rect.width;
+	u32 *dei_mmr0 = &mmr_adb->dei_regs[0];
+	bool deinterlace = true;
+	u32 val = 0;
+
+	/*
+	 * according to TRM, we should set DEI in progressive bypass mode when
+	 * the input content is progressive, however, DEI is bypassed correctly
+	 * for both progressive and interlace content in interlace bypass mode.
+	 * It has been recommended not to use progressive bypass mode.
+	 */
+	if ((!ctx->deinterlacing && (s_q_data->flags & Q_DATA_INTERLACED)) ||
+			!(s_q_data->flags & Q_DATA_INTERLACED)) {
+		deinterlace = false;
+		val = VPE_DEI_INTERLACE_BYPASS;
+	}
+
+	src_h = deinterlace ? src_h * 2 : src_h;
+
+	val |= (src_h << VPE_DEI_HEIGHT_SHIFT) |
+		(src_w << VPE_DEI_WIDTH_SHIFT) |
+		VPE_DEI_FIELD_FLUSH;
+
+	*dei_mmr0 = val;
+
+	ctx->load_mmrs = true;
+}
+
+static void set_dei_shadow_registers(struct vpe_ctx *ctx)
+{
+	struct vpe_mmr_adb *mmr_adb = ctx->mmr_adb.addr;
+	u32 *dei_mmr = &mmr_adb->dei_regs[0];
+	const struct vpe_dei_regs *cur = &dei_regs;
+
+	dei_mmr[2]  = cur->mdt_spacial_freq_thr_reg;
+	dei_mmr[3]  = cur->edi_config_reg;
+	dei_mmr[4]  = cur->edi_lut_reg0;
+	dei_mmr[5]  = cur->edi_lut_reg1;
+	dei_mmr[6]  = cur->edi_lut_reg2;
+	dei_mmr[7]  = cur->edi_lut_reg3;
+
+	ctx->load_mmrs = true;
+}
+
+/*
+ * Set the shadow registers whose values are modified when either the
+ * source or destination format is changed.
+ */
+static int set_srcdst_params(struct vpe_ctx *ctx)
+{
+	struct vpe_q_data *s_q_data =  &ctx->q_data[Q_DATA_SRC];
+	struct vpe_q_data *d_q_data =  &ctx->q_data[Q_DATA_DST];
+	struct vpe_mmr_adb *mmr_adb = ctx->mmr_adb.addr;
+	unsigned int src_w = s_q_data->c_rect.width;
+	unsigned int src_h = s_q_data->c_rect.height;
+	unsigned int dst_w = d_q_data->c_rect.width;
+	unsigned int dst_h = d_q_data->c_rect.height;
+	size_t mv_buf_size;
+	int ret;
+
+	ctx->sequence = 0;
+	ctx->field = V4L2_FIELD_TOP;
+
+	if ((s_q_data->flags & Q_DATA_INTERLACED) &&
+			!(d_q_data->flags & Q_DATA_INTERLACED)) {
+		int bytes_per_line;
+		const struct vpdma_data_format *mv =
+			&vpdma_misc_fmts[VPDMA_DATA_FMT_MV];
+
+		/*
+		 * we make sure that the source image has a 16 byte aligned
+		 * stride, we need to do the same for the motion vector buffer
+		 * by aligning it's stride to the next 16 byte boundry. this
+		 * extra space will not be used by the de-interlacer, but will
+		 * ensure that vpdma operates correctly
+		 */
+		bytes_per_line = ALIGN((s_q_data->width * mv->depth) >> 3,
+					VPDMA_STRIDE_ALIGN);
+		mv_buf_size = bytes_per_line * s_q_data->height;
+
+		ctx->deinterlacing = true;
+		src_h <<= 1;
+	} else {
+		ctx->deinterlacing = false;
+		mv_buf_size = 0;
+	}
+
+	free_vbs(ctx);
+
+	ret = realloc_mv_buffers(ctx, mv_buf_size);
+	if (ret)
+		return ret;
+
+	set_cfg_and_line_modes(ctx);
+	set_dei_regs(ctx);
+
+	csc_set_coeff(ctx->dev->csc, &mmr_adb->csc_regs[0],
+		s_q_data->colorspace, d_q_data->colorspace);
+
+	sc_set_hs_coeffs(ctx->dev->sc, ctx->sc_coeff_h.addr, src_w, dst_w);
+	sc_set_vs_coeffs(ctx->dev->sc, ctx->sc_coeff_v.addr, src_h, dst_h);
+
+	sc_config_scaler(ctx->dev->sc, &mmr_adb->sc_regs0[0],
+		&mmr_adb->sc_regs8[0], &mmr_adb->sc_regs17[0],
+		src_w, src_h, dst_w, dst_h);
+
+	return 0;
+}
+
+/*
+ * Return the vpe_ctx structure for a given struct file
+ */
+static struct vpe_ctx *file2ctx(struct file *file)
+{
+	return container_of(file->private_data, struct vpe_ctx, fh);
+}
+
+/*
+ * mem2mem callbacks
+ */
+
+/**
+ * job_ready() - check whether an instance is ready to be scheduled to run
+ */
+static int job_ready(void *priv)
+{
+	struct vpe_ctx *ctx = priv;
+	int needed = ctx->bufs_per_job;
+
+	if (ctx->deinterlacing && ctx->src_vbs[2] == NULL)
+		needed += 2;	/* need additional two most recent fields */
+
+	if (v4l2_m2m_num_src_bufs_ready(ctx->fh.m2m_ctx) < needed)
+		return 0;
+
+	if (v4l2_m2m_num_dst_bufs_ready(ctx->fh.m2m_ctx) < needed)
+		return 0;
+
+	return 1;
+}
+
+static void job_abort(void *priv)
+{
+	struct vpe_ctx *ctx = priv;
+
+	/* Will cancel the transaction in the next interrupt handler */
+	ctx->aborting = 1;
+}
+
+/*
+ * Lock access to the device
+ */
+static void vpe_lock(void *priv)
+{
+	struct vpe_ctx *ctx = priv;
+	struct vpe_dev *dev = ctx->dev;
+	mutex_lock(&dev->dev_mutex);
+}
+
+static void vpe_unlock(void *priv)
+{
+	struct vpe_ctx *ctx = priv;
+	struct vpe_dev *dev = ctx->dev;
+	mutex_unlock(&dev->dev_mutex);
+}
+
+static void vpe_dump_regs(struct vpe_dev *dev)
+{
+#define DUMPREG(r) vpe_dbg(dev, "%-35s %08x\n", #r, read_reg(dev, VPE_##r))
+
+	vpe_dbg(dev, "VPE Registers:\n");
+
+	DUMPREG(PID);
+	DUMPREG(SYSCONFIG);
+	DUMPREG(INT0_STATUS0_RAW);
+	DUMPREG(INT0_STATUS0);
+	DUMPREG(INT0_ENABLE0);
+	DUMPREG(INT0_STATUS1_RAW);
+	DUMPREG(INT0_STATUS1);
+	DUMPREG(INT0_ENABLE1);
+	DUMPREG(CLK_ENABLE);
+	DUMPREG(CLK_RESET);
+	DUMPREG(CLK_FORMAT_SELECT);
+	DUMPREG(CLK_RANGE_MAP);
+	DUMPREG(US1_R0);
+	DUMPREG(US1_R1);
+	DUMPREG(US1_R2);
+	DUMPREG(US1_R3);
+	DUMPREG(US1_R4);
+	DUMPREG(US1_R5);
+	DUMPREG(US1_R6);
+	DUMPREG(US1_R7);
+	DUMPREG(US2_R0);
+	DUMPREG(US2_R1);
+	DUMPREG(US2_R2);
+	DUMPREG(US2_R3);
+	DUMPREG(US2_R4);
+	DUMPREG(US2_R5);
+	DUMPREG(US2_R6);
+	DUMPREG(US2_R7);
+	DUMPREG(US3_R0);
+	DUMPREG(US3_R1);
+	DUMPREG(US3_R2);
+	DUMPREG(US3_R3);
+	DUMPREG(US3_R4);
+	DUMPREG(US3_R5);
+	DUMPREG(US3_R6);
+	DUMPREG(US3_R7);
+	DUMPREG(DEI_FRAME_SIZE);
+	DUMPREG(MDT_BYPASS);
+	DUMPREG(MDT_SF_THRESHOLD);
+	DUMPREG(EDI_CONFIG);
+	DUMPREG(DEI_EDI_LUT_R0);
+	DUMPREG(DEI_EDI_LUT_R1);
+	DUMPREG(DEI_EDI_LUT_R2);
+	DUMPREG(DEI_EDI_LUT_R3);
+	DUMPREG(DEI_FMD_WINDOW_R0);
+	DUMPREG(DEI_FMD_WINDOW_R1);
+	DUMPREG(DEI_FMD_CONTROL_R0);
+	DUMPREG(DEI_FMD_CONTROL_R1);
+	DUMPREG(DEI_FMD_STATUS_R0);
+	DUMPREG(DEI_FMD_STATUS_R1);
+	DUMPREG(DEI_FMD_STATUS_R2);
+#undef DUMPREG
+
+	sc_dump_regs(dev->sc);
+	csc_dump_regs(dev->csc);
+}
+
+static void add_out_dtd(struct vpe_ctx *ctx, int port)
+{
+	struct vpe_q_data *q_data = &ctx->q_data[Q_DATA_DST];
+	const struct vpe_port_data *p_data = &port_data[port];
+	struct vb2_buffer *vb = &ctx->dst_vb->vb2_buf;
+	struct vpe_fmt *fmt = q_data->fmt;
+	const struct vpdma_data_format *vpdma_fmt;
+	int mv_buf_selector = !ctx->src_mv_buf_selector;
+	dma_addr_t dma_addr;
+	u32 flags = 0;
+
+	if (port == VPE_PORT_MV_OUT) {
+		vpdma_fmt = &vpdma_misc_fmts[VPDMA_DATA_FMT_MV];
+		dma_addr = ctx->mv_buf_dma[mv_buf_selector];
+	} else {
+		/* to incorporate interleaved formats */
+		int plane = fmt->coplanar ? p_data->vb_part : 0;
+
+		vpdma_fmt = fmt->vpdma_fmt[plane];
+		dma_addr = vb2_dma_contig_plane_dma_addr(vb, plane);
+		if (!dma_addr) {
+			vpe_err(ctx->dev,
+				"acquiring output buffer(%d) dma_addr failed\n",
+				port);
+			return;
+		}
+	}
+
+	if (q_data->flags & Q_DATA_FRAME_1D)
+		flags |= VPDMA_DATA_FRAME_1D;
+	if (q_data->flags & Q_DATA_MODE_TILED)
+		flags |= VPDMA_DATA_MODE_TILED;
+
+	vpdma_add_out_dtd(&ctx->desc_list, q_data->width, &q_data->c_rect,
+		vpdma_fmt, dma_addr, p_data->channel, flags);
+}
+
+static void add_in_dtd(struct vpe_ctx *ctx, int port)
+{
+	struct vpe_q_data *q_data = &ctx->q_data[Q_DATA_SRC];
+	const struct vpe_port_data *p_data = &port_data[port];
+	struct vb2_buffer *vb = &ctx->src_vbs[p_data->vb_index]->vb2_buf;
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct vpe_fmt *fmt = q_data->fmt;
+	const struct vpdma_data_format *vpdma_fmt;
+	int mv_buf_selector = ctx->src_mv_buf_selector;
+	int field = vbuf->field == V4L2_FIELD_BOTTOM;
+	int frame_width, frame_height;
+	dma_addr_t dma_addr;
+	u32 flags = 0;
+
+	if (port == VPE_PORT_MV_IN) {
+		vpdma_fmt = &vpdma_misc_fmts[VPDMA_DATA_FMT_MV];
+		dma_addr = ctx->mv_buf_dma[mv_buf_selector];
+	} else {
+		/* to incorporate interleaved formats */
+		int plane = fmt->coplanar ? p_data->vb_part : 0;
+
+		vpdma_fmt = fmt->vpdma_fmt[plane];
+
+		dma_addr = vb2_dma_contig_plane_dma_addr(vb, plane);
+		if (!dma_addr) {
+			vpe_err(ctx->dev,
+				"acquiring input buffer(%d) dma_addr failed\n",
+				port);
+			return;
+		}
+	}
+
+	if (q_data->flags & Q_DATA_FRAME_1D)
+		flags |= VPDMA_DATA_FRAME_1D;
+	if (q_data->flags & Q_DATA_MODE_TILED)
+		flags |= VPDMA_DATA_MODE_TILED;
+
+	frame_width = q_data->c_rect.width;
+	frame_height = q_data->c_rect.height;
+
+	if (p_data->vb_part && fmt->fourcc == V4L2_PIX_FMT_NV12)
+		frame_height /= 2;
+
+	vpdma_add_in_dtd(&ctx->desc_list, q_data->width, &q_data->c_rect,
+		vpdma_fmt, dma_addr, p_data->channel, field, flags, frame_width,
+		frame_height, 0, 0);
+}
+
+/*
+ * Enable the expected IRQ sources
+ */
+static void enable_irqs(struct vpe_ctx *ctx)
+{
+	write_reg(ctx->dev, VPE_INT0_ENABLE0_SET, VPE_INT0_LIST0_COMPLETE);
+	write_reg(ctx->dev, VPE_INT0_ENABLE1_SET, VPE_DEI_ERROR_INT |
+				VPE_DS1_UV_ERROR_INT);
+
+	vpdma_enable_list_complete_irq(ctx->dev->vpdma, 0, true);
+}
+
+static void disable_irqs(struct vpe_ctx *ctx)
+{
+	write_reg(ctx->dev, VPE_INT0_ENABLE0_CLR, 0xffffffff);
+	write_reg(ctx->dev, VPE_INT0_ENABLE1_CLR, 0xffffffff);
+
+	vpdma_enable_list_complete_irq(ctx->dev->vpdma, 0, false);
+}
+
+/* device_run() - prepares and starts the device
+ *
+ * This function is only called when both the source and destination
+ * buffers are in place.
+ */
+static void device_run(void *priv)
+{
+	struct vpe_ctx *ctx = priv;
+	struct sc_data *sc = ctx->dev->sc;
+	struct vpe_q_data *d_q_data = &ctx->q_data[Q_DATA_DST];
+
+	if (ctx->deinterlacing && ctx->src_vbs[2] == NULL) {
+		ctx->src_vbs[2] = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+		WARN_ON(ctx->src_vbs[2] == NULL);
+		ctx->src_vbs[1] = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+		WARN_ON(ctx->src_vbs[1] == NULL);
+	}
+
+	ctx->src_vbs[0] = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+	WARN_ON(ctx->src_vbs[0] == NULL);
+	ctx->dst_vb = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
+	WARN_ON(ctx->dst_vb == NULL);
+
+	/* config descriptors */
+	if (ctx->dev->loaded_mmrs != ctx->mmr_adb.dma_addr || ctx->load_mmrs) {
+		vpdma_map_desc_buf(ctx->dev->vpdma, &ctx->mmr_adb);
+		vpdma_add_cfd_adb(&ctx->desc_list, CFD_MMR_CLIENT, &ctx->mmr_adb);
+		ctx->dev->loaded_mmrs = ctx->mmr_adb.dma_addr;
+		ctx->load_mmrs = false;
+	}
+
+	if (sc->loaded_coeff_h != ctx->sc_coeff_h.dma_addr ||
+			sc->load_coeff_h) {
+		vpdma_map_desc_buf(ctx->dev->vpdma, &ctx->sc_coeff_h);
+		vpdma_add_cfd_block(&ctx->desc_list, CFD_SC_CLIENT,
+			&ctx->sc_coeff_h, 0);
+
+		sc->loaded_coeff_h = ctx->sc_coeff_h.dma_addr;
+		sc->load_coeff_h = false;
+	}
+
+	if (sc->loaded_coeff_v != ctx->sc_coeff_v.dma_addr ||
+			sc->load_coeff_v) {
+		vpdma_map_desc_buf(ctx->dev->vpdma, &ctx->sc_coeff_v);
+		vpdma_add_cfd_block(&ctx->desc_list, CFD_SC_CLIENT,
+			&ctx->sc_coeff_v, SC_COEF_SRAM_SIZE >> 4);
+
+		sc->loaded_coeff_v = ctx->sc_coeff_v.dma_addr;
+		sc->load_coeff_v = false;
+	}
+
+	/* output data descriptors */
+	if (ctx->deinterlacing)
+		add_out_dtd(ctx, VPE_PORT_MV_OUT);
+
+	if (d_q_data->colorspace == V4L2_COLORSPACE_SRGB) {
+		add_out_dtd(ctx, VPE_PORT_RGB_OUT);
+	} else {
+		add_out_dtd(ctx, VPE_PORT_LUMA_OUT);
+		if (d_q_data->fmt->coplanar)
+			add_out_dtd(ctx, VPE_PORT_CHROMA_OUT);
+	}
+
+	/* input data descriptors */
+	if (ctx->deinterlacing) {
+		add_in_dtd(ctx, VPE_PORT_LUMA3_IN);
+		add_in_dtd(ctx, VPE_PORT_CHROMA3_IN);
+
+		add_in_dtd(ctx, VPE_PORT_LUMA2_IN);
+		add_in_dtd(ctx, VPE_PORT_CHROMA2_IN);
+	}
+
+	add_in_dtd(ctx, VPE_PORT_LUMA1_IN);
+	add_in_dtd(ctx, VPE_PORT_CHROMA1_IN);
+
+	if (ctx->deinterlacing)
+		add_in_dtd(ctx, VPE_PORT_MV_IN);
+
+	/* sync on channel control descriptors for input ports */
+	vpdma_add_sync_on_channel_ctd(&ctx->desc_list, VPE_CHAN_LUMA1_IN);
+	vpdma_add_sync_on_channel_ctd(&ctx->desc_list, VPE_CHAN_CHROMA1_IN);
+
+	if (ctx->deinterlacing) {
+		vpdma_add_sync_on_channel_ctd(&ctx->desc_list,
+			VPE_CHAN_LUMA2_IN);
+		vpdma_add_sync_on_channel_ctd(&ctx->desc_list,
+			VPE_CHAN_CHROMA2_IN);
+
+		vpdma_add_sync_on_channel_ctd(&ctx->desc_list,
+			VPE_CHAN_LUMA3_IN);
+		vpdma_add_sync_on_channel_ctd(&ctx->desc_list,
+			VPE_CHAN_CHROMA3_IN);
+
+		vpdma_add_sync_on_channel_ctd(&ctx->desc_list, VPE_CHAN_MV_IN);
+	}
+
+	/* sync on channel control descriptors for output ports */
+	if (d_q_data->colorspace == V4L2_COLORSPACE_SRGB) {
+		vpdma_add_sync_on_channel_ctd(&ctx->desc_list,
+			VPE_CHAN_RGB_OUT);
+	} else {
+		vpdma_add_sync_on_channel_ctd(&ctx->desc_list,
+			VPE_CHAN_LUMA_OUT);
+		if (d_q_data->fmt->coplanar)
+			vpdma_add_sync_on_channel_ctd(&ctx->desc_list,
+				VPE_CHAN_CHROMA_OUT);
+	}
+
+	if (ctx->deinterlacing)
+		vpdma_add_sync_on_channel_ctd(&ctx->desc_list, VPE_CHAN_MV_OUT);
+
+	enable_irqs(ctx);
+
+	vpdma_map_desc_buf(ctx->dev->vpdma, &ctx->desc_list.buf);
+	vpdma_submit_descs(ctx->dev->vpdma, &ctx->desc_list);
+}
+
+static void dei_error(struct vpe_ctx *ctx)
+{
+	dev_warn(ctx->dev->v4l2_dev.dev,
+		"received DEI error interrupt\n");
+}
+
+static void ds1_uv_error(struct vpe_ctx *ctx)
+{
+	dev_warn(ctx->dev->v4l2_dev.dev,
+		"received downsampler error interrupt\n");
+}
+
+static irqreturn_t vpe_irq(int irq_vpe, void *data)
+{
+	struct vpe_dev *dev = (struct vpe_dev *)data;
+	struct vpe_ctx *ctx;
+	struct vpe_q_data *d_q_data;
+	struct vb2_v4l2_buffer *s_vb, *d_vb;
+	unsigned long flags;
+	u32 irqst0, irqst1;
+
+	irqst0 = read_reg(dev, VPE_INT0_STATUS0);
+	if (irqst0) {
+		write_reg(dev, VPE_INT0_STATUS0_CLR, irqst0);
+		vpe_dbg(dev, "INT0_STATUS0 = 0x%08x\n", irqst0);
+	}
+
+	irqst1 = read_reg(dev, VPE_INT0_STATUS1);
+	if (irqst1) {
+		write_reg(dev, VPE_INT0_STATUS1_CLR, irqst1);
+		vpe_dbg(dev, "INT0_STATUS1 = 0x%08x\n", irqst1);
+	}
+
+	ctx = v4l2_m2m_get_curr_priv(dev->m2m_dev);
+	if (!ctx) {
+		vpe_err(dev, "instance released before end of transaction\n");
+		goto handled;
+	}
+
+	if (irqst1) {
+		if (irqst1 & VPE_DEI_ERROR_INT) {
+			irqst1 &= ~VPE_DEI_ERROR_INT;
+			dei_error(ctx);
+		}
+		if (irqst1 & VPE_DS1_UV_ERROR_INT) {
+			irqst1 &= ~VPE_DS1_UV_ERROR_INT;
+			ds1_uv_error(ctx);
+		}
+	}
+
+	if (irqst0) {
+		if (irqst0 & VPE_INT0_LIST0_COMPLETE)
+			vpdma_clear_list_stat(ctx->dev->vpdma);
+
+		irqst0 &= ~(VPE_INT0_LIST0_COMPLETE);
+	}
+
+	if (irqst0 | irqst1) {
+		dev_warn(dev->v4l2_dev.dev, "Unexpected interrupt: "
+			"INT0_STATUS0 = 0x%08x, INT0_STATUS1 = 0x%08x\n",
+			irqst0, irqst1);
+	}
+
+	disable_irqs(ctx);
+
+	vpdma_unmap_desc_buf(dev->vpdma, &ctx->desc_list.buf);
+	vpdma_unmap_desc_buf(dev->vpdma, &ctx->mmr_adb);
+	vpdma_unmap_desc_buf(dev->vpdma, &ctx->sc_coeff_h);
+	vpdma_unmap_desc_buf(dev->vpdma, &ctx->sc_coeff_v);
+
+	vpdma_reset_desc_list(&ctx->desc_list);
+
+	 /* the previous dst mv buffer becomes the next src mv buffer */
+	ctx->src_mv_buf_selector = !ctx->src_mv_buf_selector;
+
+	if (ctx->aborting)
+		goto finished;
+
+	s_vb = ctx->src_vbs[0];
+	d_vb = ctx->dst_vb;
+
+	d_vb->flags = s_vb->flags;
+	d_vb->timestamp = s_vb->timestamp;
+
+	if (s_vb->flags & V4L2_BUF_FLAG_TIMECODE)
+		d_vb->timecode = s_vb->timecode;
+
+	d_vb->sequence = ctx->sequence;
+
+	d_q_data = &ctx->q_data[Q_DATA_DST];
+	if (d_q_data->flags & Q_DATA_INTERLACED) {
+		d_vb->field = ctx->field;
+		if (ctx->field == V4L2_FIELD_BOTTOM) {
+			ctx->sequence++;
+			ctx->field = V4L2_FIELD_TOP;
+		} else {
+			WARN_ON(ctx->field != V4L2_FIELD_TOP);
+			ctx->field = V4L2_FIELD_BOTTOM;
+		}
+	} else {
+		d_vb->field = V4L2_FIELD_NONE;
+		ctx->sequence++;
+	}
+
+	if (ctx->deinterlacing)
+		s_vb = ctx->src_vbs[2];
+
+	spin_lock_irqsave(&dev->lock, flags);
+	v4l2_m2m_buf_done(s_vb, VB2_BUF_STATE_DONE);
+	v4l2_m2m_buf_done(d_vb, VB2_BUF_STATE_DONE);
+	spin_unlock_irqrestore(&dev->lock, flags);
+
+	if (ctx->deinterlacing) {
+		ctx->src_vbs[2] = ctx->src_vbs[1];
+		ctx->src_vbs[1] = ctx->src_vbs[0];
+	}
+
+	ctx->bufs_completed++;
+	if (ctx->bufs_completed < ctx->bufs_per_job) {
+		device_run(ctx);
+		goto handled;
+	}
+
+finished:
+	vpe_dbg(ctx->dev, "finishing transaction\n");
+	ctx->bufs_completed = 0;
+	v4l2_m2m_job_finish(dev->m2m_dev, ctx->fh.m2m_ctx);
+handled:
+	return IRQ_HANDLED;
+}
+
+/*
+ * video ioctls
+ */
+static int vpe_querycap(struct file *file, void *priv,
+			struct v4l2_capability *cap)
+{
+	strncpy(cap->driver, VPE_MODULE_NAME, sizeof(cap->driver) - 1);
+	strncpy(cap->card, VPE_MODULE_NAME, sizeof(cap->card) - 1);
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
+		VPE_MODULE_NAME);
+	cap->device_caps  = V4L2_CAP_VIDEO_M2M_MPLANE | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	return 0;
+}
+
+static int __enum_fmt(struct v4l2_fmtdesc *f, u32 type)
+{
+	int i, index;
+	struct vpe_fmt *fmt = NULL;
+
+	index = 0;
+	for (i = 0; i < ARRAY_SIZE(vpe_formats); ++i) {
+		if (vpe_formats[i].types & type) {
+			if (index == f->index) {
+				fmt = &vpe_formats[i];
+				break;
+			}
+			index++;
+		}
+	}
+
+	if (!fmt)
+		return -EINVAL;
+
+	strncpy(f->description, fmt->name, sizeof(f->description) - 1);
+	f->pixelformat = fmt->fourcc;
+	return 0;
+}
+
+static int vpe_enum_fmt(struct file *file, void *priv,
+				struct v4l2_fmtdesc *f)
+{
+	if (V4L2_TYPE_IS_OUTPUT(f->type))
+		return __enum_fmt(f, VPE_FMT_TYPE_OUTPUT);
+
+	return __enum_fmt(f, VPE_FMT_TYPE_CAPTURE);
+}
+
+static int vpe_g_fmt(struct file *file, void *priv, struct v4l2_format *f)
+{
+	struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp;
+	struct vpe_ctx *ctx = file2ctx(file);
+	struct vb2_queue *vq;
+	struct vpe_q_data *q_data;
+	int i;
+
+	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
+	if (!vq)
+		return -EINVAL;
+
+	q_data = get_q_data(ctx, f->type);
+
+	pix->width = q_data->width;
+	pix->height = q_data->height;
+	pix->pixelformat = q_data->fmt->fourcc;
+	pix->field = q_data->field;
+
+	if (V4L2_TYPE_IS_OUTPUT(f->type)) {
+		pix->colorspace = q_data->colorspace;
+	} else {
+		struct vpe_q_data *s_q_data;
+
+		/* get colorspace from the source queue */
+		s_q_data = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+
+		pix->colorspace = s_q_data->colorspace;
+	}
+
+	pix->num_planes = q_data->fmt->coplanar ? 2 : 1;
+
+	for (i = 0; i < pix->num_planes; i++) {
+		pix->plane_fmt[i].bytesperline = q_data->bytesperline[i];
+		pix->plane_fmt[i].sizeimage = q_data->sizeimage[i];
+	}
+
+	return 0;
+}
+
+static int __vpe_try_fmt(struct vpe_ctx *ctx, struct v4l2_format *f,
+		       struct vpe_fmt *fmt, int type)
+{
+	struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp;
+	struct v4l2_plane_pix_format *plane_fmt;
+	unsigned int w_align;
+	int i, depth, depth_bytes;
+
+	if (!fmt || !(fmt->types & type)) {
+		vpe_err(ctx->dev, "Fourcc format (0x%08x) invalid.\n",
+			pix->pixelformat);
+		return -EINVAL;
+	}
+
+	if (pix->field != V4L2_FIELD_NONE && pix->field != V4L2_FIELD_ALTERNATE)
+		pix->field = V4L2_FIELD_NONE;
+
+	depth = fmt->vpdma_fmt[VPE_LUMA]->depth;
+
+	/*
+	 * the line stride should 16 byte aligned for VPDMA to work, based on
+	 * the bytes per pixel, figure out how much the width should be aligned
+	 * to make sure line stride is 16 byte aligned
+	 */
+	depth_bytes = depth >> 3;
+
+	if (depth_bytes == 3)
+		/*
+		 * if bpp is 3(as in some RGB formats), the pixel width doesn't
+		 * really help in ensuring line stride is 16 byte aligned
+		 */
+		w_align = 4;
+	else
+		/*
+		 * for the remainder bpp(4, 2 and 1), the pixel width alignment
+		 * can ensure a line stride alignment of 16 bytes. For example,
+		 * if bpp is 2, then the line stride can be 16 byte aligned if
+		 * the width is 8 byte aligned
+		 */
+		w_align = order_base_2(VPDMA_DESC_ALIGN / depth_bytes);
+
+	v4l_bound_align_image(&pix->width, MIN_W, MAX_W, w_align,
+			      &pix->height, MIN_H, MAX_H, H_ALIGN,
+			      S_ALIGN);
+
+	pix->num_planes = fmt->coplanar ? 2 : 1;
+	pix->pixelformat = fmt->fourcc;
+
+	if (!pix->colorspace) {
+		if (fmt->fourcc == V4L2_PIX_FMT_RGB24 ||
+				fmt->fourcc == V4L2_PIX_FMT_BGR24 ||
+				fmt->fourcc == V4L2_PIX_FMT_RGB32 ||
+				fmt->fourcc == V4L2_PIX_FMT_BGR32) {
+			pix->colorspace = V4L2_COLORSPACE_SRGB;
+		} else {
+			if (pix->height > 1280)	/* HD */
+				pix->colorspace = V4L2_COLORSPACE_REC709;
+			else			/* SD */
+				pix->colorspace = V4L2_COLORSPACE_SMPTE170M;
+		}
+	}
+
+	memset(pix->reserved, 0, sizeof(pix->reserved));
+	for (i = 0; i < pix->num_planes; i++) {
+		plane_fmt = &pix->plane_fmt[i];
+		depth = fmt->vpdma_fmt[i]->depth;
+
+		if (i == VPE_LUMA)
+			plane_fmt->bytesperline = (pix->width * depth) >> 3;
+		else
+			plane_fmt->bytesperline = pix->width;
+
+		plane_fmt->sizeimage =
+				(pix->height * pix->width * depth) >> 3;
+
+		memset(plane_fmt->reserved, 0, sizeof(plane_fmt->reserved));
+	}
+
+	return 0;
+}
+
+static int vpe_try_fmt(struct file *file, void *priv, struct v4l2_format *f)
+{
+	struct vpe_ctx *ctx = file2ctx(file);
+	struct vpe_fmt *fmt = find_format(f);
+
+	if (V4L2_TYPE_IS_OUTPUT(f->type))
+		return __vpe_try_fmt(ctx, f, fmt, VPE_FMT_TYPE_OUTPUT);
+	else
+		return __vpe_try_fmt(ctx, f, fmt, VPE_FMT_TYPE_CAPTURE);
+}
+
+static int __vpe_s_fmt(struct vpe_ctx *ctx, struct v4l2_format *f)
+{
+	struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp;
+	struct v4l2_plane_pix_format *plane_fmt;
+	struct vpe_q_data *q_data;
+	struct vb2_queue *vq;
+	int i;
+
+	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
+	if (!vq)
+		return -EINVAL;
+
+	if (vb2_is_busy(vq)) {
+		vpe_err(ctx->dev, "queue busy\n");
+		return -EBUSY;
+	}
+
+	q_data = get_q_data(ctx, f->type);
+	if (!q_data)
+		return -EINVAL;
+
+	q_data->fmt		= find_format(f);
+	q_data->width		= pix->width;
+	q_data->height		= pix->height;
+	q_data->colorspace	= pix->colorspace;
+	q_data->field		= pix->field;
+
+	for (i = 0; i < pix->num_planes; i++) {
+		plane_fmt = &pix->plane_fmt[i];
+
+		q_data->bytesperline[i]	= plane_fmt->bytesperline;
+		q_data->sizeimage[i]	= plane_fmt->sizeimage;
+	}
+
+	q_data->c_rect.left	= 0;
+	q_data->c_rect.top	= 0;
+	q_data->c_rect.width	= q_data->width;
+	q_data->c_rect.height	= q_data->height;
+
+	if (q_data->field == V4L2_FIELD_ALTERNATE)
+		q_data->flags |= Q_DATA_INTERLACED;
+	else
+		q_data->flags &= ~Q_DATA_INTERLACED;
+
+	vpe_dbg(ctx->dev, "Setting format for type %d, wxh: %dx%d, fmt: %d bpl_y %d",
+		f->type, q_data->width, q_data->height, q_data->fmt->fourcc,
+		q_data->bytesperline[VPE_LUMA]);
+	if (q_data->fmt->coplanar)
+		vpe_dbg(ctx->dev, " bpl_uv %d\n",
+			q_data->bytesperline[VPE_CHROMA]);
+
+	return 0;
+}
+
+static int vpe_s_fmt(struct file *file, void *priv, struct v4l2_format *f)
+{
+	int ret;
+	struct vpe_ctx *ctx = file2ctx(file);
+
+	ret = vpe_try_fmt(file, priv, f);
+	if (ret)
+		return ret;
+
+	ret = __vpe_s_fmt(ctx, f);
+	if (ret)
+		return ret;
+
+	if (V4L2_TYPE_IS_OUTPUT(f->type))
+		set_src_registers(ctx);
+	else
+		set_dst_registers(ctx);
+
+	return set_srcdst_params(ctx);
+}
+
+static int __vpe_try_selection(struct vpe_ctx *ctx, struct v4l2_selection *s)
+{
+	struct vpe_q_data *q_data;
+
+	if ((s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) &&
+	    (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT))
+		return -EINVAL;
+
+	q_data = get_q_data(ctx, s->type);
+	if (!q_data)
+		return -EINVAL;
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_COMPOSE:
+		/*
+		 * COMPOSE target is only valid for capture buffer type, return
+		 * error for output buffer type
+		 */
+		if (s->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
+			return -EINVAL;
+		break;
+	case V4L2_SEL_TGT_CROP:
+		/*
+		 * CROP target is only valid for output buffer type, return
+		 * error for capture buffer type
+		 */
+		if (s->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+			return -EINVAL;
+		break;
+	/*
+	 * bound and default crop/compose targets are invalid targets to
+	 * try/set
+	 */
+	default:
+		return -EINVAL;
+	}
+
+	if (s->r.top < 0 || s->r.left < 0) {
+		vpe_err(ctx->dev, "negative values for top and left\n");
+		s->r.top = s->r.left = 0;
+	}
+
+	v4l_bound_align_image(&s->r.width, MIN_W, q_data->width, 1,
+		&s->r.height, MIN_H, q_data->height, H_ALIGN, S_ALIGN);
+
+	/* adjust left/top if cropping rectangle is out of bounds */
+	if (s->r.left + s->r.width > q_data->width)
+		s->r.left = q_data->width - s->r.width;
+	if (s->r.top + s->r.height > q_data->height)
+		s->r.top = q_data->height - s->r.height;
+
+	return 0;
+}
+
+static int vpe_g_selection(struct file *file, void *fh,
+		struct v4l2_selection *s)
+{
+	struct vpe_ctx *ctx = file2ctx(file);
+	struct vpe_q_data *q_data;
+	bool use_c_rect = false;
+
+	if ((s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) &&
+	    (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT))
+		return -EINVAL;
+
+	q_data = get_q_data(ctx, s->type);
+	if (!q_data)
+		return -EINVAL;
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+		if (s->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
+			return -EINVAL;
+		break;
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		if (s->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+			return -EINVAL;
+		break;
+	case V4L2_SEL_TGT_COMPOSE:
+		if (s->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
+			return -EINVAL;
+		use_c_rect = true;
+		break;
+	case V4L2_SEL_TGT_CROP:
+		if (s->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+			return -EINVAL;
+		use_c_rect = true;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (use_c_rect) {
+		/*
+		 * for CROP/COMPOSE target type, return c_rect params from the
+		 * respective buffer type
+		 */
+		s->r = q_data->c_rect;
+	} else {
+		/*
+		 * for DEFAULT/BOUNDS target type, return width and height from
+		 * S_FMT of the respective buffer type
+		 */
+		s->r.left = 0;
+		s->r.top = 0;
+		s->r.width = q_data->width;
+		s->r.height = q_data->height;
+	}
+
+	return 0;
+}
+
+
+static int vpe_s_selection(struct file *file, void *fh,
+		struct v4l2_selection *s)
+{
+	struct vpe_ctx *ctx = file2ctx(file);
+	struct vpe_q_data *q_data;
+	struct v4l2_selection sel = *s;
+	int ret;
+
+	ret = __vpe_try_selection(ctx, &sel);
+	if (ret)
+		return ret;
+
+	q_data = get_q_data(ctx, sel.type);
+	if (!q_data)
+		return -EINVAL;
+
+	if ((q_data->c_rect.left == sel.r.left) &&
+			(q_data->c_rect.top == sel.r.top) &&
+			(q_data->c_rect.width == sel.r.width) &&
+			(q_data->c_rect.height == sel.r.height)) {
+		vpe_dbg(ctx->dev,
+			"requested crop/compose values are already set\n");
+		return 0;
+	}
+
+	q_data->c_rect = sel.r;
+
+	return set_srcdst_params(ctx);
+}
+
+/*
+ * defines number of buffers/frames a context can process with VPE before
+ * switching to a different context. default value is 1 buffer per context
+ */
+#define V4L2_CID_VPE_BUFS_PER_JOB		(V4L2_CID_USER_TI_VPE_BASE + 0)
+
+static int vpe_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vpe_ctx *ctx =
+		container_of(ctrl->handler, struct vpe_ctx, hdl);
+
+	switch (ctrl->id) {
+	case V4L2_CID_VPE_BUFS_PER_JOB:
+		ctx->bufs_per_job = ctrl->val;
+		break;
+
+	default:
+		vpe_err(ctx->dev, "Invalid control\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops vpe_ctrl_ops = {
+	.s_ctrl = vpe_s_ctrl,
+};
+
+static const struct v4l2_ioctl_ops vpe_ioctl_ops = {
+	.vidioc_querycap		= vpe_querycap,
+
+	.vidioc_enum_fmt_vid_cap_mplane	= vpe_enum_fmt,
+	.vidioc_g_fmt_vid_cap_mplane	= vpe_g_fmt,
+	.vidioc_try_fmt_vid_cap_mplane	= vpe_try_fmt,
+	.vidioc_s_fmt_vid_cap_mplane	= vpe_s_fmt,
+
+	.vidioc_enum_fmt_vid_out_mplane	= vpe_enum_fmt,
+	.vidioc_g_fmt_vid_out_mplane	= vpe_g_fmt,
+	.vidioc_try_fmt_vid_out_mplane	= vpe_try_fmt,
+	.vidioc_s_fmt_vid_out_mplane	= vpe_s_fmt,
+
+	.vidioc_g_selection		= vpe_g_selection,
+	.vidioc_s_selection		= vpe_s_selection,
+
+	.vidioc_reqbufs			= v4l2_m2m_ioctl_reqbufs,
+	.vidioc_querybuf		= v4l2_m2m_ioctl_querybuf,
+	.vidioc_qbuf			= v4l2_m2m_ioctl_qbuf,
+	.vidioc_dqbuf			= v4l2_m2m_ioctl_dqbuf,
+	.vidioc_streamon		= v4l2_m2m_ioctl_streamon,
+	.vidioc_streamoff		= v4l2_m2m_ioctl_streamoff,
+
+	.vidioc_subscribe_event		= v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
+};
+
+/*
+ * Queue operations
+ */
+static int vpe_queue_setup(struct vb2_queue *vq,
+			   const void *parg,
+			   unsigned int *nbuffers, unsigned int *nplanes,
+			   unsigned int sizes[], void *alloc_ctxs[])
+{
+	int i;
+	struct vpe_ctx *ctx = vb2_get_drv_priv(vq);
+	struct vpe_q_data *q_data;
+
+	q_data = get_q_data(ctx, vq->type);
+
+	*nplanes = q_data->fmt->coplanar ? 2 : 1;
+
+	for (i = 0; i < *nplanes; i++) {
+		sizes[i] = q_data->sizeimage[i];
+		alloc_ctxs[i] = ctx->dev->alloc_ctx;
+	}
+
+	vpe_dbg(ctx->dev, "get %d buffer(s) of size %d", *nbuffers,
+		sizes[VPE_LUMA]);
+	if (q_data->fmt->coplanar)
+		vpe_dbg(ctx->dev, " and %d\n", sizes[VPE_CHROMA]);
+
+	return 0;
+}
+
+static int vpe_buf_prepare(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct vpe_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct vpe_q_data *q_data;
+	int i, num_planes;
+
+	vpe_dbg(ctx->dev, "type: %d\n", vb->vb2_queue->type);
+
+	q_data = get_q_data(ctx, vb->vb2_queue->type);
+	num_planes = q_data->fmt->coplanar ? 2 : 1;
+
+	if (vb->vb2_queue->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		if (!(q_data->flags & Q_DATA_INTERLACED)) {
+			vbuf->field = V4L2_FIELD_NONE;
+		} else {
+			if (vbuf->field != V4L2_FIELD_TOP &&
+					vbuf->field != V4L2_FIELD_BOTTOM)
+				return -EINVAL;
+		}
+	}
+
+	for (i = 0; i < num_planes; i++) {
+		if (vb2_plane_size(vb, i) < q_data->sizeimage[i]) {
+			vpe_err(ctx->dev,
+				"data will not fit into plane (%lu < %lu)\n",
+				vb2_plane_size(vb, i),
+				(long) q_data->sizeimage[i]);
+			return -EINVAL;
+		}
+	}
+
+	for (i = 0; i < num_planes; i++)
+		vb2_set_plane_payload(vb, i, q_data->sizeimage[i]);
+
+	return 0;
+}
+
+static void vpe_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct vpe_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+
+	v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
+}
+
+static int vpe_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	/* currently we do nothing here */
+
+	return 0;
+}
+
+static void vpe_stop_streaming(struct vb2_queue *q)
+{
+	struct vpe_ctx *ctx = vb2_get_drv_priv(q);
+
+	vpe_dump_regs(ctx->dev);
+	vpdma_dump_regs(ctx->dev->vpdma);
+}
+
+static struct vb2_ops vpe_qops = {
+	.queue_setup	 = vpe_queue_setup,
+	.buf_prepare	 = vpe_buf_prepare,
+	.buf_queue	 = vpe_buf_queue,
+	.wait_prepare	 = vb2_ops_wait_prepare,
+	.wait_finish	 = vb2_ops_wait_finish,
+	.start_streaming = vpe_start_streaming,
+	.stop_streaming  = vpe_stop_streaming,
+};
+
+static int queue_init(void *priv, struct vb2_queue *src_vq,
+		      struct vb2_queue *dst_vq)
+{
+	struct vpe_ctx *ctx = priv;
+	struct vpe_dev *dev = ctx->dev;
+	int ret;
+
+	memset(src_vq, 0, sizeof(*src_vq));
+	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+	src_vq->io_modes = VB2_MMAP | VB2_DMABUF;
+	src_vq->drv_priv = ctx;
+	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	src_vq->ops = &vpe_qops;
+	src_vq->mem_ops = &vb2_dma_contig_memops;
+	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	src_vq->lock = &dev->dev_mutex;
+
+	ret = vb2_queue_init(src_vq);
+	if (ret)
+		return ret;
+
+	memset(dst_vq, 0, sizeof(*dst_vq));
+	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	dst_vq->io_modes = VB2_MMAP | VB2_DMABUF;
+	dst_vq->drv_priv = ctx;
+	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	dst_vq->ops = &vpe_qops;
+	dst_vq->mem_ops = &vb2_dma_contig_memops;
+	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	dst_vq->lock = &dev->dev_mutex;
+
+	return vb2_queue_init(dst_vq);
+}
+
+static const struct v4l2_ctrl_config vpe_bufs_per_job = {
+	.ops = &vpe_ctrl_ops,
+	.id = V4L2_CID_VPE_BUFS_PER_JOB,
+	.name = "Buffers Per Transaction",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.def = VPE_DEF_BUFS_PER_JOB,
+	.min = 1,
+	.max = VIDEO_MAX_FRAME,
+	.step = 1,
+};
+
+/*
+ * File operations
+ */
+static int vpe_open(struct file *file)
+{
+	struct vpe_dev *dev = video_drvdata(file);
+	struct vpe_q_data *s_q_data;
+	struct v4l2_ctrl_handler *hdl;
+	struct vpe_ctx *ctx;
+	int ret;
+
+	vpe_dbg(dev, "vpe_open\n");
+
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx)
+		return -ENOMEM;
+
+	ctx->dev = dev;
+
+	if (mutex_lock_interruptible(&dev->dev_mutex)) {
+		ret = -ERESTARTSYS;
+		goto free_ctx;
+	}
+
+	ret = vpdma_create_desc_list(&ctx->desc_list, VPE_DESC_LIST_SIZE,
+			VPDMA_LIST_TYPE_NORMAL);
+	if (ret != 0)
+		goto unlock;
+
+	ret = vpdma_alloc_desc_buf(&ctx->mmr_adb, sizeof(struct vpe_mmr_adb));
+	if (ret != 0)
+		goto free_desc_list;
+
+	ret = vpdma_alloc_desc_buf(&ctx->sc_coeff_h, SC_COEF_SRAM_SIZE);
+	if (ret != 0)
+		goto free_mmr_adb;
+
+	ret = vpdma_alloc_desc_buf(&ctx->sc_coeff_v, SC_COEF_SRAM_SIZE);
+	if (ret != 0)
+		goto free_sc_h;
+
+	init_adb_hdrs(ctx);
+
+	v4l2_fh_init(&ctx->fh, video_devdata(file));
+	file->private_data = &ctx->fh;
+
+	hdl = &ctx->hdl;
+	v4l2_ctrl_handler_init(hdl, 1);
+	v4l2_ctrl_new_custom(hdl, &vpe_bufs_per_job, NULL);
+	if (hdl->error) {
+		ret = hdl->error;
+		goto exit_fh;
+	}
+	ctx->fh.ctrl_handler = hdl;
+	v4l2_ctrl_handler_setup(hdl);
+
+	s_q_data = &ctx->q_data[Q_DATA_SRC];
+	s_q_data->fmt = &vpe_formats[2];
+	s_q_data->width = 1920;
+	s_q_data->height = 1080;
+	s_q_data->bytesperline[VPE_LUMA] = (s_q_data->width *
+			s_q_data->fmt->vpdma_fmt[VPE_LUMA]->depth) >> 3;
+	s_q_data->sizeimage[VPE_LUMA] = (s_q_data->bytesperline[VPE_LUMA] *
+			s_q_data->height);
+	s_q_data->colorspace = V4L2_COLORSPACE_REC709;
+	s_q_data->field = V4L2_FIELD_NONE;
+	s_q_data->c_rect.left = 0;
+	s_q_data->c_rect.top = 0;
+	s_q_data->c_rect.width = s_q_data->width;
+	s_q_data->c_rect.height = s_q_data->height;
+	s_q_data->flags = 0;
+
+	ctx->q_data[Q_DATA_DST] = *s_q_data;
+
+	set_dei_shadow_registers(ctx);
+	set_src_registers(ctx);
+	set_dst_registers(ctx);
+	ret = set_srcdst_params(ctx);
+	if (ret)
+		goto exit_fh;
+
+	ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(dev->m2m_dev, ctx, &queue_init);
+
+	if (IS_ERR(ctx->fh.m2m_ctx)) {
+		ret = PTR_ERR(ctx->fh.m2m_ctx);
+		goto exit_fh;
+	}
+
+	v4l2_fh_add(&ctx->fh);
+
+	/*
+	 * for now, just report the creation of the first instance, we can later
+	 * optimize the driver to enable or disable clocks when the first
+	 * instance is created or the last instance released
+	 */
+	if (atomic_inc_return(&dev->num_instances) == 1)
+		vpe_dbg(dev, "first instance created\n");
+
+	ctx->bufs_per_job = VPE_DEF_BUFS_PER_JOB;
+
+	ctx->load_mmrs = true;
+
+	vpe_dbg(dev, "created instance %p, m2m_ctx: %p\n",
+		ctx, ctx->fh.m2m_ctx);
+
+	mutex_unlock(&dev->dev_mutex);
+
+	return 0;
+exit_fh:
+	v4l2_ctrl_handler_free(hdl);
+	v4l2_fh_exit(&ctx->fh);
+	vpdma_free_desc_buf(&ctx->sc_coeff_v);
+free_sc_h:
+	vpdma_free_desc_buf(&ctx->sc_coeff_h);
+free_mmr_adb:
+	vpdma_free_desc_buf(&ctx->mmr_adb);
+free_desc_list:
+	vpdma_free_desc_list(&ctx->desc_list);
+unlock:
+	mutex_unlock(&dev->dev_mutex);
+free_ctx:
+	kfree(ctx);
+	return ret;
+}
+
+static int vpe_release(struct file *file)
+{
+	struct vpe_dev *dev = video_drvdata(file);
+	struct vpe_ctx *ctx = file2ctx(file);
+
+	vpe_dbg(dev, "releasing instance %p\n", ctx);
+
+	mutex_lock(&dev->dev_mutex);
+	free_vbs(ctx);
+	free_mv_buffers(ctx);
+	vpdma_free_desc_list(&ctx->desc_list);
+	vpdma_free_desc_buf(&ctx->mmr_adb);
+
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	v4l2_ctrl_handler_free(&ctx->hdl);
+	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
+
+	kfree(ctx);
+
+	/*
+	 * for now, just report the release of the last instance, we can later
+	 * optimize the driver to enable or disable clocks when the first
+	 * instance is created or the last instance released
+	 */
+	if (atomic_dec_return(&dev->num_instances) == 0)
+		vpe_dbg(dev, "last instance released\n");
+
+	mutex_unlock(&dev->dev_mutex);
+
+	return 0;
+}
+
+static const struct v4l2_file_operations vpe_fops = {
+	.owner		= THIS_MODULE,
+	.open		= vpe_open,
+	.release	= vpe_release,
+	.poll		= v4l2_m2m_fop_poll,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap		= v4l2_m2m_fop_mmap,
+};
+
+static struct video_device vpe_videodev = {
+	.name		= VPE_MODULE_NAME,
+	.fops		= &vpe_fops,
+	.ioctl_ops	= &vpe_ioctl_ops,
+	.minor		= -1,
+	.release	= video_device_release_empty,
+	.vfl_dir	= VFL_DIR_M2M,
+};
+
+static struct v4l2_m2m_ops m2m_ops = {
+	.device_run	= device_run,
+	.job_ready	= job_ready,
+	.job_abort	= job_abort,
+	.lock		= vpe_lock,
+	.unlock		= vpe_unlock,
+};
+
+static int vpe_runtime_get(struct platform_device *pdev)
+{
+	int r;
+
+	dev_dbg(&pdev->dev, "vpe_runtime_get\n");
+
+	r = pm_runtime_get_sync(&pdev->dev);
+	WARN_ON(r < 0);
+	return r < 0 ? r : 0;
+}
+
+static void vpe_runtime_put(struct platform_device *pdev)
+{
+
+	int r;
+
+	dev_dbg(&pdev->dev, "vpe_runtime_put\n");
+
+	r = pm_runtime_put_sync(&pdev->dev);
+	WARN_ON(r < 0 && r != -ENOSYS);
+}
+
+static void vpe_fw_cb(struct platform_device *pdev)
+{
+	struct vpe_dev *dev = platform_get_drvdata(pdev);
+	struct video_device *vfd;
+	int ret;
+
+	vfd = &dev->vfd;
+	*vfd = vpe_videodev;
+	vfd->lock = &dev->dev_mutex;
+	vfd->v4l2_dev = &dev->v4l2_dev;
+
+	ret = video_register_device(vfd, VFL_TYPE_GRABBER, 0);
+	if (ret) {
+		vpe_err(dev, "Failed to register video device\n");
+
+		vpe_set_clock_enable(dev, 0);
+		vpe_runtime_put(pdev);
+		pm_runtime_disable(&pdev->dev);
+		v4l2_m2m_release(dev->m2m_dev);
+		vb2_dma_contig_cleanup_ctx(dev->alloc_ctx);
+		v4l2_device_unregister(&dev->v4l2_dev);
+
+		return;
+	}
+
+	video_set_drvdata(vfd, dev);
+	snprintf(vfd->name, sizeof(vfd->name), "%s", vpe_videodev.name);
+	dev_info(dev->v4l2_dev.dev, "Device registered as /dev/video%d\n",
+		vfd->num);
+}
+
+static int vpe_probe(struct platform_device *pdev)
+{
+	struct vpe_dev *dev;
+	int ret, irq, func;
+
+	dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
+	if (!dev)
+		return -ENOMEM;
+
+	spin_lock_init(&dev->lock);
+
+	ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
+	if (ret)
+		return ret;
+
+	atomic_set(&dev->num_instances, 0);
+	mutex_init(&dev->dev_mutex);
+
+	dev->res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
+			"vpe_top");
+	/*
+	 * HACK: we get resource info from device tree in the form of a list of
+	 * VPE sub blocks, the driver currently uses only the base of vpe_top
+	 * for register access, the driver should be changed later to access
+	 * registers based on the sub block base addresses
+	 */
+	dev->base = devm_ioremap(&pdev->dev, dev->res->start, SZ_32K);
+	if (!dev->base) {
+		ret = -ENOMEM;
+		goto v4l2_dev_unreg;
+	}
+
+	irq = platform_get_irq(pdev, 0);
+	ret = devm_request_irq(&pdev->dev, irq, vpe_irq, 0, VPE_MODULE_NAME,
+			dev);
+	if (ret)
+		goto v4l2_dev_unreg;
+
+	platform_set_drvdata(pdev, dev);
+
+	dev->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
+	if (IS_ERR(dev->alloc_ctx)) {
+		vpe_err(dev, "Failed to alloc vb2 context\n");
+		ret = PTR_ERR(dev->alloc_ctx);
+		goto v4l2_dev_unreg;
+	}
+
+	dev->m2m_dev = v4l2_m2m_init(&m2m_ops);
+	if (IS_ERR(dev->m2m_dev)) {
+		vpe_err(dev, "Failed to init mem2mem device\n");
+		ret = PTR_ERR(dev->m2m_dev);
+		goto rel_ctx;
+	}
+
+	pm_runtime_enable(&pdev->dev);
+
+	ret = vpe_runtime_get(pdev);
+	if (ret)
+		goto rel_m2m;
+
+	/* Perform clk enable followed by reset */
+	vpe_set_clock_enable(dev, 1);
+
+	vpe_top_reset(dev);
+
+	func = read_field_reg(dev, VPE_PID, VPE_PID_FUNC_MASK,
+		VPE_PID_FUNC_SHIFT);
+	vpe_dbg(dev, "VPE PID function %x\n", func);
+
+	vpe_top_vpdma_reset(dev);
+
+	dev->sc = sc_create(pdev);
+	if (IS_ERR(dev->sc)) {
+		ret = PTR_ERR(dev->sc);
+		goto runtime_put;
+	}
+
+	dev->csc = csc_create(pdev);
+	if (IS_ERR(dev->csc)) {
+		ret = PTR_ERR(dev->csc);
+		goto runtime_put;
+	}
+
+	dev->vpdma = vpdma_create(pdev, vpe_fw_cb);
+	if (IS_ERR(dev->vpdma)) {
+		ret = PTR_ERR(dev->vpdma);
+		goto runtime_put;
+	}
+
+	return 0;
+
+runtime_put:
+	vpe_runtime_put(pdev);
+rel_m2m:
+	pm_runtime_disable(&pdev->dev);
+	v4l2_m2m_release(dev->m2m_dev);
+rel_ctx:
+	vb2_dma_contig_cleanup_ctx(dev->alloc_ctx);
+v4l2_dev_unreg:
+	v4l2_device_unregister(&dev->v4l2_dev);
+
+	return ret;
+}
+
+static int vpe_remove(struct platform_device *pdev)
+{
+	struct vpe_dev *dev = platform_get_drvdata(pdev);
+
+	v4l2_info(&dev->v4l2_dev, "Removing " VPE_MODULE_NAME);
+
+	v4l2_m2m_release(dev->m2m_dev);
+	video_unregister_device(&dev->vfd);
+	v4l2_device_unregister(&dev->v4l2_dev);
+	vb2_dma_contig_cleanup_ctx(dev->alloc_ctx);
+
+	vpe_set_clock_enable(dev, 0);
+	vpe_runtime_put(pdev);
+	pm_runtime_disable(&pdev->dev);
+
+	return 0;
+}
+
+#if defined(CONFIG_OF)
+static const struct of_device_id vpe_of_match[] = {
+	{
+		.compatible = "ti,vpe",
+	},
+	{},
+};
+#endif
+
+static struct platform_driver vpe_pdrv = {
+	.probe		= vpe_probe,
+	.remove		= vpe_remove,
+	.driver		= {
+		.name	= VPE_MODULE_NAME,
+		.of_match_table = of_match_ptr(vpe_of_match),
+	},
+};
+
+module_platform_driver(vpe_pdrv);
+
+MODULE_DESCRIPTION("TI VPE driver");
+MODULE_AUTHOR("Dale Farnsworth, <dale@farnsworth.org>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/ti-vpe/vpe_regs.h b/drivers/media/platform/ti-vpe/vpe_regs.h
new file mode 100644
index 0000000..74283d7
--- /dev/null
+++ b/drivers/media/platform/ti-vpe/vpe_regs.h
@@ -0,0 +1,309 @@
+/*
+ * Copyright (c) 2013 Texas Instruments Inc.
+ *
+ * David Griego, <dagriego@biglakesoftware.com>
+ * Dale Farnsworth, <dale@farnsworth.org>
+ * Archit Taneja, <archit@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ */
+
+#ifndef __TI_VPE_REGS_H
+#define __TI_VPE_REGS_H
+
+/* VPE register offsets and field selectors */
+
+/* VPE top level regs */
+#define VPE_PID				0x0000
+#define VPE_PID_MINOR_MASK		0x3f
+#define VPE_PID_MINOR_SHIFT		0
+#define VPE_PID_CUSTOM_MASK		0x03
+#define VPE_PID_CUSTOM_SHIFT		6
+#define VPE_PID_MAJOR_MASK		0x07
+#define VPE_PID_MAJOR_SHIFT		8
+#define VPE_PID_RTL_MASK		0x1f
+#define VPE_PID_RTL_SHIFT		11
+#define VPE_PID_FUNC_MASK		0xfff
+#define VPE_PID_FUNC_SHIFT		16
+#define VPE_PID_SCHEME_MASK		0x03
+#define VPE_PID_SCHEME_SHIFT		30
+
+#define VPE_SYSCONFIG			0x0010
+#define VPE_SYSCONFIG_IDLE_MASK		0x03
+#define VPE_SYSCONFIG_IDLE_SHIFT	2
+#define VPE_SYSCONFIG_STANDBY_MASK	0x03
+#define VPE_SYSCONFIG_STANDBY_SHIFT	4
+#define VPE_FORCE_IDLE_MODE		0
+#define VPE_NO_IDLE_MODE		1
+#define VPE_SMART_IDLE_MODE		2
+#define VPE_SMART_IDLE_WAKEUP_MODE	3
+#define VPE_FORCE_STANDBY_MODE		0
+#define VPE_NO_STANDBY_MODE		1
+#define VPE_SMART_STANDBY_MODE		2
+#define VPE_SMART_STANDBY_WAKEUP_MODE	3
+
+#define VPE_INT0_STATUS0_RAW_SET	0x0020
+#define VPE_INT0_STATUS0_RAW		VPE_INT0_STATUS0_RAW_SET
+#define VPE_INT0_STATUS0_CLR		0x0028
+#define VPE_INT0_STATUS0		VPE_INT0_STATUS0_CLR
+#define VPE_INT0_ENABLE0_SET		0x0030
+#define VPE_INT0_ENABLE0		VPE_INT0_ENABLE0_SET
+#define VPE_INT0_ENABLE0_CLR		0x0038
+#define VPE_INT0_LIST0_COMPLETE		(1 << 0)
+#define VPE_INT0_LIST0_NOTIFY		(1 << 1)
+#define VPE_INT0_LIST1_COMPLETE		(1 << 2)
+#define VPE_INT0_LIST1_NOTIFY		(1 << 3)
+#define VPE_INT0_LIST2_COMPLETE		(1 << 4)
+#define VPE_INT0_LIST2_NOTIFY		(1 << 5)
+#define VPE_INT0_LIST3_COMPLETE		(1 << 6)
+#define VPE_INT0_LIST3_NOTIFY		(1 << 7)
+#define VPE_INT0_LIST4_COMPLETE		(1 << 8)
+#define VPE_INT0_LIST4_NOTIFY		(1 << 9)
+#define VPE_INT0_LIST5_COMPLETE		(1 << 10)
+#define VPE_INT0_LIST5_NOTIFY		(1 << 11)
+#define VPE_INT0_LIST6_COMPLETE		(1 << 12)
+#define VPE_INT0_LIST6_NOTIFY		(1 << 13)
+#define VPE_INT0_LIST7_COMPLETE		(1 << 14)
+#define VPE_INT0_LIST7_NOTIFY		(1 << 15)
+#define VPE_INT0_DESCRIPTOR		(1 << 16)
+#define VPE_DEI_FMD_INT			(1 << 18)
+
+#define VPE_INT0_STATUS1_RAW_SET	0x0024
+#define VPE_INT0_STATUS1_RAW		VPE_INT0_STATUS1_RAW_SET
+#define VPE_INT0_STATUS1_CLR		0x002c
+#define VPE_INT0_STATUS1		VPE_INT0_STATUS1_CLR
+#define VPE_INT0_ENABLE1_SET		0x0034
+#define VPE_INT0_ENABLE1		VPE_INT0_ENABLE1_SET
+#define VPE_INT0_ENABLE1_CLR		0x003c
+#define VPE_INT0_CHANNEL_GROUP0		(1 << 0)
+#define VPE_INT0_CHANNEL_GROUP1		(1 << 1)
+#define VPE_INT0_CHANNEL_GROUP2		(1 << 2)
+#define VPE_INT0_CHANNEL_GROUP3		(1 << 3)
+#define VPE_INT0_CHANNEL_GROUP4		(1 << 4)
+#define VPE_INT0_CHANNEL_GROUP5		(1 << 5)
+#define VPE_INT0_CLIENT			(1 << 7)
+#define VPE_DEI_ERROR_INT		(1 << 16)
+#define VPE_DS1_UV_ERROR_INT		(1 << 22)
+
+#define VPE_INTC_EOI			0x00a0
+
+#define VPE_CLK_ENABLE			0x0100
+#define VPE_VPEDMA_CLK_ENABLE		(1 << 0)
+#define VPE_DATA_PATH_CLK_ENABLE	(1 << 1)
+
+#define VPE_CLK_RESET			0x0104
+#define VPE_VPDMA_CLK_RESET_MASK	0x1
+#define VPE_VPDMA_CLK_RESET_SHIFT	0
+#define VPE_DATA_PATH_CLK_RESET_MASK	0x1
+#define VPE_DATA_PATH_CLK_RESET_SHIFT	1
+#define VPE_MAIN_RESET_MASK		0x1
+#define VPE_MAIN_RESET_SHIFT		31
+
+#define VPE_CLK_FORMAT_SELECT		0x010c
+#define VPE_CSC_SRC_SELECT_MASK		0x03
+#define VPE_CSC_SRC_SELECT_SHIFT	0
+#define VPE_RGB_OUT_SELECT		(1 << 8)
+#define VPE_DS_SRC_SELECT_MASK		0x07
+#define VPE_DS_SRC_SELECT_SHIFT		9
+#define VPE_DS_BYPASS			(1 << 16)
+#define VPE_COLOR_SEPARATE_422		(1 << 18)
+
+#define VPE_DS_SRC_DEI_SCALER		(5 << VPE_DS_SRC_SELECT_SHIFT)
+#define VPE_CSC_SRC_DEI_SCALER		(3 << VPE_CSC_SRC_SELECT_SHIFT)
+
+#define VPE_CLK_RANGE_MAP		0x011c
+#define VPE_RANGE_RANGE_MAP_Y_MASK	0x07
+#define VPE_RANGE_RANGE_MAP_Y_SHIFT	0
+#define VPE_RANGE_RANGE_MAP_UV_MASK	0x07
+#define VPE_RANGE_RANGE_MAP_UV_SHIFT	3
+#define VPE_RANGE_MAP_ON		(1 << 6)
+#define VPE_RANGE_REDUCTION_ON		(1 << 28)
+
+/* VPE chrominance upsampler regs */
+#define VPE_US1_R0			0x0304
+#define VPE_US2_R0			0x0404
+#define VPE_US3_R0			0x0504
+#define VPE_US_C1_MASK			0x3fff
+#define VPE_US_C1_SHIFT			2
+#define VPE_US_C0_MASK			0x3fff
+#define VPE_US_C0_SHIFT			18
+#define VPE_US_MODE_MASK		0x03
+#define VPE_US_MODE_SHIFT		16
+#define VPE_ANCHOR_FID0_C1_MASK		0x3fff
+#define VPE_ANCHOR_FID0_C1_SHIFT	2
+#define VPE_ANCHOR_FID0_C0_MASK		0x3fff
+#define VPE_ANCHOR_FID0_C0_SHIFT	18
+
+#define VPE_US1_R1			0x0308
+#define VPE_US2_R1			0x0408
+#define VPE_US3_R1			0x0508
+#define VPE_ANCHOR_FID0_C3_MASK		0x3fff
+#define VPE_ANCHOR_FID0_C3_SHIFT	2
+#define VPE_ANCHOR_FID0_C2_MASK		0x3fff
+#define VPE_ANCHOR_FID0_C2_SHIFT	18
+
+#define VPE_US1_R2			0x030c
+#define VPE_US2_R2			0x040c
+#define VPE_US3_R2			0x050c
+#define VPE_INTERP_FID0_C1_MASK		0x3fff
+#define VPE_INTERP_FID0_C1_SHIFT	2
+#define VPE_INTERP_FID0_C0_MASK		0x3fff
+#define VPE_INTERP_FID0_C0_SHIFT	18
+
+#define VPE_US1_R3			0x0310
+#define VPE_US2_R3			0x0410
+#define VPE_US3_R3			0x0510
+#define VPE_INTERP_FID0_C3_MASK		0x3fff
+#define VPE_INTERP_FID0_C3_SHIFT	2
+#define VPE_INTERP_FID0_C2_MASK		0x3fff
+#define VPE_INTERP_FID0_C2_SHIFT	18
+
+#define VPE_US1_R4			0x0314
+#define VPE_US2_R4			0x0414
+#define VPE_US3_R4			0x0514
+#define VPE_ANCHOR_FID1_C1_MASK		0x3fff
+#define VPE_ANCHOR_FID1_C1_SHIFT	2
+#define VPE_ANCHOR_FID1_C0_MASK		0x3fff
+#define VPE_ANCHOR_FID1_C0_SHIFT	18
+
+#define VPE_US1_R5			0x0318
+#define VPE_US2_R5			0x0418
+#define VPE_US3_R5			0x0518
+#define VPE_ANCHOR_FID1_C3_MASK		0x3fff
+#define VPE_ANCHOR_FID1_C3_SHIFT	2
+#define VPE_ANCHOR_FID1_C2_MASK		0x3fff
+#define VPE_ANCHOR_FID1_C2_SHIFT	18
+
+#define VPE_US1_R6			0x031c
+#define VPE_US2_R6			0x041c
+#define VPE_US3_R6			0x051c
+#define VPE_INTERP_FID1_C1_MASK		0x3fff
+#define VPE_INTERP_FID1_C1_SHIFT	2
+#define VPE_INTERP_FID1_C0_MASK		0x3fff
+#define VPE_INTERP_FID1_C0_SHIFT	18
+
+#define VPE_US1_R7			0x0320
+#define VPE_US2_R7			0x0420
+#define VPE_US3_R7			0x0520
+#define VPE_INTERP_FID0_C3_MASK		0x3fff
+#define VPE_INTERP_FID0_C3_SHIFT	2
+#define VPE_INTERP_FID0_C2_MASK		0x3fff
+#define VPE_INTERP_FID0_C2_SHIFT	18
+
+/* VPE de-interlacer regs */
+#define VPE_DEI_FRAME_SIZE		0x0600
+#define VPE_DEI_WIDTH_MASK		0x07ff
+#define VPE_DEI_WIDTH_SHIFT		0
+#define VPE_DEI_HEIGHT_MASK		0x07ff
+#define VPE_DEI_HEIGHT_SHIFT		16
+#define VPE_DEI_INTERLACE_BYPASS	(1 << 29)
+#define VPE_DEI_FIELD_FLUSH		(1 << 30)
+#define VPE_DEI_PROGRESSIVE		(1 << 31)
+
+#define VPE_MDT_BYPASS			0x0604
+#define VPE_MDT_TEMPMAX_BYPASS		(1 << 0)
+#define VPE_MDT_SPATMAX_BYPASS		(1 << 1)
+
+#define VPE_MDT_SF_THRESHOLD		0x0608
+#define VPE_MDT_SF_SC_THR1_MASK		0xff
+#define VPE_MDT_SF_SC_THR1_SHIFT	0
+#define VPE_MDT_SF_SC_THR2_MASK		0xff
+#define VPE_MDT_SF_SC_THR2_SHIFT	0
+#define VPE_MDT_SF_SC_THR3_MASK		0xff
+#define VPE_MDT_SF_SC_THR3_SHIFT	0
+
+#define VPE_EDI_CONFIG			0x060c
+#define VPE_EDI_INP_MODE_MASK		0x03
+#define VPE_EDI_INP_MODE_SHIFT		0
+#define VPE_EDI_ENABLE_3D		(1 << 2)
+#define VPE_EDI_ENABLE_CHROMA_3D	(1 << 3)
+#define VPE_EDI_CHROMA3D_COR_THR_MASK	0xff
+#define VPE_EDI_CHROMA3D_COR_THR_SHIFT	8
+#define VPE_EDI_DIR_COR_LOWER_THR_MASK	0xff
+#define VPE_EDI_DIR_COR_LOWER_THR_SHIFT	16
+#define VPE_EDI_COR_SCALE_FACTOR_MASK	0xff
+#define VPE_EDI_COR_SCALE_FACTOR_SHIFT	23
+
+#define VPE_DEI_EDI_LUT_R0		0x0610
+#define VPE_EDI_LUT0_MASK		0x1f
+#define VPE_EDI_LUT0_SHIFT		0
+#define VPE_EDI_LUT1_MASK		0x1f
+#define VPE_EDI_LUT1_SHIFT		8
+#define VPE_EDI_LUT2_MASK		0x1f
+#define VPE_EDI_LUT2_SHIFT		16
+#define VPE_EDI_LUT3_MASK		0x1f
+#define VPE_EDI_LUT3_SHIFT		24
+
+#define VPE_DEI_EDI_LUT_R1		0x0614
+#define VPE_EDI_LUT0_MASK		0x1f
+#define VPE_EDI_LUT0_SHIFT		0
+#define VPE_EDI_LUT1_MASK		0x1f
+#define VPE_EDI_LUT1_SHIFT		8
+#define VPE_EDI_LUT2_MASK		0x1f
+#define VPE_EDI_LUT2_SHIFT		16
+#define VPE_EDI_LUT3_MASK		0x1f
+#define VPE_EDI_LUT3_SHIFT		24
+
+#define VPE_DEI_EDI_LUT_R2		0x0618
+#define VPE_EDI_LUT4_MASK		0x1f
+#define VPE_EDI_LUT4_SHIFT		0
+#define VPE_EDI_LUT5_MASK		0x1f
+#define VPE_EDI_LUT5_SHIFT		8
+#define VPE_EDI_LUT6_MASK		0x1f
+#define VPE_EDI_LUT6_SHIFT		16
+#define VPE_EDI_LUT7_MASK		0x1f
+#define VPE_EDI_LUT7_SHIFT		24
+
+#define VPE_DEI_EDI_LUT_R3		0x061c
+#define VPE_EDI_LUT8_MASK		0x1f
+#define VPE_EDI_LUT8_SHIFT		0
+#define VPE_EDI_LUT9_MASK		0x1f
+#define VPE_EDI_LUT9_SHIFT		8
+#define VPE_EDI_LUT10_MASK		0x1f
+#define VPE_EDI_LUT10_SHIFT		16
+#define VPE_EDI_LUT11_MASK		0x1f
+#define VPE_EDI_LUT11_SHIFT		24
+
+#define VPE_DEI_FMD_WINDOW_R0		0x0620
+#define VPE_FMD_WINDOW_MINX_MASK	0x07ff
+#define VPE_FMD_WINDOW_MINX_SHIFT	0
+#define VPE_FMD_WINDOW_MAXX_MASK	0x07ff
+#define VPE_FMD_WINDOW_MAXX_SHIFT	16
+#define VPE_FMD_WINDOW_ENABLE		(1 << 31)
+
+#define VPE_DEI_FMD_WINDOW_R1		0x0624
+#define VPE_FMD_WINDOW_MINY_MASK	0x07ff
+#define VPE_FMD_WINDOW_MINY_SHIFT	0
+#define VPE_FMD_WINDOW_MAXY_MASK	0x07ff
+#define VPE_FMD_WINDOW_MAXY_SHIFT	16
+
+#define VPE_DEI_FMD_CONTROL_R0		0x0628
+#define VPE_FMD_ENABLE			(1 << 0)
+#define VPE_FMD_LOCK			(1 << 1)
+#define VPE_FMD_JAM_DIR			(1 << 2)
+#define VPE_FMD_BED_ENABLE		(1 << 3)
+#define VPE_FMD_CAF_FIELD_THR_MASK	0xff
+#define VPE_FMD_CAF_FIELD_THR_SHIFT	16
+#define VPE_FMD_CAF_LINE_THR_MASK	0xff
+#define VPE_FMD_CAF_LINE_THR_SHIFT	24
+
+#define VPE_DEI_FMD_CONTROL_R1		0x062c
+#define VPE_FMD_CAF_THR_MASK		0x000fffff
+#define VPE_FMD_CAF_THR_SHIFT		0
+
+#define VPE_DEI_FMD_STATUS_R0		0x0630
+#define VPE_FMD_CAF_MASK		0x000fffff
+#define VPE_FMD_CAF_SHIFT		0
+#define VPE_FMD_RESET			(1 << 24)
+
+#define VPE_DEI_FMD_STATUS_R1		0x0634
+#define VPE_FMD_FIELD_DIFF_MASK		0x0fffffff
+#define VPE_FMD_FIELD_DIFF_SHIFT	0
+
+#define VPE_DEI_FMD_STATUS_R2		0x0638
+#define VPE_FMD_FRAME_DIFF_MASK		0x000fffff
+#define VPE_FMD_FRAME_DIFF_SHIFT	0
+
+#endif
diff --git a/drivers/media/platform/timblogiw.c b/drivers/media/platform/timblogiw.c
new file mode 100644
index 0000000..5820e45
--- /dev/null
+++ b/drivers/media/platform/timblogiw.c
@@ -0,0 +1,870 @@
+/*
+ * timblogiw.c timberdale FPGA LogiWin Video In driver
+ * Copyright (c) 2009-2010 Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+/* Supports:
+ * Timberdale FPGA LogiWin Video In
+ */
+
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/dmaengine.h>
+#include <linux/scatterlist.h>
+#include <linux/interrupt.h>
+#include <linux/list.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-device.h>
+#include <media/videobuf-dma-contig.h>
+#include <media/timb_video.h>
+
+#define DRIVER_NAME			"timb-video"
+
+#define TIMBLOGIWIN_NAME		"Timberdale Video-In"
+#define TIMBLOGIW_VERSION_CODE		0x04
+
+#define TIMBLOGIW_LINES_PER_DESC	44
+#define TIMBLOGIW_MAX_VIDEO_MEM		16
+
+#define TIMBLOGIW_HAS_DECODER(lw)	(lw->pdata.encoder.module_name)
+
+
+struct timblogiw {
+	struct video_device		video_dev;
+	struct v4l2_device		v4l2_dev; /* mutual exclusion */
+	struct mutex			lock;
+	struct device			*dev;
+	struct timb_video_platform_data pdata;
+	struct v4l2_subdev		*sd_enc;	/* encoder */
+	bool				opened;
+};
+
+struct timblogiw_tvnorm {
+	v4l2_std_id std;
+	u16     width;
+	u16     height;
+	u8	fps;
+};
+
+struct timblogiw_fh {
+	struct videobuf_queue		vb_vidq;
+	struct timblogiw_tvnorm const	*cur_norm;
+	struct list_head		capture;
+	struct dma_chan			*chan;
+	spinlock_t			queue_lock; /* mutual exclusion */
+	unsigned int			frame_count;
+};
+
+struct timblogiw_buffer {
+	/* common v4l buffer stuff -- must be first */
+	struct videobuf_buffer	vb;
+	struct scatterlist	sg[16];
+	dma_cookie_t		cookie;
+	struct timblogiw_fh	*fh;
+};
+
+static const struct timblogiw_tvnorm timblogiw_tvnorms[] = {
+	{
+		.std			= V4L2_STD_PAL,
+		.width			= 720,
+		.height			= 576,
+		.fps			= 25
+	},
+	{
+		.std			= V4L2_STD_NTSC,
+		.width			= 720,
+		.height			= 480,
+		.fps			= 30
+	}
+};
+
+static int timblogiw_bytes_per_line(const struct timblogiw_tvnorm *norm)
+{
+	return norm->width * 2;
+}
+
+
+static int timblogiw_frame_size(const struct timblogiw_tvnorm *norm)
+{
+	return norm->height * timblogiw_bytes_per_line(norm);
+}
+
+static const struct timblogiw_tvnorm *timblogiw_get_norm(const v4l2_std_id std)
+{
+	int i;
+	for (i = 0; i < ARRAY_SIZE(timblogiw_tvnorms); i++)
+		if (timblogiw_tvnorms[i].std & std)
+			return timblogiw_tvnorms + i;
+
+	/* default to first element */
+	return timblogiw_tvnorms;
+}
+
+static void timblogiw_dma_cb(void *data)
+{
+	struct timblogiw_buffer *buf = data;
+	struct timblogiw_fh *fh = buf->fh;
+	struct videobuf_buffer *vb = &buf->vb;
+
+	spin_lock(&fh->queue_lock);
+
+	/* mark the transfer done */
+	buf->cookie = -1;
+
+	fh->frame_count++;
+
+	if (vb->state != VIDEOBUF_ERROR) {
+		list_del(&vb->queue);
+		v4l2_get_timestamp(&vb->ts);
+		vb->field_count = fh->frame_count * 2;
+		vb->state = VIDEOBUF_DONE;
+
+		wake_up(&vb->done);
+	}
+
+	if (!list_empty(&fh->capture)) {
+		vb = list_entry(fh->capture.next, struct videobuf_buffer,
+			queue);
+		vb->state = VIDEOBUF_ACTIVE;
+	}
+
+	spin_unlock(&fh->queue_lock);
+}
+
+static bool timblogiw_dma_filter_fn(struct dma_chan *chan, void *filter_param)
+{
+	return chan->chan_id == (uintptr_t)filter_param;
+}
+
+/* IOCTL functions */
+
+static int timblogiw_g_fmt(struct file *file, void  *priv,
+	struct v4l2_format *format)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct timblogiw *lw = video_get_drvdata(vdev);
+	struct timblogiw_fh *fh = priv;
+
+	dev_dbg(&vdev->dev, "%s entry\n", __func__);
+
+	if (format->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	mutex_lock(&lw->lock);
+
+	format->fmt.pix.width = fh->cur_norm->width;
+	format->fmt.pix.height = fh->cur_norm->height;
+	format->fmt.pix.pixelformat = V4L2_PIX_FMT_UYVY;
+	format->fmt.pix.bytesperline = timblogiw_bytes_per_line(fh->cur_norm);
+	format->fmt.pix.sizeimage = timblogiw_frame_size(fh->cur_norm);
+	format->fmt.pix.field = V4L2_FIELD_NONE;
+
+	mutex_unlock(&lw->lock);
+
+	return 0;
+}
+
+static int timblogiw_try_fmt(struct file *file, void  *priv,
+	struct v4l2_format *format)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct v4l2_pix_format *pix = &format->fmt.pix;
+
+	dev_dbg(&vdev->dev,
+		"%s - width=%d, height=%d, pixelformat=%d, field=%d\n"
+		"bytes per line %d, size image: %d, colorspace: %d\n",
+		__func__,
+		pix->width, pix->height, pix->pixelformat, pix->field,
+		pix->bytesperline, pix->sizeimage, pix->colorspace);
+
+	if (format->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	if (pix->field != V4L2_FIELD_NONE)
+		return -EINVAL;
+
+	if (pix->pixelformat != V4L2_PIX_FMT_UYVY)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int timblogiw_s_fmt(struct file *file, void  *priv,
+	struct v4l2_format *format)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct timblogiw *lw = video_get_drvdata(vdev);
+	struct timblogiw_fh *fh = priv;
+	struct v4l2_pix_format *pix = &format->fmt.pix;
+	int err;
+
+	mutex_lock(&lw->lock);
+
+	err = timblogiw_try_fmt(file, priv, format);
+	if (err)
+		goto out;
+
+	if (videobuf_queue_is_busy(&fh->vb_vidq)) {
+		dev_err(&vdev->dev, "%s queue busy\n", __func__);
+		err = -EBUSY;
+		goto out;
+	}
+
+	pix->width = fh->cur_norm->width;
+	pix->height = fh->cur_norm->height;
+
+out:
+	mutex_unlock(&lw->lock);
+	return err;
+}
+
+static int timblogiw_querycap(struct file *file, void  *priv,
+	struct v4l2_capability *cap)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	dev_dbg(&vdev->dev, "%s: Entry\n",  __func__);
+	strncpy(cap->card, TIMBLOGIWIN_NAME, sizeof(cap->card)-1);
+	strncpy(cap->driver, DRIVER_NAME, sizeof(cap->driver) - 1);
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s", vdev->name);
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING |
+		V4L2_CAP_READWRITE;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+
+	return 0;
+}
+
+static int timblogiw_enum_fmt(struct file *file, void  *priv,
+	struct v4l2_fmtdesc *fmt)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	dev_dbg(&vdev->dev, "%s, index: %d\n",  __func__, fmt->index);
+
+	if (fmt->index != 0)
+		return -EINVAL;
+	memset(fmt, 0, sizeof(*fmt));
+	fmt->index = 0;
+	fmt->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	strncpy(fmt->description, "4:2:2, packed, YUYV",
+		sizeof(fmt->description)-1);
+	fmt->pixelformat = V4L2_PIX_FMT_UYVY;
+
+	return 0;
+}
+
+static int timblogiw_g_parm(struct file *file, void *priv,
+	struct v4l2_streamparm *sp)
+{
+	struct timblogiw_fh *fh = priv;
+	struct v4l2_captureparm *cp = &sp->parm.capture;
+
+	cp->capability = V4L2_CAP_TIMEPERFRAME;
+	cp->timeperframe.numerator = 1;
+	cp->timeperframe.denominator = fh->cur_norm->fps;
+
+	return 0;
+}
+
+static int timblogiw_reqbufs(struct file *file, void  *priv,
+	struct v4l2_requestbuffers *rb)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct timblogiw_fh *fh = priv;
+
+	dev_dbg(&vdev->dev, "%s: entry\n",  __func__);
+
+	return videobuf_reqbufs(&fh->vb_vidq, rb);
+}
+
+static int timblogiw_querybuf(struct file *file, void  *priv,
+	struct v4l2_buffer *b)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct timblogiw_fh *fh = priv;
+
+	dev_dbg(&vdev->dev, "%s: entry\n",  __func__);
+
+	return videobuf_querybuf(&fh->vb_vidq, b);
+}
+
+static int timblogiw_qbuf(struct file *file, void  *priv, struct v4l2_buffer *b)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct timblogiw_fh *fh = priv;
+
+	dev_dbg(&vdev->dev, "%s: entry\n",  __func__);
+
+	return videobuf_qbuf(&fh->vb_vidq, b);
+}
+
+static int timblogiw_dqbuf(struct file *file, void  *priv,
+	struct v4l2_buffer *b)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct timblogiw_fh *fh = priv;
+
+	dev_dbg(&vdev->dev, "%s: entry\n",  __func__);
+
+	return videobuf_dqbuf(&fh->vb_vidq, b, file->f_flags & O_NONBLOCK);
+}
+
+static int timblogiw_g_std(struct file *file, void  *priv, v4l2_std_id *std)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct timblogiw_fh *fh = priv;
+
+	dev_dbg(&vdev->dev, "%s: entry\n",  __func__);
+
+	*std = fh->cur_norm->std;
+	return 0;
+}
+
+static int timblogiw_s_std(struct file *file, void  *priv, v4l2_std_id std)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct timblogiw *lw = video_get_drvdata(vdev);
+	struct timblogiw_fh *fh = priv;
+	int err = 0;
+
+	dev_dbg(&vdev->dev, "%s: entry\n",  __func__);
+
+	mutex_lock(&lw->lock);
+
+	if (TIMBLOGIW_HAS_DECODER(lw))
+		err = v4l2_subdev_call(lw->sd_enc, video, s_std, std);
+
+	if (!err)
+		fh->cur_norm = timblogiw_get_norm(std);
+
+	mutex_unlock(&lw->lock);
+
+	return err;
+}
+
+static int timblogiw_enuminput(struct file *file, void  *priv,
+	struct v4l2_input *inp)
+{
+	struct video_device *vdev = video_devdata(file);
+	int i;
+
+	dev_dbg(&vdev->dev, "%s: Entry\n",  __func__);
+
+	if (inp->index != 0)
+		return -EINVAL;
+
+	inp->index = 0;
+
+	strncpy(inp->name, "Timb input 1", sizeof(inp->name) - 1);
+	inp->type = V4L2_INPUT_TYPE_CAMERA;
+
+	inp->std = 0;
+	for (i = 0; i < ARRAY_SIZE(timblogiw_tvnorms); i++)
+		inp->std |= timblogiw_tvnorms[i].std;
+
+	return 0;
+}
+
+static int timblogiw_g_input(struct file *file, void  *priv,
+	unsigned int *input)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	dev_dbg(&vdev->dev, "%s: Entry\n",  __func__);
+
+	*input = 0;
+
+	return 0;
+}
+
+static int timblogiw_s_input(struct file *file, void  *priv, unsigned int input)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	dev_dbg(&vdev->dev, "%s: Entry\n",  __func__);
+
+	if (input != 0)
+		return -EINVAL;
+	return 0;
+}
+
+static int timblogiw_streamon(struct file *file, void  *priv, enum v4l2_buf_type type)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct timblogiw_fh *fh = priv;
+
+	dev_dbg(&vdev->dev, "%s: entry\n",  __func__);
+
+	if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+		dev_dbg(&vdev->dev, "%s - No capture device\n", __func__);
+		return -EINVAL;
+	}
+
+	fh->frame_count = 0;
+	return videobuf_streamon(&fh->vb_vidq);
+}
+
+static int timblogiw_streamoff(struct file *file, void  *priv,
+	enum v4l2_buf_type type)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct timblogiw_fh *fh = priv;
+
+	dev_dbg(&vdev->dev, "%s entry\n",  __func__);
+
+	if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	return videobuf_streamoff(&fh->vb_vidq);
+}
+
+static int timblogiw_querystd(struct file *file, void  *priv, v4l2_std_id *std)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct timblogiw *lw = video_get_drvdata(vdev);
+	struct timblogiw_fh *fh = priv;
+
+	dev_dbg(&vdev->dev, "%s entry\n",  __func__);
+
+	if (TIMBLOGIW_HAS_DECODER(lw))
+		return v4l2_subdev_call(lw->sd_enc, video, querystd, std);
+	else {
+		*std = fh->cur_norm->std;
+		return 0;
+	}
+}
+
+static int timblogiw_enum_framesizes(struct file *file, void  *priv,
+	struct v4l2_frmsizeenum *fsize)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct timblogiw_fh *fh = priv;
+
+	dev_dbg(&vdev->dev, "%s - index: %d, format: %d\n",  __func__,
+		fsize->index, fsize->pixel_format);
+
+	if ((fsize->index != 0) ||
+		(fsize->pixel_format != V4L2_PIX_FMT_UYVY))
+		return -EINVAL;
+
+	fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+	fsize->discrete.width = fh->cur_norm->width;
+	fsize->discrete.height = fh->cur_norm->height;
+
+	return 0;
+}
+
+/* Video buffer functions */
+
+static int buffer_setup(struct videobuf_queue *vq, unsigned int *count,
+	unsigned int *size)
+{
+	struct timblogiw_fh *fh = vq->priv_data;
+
+	*size = timblogiw_frame_size(fh->cur_norm);
+
+	if (!*count)
+		*count = 32;
+
+	while (*size * *count > TIMBLOGIW_MAX_VIDEO_MEM * 1024 * 1024)
+		(*count)--;
+
+	return 0;
+}
+
+static int buffer_prepare(struct videobuf_queue *vq, struct videobuf_buffer *vb,
+	enum v4l2_field field)
+{
+	struct timblogiw_fh *fh = vq->priv_data;
+	struct timblogiw_buffer *buf = container_of(vb, struct timblogiw_buffer,
+		vb);
+	unsigned int data_size = timblogiw_frame_size(fh->cur_norm);
+	int err = 0;
+
+	if (vb->baddr && vb->bsize < data_size)
+		/* User provided buffer, but it is too small */
+		return -ENOMEM;
+
+	vb->size = data_size;
+	vb->width = fh->cur_norm->width;
+	vb->height = fh->cur_norm->height;
+	vb->field = field;
+
+	if (vb->state == VIDEOBUF_NEEDS_INIT) {
+		int i;
+		unsigned int size;
+		unsigned int bytes_per_desc = TIMBLOGIW_LINES_PER_DESC *
+			timblogiw_bytes_per_line(fh->cur_norm);
+		dma_addr_t addr;
+
+		sg_init_table(buf->sg, ARRAY_SIZE(buf->sg));
+
+		err = videobuf_iolock(vq, vb, NULL);
+		if (err)
+			goto err;
+
+		addr = videobuf_to_dma_contig(vb);
+		for (i = 0, size = 0; size < data_size; i++) {
+			sg_dma_address(buf->sg + i) = addr + size;
+			size += bytes_per_desc;
+			sg_dma_len(buf->sg + i) = (size > data_size) ?
+				(bytes_per_desc - (size - data_size)) :
+				bytes_per_desc;
+		}
+
+		vb->state = VIDEOBUF_PREPARED;
+		buf->cookie = -1;
+		buf->fh = fh;
+	}
+
+	return 0;
+
+err:
+	videobuf_dma_contig_free(vq, vb);
+	vb->state = VIDEOBUF_NEEDS_INIT;
+	return err;
+}
+
+static void buffer_queue(struct videobuf_queue *vq, struct videobuf_buffer *vb)
+{
+	struct timblogiw_fh *fh = vq->priv_data;
+	struct timblogiw_buffer *buf = container_of(vb, struct timblogiw_buffer,
+		vb);
+	struct dma_async_tx_descriptor *desc;
+	int sg_elems;
+	int bytes_per_desc = TIMBLOGIW_LINES_PER_DESC *
+		timblogiw_bytes_per_line(fh->cur_norm);
+
+	sg_elems = timblogiw_frame_size(fh->cur_norm) / bytes_per_desc;
+	sg_elems +=
+		(timblogiw_frame_size(fh->cur_norm) % bytes_per_desc) ? 1 : 0;
+
+	if (list_empty(&fh->capture))
+		vb->state = VIDEOBUF_ACTIVE;
+	else
+		vb->state = VIDEOBUF_QUEUED;
+
+	list_add_tail(&vb->queue, &fh->capture);
+
+	spin_unlock_irq(&fh->queue_lock);
+
+	desc = dmaengine_prep_slave_sg(fh->chan,
+		buf->sg, sg_elems, DMA_DEV_TO_MEM,
+		DMA_PREP_INTERRUPT);
+	if (!desc) {
+		spin_lock_irq(&fh->queue_lock);
+		list_del_init(&vb->queue);
+		vb->state = VIDEOBUF_PREPARED;
+		return;
+	}
+
+	desc->callback_param = buf;
+	desc->callback = timblogiw_dma_cb;
+
+	buf->cookie = desc->tx_submit(desc);
+
+	spin_lock_irq(&fh->queue_lock);
+}
+
+static void buffer_release(struct videobuf_queue *vq,
+	struct videobuf_buffer *vb)
+{
+	struct timblogiw_fh *fh = vq->priv_data;
+	struct timblogiw_buffer *buf = container_of(vb, struct timblogiw_buffer,
+		vb);
+
+	videobuf_waiton(vq, vb, 0, 0);
+	if (buf->cookie >= 0)
+		dma_sync_wait(fh->chan, buf->cookie);
+
+	videobuf_dma_contig_free(vq, vb);
+	vb->state = VIDEOBUF_NEEDS_INIT;
+}
+
+static struct videobuf_queue_ops timblogiw_video_qops = {
+	.buf_setup      = buffer_setup,
+	.buf_prepare    = buffer_prepare,
+	.buf_queue      = buffer_queue,
+	.buf_release    = buffer_release,
+};
+
+/* Device Operations functions */
+
+static int timblogiw_open(struct file *file)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct timblogiw *lw = video_get_drvdata(vdev);
+	struct timblogiw_fh *fh;
+	v4l2_std_id std;
+	dma_cap_mask_t mask;
+	int err = 0;
+
+	dev_dbg(&vdev->dev, "%s: entry\n", __func__);
+
+	mutex_lock(&lw->lock);
+	if (lw->opened) {
+		err = -EBUSY;
+		goto out;
+	}
+
+	if (TIMBLOGIW_HAS_DECODER(lw) && !lw->sd_enc) {
+		struct i2c_adapter *adapt;
+
+		/* find the video decoder */
+		adapt = i2c_get_adapter(lw->pdata.i2c_adapter);
+		if (!adapt) {
+			dev_err(&vdev->dev, "No I2C bus #%d\n",
+				lw->pdata.i2c_adapter);
+			err = -ENODEV;
+			goto out;
+		}
+
+		/* now find the encoder */
+		lw->sd_enc = v4l2_i2c_new_subdev_board(&lw->v4l2_dev, adapt,
+			lw->pdata.encoder.info, NULL);
+
+		i2c_put_adapter(adapt);
+
+		if (!lw->sd_enc) {
+			dev_err(&vdev->dev, "Failed to get encoder: %s\n",
+				lw->pdata.encoder.module_name);
+			err = -ENODEV;
+			goto out;
+		}
+	}
+
+	fh = kzalloc(sizeof(*fh), GFP_KERNEL);
+	if (!fh) {
+		err = -ENOMEM;
+		goto out;
+	}
+
+	fh->cur_norm = timblogiw_tvnorms;
+	timblogiw_querystd(file, fh, &std);
+	fh->cur_norm = timblogiw_get_norm(std);
+
+	INIT_LIST_HEAD(&fh->capture);
+	spin_lock_init(&fh->queue_lock);
+
+	dma_cap_zero(mask);
+	dma_cap_set(DMA_SLAVE, mask);
+	dma_cap_set(DMA_PRIVATE, mask);
+
+	/* find the DMA channel */
+	fh->chan = dma_request_channel(mask, timblogiw_dma_filter_fn,
+			(void *)(uintptr_t)lw->pdata.dma_channel);
+	if (!fh->chan) {
+		dev_err(&vdev->dev, "Failed to get DMA channel\n");
+		kfree(fh);
+		err = -ENODEV;
+		goto out;
+	}
+
+	file->private_data = fh;
+	videobuf_queue_dma_contig_init(&fh->vb_vidq,
+		&timblogiw_video_qops, lw->dev, &fh->queue_lock,
+		V4L2_BUF_TYPE_VIDEO_CAPTURE, V4L2_FIELD_NONE,
+		sizeof(struct timblogiw_buffer), fh, NULL);
+
+	lw->opened = true;
+out:
+	mutex_unlock(&lw->lock);
+
+	return err;
+}
+
+static int timblogiw_close(struct file *file)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct timblogiw *lw = video_get_drvdata(vdev);
+	struct timblogiw_fh *fh = file->private_data;
+
+	dev_dbg(&vdev->dev, "%s: Entry\n",  __func__);
+
+	videobuf_stop(&fh->vb_vidq);
+	videobuf_mmap_free(&fh->vb_vidq);
+
+	dma_release_channel(fh->chan);
+
+	kfree(fh);
+
+	mutex_lock(&lw->lock);
+	lw->opened = false;
+	mutex_unlock(&lw->lock);
+	return 0;
+}
+
+static ssize_t timblogiw_read(struct file *file, char __user *data,
+	size_t count, loff_t *ppos)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct timblogiw_fh *fh = file->private_data;
+
+	dev_dbg(&vdev->dev, "%s: entry\n",  __func__);
+
+	return videobuf_read_stream(&fh->vb_vidq, data, count, ppos, 0,
+		file->f_flags & O_NONBLOCK);
+}
+
+static unsigned int timblogiw_poll(struct file *file,
+	struct poll_table_struct *wait)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct timblogiw_fh *fh = file->private_data;
+
+	dev_dbg(&vdev->dev, "%s: entry\n",  __func__);
+
+	return videobuf_poll_stream(file, &fh->vb_vidq, wait);
+}
+
+static int timblogiw_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct timblogiw_fh *fh = file->private_data;
+
+	dev_dbg(&vdev->dev, "%s: entry\n", __func__);
+
+	return videobuf_mmap_mapper(&fh->vb_vidq, vma);
+}
+
+/* Platform device functions */
+
+static struct v4l2_ioctl_ops timblogiw_ioctl_ops = {
+	.vidioc_querycap		= timblogiw_querycap,
+	.vidioc_enum_fmt_vid_cap	= timblogiw_enum_fmt,
+	.vidioc_g_fmt_vid_cap		= timblogiw_g_fmt,
+	.vidioc_try_fmt_vid_cap		= timblogiw_try_fmt,
+	.vidioc_s_fmt_vid_cap		= timblogiw_s_fmt,
+	.vidioc_g_parm			= timblogiw_g_parm,
+	.vidioc_reqbufs			= timblogiw_reqbufs,
+	.vidioc_querybuf		= timblogiw_querybuf,
+	.vidioc_qbuf			= timblogiw_qbuf,
+	.vidioc_dqbuf			= timblogiw_dqbuf,
+	.vidioc_g_std			= timblogiw_g_std,
+	.vidioc_s_std			= timblogiw_s_std,
+	.vidioc_enum_input		= timblogiw_enuminput,
+	.vidioc_g_input			= timblogiw_g_input,
+	.vidioc_s_input			= timblogiw_s_input,
+	.vidioc_streamon		= timblogiw_streamon,
+	.vidioc_streamoff		= timblogiw_streamoff,
+	.vidioc_querystd		= timblogiw_querystd,
+	.vidioc_enum_framesizes		= timblogiw_enum_framesizes,
+};
+
+static struct v4l2_file_operations timblogiw_fops = {
+	.owner		= THIS_MODULE,
+	.open		= timblogiw_open,
+	.release	= timblogiw_close,
+	.unlocked_ioctl		= video_ioctl2, /* V4L2 ioctl handler */
+	.mmap		= timblogiw_mmap,
+	.read		= timblogiw_read,
+	.poll		= timblogiw_poll,
+};
+
+static struct video_device timblogiw_template = {
+	.name		= TIMBLOGIWIN_NAME,
+	.fops		= &timblogiw_fops,
+	.ioctl_ops	= &timblogiw_ioctl_ops,
+	.release	= video_device_release_empty,
+	.minor		= -1,
+	.tvnorms	= V4L2_STD_PAL | V4L2_STD_NTSC
+};
+
+static int timblogiw_probe(struct platform_device *pdev)
+{
+	int err;
+	struct timblogiw *lw = NULL;
+	struct timb_video_platform_data *pdata = pdev->dev.platform_data;
+
+	if (!pdata) {
+		dev_err(&pdev->dev, "No platform data\n");
+		err = -EINVAL;
+		goto err;
+	}
+
+	if (!pdata->encoder.module_name)
+		dev_info(&pdev->dev, "Running without decoder\n");
+
+	lw = devm_kzalloc(&pdev->dev, sizeof(*lw), GFP_KERNEL);
+	if (!lw) {
+		err = -ENOMEM;
+		goto err;
+	}
+
+	if (pdev->dev.parent)
+		lw->dev = pdev->dev.parent;
+	else
+		lw->dev = &pdev->dev;
+
+	memcpy(&lw->pdata, pdata, sizeof(lw->pdata));
+
+	mutex_init(&lw->lock);
+
+	lw->video_dev = timblogiw_template;
+
+	strlcpy(lw->v4l2_dev.name, DRIVER_NAME, sizeof(lw->v4l2_dev.name));
+	err = v4l2_device_register(NULL, &lw->v4l2_dev);
+	if (err)
+		goto err;
+
+	lw->video_dev.v4l2_dev = &lw->v4l2_dev;
+
+	platform_set_drvdata(pdev, lw);
+	video_set_drvdata(&lw->video_dev, lw);
+
+	err = video_register_device(&lw->video_dev, VFL_TYPE_GRABBER, 0);
+	if (err) {
+		dev_err(&pdev->dev, "Error reg video: %d\n", err);
+		goto err_request;
+	}
+
+	return 0;
+
+err_request:
+	v4l2_device_unregister(&lw->v4l2_dev);
+err:
+	dev_err(&pdev->dev, "Failed to register: %d\n", err);
+
+	return err;
+}
+
+static int timblogiw_remove(struct platform_device *pdev)
+{
+	struct timblogiw *lw = platform_get_drvdata(pdev);
+
+	video_unregister_device(&lw->video_dev);
+
+	v4l2_device_unregister(&lw->v4l2_dev);
+
+	return 0;
+}
+
+static struct platform_driver timblogiw_platform_driver = {
+	.driver = {
+		.name	= DRIVER_NAME,
+	},
+	.probe		= timblogiw_probe,
+	.remove		= timblogiw_remove,
+};
+
+module_platform_driver(timblogiw_platform_driver);
+
+MODULE_DESCRIPTION(TIMBLOGIWIN_NAME);
+MODULE_AUTHOR("Pelagicore AB <info@pelagicore.com>");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:"DRIVER_NAME);
diff --git a/drivers/media/platform/via-camera.c b/drivers/media/platform/via-camera.c
new file mode 100644
index 0000000..32e4ff4
--- /dev/null
+++ b/drivers/media/platform/via-camera.c
@@ -0,0 +1,1481 @@
+/*
+ * Driver for the VIA Chrome integrated camera controller.
+ *
+ * Copyright 2009,2010 Jonathan Corbet <corbet@lwn.net>
+ * Distributable under the terms of the GNU General Public License, version 2
+ *
+ * This work was supported by the One Laptop Per Child project
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/list.h>
+#include <linux/pci.h>
+#include <linux/gpio.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-image-sizes.h>
+#include <media/ov7670.h>
+#include <media/videobuf-dma-sg.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/pm_qos.h>
+#include <linux/via-core.h>
+#include <linux/via-gpio.h>
+#include <linux/via_i2c.h>
+#include <asm/olpc.h>
+
+#include "via-camera.h"
+
+MODULE_ALIAS("platform:viafb-camera");
+MODULE_AUTHOR("Jonathan Corbet <corbet@lwn.net>");
+MODULE_DESCRIPTION("VIA framebuffer-based camera controller driver");
+MODULE_LICENSE("GPL");
+
+static bool flip_image;
+module_param(flip_image, bool, 0444);
+MODULE_PARM_DESC(flip_image,
+		"If set, the sensor will be instructed to flip the image "
+		"vertically.");
+
+static bool override_serial;
+module_param(override_serial, bool, 0444);
+MODULE_PARM_DESC(override_serial,
+		"The camera driver will normally refuse to load if "
+		"the XO 1.5 serial port is enabled.  Set this option "
+		"to force-enable the camera.");
+
+/*
+ * The structure describing our camera.
+ */
+enum viacam_opstate { S_IDLE = 0, S_RUNNING = 1 };
+
+struct via_camera {
+	struct v4l2_device v4l2_dev;
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct video_device vdev;
+	struct v4l2_subdev *sensor;
+	struct platform_device *platdev;
+	struct viafb_dev *viadev;
+	struct mutex lock;
+	enum viacam_opstate opstate;
+	unsigned long flags;
+	struct pm_qos_request qos_request;
+	/*
+	 * GPIO info for power/reset management
+	 */
+	int power_gpio;
+	int reset_gpio;
+	/*
+	 * I/O memory stuff.
+	 */
+	void __iomem *mmio;	/* Where the registers live */
+	void __iomem *fbmem;	/* Frame buffer memory */
+	u32 fb_offset;		/* Reserved memory offset (FB) */
+	/*
+	 * Capture buffers and related.	 The controller supports
+	 * up to three, so that's what we have here.  These buffers
+	 * live in frame buffer memory, so we don't call them "DMA".
+	 */
+	unsigned int cb_offsets[3];	/* offsets into fb mem */
+	u8 __iomem *cb_addrs[3];		/* Kernel-space addresses */
+	int n_cap_bufs;			/* How many are we using? */
+	int next_buf;
+	struct videobuf_queue vb_queue;
+	struct list_head buffer_queue;	/* prot. by reg_lock */
+	/*
+	 * User tracking.
+	 */
+	int users;
+	struct file *owner;
+	/*
+	 * Video format information.  sensor_format is kept in a form
+	 * that we can use to pass to the sensor.  We always run the
+	 * sensor in VGA resolution, though, and let the controller
+	 * downscale things if need be.	 So we keep the "real*
+	 * dimensions separately.
+	 */
+	struct v4l2_pix_format sensor_format;
+	struct v4l2_pix_format user_format;
+	u32 mbus_code;
+};
+
+/*
+ * Yes, this is a hack, but there's only going to be one of these
+ * on any system we know of.
+ */
+static struct via_camera *via_cam_info;
+
+/*
+ * Flag values, manipulated with bitops
+ */
+#define CF_DMA_ACTIVE	 0	/* A frame is incoming */
+#define CF_CONFIG_NEEDED 1	/* Must configure hardware */
+
+
+/*
+ * Nasty ugly v4l2 boilerplate.
+ */
+#define sensor_call(cam, optype, func, args...) \
+	v4l2_subdev_call(cam->sensor, optype, func, ##args)
+
+/*
+ * Debugging and related.
+ */
+#define cam_err(cam, fmt, arg...) \
+	dev_err(&(cam)->platdev->dev, fmt, ##arg);
+#define cam_warn(cam, fmt, arg...) \
+	dev_warn(&(cam)->platdev->dev, fmt, ##arg);
+#define cam_dbg(cam, fmt, arg...) \
+	dev_dbg(&(cam)->platdev->dev, fmt, ##arg);
+
+/*
+ * Format handling.  This is ripped almost directly from Hans's changes
+ * to cafe_ccic.c.  It's a little unfortunate; until this change, we
+ * didn't need to know anything about the format except its byte depth;
+ * now this information must be managed at this level too.
+ */
+static struct via_format {
+	__u8 *desc;
+	__u32 pixelformat;
+	int bpp;   /* Bytes per pixel */
+	u32 mbus_code;
+} via_formats[] = {
+	{
+		.desc		= "YUYV 4:2:2",
+		.pixelformat	= V4L2_PIX_FMT_YUYV,
+		.mbus_code	= MEDIA_BUS_FMT_YUYV8_2X8,
+		.bpp		= 2,
+	},
+	/* RGB444 and Bayer should be doable, but have never been
+	   tested with this driver. RGB565 seems to work at the default
+	   resolution, but results in color corruption when being scaled by
+	   viacam_set_scaled(), and is disabled as a result. */
+};
+#define N_VIA_FMTS ARRAY_SIZE(via_formats)
+
+static struct via_format *via_find_format(u32 pixelformat)
+{
+	unsigned i;
+
+	for (i = 0; i < N_VIA_FMTS; i++)
+		if (via_formats[i].pixelformat == pixelformat)
+			return via_formats + i;
+	/* Not found? Then return the first format. */
+	return via_formats;
+}
+
+
+/*--------------------------------------------------------------------------*/
+/*
+ * Sensor power/reset management.  This piece is OLPC-specific for
+ * sure; other configurations will have things connected differently.
+ */
+static int via_sensor_power_setup(struct via_camera *cam)
+{
+	int ret;
+
+	cam->power_gpio = viafb_gpio_lookup("VGPIO3");
+	cam->reset_gpio = viafb_gpio_lookup("VGPIO2");
+	if (cam->power_gpio < 0 || cam->reset_gpio < 0) {
+		dev_err(&cam->platdev->dev, "Unable to find GPIO lines\n");
+		return -EINVAL;
+	}
+	ret = gpio_request(cam->power_gpio, "viafb-camera");
+	if (ret) {
+		dev_err(&cam->platdev->dev, "Unable to request power GPIO\n");
+		return ret;
+	}
+	ret = gpio_request(cam->reset_gpio, "viafb-camera");
+	if (ret) {
+		dev_err(&cam->platdev->dev, "Unable to request reset GPIO\n");
+		gpio_free(cam->power_gpio);
+		return ret;
+	}
+	gpio_direction_output(cam->power_gpio, 0);
+	gpio_direction_output(cam->reset_gpio, 0);
+	return 0;
+}
+
+/*
+ * Power up the sensor and perform the reset dance.
+ */
+static void via_sensor_power_up(struct via_camera *cam)
+{
+	gpio_set_value(cam->power_gpio, 1);
+	gpio_set_value(cam->reset_gpio, 0);
+	msleep(20);  /* Probably excessive */
+	gpio_set_value(cam->reset_gpio, 1);
+	msleep(20);
+}
+
+static void via_sensor_power_down(struct via_camera *cam)
+{
+	gpio_set_value(cam->power_gpio, 0);
+	gpio_set_value(cam->reset_gpio, 0);
+}
+
+
+static void via_sensor_power_release(struct via_camera *cam)
+{
+	via_sensor_power_down(cam);
+	gpio_free(cam->power_gpio);
+	gpio_free(cam->reset_gpio);
+}
+
+/* --------------------------------------------------------------------------*/
+/* Sensor ops */
+
+/*
+ * Manage the ov7670 "flip" bit, which needs special help.
+ */
+static int viacam_set_flip(struct via_camera *cam)
+{
+	struct v4l2_control ctrl;
+
+	memset(&ctrl, 0, sizeof(ctrl));
+	ctrl.id = V4L2_CID_VFLIP;
+	ctrl.value = flip_image;
+	return sensor_call(cam, core, s_ctrl, &ctrl);
+}
+
+/*
+ * Configure the sensor.  It's up to the caller to ensure
+ * that the camera is in the correct operating state.
+ */
+static int viacam_configure_sensor(struct via_camera *cam)
+{
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	int ret;
+
+	v4l2_fill_mbus_format(&format.format, &cam->sensor_format, cam->mbus_code);
+	ret = sensor_call(cam, core, init, 0);
+	if (ret == 0)
+		ret = sensor_call(cam, pad, set_fmt, NULL, &format);
+	/*
+	 * OV7670 does weird things if flip is set *before* format...
+	 */
+	if (ret == 0)
+		ret = viacam_set_flip(cam);
+	return ret;
+}
+
+
+
+/* --------------------------------------------------------------------------*/
+/*
+ * Some simple register accessors; they assume that the lock is held.
+ *
+ * Should we want to support the second capture engine, we could
+ * hide the register difference by adding 0x1000 to registers in the
+ * 0x300-350 range.
+ */
+static inline void viacam_write_reg(struct via_camera *cam,
+		int reg, int value)
+{
+	iowrite32(value, cam->mmio + reg);
+}
+
+static inline int viacam_read_reg(struct via_camera *cam, int reg)
+{
+	return ioread32(cam->mmio + reg);
+}
+
+static inline void viacam_write_reg_mask(struct via_camera *cam,
+		int reg, int value, int mask)
+{
+	int tmp = viacam_read_reg(cam, reg);
+
+	tmp = (tmp & ~mask) | (value & mask);
+	viacam_write_reg(cam, reg, tmp);
+}
+
+
+/* --------------------------------------------------------------------------*/
+/* Interrupt management and handling */
+
+static irqreturn_t viacam_quick_irq(int irq, void *data)
+{
+	struct via_camera *cam = data;
+	irqreturn_t ret = IRQ_NONE;
+	int icv;
+
+	/*
+	 * All we do here is to clear the interrupts and tell
+	 * the handler thread to wake up.
+	 */
+	spin_lock(&cam->viadev->reg_lock);
+	icv = viacam_read_reg(cam, VCR_INTCTRL);
+	if (icv & VCR_IC_EAV) {
+		icv |= VCR_IC_EAV|VCR_IC_EVBI|VCR_IC_FFULL;
+		viacam_write_reg(cam, VCR_INTCTRL, icv);
+		ret = IRQ_WAKE_THREAD;
+	}
+	spin_unlock(&cam->viadev->reg_lock);
+	return ret;
+}
+
+/*
+ * Find the next videobuf buffer which has somebody waiting on it.
+ */
+static struct videobuf_buffer *viacam_next_buffer(struct via_camera *cam)
+{
+	unsigned long flags;
+	struct videobuf_buffer *buf = NULL;
+
+	spin_lock_irqsave(&cam->viadev->reg_lock, flags);
+	if (cam->opstate != S_RUNNING)
+		goto out;
+	if (list_empty(&cam->buffer_queue))
+		goto out;
+	buf = list_entry(cam->buffer_queue.next, struct videobuf_buffer, queue);
+	if (!waitqueue_active(&buf->done)) {/* Nobody waiting */
+		buf = NULL;
+		goto out;
+	}
+	list_del(&buf->queue);
+	buf->state = VIDEOBUF_ACTIVE;
+out:
+	spin_unlock_irqrestore(&cam->viadev->reg_lock, flags);
+	return buf;
+}
+
+/*
+ * The threaded IRQ handler.
+ */
+static irqreturn_t viacam_irq(int irq, void *data)
+{
+	int bufn;
+	struct videobuf_buffer *vb;
+	struct via_camera *cam = data;
+	struct videobuf_dmabuf *vdma;
+
+	/*
+	 * If there is no place to put the data frame, don't bother
+	 * with anything else.
+	 */
+	vb = viacam_next_buffer(cam);
+	if (vb == NULL)
+		goto done;
+	/*
+	 * Figure out which buffer we just completed.
+	 */
+	bufn = (viacam_read_reg(cam, VCR_INTCTRL) & VCR_IC_ACTBUF) >> 3;
+	bufn -= 1;
+	if (bufn < 0)
+		bufn = cam->n_cap_bufs - 1;
+	/*
+	 * Copy over the data and let any waiters know.
+	 */
+	vdma = videobuf_to_dma(vb);
+	viafb_dma_copy_out_sg(cam->cb_offsets[bufn], vdma->sglist, vdma->sglen);
+	vb->state = VIDEOBUF_DONE;
+	vb->size = cam->user_format.sizeimage;
+	wake_up(&vb->done);
+done:
+	return IRQ_HANDLED;
+}
+
+
+/*
+ * These functions must mess around with the general interrupt
+ * control register, which is relevant to much more than just the
+ * camera.  Nothing else uses interrupts, though, as of this writing.
+ * Should that situation change, we'll have to improve support at
+ * the via-core level.
+ */
+static void viacam_int_enable(struct via_camera *cam)
+{
+	viacam_write_reg(cam, VCR_INTCTRL,
+			VCR_IC_INTEN|VCR_IC_EAV|VCR_IC_EVBI|VCR_IC_FFULL);
+	viafb_irq_enable(VDE_I_C0AVEN);
+}
+
+static void viacam_int_disable(struct via_camera *cam)
+{
+	viafb_irq_disable(VDE_I_C0AVEN);
+	viacam_write_reg(cam, VCR_INTCTRL, 0);
+}
+
+
+
+/* --------------------------------------------------------------------------*/
+/* Controller operations */
+
+/*
+ * Set up our capture buffers in framebuffer memory.
+ */
+static int viacam_ctlr_cbufs(struct via_camera *cam)
+{
+	int nbuf = cam->viadev->camera_fbmem_size/cam->sensor_format.sizeimage;
+	int i;
+	unsigned int offset;
+
+	/*
+	 * See how many buffers we can work with.
+	 */
+	if (nbuf >= 3) {
+		cam->n_cap_bufs = 3;
+		viacam_write_reg_mask(cam, VCR_CAPINTC, VCR_CI_3BUFS,
+				VCR_CI_3BUFS);
+	} else if (nbuf == 2) {
+		cam->n_cap_bufs = 2;
+		viacam_write_reg_mask(cam, VCR_CAPINTC, 0, VCR_CI_3BUFS);
+	} else {
+		cam_warn(cam, "Insufficient frame buffer memory\n");
+		return -ENOMEM;
+	}
+	/*
+	 * Set them up.
+	 */
+	offset = cam->fb_offset;
+	for (i = 0; i < cam->n_cap_bufs; i++) {
+		cam->cb_offsets[i] = offset;
+		cam->cb_addrs[i] = cam->fbmem + offset;
+		viacam_write_reg(cam, VCR_VBUF1 + i*4, offset & VCR_VBUF_MASK);
+		offset += cam->sensor_format.sizeimage;
+	}
+	return 0;
+}
+
+/*
+ * Set the scaling register for downscaling the image.
+ *
+ * This register works like this...  Vertical scaling is enabled
+ * by bit 26; if that bit is set, downscaling is controlled by the
+ * value in bits 16:25.	 Those bits are divided by 1024 to get
+ * the scaling factor; setting just bit 25 thus cuts the height
+ * in half.
+ *
+ * Horizontal scaling works about the same, but it's enabled by
+ * bit 11, with bits 0:10 giving the numerator of a fraction
+ * (over 2048) for the scaling value.
+ *
+ * This function is naive in that, if the user departs from
+ * the 3x4 VGA scaling factor, the image will distort.	We
+ * could work around that if it really seemed important.
+ */
+static void viacam_set_scale(struct via_camera *cam)
+{
+	unsigned int avscale;
+	int sf;
+
+	if (cam->user_format.width == VGA_WIDTH)
+		avscale = 0;
+	else {
+		sf = (cam->user_format.width*2048)/VGA_WIDTH;
+		avscale = VCR_AVS_HEN | sf;
+	}
+	if (cam->user_format.height < VGA_HEIGHT) {
+		sf = (1024*cam->user_format.height)/VGA_HEIGHT;
+		avscale |= VCR_AVS_VEN | (sf << 16);
+	}
+	viacam_write_reg(cam, VCR_AVSCALE, avscale);
+}
+
+
+/*
+ * Configure image-related information into the capture engine.
+ */
+static void viacam_ctlr_image(struct via_camera *cam)
+{
+	int cicreg;
+
+	/*
+	 * Disable clock before messing with stuff - from the via
+	 * sample driver.
+	 */
+	viacam_write_reg(cam, VCR_CAPINTC, ~(VCR_CI_ENABLE|VCR_CI_CLKEN));
+	/*
+	 * Set up the controller for VGA resolution, modulo magic
+	 * offsets from the via sample driver.
+	 */
+	viacam_write_reg(cam, VCR_HORRANGE, 0x06200120);
+	viacam_write_reg(cam, VCR_VERTRANGE, 0x01de0000);
+	viacam_set_scale(cam);
+	/*
+	 * Image size info.
+	 */
+	viacam_write_reg(cam, VCR_MAXDATA,
+			(cam->sensor_format.height << 16) |
+			(cam->sensor_format.bytesperline >> 3));
+	viacam_write_reg(cam, VCR_MAXVBI, 0);
+	viacam_write_reg(cam, VCR_VSTRIDE,
+			cam->user_format.bytesperline & VCR_VS_STRIDE);
+	/*
+	 * Set up the capture interface control register,
+	 * everything but the "go" bit.
+	 *
+	 * The FIFO threshold is a bit of a magic number; 8 is what
+	 * VIA's sample code uses.
+	 */
+	cicreg = VCR_CI_CLKEN |
+		0x08000000 |		/* FIFO threshold */
+		VCR_CI_FLDINV |		/* OLPC-specific? */
+		VCR_CI_VREFINV |	/* OLPC-specific? */
+		VCR_CI_DIBOTH |		/* Capture both fields */
+		VCR_CI_CCIR601_8;
+	if (cam->n_cap_bufs == 3)
+		cicreg |= VCR_CI_3BUFS;
+	/*
+	 * YUV formats need different byte swapping than RGB.
+	 */
+	if (cam->user_format.pixelformat == V4L2_PIX_FMT_YUYV)
+		cicreg |= VCR_CI_YUYV;
+	else
+		cicreg |= VCR_CI_UYVY;
+	viacam_write_reg(cam, VCR_CAPINTC, cicreg);
+}
+
+
+static int viacam_config_controller(struct via_camera *cam)
+{
+	int ret;
+	unsigned long flags;
+
+	spin_lock_irqsave(&cam->viadev->reg_lock, flags);
+	ret = viacam_ctlr_cbufs(cam);
+	if (!ret)
+		viacam_ctlr_image(cam);
+	spin_unlock_irqrestore(&cam->viadev->reg_lock, flags);
+	clear_bit(CF_CONFIG_NEEDED, &cam->flags);
+	return ret;
+}
+
+/*
+ * Make it start grabbing data.
+ */
+static void viacam_start_engine(struct via_camera *cam)
+{
+	spin_lock_irq(&cam->viadev->reg_lock);
+	cam->next_buf = 0;
+	viacam_write_reg_mask(cam, VCR_CAPINTC, VCR_CI_ENABLE, VCR_CI_ENABLE);
+	viacam_int_enable(cam);
+	(void) viacam_read_reg(cam, VCR_CAPINTC); /* Force post */
+	cam->opstate = S_RUNNING;
+	spin_unlock_irq(&cam->viadev->reg_lock);
+}
+
+
+static void viacam_stop_engine(struct via_camera *cam)
+{
+	spin_lock_irq(&cam->viadev->reg_lock);
+	viacam_int_disable(cam);
+	viacam_write_reg_mask(cam, VCR_CAPINTC, 0, VCR_CI_ENABLE);
+	(void) viacam_read_reg(cam, VCR_CAPINTC); /* Force post */
+	cam->opstate = S_IDLE;
+	spin_unlock_irq(&cam->viadev->reg_lock);
+}
+
+
+/* --------------------------------------------------------------------------*/
+/* Videobuf callback ops */
+
+/*
+ * buffer_setup.  The purpose of this one would appear to be to tell
+ * videobuf how big a single image is.	It's also evidently up to us
+ * to put some sort of limit on the maximum number of buffers allowed.
+ */
+static int viacam_vb_buf_setup(struct videobuf_queue *q,
+		unsigned int *count, unsigned int *size)
+{
+	struct via_camera *cam = q->priv_data;
+
+	*size = cam->user_format.sizeimage;
+	if (*count == 0 || *count > 6)	/* Arbitrary number */
+		*count = 6;
+	return 0;
+}
+
+/*
+ * Prepare a buffer.
+ */
+static int viacam_vb_buf_prepare(struct videobuf_queue *q,
+		struct videobuf_buffer *vb, enum v4l2_field field)
+{
+	struct via_camera *cam = q->priv_data;
+
+	vb->size = cam->user_format.sizeimage;
+	vb->width = cam->user_format.width; /* bytesperline???? */
+	vb->height = cam->user_format.height;
+	vb->field = field;
+	if (vb->state == VIDEOBUF_NEEDS_INIT) {
+		int ret = videobuf_iolock(q, vb, NULL);
+		if (ret)
+			return ret;
+	}
+	vb->state = VIDEOBUF_PREPARED;
+	return 0;
+}
+
+/*
+ * We've got a buffer to put data into.
+ *
+ * FIXME: check for a running engine and valid buffers?
+ */
+static void viacam_vb_buf_queue(struct videobuf_queue *q,
+		struct videobuf_buffer *vb)
+{
+	struct via_camera *cam = q->priv_data;
+
+	/*
+	 * Note that videobuf holds the lock when it calls
+	 * us, so we need not (indeed, cannot) take it here.
+	 */
+	vb->state = VIDEOBUF_QUEUED;
+	list_add_tail(&vb->queue, &cam->buffer_queue);
+}
+
+/*
+ * Free a buffer.
+ */
+static void viacam_vb_buf_release(struct videobuf_queue *q,
+		struct videobuf_buffer *vb)
+{
+	struct via_camera *cam = q->priv_data;
+
+	videobuf_dma_unmap(&cam->platdev->dev, videobuf_to_dma(vb));
+	videobuf_dma_free(videobuf_to_dma(vb));
+	vb->state = VIDEOBUF_NEEDS_INIT;
+}
+
+static const struct videobuf_queue_ops viacam_vb_ops = {
+	.buf_setup	= viacam_vb_buf_setup,
+	.buf_prepare	= viacam_vb_buf_prepare,
+	.buf_queue	= viacam_vb_buf_queue,
+	.buf_release	= viacam_vb_buf_release,
+};
+
+/* --------------------------------------------------------------------------*/
+/* File operations */
+
+static int viacam_open(struct file *filp)
+{
+	struct via_camera *cam = video_drvdata(filp);
+
+	filp->private_data = cam;
+	/*
+	 * Note the new user.  If this is the first one, we'll also
+	 * need to power up the sensor.
+	 */
+	mutex_lock(&cam->lock);
+	if (cam->users == 0) {
+		int ret = viafb_request_dma();
+
+		if (ret) {
+			mutex_unlock(&cam->lock);
+			return ret;
+		}
+		via_sensor_power_up(cam);
+		set_bit(CF_CONFIG_NEEDED, &cam->flags);
+		/*
+		 * Hook into videobuf.	Evidently this cannot fail.
+		 */
+		videobuf_queue_sg_init(&cam->vb_queue, &viacam_vb_ops,
+				&cam->platdev->dev, &cam->viadev->reg_lock,
+				V4L2_BUF_TYPE_VIDEO_CAPTURE, V4L2_FIELD_NONE,
+				sizeof(struct videobuf_buffer), cam, NULL);
+	}
+	(cam->users)++;
+	mutex_unlock(&cam->lock);
+	return 0;
+}
+
+static int viacam_release(struct file *filp)
+{
+	struct via_camera *cam = video_drvdata(filp);
+
+	mutex_lock(&cam->lock);
+	(cam->users)--;
+	/*
+	 * If the "owner" is closing, shut down any ongoing
+	 * operations.
+	 */
+	if (filp == cam->owner) {
+		videobuf_stop(&cam->vb_queue);
+		/*
+		 * We don't hold the spinlock here, but, if release()
+		 * is being called by the owner, nobody else will
+		 * be changing the state.  And an extra stop would
+		 * not hurt anyway.
+		 */
+		if (cam->opstate != S_IDLE)
+			viacam_stop_engine(cam);
+		cam->owner = NULL;
+	}
+	/*
+	 * Last one out needs to turn out the lights.
+	 */
+	if (cam->users == 0) {
+		videobuf_mmap_free(&cam->vb_queue);
+		via_sensor_power_down(cam);
+		viafb_release_dma();
+	}
+	mutex_unlock(&cam->lock);
+	return 0;
+}
+
+/*
+ * Read a frame from the device.
+ */
+static ssize_t viacam_read(struct file *filp, char __user *buffer,
+		size_t len, loff_t *pos)
+{
+	struct via_camera *cam = video_drvdata(filp);
+	int ret;
+
+	mutex_lock(&cam->lock);
+	/*
+	 * Enforce the V4l2 "only one owner gets to read data" rule.
+	 */
+	if (cam->owner && cam->owner != filp) {
+		ret = -EBUSY;
+		goto out_unlock;
+	}
+	cam->owner = filp;
+	/*
+	 * Do we need to configure the hardware?
+	 */
+	if (test_bit(CF_CONFIG_NEEDED, &cam->flags)) {
+		ret = viacam_configure_sensor(cam);
+		if (!ret)
+			ret = viacam_config_controller(cam);
+		if (ret)
+			goto out_unlock;
+	}
+	/*
+	 * Fire up the capture engine, then have videobuf do
+	 * the heavy lifting.  Someday it would be good to avoid
+	 * stopping and restarting the engine each time.
+	 */
+	INIT_LIST_HEAD(&cam->buffer_queue);
+	viacam_start_engine(cam);
+	ret = videobuf_read_stream(&cam->vb_queue, buffer, len, pos, 0,
+			filp->f_flags & O_NONBLOCK);
+	viacam_stop_engine(cam);
+	/* videobuf_stop() ?? */
+
+out_unlock:
+	mutex_unlock(&cam->lock);
+	return ret;
+}
+
+
+static unsigned int viacam_poll(struct file *filp, struct poll_table_struct *pt)
+{
+	struct via_camera *cam = video_drvdata(filp);
+
+	return videobuf_poll_stream(filp, &cam->vb_queue, pt);
+}
+
+
+static int viacam_mmap(struct file *filp, struct vm_area_struct *vma)
+{
+	struct via_camera *cam = video_drvdata(filp);
+
+	return videobuf_mmap_mapper(&cam->vb_queue, vma);
+}
+
+
+
+static const struct v4l2_file_operations viacam_fops = {
+	.owner		= THIS_MODULE,
+	.open		= viacam_open,
+	.release	= viacam_release,
+	.read		= viacam_read,
+	.poll		= viacam_poll,
+	.mmap		= viacam_mmap,
+	.unlocked_ioctl	= video_ioctl2,
+};
+
+/*----------------------------------------------------------------------------*/
+/*
+ * The long list of v4l2 ioctl ops
+ */
+
+/*
+ * Only one input.
+ */
+static int viacam_enum_input(struct file *filp, void *priv,
+		struct v4l2_input *input)
+{
+	if (input->index != 0)
+		return -EINVAL;
+
+	input->type = V4L2_INPUT_TYPE_CAMERA;
+	input->std = V4L2_STD_ALL; /* Not sure what should go here */
+	strcpy(input->name, "Camera");
+	return 0;
+}
+
+static int viacam_g_input(struct file *filp, void *priv, unsigned int *i)
+{
+	*i = 0;
+	return 0;
+}
+
+static int viacam_s_input(struct file *filp, void *priv, unsigned int i)
+{
+	if (i != 0)
+		return -EINVAL;
+	return 0;
+}
+
+static int viacam_s_std(struct file *filp, void *priv, v4l2_std_id std)
+{
+	return 0;
+}
+
+static int viacam_g_std(struct file *filp, void *priv, v4l2_std_id *std)
+{
+	*std = V4L2_STD_NTSC_M;
+	return 0;
+}
+
+/*
+ * Video format stuff.	Here is our default format until
+ * user space messes with things.
+ */
+static const struct v4l2_pix_format viacam_def_pix_format = {
+	.width		= VGA_WIDTH,
+	.height		= VGA_HEIGHT,
+	.pixelformat	= V4L2_PIX_FMT_YUYV,
+	.field		= V4L2_FIELD_NONE,
+	.bytesperline	= VGA_WIDTH * 2,
+	.sizeimage	= VGA_WIDTH * VGA_HEIGHT * 2,
+};
+
+static const u32 via_def_mbus_code = MEDIA_BUS_FMT_YUYV8_2X8;
+
+static int viacam_enum_fmt_vid_cap(struct file *filp, void *priv,
+		struct v4l2_fmtdesc *fmt)
+{
+	if (fmt->index >= N_VIA_FMTS)
+		return -EINVAL;
+	strlcpy(fmt->description, via_formats[fmt->index].desc,
+			sizeof(fmt->description));
+	fmt->pixelformat = via_formats[fmt->index].pixelformat;
+	return 0;
+}
+
+/*
+ * Figure out proper image dimensions, but always force the
+ * sensor to VGA.
+ */
+static void viacam_fmt_pre(struct v4l2_pix_format *userfmt,
+		struct v4l2_pix_format *sensorfmt)
+{
+	*sensorfmt = *userfmt;
+	if (userfmt->width < QCIF_WIDTH || userfmt->height < QCIF_HEIGHT) {
+		userfmt->width = QCIF_WIDTH;
+		userfmt->height = QCIF_HEIGHT;
+	}
+	if (userfmt->width > VGA_WIDTH || userfmt->height > VGA_HEIGHT) {
+		userfmt->width = VGA_WIDTH;
+		userfmt->height = VGA_HEIGHT;
+	}
+	sensorfmt->width = VGA_WIDTH;
+	sensorfmt->height = VGA_HEIGHT;
+}
+
+static void viacam_fmt_post(struct v4l2_pix_format *userfmt,
+		struct v4l2_pix_format *sensorfmt)
+{
+	struct via_format *f = via_find_format(userfmt->pixelformat);
+
+	sensorfmt->bytesperline = sensorfmt->width * f->bpp;
+	sensorfmt->sizeimage = sensorfmt->height * sensorfmt->bytesperline;
+	userfmt->pixelformat = sensorfmt->pixelformat;
+	userfmt->field = sensorfmt->field;
+	userfmt->bytesperline = 2 * userfmt->width;
+	userfmt->sizeimage = userfmt->bytesperline * userfmt->height;
+}
+
+
+/*
+ * The real work of figuring out a workable format.
+ */
+static int viacam_do_try_fmt(struct via_camera *cam,
+		struct v4l2_pix_format *upix, struct v4l2_pix_format *spix)
+{
+	int ret;
+	struct v4l2_subdev_pad_config pad_cfg;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+	};
+	struct via_format *f = via_find_format(upix->pixelformat);
+
+	upix->pixelformat = f->pixelformat;
+	viacam_fmt_pre(upix, spix);
+	v4l2_fill_mbus_format(&format.format, spix, f->mbus_code);
+	ret = sensor_call(cam, pad, set_fmt, &pad_cfg, &format);
+	v4l2_fill_pix_format(spix, &format.format);
+	viacam_fmt_post(upix, spix);
+	return ret;
+}
+
+
+
+static int viacam_try_fmt_vid_cap(struct file *filp, void *priv,
+		struct v4l2_format *fmt)
+{
+	struct via_camera *cam = priv;
+	struct v4l2_format sfmt;
+	int ret;
+
+	mutex_lock(&cam->lock);
+	ret = viacam_do_try_fmt(cam, &fmt->fmt.pix, &sfmt.fmt.pix);
+	mutex_unlock(&cam->lock);
+	return ret;
+}
+
+
+static int viacam_g_fmt_vid_cap(struct file *filp, void *priv,
+		struct v4l2_format *fmt)
+{
+	struct via_camera *cam = priv;
+
+	mutex_lock(&cam->lock);
+	fmt->fmt.pix = cam->user_format;
+	mutex_unlock(&cam->lock);
+	return 0;
+}
+
+static int viacam_s_fmt_vid_cap(struct file *filp, void *priv,
+		struct v4l2_format *fmt)
+{
+	struct via_camera *cam = priv;
+	int ret;
+	struct v4l2_format sfmt;
+	struct via_format *f = via_find_format(fmt->fmt.pix.pixelformat);
+
+	/*
+	 * Camera must be idle or we can't mess with the
+	 * video setup.
+	 */
+	mutex_lock(&cam->lock);
+	if (cam->opstate != S_IDLE) {
+		ret = -EBUSY;
+		goto out;
+	}
+	/*
+	 * Let the sensor code look over and tweak the
+	 * requested formatting.
+	 */
+	ret = viacam_do_try_fmt(cam, &fmt->fmt.pix, &sfmt.fmt.pix);
+	if (ret)
+		goto out;
+	/*
+	 * OK, let's commit to the new format.
+	 */
+	cam->user_format = fmt->fmt.pix;
+	cam->sensor_format = sfmt.fmt.pix;
+	cam->mbus_code = f->mbus_code;
+	ret = viacam_configure_sensor(cam);
+	if (!ret)
+		ret = viacam_config_controller(cam);
+out:
+	mutex_unlock(&cam->lock);
+	return ret;
+}
+
+static int viacam_querycap(struct file *filp, void *priv,
+		struct v4l2_capability *cap)
+{
+	strcpy(cap->driver, "via-camera");
+	strcpy(cap->card, "via-camera");
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE |
+		V4L2_CAP_READWRITE | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	return 0;
+}
+
+/*
+ * Streaming operations - pure videobuf stuff.
+ */
+static int viacam_reqbufs(struct file *filp, void *priv,
+		struct v4l2_requestbuffers *rb)
+{
+	struct via_camera *cam = priv;
+
+	return videobuf_reqbufs(&cam->vb_queue, rb);
+}
+
+static int viacam_querybuf(struct file *filp, void *priv,
+		struct v4l2_buffer *buf)
+{
+	struct via_camera *cam = priv;
+
+	return videobuf_querybuf(&cam->vb_queue, buf);
+}
+
+static int viacam_qbuf(struct file *filp, void *priv, struct v4l2_buffer *buf)
+{
+	struct via_camera *cam = priv;
+
+	return videobuf_qbuf(&cam->vb_queue, buf);
+}
+
+static int viacam_dqbuf(struct file *filp, void *priv, struct v4l2_buffer *buf)
+{
+	struct via_camera *cam = priv;
+
+	return videobuf_dqbuf(&cam->vb_queue, buf, filp->f_flags & O_NONBLOCK);
+}
+
+static int viacam_streamon(struct file *filp, void *priv, enum v4l2_buf_type t)
+{
+	struct via_camera *cam = priv;
+	int ret = 0;
+
+	if (t != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	mutex_lock(&cam->lock);
+	if (cam->opstate != S_IDLE) {
+		ret = -EBUSY;
+		goto out;
+	}
+	/*
+	 * Enforce the V4l2 "only one owner gets to read data" rule.
+	 */
+	if (cam->owner && cam->owner != filp) {
+		ret = -EBUSY;
+		goto out;
+	}
+	cam->owner = filp;
+	/*
+	 * Configure things if need be.
+	 */
+	if (test_bit(CF_CONFIG_NEEDED, &cam->flags)) {
+		ret = viacam_configure_sensor(cam);
+		if (ret)
+			goto out;
+		ret = viacam_config_controller(cam);
+		if (ret)
+			goto out;
+	}
+	/*
+	 * If the CPU goes into C3, the DMA transfer gets corrupted and
+	 * users start filing unsightly bug reports.  Put in a "latency"
+	 * requirement which will keep the CPU out of the deeper sleep
+	 * states.
+	 */
+	pm_qos_add_request(&cam->qos_request, PM_QOS_CPU_DMA_LATENCY, 50);
+	/*
+	 * Fire things up.
+	 */
+	INIT_LIST_HEAD(&cam->buffer_queue);
+	ret = videobuf_streamon(&cam->vb_queue);
+	if (!ret)
+		viacam_start_engine(cam);
+out:
+	mutex_unlock(&cam->lock);
+	return ret;
+}
+
+static int viacam_streamoff(struct file *filp, void *priv, enum v4l2_buf_type t)
+{
+	struct via_camera *cam = priv;
+	int ret;
+
+	if (t != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+	mutex_lock(&cam->lock);
+	if (cam->opstate != S_RUNNING) {
+		ret = -EINVAL;
+		goto out;
+	}
+	pm_qos_remove_request(&cam->qos_request);
+	viacam_stop_engine(cam);
+	/*
+	 * Videobuf will recycle all of the outstanding buffers, but
+	 * we should be sure we don't retain any references to
+	 * any of them.
+	 */
+	ret = videobuf_streamoff(&cam->vb_queue);
+	INIT_LIST_HEAD(&cam->buffer_queue);
+out:
+	mutex_unlock(&cam->lock);
+	return ret;
+}
+
+/* G/S_PARM */
+
+static int viacam_g_parm(struct file *filp, void *priv,
+		struct v4l2_streamparm *parm)
+{
+	struct via_camera *cam = priv;
+	int ret;
+
+	mutex_lock(&cam->lock);
+	ret = sensor_call(cam, video, g_parm, parm);
+	mutex_unlock(&cam->lock);
+	parm->parm.capture.readbuffers = cam->n_cap_bufs;
+	return ret;
+}
+
+static int viacam_s_parm(struct file *filp, void *priv,
+		struct v4l2_streamparm *parm)
+{
+	struct via_camera *cam = priv;
+	int ret;
+
+	mutex_lock(&cam->lock);
+	ret = sensor_call(cam, video, s_parm, parm);
+	mutex_unlock(&cam->lock);
+	parm->parm.capture.readbuffers = cam->n_cap_bufs;
+	return ret;
+}
+
+static int viacam_enum_framesizes(struct file *filp, void *priv,
+		struct v4l2_frmsizeenum *sizes)
+{
+	if (sizes->index != 0)
+		return -EINVAL;
+	sizes->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
+	sizes->stepwise.min_width = QCIF_WIDTH;
+	sizes->stepwise.min_height = QCIF_HEIGHT;
+	sizes->stepwise.max_width = VGA_WIDTH;
+	sizes->stepwise.max_height = VGA_HEIGHT;
+	sizes->stepwise.step_width = sizes->stepwise.step_height = 1;
+	return 0;
+}
+
+static int viacam_enum_frameintervals(struct file *filp, void *priv,
+		struct v4l2_frmivalenum *interval)
+{
+	struct via_camera *cam = priv;
+	struct v4l2_subdev_frame_interval_enum fie = {
+		.index = interval->index,
+		.code = cam->mbus_code,
+		.width = cam->sensor_format.width,
+		.height = cam->sensor_format.height,
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	int ret;
+
+	mutex_lock(&cam->lock);
+	ret = sensor_call(cam, pad, enum_frame_interval, NULL, &fie);
+	mutex_unlock(&cam->lock);
+	if (ret)
+		return ret;
+	interval->type = V4L2_FRMIVAL_TYPE_DISCRETE;
+	interval->discrete = fie.interval;
+	return 0;
+}
+
+
+
+static const struct v4l2_ioctl_ops viacam_ioctl_ops = {
+	.vidioc_enum_input	= viacam_enum_input,
+	.vidioc_g_input		= viacam_g_input,
+	.vidioc_s_input		= viacam_s_input,
+	.vidioc_s_std		= viacam_s_std,
+	.vidioc_g_std		= viacam_g_std,
+	.vidioc_enum_fmt_vid_cap = viacam_enum_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap = viacam_try_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap	= viacam_g_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap	= viacam_s_fmt_vid_cap,
+	.vidioc_querycap	= viacam_querycap,
+	.vidioc_reqbufs		= viacam_reqbufs,
+	.vidioc_querybuf	= viacam_querybuf,
+	.vidioc_qbuf		= viacam_qbuf,
+	.vidioc_dqbuf		= viacam_dqbuf,
+	.vidioc_streamon	= viacam_streamon,
+	.vidioc_streamoff	= viacam_streamoff,
+	.vidioc_g_parm		= viacam_g_parm,
+	.vidioc_s_parm		= viacam_s_parm,
+	.vidioc_enum_framesizes = viacam_enum_framesizes,
+	.vidioc_enum_frameintervals = viacam_enum_frameintervals,
+};
+
+/*----------------------------------------------------------------------------*/
+
+/*
+ * Power management.
+ */
+#ifdef CONFIG_PM
+
+static int viacam_suspend(void *priv)
+{
+	struct via_camera *cam = priv;
+	enum viacam_opstate state = cam->opstate;
+
+	if (cam->opstate != S_IDLE) {
+		viacam_stop_engine(cam);
+		cam->opstate = state; /* So resume restarts */
+	}
+
+	return 0;
+}
+
+static int viacam_resume(void *priv)
+{
+	struct via_camera *cam = priv;
+	int ret = 0;
+
+	/*
+	 * Get back to a reasonable operating state.
+	 */
+	via_write_reg_mask(VIASR, 0x78, 0, 0x80);
+	via_write_reg_mask(VIASR, 0x1e, 0xc0, 0xc0);
+	viacam_int_disable(cam);
+	set_bit(CF_CONFIG_NEEDED, &cam->flags);
+	/*
+	 * Make sure the sensor's power state is correct
+	 */
+	if (cam->users > 0)
+		via_sensor_power_up(cam);
+	else
+		via_sensor_power_down(cam);
+	/*
+	 * If it was operating, try to restart it.
+	 */
+	if (cam->opstate != S_IDLE) {
+		mutex_lock(&cam->lock);
+		ret = viacam_configure_sensor(cam);
+		if (!ret)
+			ret = viacam_config_controller(cam);
+		mutex_unlock(&cam->lock);
+		if (!ret)
+			viacam_start_engine(cam);
+	}
+
+	return ret;
+}
+
+static struct viafb_pm_hooks viacam_pm_hooks = {
+	.suspend = viacam_suspend,
+	.resume = viacam_resume
+};
+
+#endif /* CONFIG_PM */
+
+/*
+ * Setup stuff.
+ */
+
+static struct video_device viacam_v4l_template = {
+	.name		= "via-camera",
+	.minor		= -1,
+	.tvnorms	= V4L2_STD_NTSC_M,
+	.fops		= &viacam_fops,
+	.ioctl_ops	= &viacam_ioctl_ops,
+	.release	= video_device_release_empty, /* Check this */
+};
+
+/*
+ * The OLPC folks put the serial port on the same pin as
+ * the camera.	They also get grumpy if we break the
+ * serial port and keep them from using it.  So we have
+ * to check the serial enable bit and not step on it.
+ */
+#define VIACAM_SERIAL_DEVFN 0x88
+#define VIACAM_SERIAL_CREG 0x46
+#define VIACAM_SERIAL_BIT 0x40
+
+static bool viacam_serial_is_enabled(void)
+{
+	struct pci_bus *pbus = pci_find_bus(0, 0);
+	u8 cbyte;
+
+	if (!pbus)
+		return false;
+	pci_bus_read_config_byte(pbus, VIACAM_SERIAL_DEVFN,
+			VIACAM_SERIAL_CREG, &cbyte);
+	if ((cbyte & VIACAM_SERIAL_BIT) == 0)
+		return false; /* Not enabled */
+	if (!override_serial) {
+		printk(KERN_NOTICE "Via camera: serial port is enabled, " \
+				"refusing to load.\n");
+		printk(KERN_NOTICE "Specify override_serial=1 to force " \
+				"module loading.\n");
+		return true;
+	}
+	printk(KERN_NOTICE "Via camera: overriding serial port\n");
+	pci_bus_write_config_byte(pbus, VIACAM_SERIAL_DEVFN,
+			VIACAM_SERIAL_CREG, cbyte & ~VIACAM_SERIAL_BIT);
+	return false;
+}
+
+static struct ov7670_config sensor_cfg = {
+	/* The XO-1.5 (only known user) clocks the camera at 90MHz. */
+	.clock_speed = 90,
+};
+
+static int viacam_probe(struct platform_device *pdev)
+{
+	int ret;
+	struct i2c_adapter *sensor_adapter;
+	struct viafb_dev *viadev = pdev->dev.platform_data;
+	struct i2c_board_info ov7670_info = {
+		.type = "ov7670",
+		.addr = 0x42 >> 1,
+		.platform_data = &sensor_cfg,
+	};
+
+	/*
+	 * Note that there are actually two capture channels on
+	 * the device.	We only deal with one for now.	That
+	 * is encoded here; nothing else assumes it's dealing with
+	 * a unique capture device.
+	 */
+	struct via_camera *cam;
+
+	/*
+	 * Ensure that frame buffer memory has been set aside for
+	 * this purpose.  As an arbitrary limit, refuse to work
+	 * with less than two frames of VGA 16-bit data.
+	 *
+	 * If we ever support the second port, we'll need to set
+	 * aside more memory.
+	 */
+	if (viadev->camera_fbmem_size < (VGA_HEIGHT*VGA_WIDTH*4)) {
+		printk(KERN_ERR "viacam: insufficient FB memory reserved\n");
+		return -ENOMEM;
+	}
+	if (viadev->engine_mmio == NULL) {
+		printk(KERN_ERR "viacam: No I/O memory, so no pictures\n");
+		return -ENOMEM;
+	}
+
+	if (machine_is_olpc() && viacam_serial_is_enabled())
+		return -EBUSY;
+
+	/*
+	 * Basic structure initialization.
+	 */
+	cam = kzalloc (sizeof(struct via_camera), GFP_KERNEL);
+	if (cam == NULL)
+		return -ENOMEM;
+	via_cam_info = cam;
+	cam->platdev = pdev;
+	cam->viadev = viadev;
+	cam->users = 0;
+	cam->owner = NULL;
+	cam->opstate = S_IDLE;
+	cam->user_format = cam->sensor_format = viacam_def_pix_format;
+	mutex_init(&cam->lock);
+	INIT_LIST_HEAD(&cam->buffer_queue);
+	cam->mmio = viadev->engine_mmio;
+	cam->fbmem = viadev->fbmem;
+	cam->fb_offset = viadev->camera_fbmem_offset;
+	cam->flags = 1 << CF_CONFIG_NEEDED;
+	cam->mbus_code = via_def_mbus_code;
+	/*
+	 * Tell V4L that we exist.
+	 */
+	ret = v4l2_device_register(&pdev->dev, &cam->v4l2_dev);
+	if (ret) {
+		dev_err(&pdev->dev, "Unable to register v4l2 device\n");
+		goto out_free;
+	}
+	ret = v4l2_ctrl_handler_init(&cam->ctrl_handler, 10);
+	if (ret)
+		goto out_unregister;
+	cam->v4l2_dev.ctrl_handler = &cam->ctrl_handler;
+	/*
+	 * Convince the system that we can do DMA.
+	 */
+	pdev->dev.dma_mask = &viadev->pdev->dma_mask;
+	dma_set_mask(&pdev->dev, 0xffffffff);
+	/*
+	 * Fire up the capture port.  The write to 0x78 looks purely
+	 * OLPCish; any system will need to tweak 0x1e.
+	 */
+	via_write_reg_mask(VIASR, 0x78, 0, 0x80);
+	via_write_reg_mask(VIASR, 0x1e, 0xc0, 0xc0);
+	/*
+	 * Get the sensor powered up.
+	 */
+	ret = via_sensor_power_setup(cam);
+	if (ret)
+		goto out_ctrl_hdl_free;
+	via_sensor_power_up(cam);
+
+	/*
+	 * See if we can't find it on the bus.	The VIA_PORT_31 assumption
+	 * is OLPC-specific.  0x42 assumption is ov7670-specific.
+	 */
+	sensor_adapter = viafb_find_i2c_adapter(VIA_PORT_31);
+	cam->sensor = v4l2_i2c_new_subdev_board(&cam->v4l2_dev, sensor_adapter,
+			&ov7670_info, NULL);
+	if (cam->sensor == NULL) {
+		dev_err(&pdev->dev, "Unable to find the sensor!\n");
+		ret = -ENODEV;
+		goto out_power_down;
+	}
+	/*
+	 * Get the IRQ.
+	 */
+	viacam_int_disable(cam);
+	ret = request_threaded_irq(viadev->pdev->irq, viacam_quick_irq,
+			viacam_irq, IRQF_SHARED, "via-camera", cam);
+	if (ret)
+		goto out_power_down;
+	/*
+	 * Tell V4l2 that we exist.
+	 */
+	cam->vdev = viacam_v4l_template;
+	cam->vdev.v4l2_dev = &cam->v4l2_dev;
+	ret = video_register_device(&cam->vdev, VFL_TYPE_GRABBER, -1);
+	if (ret)
+		goto out_irq;
+	video_set_drvdata(&cam->vdev, cam);
+
+#ifdef CONFIG_PM
+	/*
+	 * Hook into PM events
+	 */
+	viacam_pm_hooks.private = cam;
+	viafb_pm_register(&viacam_pm_hooks);
+#endif
+
+	/* Power the sensor down until somebody opens the device */
+	via_sensor_power_down(cam);
+	return 0;
+
+out_irq:
+	free_irq(viadev->pdev->irq, cam);
+out_power_down:
+	via_sensor_power_release(cam);
+out_ctrl_hdl_free:
+	v4l2_ctrl_handler_free(&cam->ctrl_handler);
+out_unregister:
+	v4l2_device_unregister(&cam->v4l2_dev);
+out_free:
+	kfree(cam);
+	return ret;
+}
+
+static int viacam_remove(struct platform_device *pdev)
+{
+	struct via_camera *cam = via_cam_info;
+	struct viafb_dev *viadev = pdev->dev.platform_data;
+
+	video_unregister_device(&cam->vdev);
+	v4l2_device_unregister(&cam->v4l2_dev);
+	free_irq(viadev->pdev->irq, cam);
+	via_sensor_power_release(cam);
+	v4l2_ctrl_handler_free(&cam->ctrl_handler);
+	kfree(cam);
+	via_cam_info = NULL;
+	return 0;
+}
+
+static struct platform_driver viacam_driver = {
+	.driver = {
+		.name = "viafb-camera",
+	},
+	.probe = viacam_probe,
+	.remove = viacam_remove,
+};
+
+module_platform_driver(viacam_driver);
diff --git a/drivers/media/platform/via-camera.h b/drivers/media/platform/via-camera.h
new file mode 100644
index 0000000..b12a4b3
--- /dev/null
+++ b/drivers/media/platform/via-camera.h
@@ -0,0 +1,93 @@
+/*
+ * VIA Camera register definitions.
+ */
+#define VCR_INTCTRL	0x300	/* Capture interrupt control */
+#define   VCR_IC_EAV	  0x0001   /* End of active video status */
+#define	  VCR_IC_EVBI	  0x0002   /* End of VBI status */
+#define   VCR_IC_FBOTFLD  0x0004   /* "flipping" Bottom field is active */
+#define   VCR_IC_ACTBUF	  0x0018   /* Active video buffer  */
+#define   VCR_IC_VSYNC	  0x0020   /* 0 = VB, 1 = active video */
+#define   VCR_IC_BOTFLD	  0x0040   /* Bottom field is active */
+#define   VCR_IC_FFULL	  0x0080   /* FIFO full */
+#define   VCR_IC_INTEN	  0x0100   /* End of active video int. enable */
+#define   VCR_IC_VBIINT	  0x0200   /* End of VBI int enable */
+#define   VCR_IC_VBIBUF	  0x0400   /* Current VBI buffer */
+
+#define VCR_TSC		0x308	/* Transport stream control */
+#define VCR_TSC_ENABLE    0x000001   /* Transport stream input enable */
+#define VCR_TSC_DROPERR   0x000002   /* Drop error packets */
+#define VCR_TSC_METHOD    0x00000c   /* DMA method (non-functional) */
+#define VCR_TSC_COUNT     0x07fff0   /* KByte or packet count */
+#define VCR_TSC_CBMODE	  0x080000   /* Change buffer by byte count */
+#define VCR_TSC_PSSIG	  0x100000   /* Packet starting signal disable */
+#define VCR_TSC_BE	  0x200000   /* MSB first (serial mode) */
+#define VCR_TSC_SERIAL	  0x400000   /* Serial input (0 = parallel) */
+
+#define VCR_CAPINTC	0x310	/* Capture interface control */
+#define   VCR_CI_ENABLE   0x00000001  /* Capture enable */
+#define   VCR_CI_BSS	  0x00000002  /* WTF "bit stream selection" */
+#define   VCR_CI_3BUFS	  0x00000004  /* 1 = 3 buffers, 0 = 2 buffers */
+#define   VCR_CI_VIPEN	  0x00000008  /* VIP enable */
+#define   VCR_CI_CCIR601_8  0	        /* CCIR601 input stream, 8 bit */
+#define   VCR_CI_CCIR656_8  0x00000010  /* ... CCIR656, 8 bit */
+#define   VCR_CI_CCIR601_16 0x00000020  /* ... CCIR601, 16 bit */
+#define   VCR_CI_CCIR656_16 0x00000030  /* ... CCIR656, 16 bit */
+#define   VCR_CI_HDMODE   0x00000040  /* CCIR656-16 hdr decode mode; 1=16b */
+#define   VCR_CI_BSWAP    0x00000080  /* Swap bytes (16-bit) */
+#define   VCR_CI_YUYV	  0	      /* Byte order 0123 */
+#define   VCR_CI_UYVY	  0x00000100  /* Byte order 1032 */
+#define   VCR_CI_YVYU	  0x00000200  /* Byte order 0321 */
+#define   VCR_CI_VYUY	  0x00000300  /* Byte order 3012 */
+#define   VCR_CI_VIPTYPE  0x00000400  /* VIP type */
+#define   VCR_CI_IFSEN    0x00000800  /* Input field signal enable */
+#define   VCR_CI_DIODD	  0	      /* De-interlace odd, 30fps */
+#define   VCR_CI_DIEVEN   0x00001000  /*    ...even field, 30fps */
+#define   VCR_CI_DIBOTH   0x00002000  /*    ...both fields, 60fps */
+#define   VCR_CI_DIBOTH30 0x00003000  /*    ...both fields, 30fps interlace */
+#define   VCR_CI_CONVTYPE 0x00004000  /* 4:2:2 to 4:4:4; 1 = interpolate */
+#define   VCR_CI_CFC	  0x00008000  /* Capture flipping control */
+#define   VCR_CI_FILTER   0x00070000  /* Horiz filter mode select
+					 000 = none
+					 001 = 2 tap
+					 010 = 3 tap
+					 011 = 4 tap
+					 100 = 5 tap */
+#define   VCR_CI_CLKINV   0x00080000  /* Input CLK inverted */
+#define   VCR_CI_VREFINV  0x00100000  /* VREF inverted */
+#define   VCR_CI_HREFINV  0x00200000  /* HREF inverted */
+#define   VCR_CI_FLDINV   0x00400000  /* Field inverted */
+#define   VCR_CI_CLKPIN	  0x00800000  /* Capture clock pin */
+#define   VCR_CI_THRESH   0x0f000000  /* Capture fifo threshold */
+#define   VCR_CI_HRLE     0x10000000  /* Positive edge of HREF */
+#define   VCR_CI_VRLE     0x20000000  /* Positive edge of VREF */
+#define   VCR_CI_OFLDINV  0x40000000  /* Field output inverted */
+#define   VCR_CI_CLKEN    0x80000000  /* Capture clock enable */
+
+#define VCR_HORRANGE	0x314	/* Active video horizontal range */
+#define VCR_VERTRANGE	0x318	/* Active video vertical range */
+#define VCR_AVSCALE	0x31c	/* Active video scaling control */
+#define   VCR_AVS_HEN	  0x00000800   /* Horizontal scale enable */
+#define   VCR_AVS_VEN	  0x04000000   /* Vertical enable */
+#define VCR_VBIHOR	0x320	/* VBI Data horizontal range */
+#define VCR_VBIVERT	0x324	/* VBI data vertical range */
+#define VCR_VBIBUF1	0x328	/* First VBI buffer */
+#define VCR_VBISTRIDE	0x32c	/* VBI stride */
+#define VCR_ANCDATACNT	0x330	/* Ancillary data count setting */
+#define VCR_MAXDATA	0x334	/* Active data count of active video */
+#define VCR_MAXVBI	0x338	/* Maximum data count of VBI */
+#define VCR_CAPDATA	0x33c	/* Capture data count */
+#define VCR_VBUF1	0x340	/* First video buffer */
+#define VCR_VBUF2	0x344	/* Second video buffer */
+#define VCR_VBUF3	0x348	/* Third video buffer */
+#define VCR_VBUF_MASK	0x1ffffff0	/* Bits 28:4 */
+#define VCR_VBIBUF2	0x34c	/* Second VBI buffer */
+#define VCR_VSTRIDE	0x350	/* Stride of video + coring control */
+#define   VCR_VS_STRIDE_SHIFT 4
+#define   VCR_VS_STRIDE   0x00001ff0  /* Stride (8-byte units) */
+#define   VCR_VS_CCD	  0x007f0000  /* Coring compare data */
+#define   VCR_VS_COREEN   0x00800000  /* Coring enable */
+#define VCR_TS0ERR	0x354	/* TS buffer 0 error indicator */
+#define VCR_TS1ERR	0x358	/* TS buffer 0 error indicator */
+#define VCR_TS2ERR	0x35c	/* TS buffer 0 error indicator */
+
+/* Add 0x1000 for the second capture engine registers */
diff --git a/drivers/media/platform/vim2m.c b/drivers/media/platform/vim2m.c
new file mode 100644
index 0000000..e18fb9f
--- /dev/null
+++ b/drivers/media/platform/vim2m.c
@@ -0,0 +1,1086 @@
+/*
+ * A virtual v4l2-mem2mem example device.
+ *
+ * This is a virtual device driver for testing mem-to-mem videobuf framework.
+ * It simulates a device that uses memory buffers for both source and
+ * destination, processes the data and issues an "irq" (simulated by a timer).
+ * The device is capable of multi-instance, multi-buffer-per-transaction
+ * operation (via the mem2mem framework).
+ *
+ * Copyright (c) 2009-2010 Samsung Electronics Co., Ltd.
+ * Pawel Osciak, <pawel@osciak.com>
+ * Marek Szyprowski, <m.szyprowski@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation; either version 2 of the
+ * License, or (at your option) any later version
+ */
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/timer.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+
+#include <linux/platform_device.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
+#include <media/videobuf2-vmalloc.h>
+
+MODULE_DESCRIPTION("Virtual device for mem2mem framework testing");
+MODULE_AUTHOR("Pawel Osciak, <pawel@osciak.com>");
+MODULE_LICENSE("GPL");
+MODULE_VERSION("0.1.1");
+MODULE_ALIAS("mem2mem_testdev");
+
+static unsigned debug;
+module_param(debug, uint, 0644);
+MODULE_PARM_DESC(debug, "activates debug info");
+
+#define MIN_W 32
+#define MIN_H 32
+#define MAX_W 640
+#define MAX_H 480
+#define DIM_ALIGN_MASK 7 /* 8-byte alignment for line length */
+
+/* Flags that indicate a format can be used for capture/output */
+#define MEM2MEM_CAPTURE	(1 << 0)
+#define MEM2MEM_OUTPUT	(1 << 1)
+
+#define MEM2MEM_NAME		"vim2m"
+
+/* Per queue */
+#define MEM2MEM_DEF_NUM_BUFS	VIDEO_MAX_FRAME
+/* In bytes, per queue */
+#define MEM2MEM_VID_MEM_LIMIT	(16 * 1024 * 1024)
+
+/* Default transaction time in msec */
+#define MEM2MEM_DEF_TRANSTIME	40
+#define MEM2MEM_COLOR_STEP	(0xff >> 4)
+#define MEM2MEM_NUM_TILES	8
+
+/* Flags that indicate processing mode */
+#define MEM2MEM_HFLIP	(1 << 0)
+#define MEM2MEM_VFLIP	(1 << 1)
+
+#define dprintk(dev, fmt, arg...) \
+	v4l2_dbg(1, debug, &dev->v4l2_dev, "%s: " fmt, __func__, ## arg)
+
+
+static void vim2m_dev_release(struct device *dev)
+{}
+
+static struct platform_device vim2m_pdev = {
+	.name		= MEM2MEM_NAME,
+	.dev.release	= vim2m_dev_release,
+};
+
+struct vim2m_fmt {
+	u32	fourcc;
+	int	depth;
+	/* Types the format can be used for */
+	u32	types;
+};
+
+static struct vim2m_fmt formats[] = {
+	{
+		.fourcc	= V4L2_PIX_FMT_RGB565X, /* rrrrrggg gggbbbbb */
+		.depth	= 16,
+		/* Both capture and output format */
+		.types	= MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
+	},
+	{
+		.fourcc	= V4L2_PIX_FMT_YUYV,
+		.depth	= 16,
+		/* Output-only format */
+		.types	= MEM2MEM_OUTPUT,
+	},
+};
+
+#define NUM_FORMATS ARRAY_SIZE(formats)
+
+/* Per-queue, driver-specific private data */
+struct vim2m_q_data {
+	unsigned int		width;
+	unsigned int		height;
+	unsigned int		sizeimage;
+	unsigned int		sequence;
+	struct vim2m_fmt	*fmt;
+};
+
+enum {
+	V4L2_M2M_SRC = 0,
+	V4L2_M2M_DST = 1,
+};
+
+#define V4L2_CID_TRANS_TIME_MSEC	(V4L2_CID_USER_BASE + 0x1000)
+#define V4L2_CID_TRANS_NUM_BUFS		(V4L2_CID_USER_BASE + 0x1001)
+
+static struct vim2m_fmt *find_format(struct v4l2_format *f)
+{
+	struct vim2m_fmt *fmt;
+	unsigned int k;
+
+	for (k = 0; k < NUM_FORMATS; k++) {
+		fmt = &formats[k];
+		if (fmt->fourcc == f->fmt.pix.pixelformat)
+			break;
+	}
+
+	if (k == NUM_FORMATS)
+		return NULL;
+
+	return &formats[k];
+}
+
+struct vim2m_dev {
+	struct v4l2_device	v4l2_dev;
+	struct video_device	vfd;
+
+	atomic_t		num_inst;
+	struct mutex		dev_mutex;
+	spinlock_t		irqlock;
+
+	struct timer_list	timer;
+
+	struct v4l2_m2m_dev	*m2m_dev;
+};
+
+struct vim2m_ctx {
+	struct v4l2_fh		fh;
+	struct vim2m_dev	*dev;
+
+	struct v4l2_ctrl_handler hdl;
+
+	/* Processed buffers in this transaction */
+	u8			num_processed;
+
+	/* Transaction length (i.e. how many buffers per transaction) */
+	u32			translen;
+	/* Transaction time (i.e. simulated processing time) in milliseconds */
+	u32			transtime;
+
+	/* Abort requested by m2m */
+	int			aborting;
+
+	/* Processing mode */
+	int			mode;
+
+	enum v4l2_colorspace	colorspace;
+
+	/* Source and destination queue data */
+	struct vim2m_q_data   q_data[2];
+};
+
+static inline struct vim2m_ctx *file2ctx(struct file *file)
+{
+	return container_of(file->private_data, struct vim2m_ctx, fh);
+}
+
+static struct vim2m_q_data *get_q_data(struct vim2m_ctx *ctx,
+					 enum v4l2_buf_type type)
+{
+	switch (type) {
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+		return &ctx->q_data[V4L2_M2M_SRC];
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		return &ctx->q_data[V4L2_M2M_DST];
+	default:
+		BUG();
+	}
+	return NULL;
+}
+
+
+static int device_process(struct vim2m_ctx *ctx,
+			  struct vb2_v4l2_buffer *in_vb,
+			  struct vb2_v4l2_buffer *out_vb)
+{
+	struct vim2m_dev *dev = ctx->dev;
+	struct vim2m_q_data *q_data;
+	u8 *p_in, *p_out;
+	int x, y, t, w;
+	int tile_w, bytes_left;
+	int width, height, bytesperline;
+
+	q_data = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
+
+	width	= q_data->width;
+	height	= q_data->height;
+	bytesperline	= (q_data->width * q_data->fmt->depth) >> 3;
+
+	p_in = vb2_plane_vaddr(&in_vb->vb2_buf, 0);
+	p_out = vb2_plane_vaddr(&out_vb->vb2_buf, 0);
+	if (!p_in || !p_out) {
+		v4l2_err(&dev->v4l2_dev,
+			 "Acquiring kernel pointers to buffers failed\n");
+		return -EFAULT;
+	}
+
+	if (vb2_plane_size(&in_vb->vb2_buf, 0) >
+			vb2_plane_size(&out_vb->vb2_buf, 0)) {
+		v4l2_err(&dev->v4l2_dev, "Output buffer is too small\n");
+		return -EINVAL;
+	}
+
+	tile_w = (width * (q_data[V4L2_M2M_DST].fmt->depth >> 3))
+		/ MEM2MEM_NUM_TILES;
+	bytes_left = bytesperline - tile_w * MEM2MEM_NUM_TILES;
+	w = 0;
+
+	out_vb->sequence =
+		get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE)->sequence++;
+	in_vb->sequence = q_data->sequence++;
+	out_vb->timestamp = in_vb->timestamp;
+
+	if (in_vb->flags & V4L2_BUF_FLAG_TIMECODE)
+		out_vb->timecode = in_vb->timecode;
+	out_vb->field = in_vb->field;
+	out_vb->flags = in_vb->flags &
+		(V4L2_BUF_FLAG_TIMECODE |
+		 V4L2_BUF_FLAG_KEYFRAME |
+		 V4L2_BUF_FLAG_PFRAME |
+		 V4L2_BUF_FLAG_BFRAME |
+		 V4L2_BUF_FLAG_TSTAMP_SRC_MASK);
+
+	switch (ctx->mode) {
+	case MEM2MEM_HFLIP | MEM2MEM_VFLIP:
+		p_out += bytesperline * height - bytes_left;
+		for (y = 0; y < height; ++y) {
+			for (t = 0; t < MEM2MEM_NUM_TILES; ++t) {
+				if (w & 0x1) {
+					for (x = 0; x < tile_w; ++x)
+						*--p_out = *p_in++ +
+							MEM2MEM_COLOR_STEP;
+				} else {
+					for (x = 0; x < tile_w; ++x)
+						*--p_out = *p_in++ -
+							MEM2MEM_COLOR_STEP;
+				}
+				++w;
+			}
+			p_in += bytes_left;
+			p_out -= bytes_left;
+		}
+		break;
+
+	case MEM2MEM_HFLIP:
+		for (y = 0; y < height; ++y) {
+			p_out += MEM2MEM_NUM_TILES * tile_w;
+			for (t = 0; t < MEM2MEM_NUM_TILES; ++t) {
+				if (w & 0x01) {
+					for (x = 0; x < tile_w; ++x)
+						*--p_out = *p_in++ +
+							MEM2MEM_COLOR_STEP;
+				} else {
+					for (x = 0; x < tile_w; ++x)
+						*--p_out = *p_in++ -
+							MEM2MEM_COLOR_STEP;
+				}
+				++w;
+			}
+			p_in += bytes_left;
+			p_out += bytesperline;
+		}
+		break;
+
+	case MEM2MEM_VFLIP:
+		p_out += bytesperline * (height - 1);
+		for (y = 0; y < height; ++y) {
+			for (t = 0; t < MEM2MEM_NUM_TILES; ++t) {
+				if (w & 0x1) {
+					for (x = 0; x < tile_w; ++x)
+						*p_out++ = *p_in++ +
+							MEM2MEM_COLOR_STEP;
+				} else {
+					for (x = 0; x < tile_w; ++x)
+						*p_out++ = *p_in++ -
+							MEM2MEM_COLOR_STEP;
+				}
+				++w;
+			}
+			p_in += bytes_left;
+			p_out += bytes_left - 2 * bytesperline;
+		}
+		break;
+
+	default:
+		for (y = 0; y < height; ++y) {
+			for (t = 0; t < MEM2MEM_NUM_TILES; ++t) {
+				if (w & 0x1) {
+					for (x = 0; x < tile_w; ++x)
+						*p_out++ = *p_in++ +
+							MEM2MEM_COLOR_STEP;
+				} else {
+					for (x = 0; x < tile_w; ++x)
+						*p_out++ = *p_in++ -
+							MEM2MEM_COLOR_STEP;
+				}
+				++w;
+			}
+			p_in += bytes_left;
+			p_out += bytes_left;
+		}
+	}
+
+	return 0;
+}
+
+static void schedule_irq(struct vim2m_dev *dev, int msec_timeout)
+{
+	dprintk(dev, "Scheduling a simulated irq\n");
+	mod_timer(&dev->timer, jiffies + msecs_to_jiffies(msec_timeout));
+}
+
+/*
+ * mem2mem callbacks
+ */
+
+/**
+ * job_ready() - check whether an instance is ready to be scheduled to run
+ */
+static int job_ready(void *priv)
+{
+	struct vim2m_ctx *ctx = priv;
+
+	if (v4l2_m2m_num_src_bufs_ready(ctx->fh.m2m_ctx) < ctx->translen
+	    || v4l2_m2m_num_dst_bufs_ready(ctx->fh.m2m_ctx) < ctx->translen) {
+		dprintk(ctx->dev, "Not enough buffers available\n");
+		return 0;
+	}
+
+	return 1;
+}
+
+static void job_abort(void *priv)
+{
+	struct vim2m_ctx *ctx = priv;
+
+	/* Will cancel the transaction in the next interrupt handler */
+	ctx->aborting = 1;
+}
+
+/* device_run() - prepares and starts the device
+ *
+ * This simulates all the immediate preparations required before starting
+ * a device. This will be called by the framework when it decides to schedule
+ * a particular instance.
+ */
+static void device_run(void *priv)
+{
+	struct vim2m_ctx *ctx = priv;
+	struct vim2m_dev *dev = ctx->dev;
+	struct vb2_v4l2_buffer *src_buf, *dst_buf;
+
+	src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+	dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
+
+	device_process(ctx, src_buf, dst_buf);
+
+	/* Run a timer, which simulates a hardware irq  */
+	schedule_irq(dev, ctx->transtime);
+}
+
+static void device_isr(unsigned long priv)
+{
+	struct vim2m_dev *vim2m_dev = (struct vim2m_dev *)priv;
+	struct vim2m_ctx *curr_ctx;
+	struct vb2_v4l2_buffer *src_vb, *dst_vb;
+	unsigned long flags;
+
+	curr_ctx = v4l2_m2m_get_curr_priv(vim2m_dev->m2m_dev);
+
+	if (NULL == curr_ctx) {
+		pr_err("Instance released before the end of transaction\n");
+		return;
+	}
+
+	src_vb = v4l2_m2m_src_buf_remove(curr_ctx->fh.m2m_ctx);
+	dst_vb = v4l2_m2m_dst_buf_remove(curr_ctx->fh.m2m_ctx);
+
+	curr_ctx->num_processed++;
+
+	spin_lock_irqsave(&vim2m_dev->irqlock, flags);
+	v4l2_m2m_buf_done(src_vb, VB2_BUF_STATE_DONE);
+	v4l2_m2m_buf_done(dst_vb, VB2_BUF_STATE_DONE);
+	spin_unlock_irqrestore(&vim2m_dev->irqlock, flags);
+
+	if (curr_ctx->num_processed == curr_ctx->translen
+	    || curr_ctx->aborting) {
+		dprintk(curr_ctx->dev, "Finishing transaction\n");
+		curr_ctx->num_processed = 0;
+		v4l2_m2m_job_finish(vim2m_dev->m2m_dev, curr_ctx->fh.m2m_ctx);
+	} else {
+		device_run(curr_ctx);
+	}
+}
+
+/*
+ * video ioctls
+ */
+static int vidioc_querycap(struct file *file, void *priv,
+			   struct v4l2_capability *cap)
+{
+	strncpy(cap->driver, MEM2MEM_NAME, sizeof(cap->driver) - 1);
+	strncpy(cap->card, MEM2MEM_NAME, sizeof(cap->card) - 1);
+	snprintf(cap->bus_info, sizeof(cap->bus_info),
+			"platform:%s", MEM2MEM_NAME);
+	cap->device_caps = V4L2_CAP_VIDEO_M2M | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	return 0;
+}
+
+static int enum_fmt(struct v4l2_fmtdesc *f, u32 type)
+{
+	int i, num;
+	struct vim2m_fmt *fmt;
+
+	num = 0;
+
+	for (i = 0; i < NUM_FORMATS; ++i) {
+		if (formats[i].types & type) {
+			/* index-th format of type type found ? */
+			if (num == f->index)
+				break;
+			/* Correct type but haven't reached our index yet,
+			 * just increment per-type index */
+			++num;
+		}
+	}
+
+	if (i < NUM_FORMATS) {
+		/* Format found */
+		fmt = &formats[i];
+		f->pixelformat = fmt->fourcc;
+		return 0;
+	}
+
+	/* Format not found */
+	return -EINVAL;
+}
+
+static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
+				   struct v4l2_fmtdesc *f)
+{
+	return enum_fmt(f, MEM2MEM_CAPTURE);
+}
+
+static int vidioc_enum_fmt_vid_out(struct file *file, void *priv,
+				   struct v4l2_fmtdesc *f)
+{
+	return enum_fmt(f, MEM2MEM_OUTPUT);
+}
+
+static int vidioc_g_fmt(struct vim2m_ctx *ctx, struct v4l2_format *f)
+{
+	struct vb2_queue *vq;
+	struct vim2m_q_data *q_data;
+
+	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
+	if (!vq)
+		return -EINVAL;
+
+	q_data = get_q_data(ctx, f->type);
+
+	f->fmt.pix.width	= q_data->width;
+	f->fmt.pix.height	= q_data->height;
+	f->fmt.pix.field	= V4L2_FIELD_NONE;
+	f->fmt.pix.pixelformat	= q_data->fmt->fourcc;
+	f->fmt.pix.bytesperline	= (q_data->width * q_data->fmt->depth) >> 3;
+	f->fmt.pix.sizeimage	= q_data->sizeimage;
+	f->fmt.pix.colorspace	= ctx->colorspace;
+
+	return 0;
+}
+
+static int vidioc_g_fmt_vid_out(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	return vidioc_g_fmt(file2ctx(file), f);
+}
+
+static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	return vidioc_g_fmt(file2ctx(file), f);
+}
+
+static int vidioc_try_fmt(struct v4l2_format *f, struct vim2m_fmt *fmt)
+{
+	/* V4L2 specification suggests the driver corrects the format struct
+	 * if any of the dimensions is unsupported */
+	if (f->fmt.pix.height < MIN_H)
+		f->fmt.pix.height = MIN_H;
+	else if (f->fmt.pix.height > MAX_H)
+		f->fmt.pix.height = MAX_H;
+
+	if (f->fmt.pix.width < MIN_W)
+		f->fmt.pix.width = MIN_W;
+	else if (f->fmt.pix.width > MAX_W)
+		f->fmt.pix.width = MAX_W;
+
+	f->fmt.pix.width &= ~DIM_ALIGN_MASK;
+	f->fmt.pix.bytesperline = (f->fmt.pix.width * fmt->depth) >> 3;
+	f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
+	f->fmt.pix.field = V4L2_FIELD_NONE;
+
+	return 0;
+}
+
+static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
+				  struct v4l2_format *f)
+{
+	struct vim2m_fmt *fmt;
+	struct vim2m_ctx *ctx = file2ctx(file);
+
+	fmt = find_format(f);
+	if (!fmt) {
+		f->fmt.pix.pixelformat = formats[0].fourcc;
+		fmt = find_format(f);
+	}
+	if (!(fmt->types & MEM2MEM_CAPTURE)) {
+		v4l2_err(&ctx->dev->v4l2_dev,
+			 "Fourcc format (0x%08x) invalid.\n",
+			 f->fmt.pix.pixelformat);
+		return -EINVAL;
+	}
+	f->fmt.pix.colorspace = ctx->colorspace;
+
+	return vidioc_try_fmt(f, fmt);
+}
+
+static int vidioc_try_fmt_vid_out(struct file *file, void *priv,
+				  struct v4l2_format *f)
+{
+	struct vim2m_fmt *fmt;
+	struct vim2m_ctx *ctx = file2ctx(file);
+
+	fmt = find_format(f);
+	if (!fmt) {
+		f->fmt.pix.pixelformat = formats[0].fourcc;
+		fmt = find_format(f);
+	}
+	if (!(fmt->types & MEM2MEM_OUTPUT)) {
+		v4l2_err(&ctx->dev->v4l2_dev,
+			 "Fourcc format (0x%08x) invalid.\n",
+			 f->fmt.pix.pixelformat);
+		return -EINVAL;
+	}
+	if (!f->fmt.pix.colorspace)
+		f->fmt.pix.colorspace = V4L2_COLORSPACE_REC709;
+
+	return vidioc_try_fmt(f, fmt);
+}
+
+static int vidioc_s_fmt(struct vim2m_ctx *ctx, struct v4l2_format *f)
+{
+	struct vim2m_q_data *q_data;
+	struct vb2_queue *vq;
+
+	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
+	if (!vq)
+		return -EINVAL;
+
+	q_data = get_q_data(ctx, f->type);
+	if (!q_data)
+		return -EINVAL;
+
+	if (vb2_is_busy(vq)) {
+		v4l2_err(&ctx->dev->v4l2_dev, "%s queue busy\n", __func__);
+		return -EBUSY;
+	}
+
+	q_data->fmt		= find_format(f);
+	q_data->width		= f->fmt.pix.width;
+	q_data->height		= f->fmt.pix.height;
+	q_data->sizeimage	= q_data->width * q_data->height
+				* q_data->fmt->depth >> 3;
+
+	dprintk(ctx->dev,
+		"Setting format for type %d, wxh: %dx%d, fmt: %d\n",
+		f->type, q_data->width, q_data->height, q_data->fmt->fourcc);
+
+	return 0;
+}
+
+static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	int ret;
+
+	ret = vidioc_try_fmt_vid_cap(file, priv, f);
+	if (ret)
+		return ret;
+
+	return vidioc_s_fmt(file2ctx(file), f);
+}
+
+static int vidioc_s_fmt_vid_out(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	struct vim2m_ctx *ctx = file2ctx(file);
+	int ret;
+
+	ret = vidioc_try_fmt_vid_out(file, priv, f);
+	if (ret)
+		return ret;
+
+	ret = vidioc_s_fmt(file2ctx(file), f);
+	if (!ret)
+		ctx->colorspace = f->fmt.pix.colorspace;
+	return ret;
+}
+
+static int vim2m_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vim2m_ctx *ctx =
+		container_of(ctrl->handler, struct vim2m_ctx, hdl);
+
+	switch (ctrl->id) {
+	case V4L2_CID_HFLIP:
+		if (ctrl->val)
+			ctx->mode |= MEM2MEM_HFLIP;
+		else
+			ctx->mode &= ~MEM2MEM_HFLIP;
+		break;
+
+	case V4L2_CID_VFLIP:
+		if (ctrl->val)
+			ctx->mode |= MEM2MEM_VFLIP;
+		else
+			ctx->mode &= ~MEM2MEM_VFLIP;
+		break;
+
+	case V4L2_CID_TRANS_TIME_MSEC:
+		ctx->transtime = ctrl->val;
+		break;
+
+	case V4L2_CID_TRANS_NUM_BUFS:
+		ctx->translen = ctrl->val;
+		break;
+
+	default:
+		v4l2_err(&ctx->dev->v4l2_dev, "Invalid control\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops vim2m_ctrl_ops = {
+	.s_ctrl = vim2m_s_ctrl,
+};
+
+
+static const struct v4l2_ioctl_ops vim2m_ioctl_ops = {
+	.vidioc_querycap	= vidioc_querycap,
+
+	.vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap	= vidioc_g_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap	= vidioc_try_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap	= vidioc_s_fmt_vid_cap,
+
+	.vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out,
+	.vidioc_g_fmt_vid_out	= vidioc_g_fmt_vid_out,
+	.vidioc_try_fmt_vid_out	= vidioc_try_fmt_vid_out,
+	.vidioc_s_fmt_vid_out	= vidioc_s_fmt_vid_out,
+
+	.vidioc_reqbufs		= v4l2_m2m_ioctl_reqbufs,
+	.vidioc_querybuf	= v4l2_m2m_ioctl_querybuf,
+	.vidioc_qbuf		= v4l2_m2m_ioctl_qbuf,
+	.vidioc_dqbuf		= v4l2_m2m_ioctl_dqbuf,
+	.vidioc_prepare_buf	= v4l2_m2m_ioctl_prepare_buf,
+	.vidioc_create_bufs	= v4l2_m2m_ioctl_create_bufs,
+	.vidioc_expbuf		= v4l2_m2m_ioctl_expbuf,
+
+	.vidioc_streamon	= v4l2_m2m_ioctl_streamon,
+	.vidioc_streamoff	= v4l2_m2m_ioctl_streamoff,
+
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+};
+
+
+/*
+ * Queue operations
+ */
+
+static int vim2m_queue_setup(struct vb2_queue *vq,
+				const void *parg,
+				unsigned int *nbuffers, unsigned int *nplanes,
+				unsigned int sizes[], void *alloc_ctxs[])
+{
+	const struct v4l2_format *fmt = parg;
+	struct vim2m_ctx *ctx = vb2_get_drv_priv(vq);
+	struct vim2m_q_data *q_data;
+	unsigned int size, count = *nbuffers;
+
+	q_data = get_q_data(ctx, vq->type);
+
+	size = q_data->width * q_data->height * q_data->fmt->depth >> 3;
+
+	if (fmt) {
+		if (fmt->fmt.pix.sizeimage < size)
+			return -EINVAL;
+		size = fmt->fmt.pix.sizeimage;
+	}
+
+	while (size * count > MEM2MEM_VID_MEM_LIMIT)
+		(count)--;
+
+	*nplanes = 1;
+	*nbuffers = count;
+	sizes[0] = size;
+
+	/*
+	 * videobuf2-vmalloc allocator is context-less so no need to set
+	 * alloc_ctxs array.
+	 */
+
+	dprintk(ctx->dev, "get %d buffer(s) of size %d each.\n", count, size);
+
+	return 0;
+}
+
+static int vim2m_buf_prepare(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct vim2m_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct vim2m_q_data *q_data;
+
+	dprintk(ctx->dev, "type: %d\n", vb->vb2_queue->type);
+
+	q_data = get_q_data(ctx, vb->vb2_queue->type);
+	if (V4L2_TYPE_IS_OUTPUT(vb->vb2_queue->type)) {
+		if (vbuf->field == V4L2_FIELD_ANY)
+			vbuf->field = V4L2_FIELD_NONE;
+		if (vbuf->field != V4L2_FIELD_NONE) {
+			dprintk(ctx->dev, "%s field isn't supported\n",
+					__func__);
+			return -EINVAL;
+		}
+	}
+
+	if (vb2_plane_size(vb, 0) < q_data->sizeimage) {
+		dprintk(ctx->dev, "%s data will not fit into plane (%lu < %lu)\n",
+				__func__, vb2_plane_size(vb, 0), (long)q_data->sizeimage);
+		return -EINVAL;
+	}
+
+	vb2_set_plane_payload(vb, 0, q_data->sizeimage);
+
+	return 0;
+}
+
+static void vim2m_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct vim2m_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+
+	v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
+}
+
+static int vim2m_start_streaming(struct vb2_queue *q, unsigned count)
+{
+	struct vim2m_ctx *ctx = vb2_get_drv_priv(q);
+	struct vim2m_q_data *q_data = get_q_data(ctx, q->type);
+
+	q_data->sequence = 0;
+	return 0;
+}
+
+static void vim2m_stop_streaming(struct vb2_queue *q)
+{
+	struct vim2m_ctx *ctx = vb2_get_drv_priv(q);
+	struct vb2_v4l2_buffer *vbuf;
+	unsigned long flags;
+
+	for (;;) {
+		if (V4L2_TYPE_IS_OUTPUT(q->type))
+			vbuf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+		else
+			vbuf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
+		if (vbuf == NULL)
+			return;
+		spin_lock_irqsave(&ctx->dev->irqlock, flags);
+		v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_ERROR);
+		spin_unlock_irqrestore(&ctx->dev->irqlock, flags);
+	}
+}
+
+static struct vb2_ops vim2m_qops = {
+	.queue_setup	 = vim2m_queue_setup,
+	.buf_prepare	 = vim2m_buf_prepare,
+	.buf_queue	 = vim2m_buf_queue,
+	.start_streaming = vim2m_start_streaming,
+	.stop_streaming  = vim2m_stop_streaming,
+	.wait_prepare	 = vb2_ops_wait_prepare,
+	.wait_finish	 = vb2_ops_wait_finish,
+};
+
+static int queue_init(void *priv, struct vb2_queue *src_vq, struct vb2_queue *dst_vq)
+{
+	struct vim2m_ctx *ctx = priv;
+	int ret;
+
+	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	src_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
+	src_vq->drv_priv = ctx;
+	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	src_vq->ops = &vim2m_qops;
+	src_vq->mem_ops = &vb2_vmalloc_memops;
+	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	src_vq->lock = &ctx->dev->dev_mutex;
+
+	ret = vb2_queue_init(src_vq);
+	if (ret)
+		return ret;
+
+	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	dst_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
+	dst_vq->drv_priv = ctx;
+	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	dst_vq->ops = &vim2m_qops;
+	dst_vq->mem_ops = &vb2_vmalloc_memops;
+	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	dst_vq->lock = &ctx->dev->dev_mutex;
+
+	return vb2_queue_init(dst_vq);
+}
+
+static const struct v4l2_ctrl_config vim2m_ctrl_trans_time_msec = {
+	.ops = &vim2m_ctrl_ops,
+	.id = V4L2_CID_TRANS_TIME_MSEC,
+	.name = "Transaction Time (msec)",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.def = MEM2MEM_DEF_TRANSTIME,
+	.min = 1,
+	.max = 10001,
+	.step = 1,
+};
+
+static const struct v4l2_ctrl_config vim2m_ctrl_trans_num_bufs = {
+	.ops = &vim2m_ctrl_ops,
+	.id = V4L2_CID_TRANS_NUM_BUFS,
+	.name = "Buffers Per Transaction",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.def = 1,
+	.min = 1,
+	.max = MEM2MEM_DEF_NUM_BUFS,
+	.step = 1,
+};
+
+/*
+ * File operations
+ */
+static int vim2m_open(struct file *file)
+{
+	struct vim2m_dev *dev = video_drvdata(file);
+	struct vim2m_ctx *ctx = NULL;
+	struct v4l2_ctrl_handler *hdl;
+	int rc = 0;
+
+	if (mutex_lock_interruptible(&dev->dev_mutex))
+		return -ERESTARTSYS;
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx) {
+		rc = -ENOMEM;
+		goto open_unlock;
+	}
+
+	v4l2_fh_init(&ctx->fh, video_devdata(file));
+	file->private_data = &ctx->fh;
+	ctx->dev = dev;
+	hdl = &ctx->hdl;
+	v4l2_ctrl_handler_init(hdl, 4);
+	v4l2_ctrl_new_std(hdl, &vim2m_ctrl_ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(hdl, &vim2m_ctrl_ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_custom(hdl, &vim2m_ctrl_trans_time_msec, NULL);
+	v4l2_ctrl_new_custom(hdl, &vim2m_ctrl_trans_num_bufs, NULL);
+	if (hdl->error) {
+		rc = hdl->error;
+		v4l2_ctrl_handler_free(hdl);
+		goto open_unlock;
+	}
+	ctx->fh.ctrl_handler = hdl;
+	v4l2_ctrl_handler_setup(hdl);
+
+	ctx->q_data[V4L2_M2M_SRC].fmt = &formats[0];
+	ctx->q_data[V4L2_M2M_SRC].width = 640;
+	ctx->q_data[V4L2_M2M_SRC].height = 480;
+	ctx->q_data[V4L2_M2M_SRC].sizeimage =
+		ctx->q_data[V4L2_M2M_SRC].width *
+		ctx->q_data[V4L2_M2M_SRC].height *
+		(ctx->q_data[V4L2_M2M_SRC].fmt->depth >> 3);
+	ctx->q_data[V4L2_M2M_DST] = ctx->q_data[V4L2_M2M_SRC];
+	ctx->colorspace = V4L2_COLORSPACE_REC709;
+
+	ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(dev->m2m_dev, ctx, &queue_init);
+
+	if (IS_ERR(ctx->fh.m2m_ctx)) {
+		rc = PTR_ERR(ctx->fh.m2m_ctx);
+
+		v4l2_ctrl_handler_free(hdl);
+		kfree(ctx);
+		goto open_unlock;
+	}
+
+	v4l2_fh_add(&ctx->fh);
+	atomic_inc(&dev->num_inst);
+
+	dprintk(dev, "Created instance: %p, m2m_ctx: %p\n",
+		ctx, ctx->fh.m2m_ctx);
+
+open_unlock:
+	mutex_unlock(&dev->dev_mutex);
+	return rc;
+}
+
+static int vim2m_release(struct file *file)
+{
+	struct vim2m_dev *dev = video_drvdata(file);
+	struct vim2m_ctx *ctx = file2ctx(file);
+
+	dprintk(dev, "Releasing instance %p\n", ctx);
+
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	v4l2_ctrl_handler_free(&ctx->hdl);
+	mutex_lock(&dev->dev_mutex);
+	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
+	mutex_unlock(&dev->dev_mutex);
+	kfree(ctx);
+
+	atomic_dec(&dev->num_inst);
+
+	return 0;
+}
+
+static const struct v4l2_file_operations vim2m_fops = {
+	.owner		= THIS_MODULE,
+	.open		= vim2m_open,
+	.release	= vim2m_release,
+	.poll		= v4l2_m2m_fop_poll,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap		= v4l2_m2m_fop_mmap,
+};
+
+static struct video_device vim2m_videodev = {
+	.name		= MEM2MEM_NAME,
+	.vfl_dir	= VFL_DIR_M2M,
+	.fops		= &vim2m_fops,
+	.ioctl_ops	= &vim2m_ioctl_ops,
+	.minor		= -1,
+	.release	= video_device_release_empty,
+};
+
+static struct v4l2_m2m_ops m2m_ops = {
+	.device_run	= device_run,
+	.job_ready	= job_ready,
+	.job_abort	= job_abort,
+};
+
+static int vim2m_probe(struct platform_device *pdev)
+{
+	struct vim2m_dev *dev;
+	struct video_device *vfd;
+	int ret;
+
+	dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
+	if (!dev)
+		return -ENOMEM;
+
+	spin_lock_init(&dev->irqlock);
+
+	ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
+	if (ret)
+		return ret;
+
+	atomic_set(&dev->num_inst, 0);
+	mutex_init(&dev->dev_mutex);
+
+	dev->vfd = vim2m_videodev;
+	vfd = &dev->vfd;
+	vfd->lock = &dev->dev_mutex;
+	vfd->v4l2_dev = &dev->v4l2_dev;
+
+	ret = video_register_device(vfd, VFL_TYPE_GRABBER, 0);
+	if (ret) {
+		v4l2_err(&dev->v4l2_dev, "Failed to register video device\n");
+		goto unreg_dev;
+	}
+
+	video_set_drvdata(vfd, dev);
+	snprintf(vfd->name, sizeof(vfd->name), "%s", vim2m_videodev.name);
+	v4l2_info(&dev->v4l2_dev,
+			"Device registered as /dev/video%d\n", vfd->num);
+
+	setup_timer(&dev->timer, device_isr, (long)dev);
+	platform_set_drvdata(pdev, dev);
+
+	dev->m2m_dev = v4l2_m2m_init(&m2m_ops);
+	if (IS_ERR(dev->m2m_dev)) {
+		v4l2_err(&dev->v4l2_dev, "Failed to init mem2mem device\n");
+		ret = PTR_ERR(dev->m2m_dev);
+		goto err_m2m;
+	}
+
+	return 0;
+
+err_m2m:
+	v4l2_m2m_release(dev->m2m_dev);
+	video_unregister_device(&dev->vfd);
+unreg_dev:
+	v4l2_device_unregister(&dev->v4l2_dev);
+
+	return ret;
+}
+
+static int vim2m_remove(struct platform_device *pdev)
+{
+	struct vim2m_dev *dev = platform_get_drvdata(pdev);
+
+	v4l2_info(&dev->v4l2_dev, "Removing " MEM2MEM_NAME);
+	v4l2_m2m_release(dev->m2m_dev);
+	del_timer_sync(&dev->timer);
+	video_unregister_device(&dev->vfd);
+	v4l2_device_unregister(&dev->v4l2_dev);
+
+	return 0;
+}
+
+static struct platform_driver vim2m_pdrv = {
+	.probe		= vim2m_probe,
+	.remove		= vim2m_remove,
+	.driver		= {
+		.name	= MEM2MEM_NAME,
+	},
+};
+
+static void __exit vim2m_exit(void)
+{
+	platform_driver_unregister(&vim2m_pdrv);
+	platform_device_unregister(&vim2m_pdev);
+}
+
+static int __init vim2m_init(void)
+{
+	int ret;
+
+	ret = platform_device_register(&vim2m_pdev);
+	if (ret)
+		return ret;
+
+	ret = platform_driver_register(&vim2m_pdrv);
+	if (ret)
+		platform_device_unregister(&vim2m_pdev);
+
+	return 0;
+}
+
+module_init(vim2m_init);
+module_exit(vim2m_exit);
diff --git a/drivers/media/platform/vivid/Kconfig b/drivers/media/platform/vivid/Kconfig
new file mode 100644
index 0000000..0885e93
--- /dev/null
+++ b/drivers/media/platform/vivid/Kconfig
@@ -0,0 +1,30 @@
+config VIDEO_VIVID
+	tristate "Virtual Video Test Driver"
+	depends on VIDEO_DEV && VIDEO_V4L2 && !SPARC32 && !SPARC64 && FB
+	select FONT_SUPPORT
+	select FONT_8x16
+	select FB_CFB_FILLRECT
+	select FB_CFB_COPYAREA
+	select FB_CFB_IMAGEBLIT
+	select VIDEOBUF2_VMALLOC
+	default n
+	---help---
+	  Enables a virtual video driver. This driver emulates a webcam,
+	  TV, S-Video and HDMI capture hardware, including VBI support for
+	  the SDTV inputs. Also video output, VBI output, radio receivers,
+	  transmitters and software defined radio capture is emulated.
+
+	  It is highly configurable and is ideal for testing applications.
+	  Error injection is supported to test rare errors that are hard
+	  to reproduce in real hardware.
+
+	  Say Y here if you want to test video apps or debug V4L devices.
+	  When in doubt, say N.
+
+config VIDEO_VIVID_MAX_DEVS
+	int "Maximum number of devices"
+	depends on VIDEO_VIVID
+	default "64"
+	---help---
+	  This allows you to specify the maximum number of devices supported
+	  by the vivid driver.
diff --git a/drivers/media/platform/vivid/Makefile b/drivers/media/platform/vivid/Makefile
new file mode 100644
index 0000000..756fc12
--- /dev/null
+++ b/drivers/media/platform/vivid/Makefile
@@ -0,0 +1,6 @@
+vivid-objs := vivid-core.o vivid-ctrls.o vivid-vid-common.o vivid-vbi-gen.o \
+		vivid-vid-cap.o vivid-vid-out.o vivid-kthread-cap.o vivid-kthread-out.o \
+		vivid-radio-rx.o vivid-radio-tx.o vivid-radio-common.o \
+		vivid-rds-gen.o vivid-sdr-cap.o vivid-vbi-cap.o vivid-vbi-out.o \
+		vivid-osd.o vivid-tpg.o vivid-tpg-colors.o
+obj-$(CONFIG_VIDEO_VIVID) += vivid.o
diff --git a/drivers/media/platform/vivid/vivid-core.c b/drivers/media/platform/vivid/vivid-core.c
new file mode 100644
index 0000000..ec125be
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-core.c
@@ -0,0 +1,1436 @@
+/*
+ * vivid-core.c - A Virtual Video Test Driver, core initialization
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/vmalloc.h>
+#include <linux/font.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/videodev2.h>
+#include <linux/v4l2-dv-timings.h>
+#include <media/videobuf2-vmalloc.h>
+#include <media/v4l2-dv-timings.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-event.h>
+
+#include "vivid-core.h"
+#include "vivid-vid-common.h"
+#include "vivid-vid-cap.h"
+#include "vivid-vid-out.h"
+#include "vivid-radio-common.h"
+#include "vivid-radio-rx.h"
+#include "vivid-radio-tx.h"
+#include "vivid-sdr-cap.h"
+#include "vivid-vbi-cap.h"
+#include "vivid-vbi-out.h"
+#include "vivid-osd.h"
+#include "vivid-ctrls.h"
+
+#define VIVID_MODULE_NAME "vivid"
+
+/* The maximum number of vivid devices */
+#define VIVID_MAX_DEVS CONFIG_VIDEO_VIVID_MAX_DEVS
+
+MODULE_DESCRIPTION("Virtual Video Test Driver");
+MODULE_AUTHOR("Hans Verkuil");
+MODULE_LICENSE("GPL");
+
+static unsigned n_devs = 1;
+module_param(n_devs, uint, 0444);
+MODULE_PARM_DESC(n_devs, " number of driver instances to create");
+
+static int vid_cap_nr[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = -1 };
+module_param_array(vid_cap_nr, int, NULL, 0444);
+MODULE_PARM_DESC(vid_cap_nr, " videoX start number, -1 is autodetect");
+
+static int vid_out_nr[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = -1 };
+module_param_array(vid_out_nr, int, NULL, 0444);
+MODULE_PARM_DESC(vid_out_nr, " videoX start number, -1 is autodetect");
+
+static int vbi_cap_nr[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = -1 };
+module_param_array(vbi_cap_nr, int, NULL, 0444);
+MODULE_PARM_DESC(vbi_cap_nr, " vbiX start number, -1 is autodetect");
+
+static int vbi_out_nr[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = -1 };
+module_param_array(vbi_out_nr, int, NULL, 0444);
+MODULE_PARM_DESC(vbi_out_nr, " vbiX start number, -1 is autodetect");
+
+static int sdr_cap_nr[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = -1 };
+module_param_array(sdr_cap_nr, int, NULL, 0444);
+MODULE_PARM_DESC(sdr_cap_nr, " swradioX start number, -1 is autodetect");
+
+static int radio_rx_nr[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = -1 };
+module_param_array(radio_rx_nr, int, NULL, 0444);
+MODULE_PARM_DESC(radio_rx_nr, " radioX start number, -1 is autodetect");
+
+static int radio_tx_nr[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = -1 };
+module_param_array(radio_tx_nr, int, NULL, 0444);
+MODULE_PARM_DESC(radio_tx_nr, " radioX start number, -1 is autodetect");
+
+static int ccs_cap_mode[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = -1 };
+module_param_array(ccs_cap_mode, int, NULL, 0444);
+MODULE_PARM_DESC(ccs_cap_mode, " capture crop/compose/scale mode:\n"
+			   "\t\t    bit 0=crop, 1=compose, 2=scale,\n"
+			   "\t\t    -1=user-controlled (default)");
+
+static int ccs_out_mode[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = -1 };
+module_param_array(ccs_out_mode, int, NULL, 0444);
+MODULE_PARM_DESC(ccs_out_mode, " output crop/compose/scale mode:\n"
+			   "\t\t    bit 0=crop, 1=compose, 2=scale,\n"
+			   "\t\t    -1=user-controlled (default)");
+
+static unsigned multiplanar[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = 1 };
+module_param_array(multiplanar, uint, NULL, 0444);
+MODULE_PARM_DESC(multiplanar, " 1 (default) creates a single planar device, 2 creates a multiplanar device.");
+
+/* Default: video + vbi-cap (raw and sliced) + radio rx + radio tx + sdr + vbi-out + vid-out */
+static unsigned node_types[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = 0x1d3d };
+module_param_array(node_types, uint, NULL, 0444);
+MODULE_PARM_DESC(node_types, " node types, default is 0x1d3d. Bitmask with the following meaning:\n"
+			     "\t\t    bit 0: Video Capture node\n"
+			     "\t\t    bit 2-3: VBI Capture node: 0 = none, 1 = raw vbi, 2 = sliced vbi, 3 = both\n"
+			     "\t\t    bit 4: Radio Receiver node\n"
+			     "\t\t    bit 5: Software Defined Radio Receiver node\n"
+			     "\t\t    bit 8: Video Output node\n"
+			     "\t\t    bit 10-11: VBI Output node: 0 = none, 1 = raw vbi, 2 = sliced vbi, 3 = both\n"
+			     "\t\t    bit 12: Radio Transmitter node\n"
+			     "\t\t    bit 16: Framebuffer for testing overlays");
+
+/* Default: 4 inputs */
+static unsigned num_inputs[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = 4 };
+module_param_array(num_inputs, uint, NULL, 0444);
+MODULE_PARM_DESC(num_inputs, " number of inputs, default is 4");
+
+/* Default: input 0 = WEBCAM, 1 = TV, 2 = SVID, 3 = HDMI */
+static unsigned input_types[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = 0xe4 };
+module_param_array(input_types, uint, NULL, 0444);
+MODULE_PARM_DESC(input_types, " input types, default is 0xe4. Two bits per input,\n"
+			      "\t\t    bits 0-1 == input 0, bits 31-30 == input 15.\n"
+			      "\t\t    Type 0 == webcam, 1 == TV, 2 == S-Video, 3 == HDMI");
+
+/* Default: 2 outputs */
+static unsigned num_outputs[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = 2 };
+module_param_array(num_outputs, uint, NULL, 0444);
+MODULE_PARM_DESC(num_outputs, " number of outputs, default is 2");
+
+/* Default: output 0 = SVID, 1 = HDMI */
+static unsigned output_types[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = 2 };
+module_param_array(output_types, uint, NULL, 0444);
+MODULE_PARM_DESC(output_types, " output types, default is 0x02. One bit per output,\n"
+			      "\t\t    bit 0 == output 0, bit 15 == output 15.\n"
+			      "\t\t    Type 0 == S-Video, 1 == HDMI");
+
+unsigned vivid_debug;
+module_param(vivid_debug, uint, 0644);
+MODULE_PARM_DESC(vivid_debug, " activates debug info");
+
+static bool no_error_inj;
+module_param(no_error_inj, bool, 0444);
+MODULE_PARM_DESC(no_error_inj, " if set disable the error injecting controls");
+
+static struct vivid_dev *vivid_devs[VIVID_MAX_DEVS];
+
+const struct v4l2_rect vivid_min_rect = {
+	0, 0, MIN_WIDTH, MIN_HEIGHT
+};
+
+const struct v4l2_rect vivid_max_rect = {
+	0, 0, MAX_WIDTH * MAX_ZOOM, MAX_HEIGHT * MAX_ZOOM
+};
+
+static const u8 vivid_hdmi_edid[256] = {
+	0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00,
+	0x63, 0x3a, 0xaa, 0x55, 0x00, 0x00, 0x00, 0x00,
+	0x0a, 0x18, 0x01, 0x03, 0x80, 0x10, 0x09, 0x78,
+	0x0e, 0x00, 0xb2, 0xa0, 0x57, 0x49, 0x9b, 0x26,
+	0x10, 0x48, 0x4f, 0x2f, 0xcf, 0x00, 0x31, 0x59,
+	0x45, 0x59, 0x81, 0x80, 0x81, 0x40, 0x90, 0x40,
+	0x95, 0x00, 0xa9, 0x40, 0xb3, 0x00, 0x02, 0x3a,
+	0x80, 0x18, 0x71, 0x38, 0x2d, 0x40, 0x58, 0x2c,
+	0x46, 0x00, 0x10, 0x09, 0x00, 0x00, 0x00, 0x1e,
+	0x00, 0x00, 0x00, 0xfd, 0x00, 0x18, 0x55, 0x18,
+	0x5e, 0x11, 0x00, 0x0a, 0x20, 0x20, 0x20, 0x20,
+	0x20, 0x20, 0x00, 0x00, 0x00, 0xfc, 0x00,  'v',
+	'4',   'l',  '2',  '-',  'h',  'd',  'm',  'i',
+	0x0a, 0x0a, 0x0a, 0x0a, 0x00, 0x00, 0x00, 0x10,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf0,
+
+	0x02, 0x03, 0x1a, 0xc0, 0x48, 0xa2, 0x10, 0x04,
+	0x02, 0x01, 0x21, 0x14, 0x13, 0x23, 0x09, 0x07,
+	0x07, 0x65, 0x03, 0x0c, 0x00, 0x10, 0x00, 0xe2,
+	0x00, 0x2a, 0x01, 0x1d, 0x00, 0x80, 0x51, 0xd0,
+	0x1c, 0x20, 0x40, 0x80, 0x35, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x1e, 0x8c, 0x0a, 0xd0, 0x8a,
+	0x20, 0xe0, 0x2d, 0x10, 0x10, 0x3e, 0x96, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xd7
+};
+
+static int vidioc_querycap(struct file *file, void  *priv,
+					struct v4l2_capability *cap)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct video_device *vdev = video_devdata(file);
+
+	strcpy(cap->driver, "vivid");
+	strcpy(cap->card, "vivid");
+	snprintf(cap->bus_info, sizeof(cap->bus_info),
+			"platform:%s", dev->v4l2_dev.name);
+
+	if (vdev->vfl_type == VFL_TYPE_GRABBER && vdev->vfl_dir == VFL_DIR_RX)
+		cap->device_caps = dev->vid_cap_caps;
+	if (vdev->vfl_type == VFL_TYPE_GRABBER && vdev->vfl_dir == VFL_DIR_TX)
+		cap->device_caps = dev->vid_out_caps;
+	else if (vdev->vfl_type == VFL_TYPE_VBI && vdev->vfl_dir == VFL_DIR_RX)
+		cap->device_caps = dev->vbi_cap_caps;
+	else if (vdev->vfl_type == VFL_TYPE_VBI && vdev->vfl_dir == VFL_DIR_TX)
+		cap->device_caps = dev->vbi_out_caps;
+	else if (vdev->vfl_type == VFL_TYPE_SDR)
+		cap->device_caps = dev->sdr_cap_caps;
+	else if (vdev->vfl_type == VFL_TYPE_RADIO && vdev->vfl_dir == VFL_DIR_RX)
+		cap->device_caps = dev->radio_rx_caps;
+	else if (vdev->vfl_type == VFL_TYPE_RADIO && vdev->vfl_dir == VFL_DIR_TX)
+		cap->device_caps = dev->radio_tx_caps;
+	cap->capabilities = dev->vid_cap_caps | dev->vid_out_caps |
+		dev->vbi_cap_caps | dev->vbi_out_caps |
+		dev->radio_rx_caps | dev->radio_tx_caps |
+		dev->sdr_cap_caps | V4L2_CAP_DEVICE_CAPS;
+	return 0;
+}
+
+static int vidioc_s_hw_freq_seek(struct file *file, void *fh, const struct v4l2_hw_freq_seek *a)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_type == VFL_TYPE_RADIO)
+		return vivid_radio_rx_s_hw_freq_seek(file, fh, a);
+	return -ENOTTY;
+}
+
+static int vidioc_enum_freq_bands(struct file *file, void *fh, struct v4l2_frequency_band *band)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_type == VFL_TYPE_RADIO)
+		return vivid_radio_rx_enum_freq_bands(file, fh, band);
+	if (vdev->vfl_type == VFL_TYPE_SDR)
+		return vivid_sdr_enum_freq_bands(file, fh, band);
+	return -ENOTTY;
+}
+
+static int vidioc_g_tuner(struct file *file, void *fh, struct v4l2_tuner *vt)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_type == VFL_TYPE_RADIO)
+		return vivid_radio_rx_g_tuner(file, fh, vt);
+	if (vdev->vfl_type == VFL_TYPE_SDR)
+		return vivid_sdr_g_tuner(file, fh, vt);
+	return vivid_video_g_tuner(file, fh, vt);
+}
+
+static int vidioc_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *vt)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_type == VFL_TYPE_RADIO)
+		return vivid_radio_rx_s_tuner(file, fh, vt);
+	if (vdev->vfl_type == VFL_TYPE_SDR)
+		return vivid_sdr_s_tuner(file, fh, vt);
+	return vivid_video_s_tuner(file, fh, vt);
+}
+
+static int vidioc_g_frequency(struct file *file, void *fh, struct v4l2_frequency *vf)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_type == VFL_TYPE_RADIO)
+		return vivid_radio_g_frequency(file,
+			vdev->vfl_dir == VFL_DIR_RX ?
+			&dev->radio_rx_freq : &dev->radio_tx_freq, vf);
+	if (vdev->vfl_type == VFL_TYPE_SDR)
+		return vivid_sdr_g_frequency(file, fh, vf);
+	return vivid_video_g_frequency(file, fh, vf);
+}
+
+static int vidioc_s_frequency(struct file *file, void *fh, const struct v4l2_frequency *vf)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_type == VFL_TYPE_RADIO)
+		return vivid_radio_s_frequency(file,
+			vdev->vfl_dir == VFL_DIR_RX ?
+			&dev->radio_rx_freq : &dev->radio_tx_freq, vf);
+	if (vdev->vfl_type == VFL_TYPE_SDR)
+		return vivid_sdr_s_frequency(file, fh, vf);
+	return vivid_video_s_frequency(file, fh, vf);
+}
+
+static int vidioc_overlay(struct file *file, void *fh, unsigned i)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_dir == VFL_DIR_RX)
+		return vivid_vid_cap_overlay(file, fh, i);
+	return vivid_vid_out_overlay(file, fh, i);
+}
+
+static int vidioc_g_fbuf(struct file *file, void *fh, struct v4l2_framebuffer *a)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_dir == VFL_DIR_RX)
+		return vivid_vid_cap_g_fbuf(file, fh, a);
+	return vivid_vid_out_g_fbuf(file, fh, a);
+}
+
+static int vidioc_s_fbuf(struct file *file, void *fh, const struct v4l2_framebuffer *a)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_dir == VFL_DIR_RX)
+		return vivid_vid_cap_s_fbuf(file, fh, a);
+	return vivid_vid_out_s_fbuf(file, fh, a);
+}
+
+static int vidioc_s_std(struct file *file, void *fh, v4l2_std_id id)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_dir == VFL_DIR_RX)
+		return vivid_vid_cap_s_std(file, fh, id);
+	return vivid_vid_out_s_std(file, fh, id);
+}
+
+static int vidioc_s_dv_timings(struct file *file, void *fh, struct v4l2_dv_timings *timings)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_dir == VFL_DIR_RX)
+		return vivid_vid_cap_s_dv_timings(file, fh, timings);
+	return vivid_vid_out_s_dv_timings(file, fh, timings);
+}
+
+static int vidioc_cropcap(struct file *file, void *fh, struct v4l2_cropcap *cc)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_dir == VFL_DIR_RX)
+		return vivid_vid_cap_cropcap(file, fh, cc);
+	return vivid_vid_out_cropcap(file, fh, cc);
+}
+
+static int vidioc_g_selection(struct file *file, void *fh,
+			      struct v4l2_selection *sel)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_dir == VFL_DIR_RX)
+		return vivid_vid_cap_g_selection(file, fh, sel);
+	return vivid_vid_out_g_selection(file, fh, sel);
+}
+
+static int vidioc_s_selection(struct file *file, void *fh,
+			      struct v4l2_selection *sel)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_dir == VFL_DIR_RX)
+		return vivid_vid_cap_s_selection(file, fh, sel);
+	return vivid_vid_out_s_selection(file, fh, sel);
+}
+
+static int vidioc_g_parm(struct file *file, void *fh,
+			  struct v4l2_streamparm *parm)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_dir == VFL_DIR_RX)
+		return vivid_vid_cap_g_parm(file, fh, parm);
+	return vivid_vid_out_g_parm(file, fh, parm);
+}
+
+static int vidioc_s_parm(struct file *file, void *fh,
+			  struct v4l2_streamparm *parm)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_dir == VFL_DIR_RX)
+		return vivid_vid_cap_s_parm(file, fh, parm);
+	return vivid_vid_out_g_parm(file, fh, parm);
+}
+
+static int vidioc_log_status(struct file *file, void *fh)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct video_device *vdev = video_devdata(file);
+
+	v4l2_ctrl_log_status(file, fh);
+	if (vdev->vfl_dir == VFL_DIR_RX && vdev->vfl_type == VFL_TYPE_GRABBER)
+		tpg_log_status(&dev->tpg);
+	return 0;
+}
+
+static ssize_t vivid_radio_read(struct file *file, char __user *buf,
+			 size_t size, loff_t *offset)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_dir == VFL_DIR_TX)
+		return -EINVAL;
+	return vivid_radio_rx_read(file, buf, size, offset);
+}
+
+static ssize_t vivid_radio_write(struct file *file, const char __user *buf,
+			  size_t size, loff_t *offset)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_dir == VFL_DIR_RX)
+		return -EINVAL;
+	return vivid_radio_tx_write(file, buf, size, offset);
+}
+
+static unsigned int vivid_radio_poll(struct file *file, struct poll_table_struct *wait)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_dir == VFL_DIR_RX)
+		return vivid_radio_rx_poll(file, wait);
+	return vivid_radio_tx_poll(file, wait);
+}
+
+static bool vivid_is_in_use(struct video_device *vdev)
+{
+	unsigned long flags;
+	bool res;
+
+	spin_lock_irqsave(&vdev->fh_lock, flags);
+	res = !list_empty(&vdev->fh_list);
+	spin_unlock_irqrestore(&vdev->fh_lock, flags);
+	return res;
+}
+
+static bool vivid_is_last_user(struct vivid_dev *dev)
+{
+	unsigned uses = vivid_is_in_use(&dev->vid_cap_dev) +
+			vivid_is_in_use(&dev->vid_out_dev) +
+			vivid_is_in_use(&dev->vbi_cap_dev) +
+			vivid_is_in_use(&dev->vbi_out_dev) +
+			vivid_is_in_use(&dev->sdr_cap_dev) +
+			vivid_is_in_use(&dev->radio_rx_dev) +
+			vivid_is_in_use(&dev->radio_tx_dev);
+
+	return uses == 1;
+}
+
+static int vivid_fop_release(struct file *file)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct video_device *vdev = video_devdata(file);
+
+	mutex_lock(&dev->mutex);
+	if (!no_error_inj && v4l2_fh_is_singular_file(file) &&
+	    !video_is_registered(vdev) && vivid_is_last_user(dev)) {
+		/*
+		 * I am the last user of this driver, and a disconnect
+		 * was forced (since this video_device is unregistered),
+		 * so re-register all video_device's again.
+		 */
+		v4l2_info(&dev->v4l2_dev, "reconnect\n");
+		set_bit(V4L2_FL_REGISTERED, &dev->vid_cap_dev.flags);
+		set_bit(V4L2_FL_REGISTERED, &dev->vid_out_dev.flags);
+		set_bit(V4L2_FL_REGISTERED, &dev->vbi_cap_dev.flags);
+		set_bit(V4L2_FL_REGISTERED, &dev->vbi_out_dev.flags);
+		set_bit(V4L2_FL_REGISTERED, &dev->sdr_cap_dev.flags);
+		set_bit(V4L2_FL_REGISTERED, &dev->radio_rx_dev.flags);
+		set_bit(V4L2_FL_REGISTERED, &dev->radio_tx_dev.flags);
+	}
+	mutex_unlock(&dev->mutex);
+	if (file->private_data == dev->overlay_cap_owner)
+		dev->overlay_cap_owner = NULL;
+	if (file->private_data == dev->radio_rx_rds_owner) {
+		dev->radio_rx_rds_last_block = 0;
+		dev->radio_rx_rds_owner = NULL;
+	}
+	if (file->private_data == dev->radio_tx_rds_owner) {
+		dev->radio_tx_rds_last_block = 0;
+		dev->radio_tx_rds_owner = NULL;
+	}
+	if (vdev->queue)
+		return vb2_fop_release(file);
+	return v4l2_fh_release(file);
+}
+
+static const struct v4l2_file_operations vivid_fops = {
+	.owner		= THIS_MODULE,
+	.open           = v4l2_fh_open,
+	.release        = vivid_fop_release,
+	.read           = vb2_fop_read,
+	.write          = vb2_fop_write,
+	.poll		= vb2_fop_poll,
+	.unlocked_ioctl = video_ioctl2,
+	.mmap           = vb2_fop_mmap,
+};
+
+static const struct v4l2_file_operations vivid_radio_fops = {
+	.owner		= THIS_MODULE,
+	.open           = v4l2_fh_open,
+	.release        = vivid_fop_release,
+	.read           = vivid_radio_read,
+	.write          = vivid_radio_write,
+	.poll		= vivid_radio_poll,
+	.unlocked_ioctl = video_ioctl2,
+};
+
+static const struct v4l2_ioctl_ops vivid_ioctl_ops = {
+	.vidioc_querycap		= vidioc_querycap,
+
+	.vidioc_enum_fmt_vid_cap	= vidioc_enum_fmt_vid,
+	.vidioc_g_fmt_vid_cap		= vidioc_g_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap		= vidioc_try_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap		= vidioc_s_fmt_vid_cap,
+	.vidioc_enum_fmt_vid_cap_mplane = vidioc_enum_fmt_vid_mplane,
+	.vidioc_g_fmt_vid_cap_mplane	= vidioc_g_fmt_vid_cap_mplane,
+	.vidioc_try_fmt_vid_cap_mplane	= vidioc_try_fmt_vid_cap_mplane,
+	.vidioc_s_fmt_vid_cap_mplane	= vidioc_s_fmt_vid_cap_mplane,
+
+	.vidioc_enum_fmt_vid_out	= vidioc_enum_fmt_vid,
+	.vidioc_g_fmt_vid_out		= vidioc_g_fmt_vid_out,
+	.vidioc_try_fmt_vid_out		= vidioc_try_fmt_vid_out,
+	.vidioc_s_fmt_vid_out		= vidioc_s_fmt_vid_out,
+	.vidioc_enum_fmt_vid_out_mplane = vidioc_enum_fmt_vid_mplane,
+	.vidioc_g_fmt_vid_out_mplane	= vidioc_g_fmt_vid_out_mplane,
+	.vidioc_try_fmt_vid_out_mplane	= vidioc_try_fmt_vid_out_mplane,
+	.vidioc_s_fmt_vid_out_mplane	= vidioc_s_fmt_vid_out_mplane,
+
+	.vidioc_g_selection		= vidioc_g_selection,
+	.vidioc_s_selection		= vidioc_s_selection,
+	.vidioc_cropcap			= vidioc_cropcap,
+
+	.vidioc_g_fmt_vbi_cap		= vidioc_g_fmt_vbi_cap,
+	.vidioc_try_fmt_vbi_cap		= vidioc_g_fmt_vbi_cap,
+	.vidioc_s_fmt_vbi_cap		= vidioc_s_fmt_vbi_cap,
+
+	.vidioc_g_fmt_sliced_vbi_cap    = vidioc_g_fmt_sliced_vbi_cap,
+	.vidioc_try_fmt_sliced_vbi_cap  = vidioc_try_fmt_sliced_vbi_cap,
+	.vidioc_s_fmt_sliced_vbi_cap    = vidioc_s_fmt_sliced_vbi_cap,
+	.vidioc_g_sliced_vbi_cap	= vidioc_g_sliced_vbi_cap,
+
+	.vidioc_g_fmt_vbi_out		= vidioc_g_fmt_vbi_out,
+	.vidioc_try_fmt_vbi_out		= vidioc_g_fmt_vbi_out,
+	.vidioc_s_fmt_vbi_out		= vidioc_s_fmt_vbi_out,
+
+	.vidioc_g_fmt_sliced_vbi_out    = vidioc_g_fmt_sliced_vbi_out,
+	.vidioc_try_fmt_sliced_vbi_out  = vidioc_try_fmt_sliced_vbi_out,
+	.vidioc_s_fmt_sliced_vbi_out    = vidioc_s_fmt_sliced_vbi_out,
+
+	.vidioc_enum_fmt_sdr_cap	= vidioc_enum_fmt_sdr_cap,
+	.vidioc_g_fmt_sdr_cap		= vidioc_g_fmt_sdr_cap,
+	.vidioc_try_fmt_sdr_cap		= vidioc_try_fmt_sdr_cap,
+	.vidioc_s_fmt_sdr_cap		= vidioc_s_fmt_sdr_cap,
+
+	.vidioc_overlay			= vidioc_overlay,
+	.vidioc_enum_framesizes		= vidioc_enum_framesizes,
+	.vidioc_enum_frameintervals	= vidioc_enum_frameintervals,
+	.vidioc_g_parm			= vidioc_g_parm,
+	.vidioc_s_parm			= vidioc_s_parm,
+
+	.vidioc_enum_fmt_vid_overlay	= vidioc_enum_fmt_vid_overlay,
+	.vidioc_g_fmt_vid_overlay	= vidioc_g_fmt_vid_overlay,
+	.vidioc_try_fmt_vid_overlay	= vidioc_try_fmt_vid_overlay,
+	.vidioc_s_fmt_vid_overlay	= vidioc_s_fmt_vid_overlay,
+	.vidioc_g_fmt_vid_out_overlay	= vidioc_g_fmt_vid_out_overlay,
+	.vidioc_try_fmt_vid_out_overlay	= vidioc_try_fmt_vid_out_overlay,
+	.vidioc_s_fmt_vid_out_overlay	= vidioc_s_fmt_vid_out_overlay,
+	.vidioc_g_fbuf			= vidioc_g_fbuf,
+	.vidioc_s_fbuf			= vidioc_s_fbuf,
+
+	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
+	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
+	.vidioc_prepare_buf		= vb2_ioctl_prepare_buf,
+	.vidioc_querybuf		= vb2_ioctl_querybuf,
+	.vidioc_qbuf			= vb2_ioctl_qbuf,
+	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
+	.vidioc_expbuf			= vb2_ioctl_expbuf,
+	.vidioc_streamon		= vb2_ioctl_streamon,
+	.vidioc_streamoff		= vb2_ioctl_streamoff,
+
+	.vidioc_enum_input		= vidioc_enum_input,
+	.vidioc_g_input			= vidioc_g_input,
+	.vidioc_s_input			= vidioc_s_input,
+	.vidioc_s_audio			= vidioc_s_audio,
+	.vidioc_g_audio			= vidioc_g_audio,
+	.vidioc_enumaudio		= vidioc_enumaudio,
+	.vidioc_s_frequency		= vidioc_s_frequency,
+	.vidioc_g_frequency		= vidioc_g_frequency,
+	.vidioc_s_tuner			= vidioc_s_tuner,
+	.vidioc_g_tuner			= vidioc_g_tuner,
+	.vidioc_s_modulator		= vidioc_s_modulator,
+	.vidioc_g_modulator		= vidioc_g_modulator,
+	.vidioc_s_hw_freq_seek		= vidioc_s_hw_freq_seek,
+	.vidioc_enum_freq_bands		= vidioc_enum_freq_bands,
+
+	.vidioc_enum_output		= vidioc_enum_output,
+	.vidioc_g_output		= vidioc_g_output,
+	.vidioc_s_output		= vidioc_s_output,
+	.vidioc_s_audout		= vidioc_s_audout,
+	.vidioc_g_audout		= vidioc_g_audout,
+	.vidioc_enumaudout		= vidioc_enumaudout,
+
+	.vidioc_querystd		= vidioc_querystd,
+	.vidioc_g_std			= vidioc_g_std,
+	.vidioc_s_std			= vidioc_s_std,
+	.vidioc_s_dv_timings		= vidioc_s_dv_timings,
+	.vidioc_g_dv_timings		= vidioc_g_dv_timings,
+	.vidioc_query_dv_timings	= vidioc_query_dv_timings,
+	.vidioc_enum_dv_timings		= vidioc_enum_dv_timings,
+	.vidioc_dv_timings_cap		= vidioc_dv_timings_cap,
+	.vidioc_g_edid			= vidioc_g_edid,
+	.vidioc_s_edid			= vidioc_s_edid,
+
+	.vidioc_log_status		= vidioc_log_status,
+	.vidioc_subscribe_event		= vidioc_subscribe_event,
+	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
+};
+
+/* -----------------------------------------------------------------
+	Initialization and module stuff
+   ------------------------------------------------------------------*/
+
+static void vivid_dev_release(struct v4l2_device *v4l2_dev)
+{
+	struct vivid_dev *dev = container_of(v4l2_dev, struct vivid_dev, v4l2_dev);
+
+	vivid_free_controls(dev);
+	v4l2_device_unregister(&dev->v4l2_dev);
+	vfree(dev->scaled_line);
+	vfree(dev->blended_line);
+	vfree(dev->edid);
+	vfree(dev->bitmap_cap);
+	vfree(dev->bitmap_out);
+	tpg_free(&dev->tpg);
+	kfree(dev->query_dv_timings_qmenu);
+	kfree(dev);
+}
+
+static int vivid_create_instance(struct platform_device *pdev, int inst)
+{
+	static const struct v4l2_dv_timings def_dv_timings =
+					V4L2_DV_BT_CEA_1280X720P60;
+	unsigned in_type_counter[4] = { 0, 0, 0, 0 };
+	unsigned out_type_counter[4] = { 0, 0, 0, 0 };
+	int ccs_cap = ccs_cap_mode[inst];
+	int ccs_out = ccs_out_mode[inst];
+	bool has_tuner;
+	bool has_modulator;
+	struct vivid_dev *dev;
+	struct video_device *vfd;
+	struct vb2_queue *q;
+	unsigned node_type = node_types[inst];
+	v4l2_std_id tvnorms_cap = 0, tvnorms_out = 0;
+	int ret;
+	int i;
+
+	/* allocate main vivid state structure */
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev)
+		return -ENOMEM;
+
+	dev->inst = inst;
+
+	/* register v4l2_device */
+	snprintf(dev->v4l2_dev.name, sizeof(dev->v4l2_dev.name),
+			"%s-%03d", VIVID_MODULE_NAME, inst);
+	ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
+	if (ret) {
+		kfree(dev);
+		return ret;
+	}
+	dev->v4l2_dev.release = vivid_dev_release;
+
+	/* start detecting feature set */
+
+	/* do we use single- or multi-planar? */
+	dev->multiplanar = multiplanar[inst] > 1;
+	v4l2_info(&dev->v4l2_dev, "using %splanar format API\n",
+			dev->multiplanar ? "multi" : "single ");
+
+	/* how many inputs do we have and of what type? */
+	dev->num_inputs = num_inputs[inst];
+	if (dev->num_inputs < 1)
+		dev->num_inputs = 1;
+	if (dev->num_inputs >= MAX_INPUTS)
+		dev->num_inputs = MAX_INPUTS;
+	for (i = 0; i < dev->num_inputs; i++) {
+		dev->input_type[i] = (input_types[inst] >> (i * 2)) & 0x3;
+		dev->input_name_counter[i] = in_type_counter[dev->input_type[i]]++;
+	}
+	dev->has_audio_inputs = in_type_counter[TV] && in_type_counter[SVID];
+
+	/* how many outputs do we have and of what type? */
+	dev->num_outputs = num_outputs[inst];
+	if (dev->num_outputs < 1)
+		dev->num_outputs = 1;
+	if (dev->num_outputs >= MAX_OUTPUTS)
+		dev->num_outputs = MAX_OUTPUTS;
+	for (i = 0; i < dev->num_outputs; i++) {
+		dev->output_type[i] = ((output_types[inst] >> i) & 1) ? HDMI : SVID;
+		dev->output_name_counter[i] = out_type_counter[dev->output_type[i]]++;
+	}
+	dev->has_audio_outputs = out_type_counter[SVID];
+
+	/* do we create a video capture device? */
+	dev->has_vid_cap = node_type & 0x0001;
+
+	/* do we create a vbi capture device? */
+	if (in_type_counter[TV] || in_type_counter[SVID]) {
+		dev->has_raw_vbi_cap = node_type & 0x0004;
+		dev->has_sliced_vbi_cap = node_type & 0x0008;
+		dev->has_vbi_cap = dev->has_raw_vbi_cap | dev->has_sliced_vbi_cap;
+	}
+
+	/* do we create a video output device? */
+	dev->has_vid_out = node_type & 0x0100;
+
+	/* do we create a vbi output device? */
+	if (out_type_counter[SVID]) {
+		dev->has_raw_vbi_out = node_type & 0x0400;
+		dev->has_sliced_vbi_out = node_type & 0x0800;
+		dev->has_vbi_out = dev->has_raw_vbi_out | dev->has_sliced_vbi_out;
+	}
+
+	/* do we create a radio receiver device? */
+	dev->has_radio_rx = node_type & 0x0010;
+
+	/* do we create a radio transmitter device? */
+	dev->has_radio_tx = node_type & 0x1000;
+
+	/* do we create a software defined radio capture device? */
+	dev->has_sdr_cap = node_type & 0x0020;
+
+	/* do we have a tuner? */
+	has_tuner = ((dev->has_vid_cap || dev->has_vbi_cap) && in_type_counter[TV]) ||
+		    dev->has_radio_rx || dev->has_sdr_cap;
+
+	/* do we have a modulator? */
+	has_modulator = dev->has_radio_tx;
+
+	if (dev->has_vid_cap)
+		/* do we have a framebuffer for overlay testing? */
+		dev->has_fb = node_type & 0x10000;
+
+	/* can we do crop/compose/scaling while capturing? */
+	if (no_error_inj && ccs_cap == -1)
+		ccs_cap = 7;
+
+	/* if ccs_cap == -1, then the use can select it using controls */
+	if (ccs_cap != -1) {
+		dev->has_crop_cap = ccs_cap & 1;
+		dev->has_compose_cap = ccs_cap & 2;
+		dev->has_scaler_cap = ccs_cap & 4;
+		v4l2_info(&dev->v4l2_dev, "Capture Crop: %c Compose: %c Scaler: %c\n",
+			dev->has_crop_cap ? 'Y' : 'N',
+			dev->has_compose_cap ? 'Y' : 'N',
+			dev->has_scaler_cap ? 'Y' : 'N');
+	}
+
+	/* can we do crop/compose/scaling with video output? */
+	if (no_error_inj && ccs_out == -1)
+		ccs_out = 7;
+
+	/* if ccs_out == -1, then the use can select it using controls */
+	if (ccs_out != -1) {
+		dev->has_crop_out = ccs_out & 1;
+		dev->has_compose_out = ccs_out & 2;
+		dev->has_scaler_out = ccs_out & 4;
+		v4l2_info(&dev->v4l2_dev, "Output Crop: %c Compose: %c Scaler: %c\n",
+			dev->has_crop_out ? 'Y' : 'N',
+			dev->has_compose_out ? 'Y' : 'N',
+			dev->has_scaler_out ? 'Y' : 'N');
+	}
+
+	/* end detecting feature set */
+
+	if (dev->has_vid_cap) {
+		/* set up the capabilities of the video capture device */
+		dev->vid_cap_caps = dev->multiplanar ?
+			V4L2_CAP_VIDEO_CAPTURE_MPLANE :
+			V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OVERLAY;
+		dev->vid_cap_caps |= V4L2_CAP_STREAMING | V4L2_CAP_READWRITE;
+		if (dev->has_audio_inputs)
+			dev->vid_cap_caps |= V4L2_CAP_AUDIO;
+		if (in_type_counter[TV])
+			dev->vid_cap_caps |= V4L2_CAP_TUNER;
+	}
+	if (dev->has_vid_out) {
+		/* set up the capabilities of the video output device */
+		dev->vid_out_caps = dev->multiplanar ?
+			V4L2_CAP_VIDEO_OUTPUT_MPLANE :
+			V4L2_CAP_VIDEO_OUTPUT;
+		if (dev->has_fb)
+			dev->vid_out_caps |= V4L2_CAP_VIDEO_OUTPUT_OVERLAY;
+		dev->vid_out_caps |= V4L2_CAP_STREAMING | V4L2_CAP_READWRITE;
+		if (dev->has_audio_outputs)
+			dev->vid_out_caps |= V4L2_CAP_AUDIO;
+	}
+	if (dev->has_vbi_cap) {
+		/* set up the capabilities of the vbi capture device */
+		dev->vbi_cap_caps = (dev->has_raw_vbi_cap ? V4L2_CAP_VBI_CAPTURE : 0) |
+				    (dev->has_sliced_vbi_cap ? V4L2_CAP_SLICED_VBI_CAPTURE : 0);
+		dev->vbi_cap_caps |= V4L2_CAP_STREAMING | V4L2_CAP_READWRITE;
+		if (dev->has_audio_inputs)
+			dev->vbi_cap_caps |= V4L2_CAP_AUDIO;
+		if (in_type_counter[TV])
+			dev->vbi_cap_caps |= V4L2_CAP_TUNER;
+	}
+	if (dev->has_vbi_out) {
+		/* set up the capabilities of the vbi output device */
+		dev->vbi_out_caps = (dev->has_raw_vbi_out ? V4L2_CAP_VBI_OUTPUT : 0) |
+				    (dev->has_sliced_vbi_out ? V4L2_CAP_SLICED_VBI_OUTPUT : 0);
+		dev->vbi_out_caps |= V4L2_CAP_STREAMING | V4L2_CAP_READWRITE;
+		if (dev->has_audio_outputs)
+			dev->vbi_out_caps |= V4L2_CAP_AUDIO;
+	}
+	if (dev->has_sdr_cap) {
+		/* set up the capabilities of the sdr capture device */
+		dev->sdr_cap_caps = V4L2_CAP_SDR_CAPTURE | V4L2_CAP_TUNER;
+		dev->sdr_cap_caps |= V4L2_CAP_STREAMING | V4L2_CAP_READWRITE;
+	}
+	/* set up the capabilities of the radio receiver device */
+	if (dev->has_radio_rx)
+		dev->radio_rx_caps = V4L2_CAP_RADIO | V4L2_CAP_RDS_CAPTURE |
+				     V4L2_CAP_HW_FREQ_SEEK | V4L2_CAP_TUNER |
+				     V4L2_CAP_READWRITE;
+	/* set up the capabilities of the radio transmitter device */
+	if (dev->has_radio_tx)
+		dev->radio_tx_caps = V4L2_CAP_RDS_OUTPUT | V4L2_CAP_MODULATOR |
+				     V4L2_CAP_READWRITE;
+
+	/* initialize the test pattern generator */
+	tpg_init(&dev->tpg, 640, 360);
+	if (tpg_alloc(&dev->tpg, MAX_ZOOM * MAX_WIDTH))
+		goto free_dev;
+	dev->scaled_line = vzalloc(MAX_ZOOM * MAX_WIDTH);
+	if (!dev->scaled_line)
+		goto free_dev;
+	dev->blended_line = vzalloc(MAX_ZOOM * MAX_WIDTH);
+	if (!dev->blended_line)
+		goto free_dev;
+
+	/* load the edid */
+	dev->edid = vmalloc(256 * 128);
+	if (!dev->edid)
+		goto free_dev;
+
+	/* create a string array containing the names of all the preset timings */
+	while (v4l2_dv_timings_presets[dev->query_dv_timings_size].bt.width)
+		dev->query_dv_timings_size++;
+	dev->query_dv_timings_qmenu = kmalloc(dev->query_dv_timings_size *
+					   (sizeof(void *) + 32), GFP_KERNEL);
+	if (dev->query_dv_timings_qmenu == NULL)
+		goto free_dev;
+	for (i = 0; i < dev->query_dv_timings_size; i++) {
+		const struct v4l2_bt_timings *bt = &v4l2_dv_timings_presets[i].bt;
+		char *p = (char *)&dev->query_dv_timings_qmenu[dev->query_dv_timings_size];
+		u32 htot, vtot;
+
+		p += i * 32;
+		dev->query_dv_timings_qmenu[i] = p;
+
+		htot = V4L2_DV_BT_FRAME_WIDTH(bt);
+		vtot = V4L2_DV_BT_FRAME_HEIGHT(bt);
+		snprintf(p, 32, "%ux%u%s%u",
+			bt->width, bt->height, bt->interlaced ? "i" : "p",
+			(u32)bt->pixelclock / (htot * vtot));
+	}
+
+	/* disable invalid ioctls based on the feature set */
+	if (!dev->has_audio_inputs) {
+		v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_S_AUDIO);
+		v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_G_AUDIO);
+		v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_ENUMAUDIO);
+		v4l2_disable_ioctl(&dev->vbi_cap_dev, VIDIOC_S_AUDIO);
+		v4l2_disable_ioctl(&dev->vbi_cap_dev, VIDIOC_G_AUDIO);
+		v4l2_disable_ioctl(&dev->vbi_cap_dev, VIDIOC_ENUMAUDIO);
+	}
+	if (!dev->has_audio_outputs) {
+		v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_S_AUDOUT);
+		v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_G_AUDOUT);
+		v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_ENUMAUDOUT);
+		v4l2_disable_ioctl(&dev->vbi_out_dev, VIDIOC_S_AUDOUT);
+		v4l2_disable_ioctl(&dev->vbi_out_dev, VIDIOC_G_AUDOUT);
+		v4l2_disable_ioctl(&dev->vbi_out_dev, VIDIOC_ENUMAUDOUT);
+	}
+	if (!in_type_counter[TV] && !in_type_counter[SVID]) {
+		v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_S_STD);
+		v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_G_STD);
+		v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_ENUMSTD);
+		v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_QUERYSTD);
+	}
+	if (!out_type_counter[SVID]) {
+		v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_S_STD);
+		v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_G_STD);
+		v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_ENUMSTD);
+	}
+	if (!has_tuner && !has_modulator) {
+		v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_S_FREQUENCY);
+		v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_G_FREQUENCY);
+		v4l2_disable_ioctl(&dev->vbi_cap_dev, VIDIOC_S_FREQUENCY);
+		v4l2_disable_ioctl(&dev->vbi_cap_dev, VIDIOC_G_FREQUENCY);
+	}
+	if (!has_tuner) {
+		v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_S_TUNER);
+		v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_G_TUNER);
+		v4l2_disable_ioctl(&dev->vbi_cap_dev, VIDIOC_S_TUNER);
+		v4l2_disable_ioctl(&dev->vbi_cap_dev, VIDIOC_G_TUNER);
+	}
+	if (in_type_counter[HDMI] == 0) {
+		v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_S_EDID);
+		v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_G_EDID);
+		v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_DV_TIMINGS_CAP);
+		v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_G_DV_TIMINGS);
+		v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_S_DV_TIMINGS);
+		v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_ENUM_DV_TIMINGS);
+		v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_QUERY_DV_TIMINGS);
+	}
+	if (out_type_counter[HDMI] == 0) {
+		v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_G_EDID);
+		v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_DV_TIMINGS_CAP);
+		v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_G_DV_TIMINGS);
+		v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_S_DV_TIMINGS);
+		v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_ENUM_DV_TIMINGS);
+	}
+	if (!dev->has_fb) {
+		v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_G_FBUF);
+		v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_S_FBUF);
+		v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_OVERLAY);
+	}
+	v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_S_HW_FREQ_SEEK);
+	v4l2_disable_ioctl(&dev->vbi_cap_dev, VIDIOC_S_HW_FREQ_SEEK);
+	v4l2_disable_ioctl(&dev->sdr_cap_dev, VIDIOC_S_HW_FREQ_SEEK);
+	v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_S_FREQUENCY);
+	v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_G_FREQUENCY);
+	v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_ENUM_FRAMESIZES);
+	v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_ENUM_FRAMEINTERVALS);
+	v4l2_disable_ioctl(&dev->vbi_out_dev, VIDIOC_S_FREQUENCY);
+	v4l2_disable_ioctl(&dev->vbi_out_dev, VIDIOC_G_FREQUENCY);
+
+	/* configure internal data */
+	dev->fmt_cap = &vivid_formats[0];
+	dev->fmt_out = &vivid_formats[0];
+	if (!dev->multiplanar)
+		vivid_formats[0].data_offset[0] = 0;
+	dev->webcam_size_idx = 1;
+	dev->webcam_ival_idx = 3;
+	tpg_s_fourcc(&dev->tpg, dev->fmt_cap->fourcc);
+	dev->std_cap = V4L2_STD_PAL;
+	dev->std_out = V4L2_STD_PAL;
+	if (dev->input_type[0] == TV || dev->input_type[0] == SVID)
+		tvnorms_cap = V4L2_STD_ALL;
+	if (dev->output_type[0] == SVID)
+		tvnorms_out = V4L2_STD_ALL;
+	dev->dv_timings_cap = def_dv_timings;
+	dev->dv_timings_out = def_dv_timings;
+	dev->tv_freq = 2804 /* 175.25 * 16 */;
+	dev->tv_audmode = V4L2_TUNER_MODE_STEREO;
+	dev->tv_field_cap = V4L2_FIELD_INTERLACED;
+	dev->tv_field_out = V4L2_FIELD_INTERLACED;
+	dev->radio_rx_freq = 95000 * 16;
+	dev->radio_rx_audmode = V4L2_TUNER_MODE_STEREO;
+	if (dev->has_radio_tx) {
+		dev->radio_tx_freq = 95500 * 16;
+		dev->radio_rds_loop = false;
+	}
+	dev->radio_tx_subchans = V4L2_TUNER_SUB_STEREO | V4L2_TUNER_SUB_RDS;
+	dev->sdr_adc_freq = 300000;
+	dev->sdr_fm_freq = 50000000;
+	dev->sdr_pixelformat = V4L2_SDR_FMT_CU8;
+	dev->sdr_buffersize = SDR_CAP_SAMPLES_PER_BUF * 2;
+
+	dev->edid_max_blocks = dev->edid_blocks = 2;
+	memcpy(dev->edid, vivid_hdmi_edid, sizeof(vivid_hdmi_edid));
+	ktime_get_ts(&dev->radio_rds_init_ts);
+
+	/* create all controls */
+	ret = vivid_create_controls(dev, ccs_cap == -1, ccs_out == -1, no_error_inj,
+			in_type_counter[TV] || in_type_counter[SVID] ||
+			out_type_counter[SVID],
+			in_type_counter[HDMI] || out_type_counter[HDMI]);
+	if (ret)
+		goto unreg_dev;
+
+	/*
+	 * update the capture and output formats to do a proper initial
+	 * configuration.
+	 */
+	vivid_update_format_cap(dev, false);
+	vivid_update_format_out(dev);
+
+	v4l2_ctrl_handler_setup(&dev->ctrl_hdl_vid_cap);
+	v4l2_ctrl_handler_setup(&dev->ctrl_hdl_vid_out);
+	v4l2_ctrl_handler_setup(&dev->ctrl_hdl_vbi_cap);
+	v4l2_ctrl_handler_setup(&dev->ctrl_hdl_vbi_out);
+	v4l2_ctrl_handler_setup(&dev->ctrl_hdl_radio_rx);
+	v4l2_ctrl_handler_setup(&dev->ctrl_hdl_radio_tx);
+	v4l2_ctrl_handler_setup(&dev->ctrl_hdl_sdr_cap);
+
+	/* initialize overlay */
+	dev->fb_cap.fmt.width = dev->src_rect.width;
+	dev->fb_cap.fmt.height = dev->src_rect.height;
+	dev->fb_cap.fmt.pixelformat = dev->fmt_cap->fourcc;
+	dev->fb_cap.fmt.bytesperline = dev->src_rect.width * tpg_g_twopixelsize(&dev->tpg, 0) / 2;
+	dev->fb_cap.fmt.sizeimage = dev->src_rect.height * dev->fb_cap.fmt.bytesperline;
+
+	/* initialize locks */
+	spin_lock_init(&dev->slock);
+	mutex_init(&dev->mutex);
+
+	/* init dma queues */
+	INIT_LIST_HEAD(&dev->vid_cap_active);
+	INIT_LIST_HEAD(&dev->vid_out_active);
+	INIT_LIST_HEAD(&dev->vbi_cap_active);
+	INIT_LIST_HEAD(&dev->vbi_out_active);
+	INIT_LIST_HEAD(&dev->sdr_cap_active);
+
+	/* start creating the vb2 queues */
+	if (dev->has_vid_cap) {
+		/* initialize vid_cap queue */
+		q = &dev->vb_vid_cap_q;
+		q->type = dev->multiplanar ? V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE :
+			V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF | VB2_READ;
+		q->drv_priv = dev;
+		q->buf_struct_size = sizeof(struct vivid_buffer);
+		q->ops = &vivid_vid_cap_qops;
+		q->mem_ops = &vb2_vmalloc_memops;
+		q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+		q->min_buffers_needed = 2;
+		q->lock = &dev->mutex;
+
+		ret = vb2_queue_init(q);
+		if (ret)
+			goto unreg_dev;
+	}
+
+	if (dev->has_vid_out) {
+		/* initialize vid_out queue */
+		q = &dev->vb_vid_out_q;
+		q->type = dev->multiplanar ? V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE :
+			V4L2_BUF_TYPE_VIDEO_OUTPUT;
+		q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF | VB2_WRITE;
+		q->drv_priv = dev;
+		q->buf_struct_size = sizeof(struct vivid_buffer);
+		q->ops = &vivid_vid_out_qops;
+		q->mem_ops = &vb2_vmalloc_memops;
+		q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+		q->min_buffers_needed = 2;
+		q->lock = &dev->mutex;
+
+		ret = vb2_queue_init(q);
+		if (ret)
+			goto unreg_dev;
+	}
+
+	if (dev->has_vbi_cap) {
+		/* initialize vbi_cap queue */
+		q = &dev->vb_vbi_cap_q;
+		q->type = dev->has_raw_vbi_cap ? V4L2_BUF_TYPE_VBI_CAPTURE :
+					      V4L2_BUF_TYPE_SLICED_VBI_CAPTURE;
+		q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF | VB2_READ;
+		q->drv_priv = dev;
+		q->buf_struct_size = sizeof(struct vivid_buffer);
+		q->ops = &vivid_vbi_cap_qops;
+		q->mem_ops = &vb2_vmalloc_memops;
+		q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+		q->min_buffers_needed = 2;
+		q->lock = &dev->mutex;
+
+		ret = vb2_queue_init(q);
+		if (ret)
+			goto unreg_dev;
+	}
+
+	if (dev->has_vbi_out) {
+		/* initialize vbi_out queue */
+		q = &dev->vb_vbi_out_q;
+		q->type = dev->has_raw_vbi_out ? V4L2_BUF_TYPE_VBI_OUTPUT :
+					      V4L2_BUF_TYPE_SLICED_VBI_OUTPUT;
+		q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF | VB2_WRITE;
+		q->drv_priv = dev;
+		q->buf_struct_size = sizeof(struct vivid_buffer);
+		q->ops = &vivid_vbi_out_qops;
+		q->mem_ops = &vb2_vmalloc_memops;
+		q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+		q->min_buffers_needed = 2;
+		q->lock = &dev->mutex;
+
+		ret = vb2_queue_init(q);
+		if (ret)
+			goto unreg_dev;
+	}
+
+	if (dev->has_sdr_cap) {
+		/* initialize sdr_cap queue */
+		q = &dev->vb_sdr_cap_q;
+		q->type = V4L2_BUF_TYPE_SDR_CAPTURE;
+		q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF | VB2_READ;
+		q->drv_priv = dev;
+		q->buf_struct_size = sizeof(struct vivid_buffer);
+		q->ops = &vivid_sdr_cap_qops;
+		q->mem_ops = &vb2_vmalloc_memops;
+		q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+		q->min_buffers_needed = 8;
+		q->lock = &dev->mutex;
+
+		ret = vb2_queue_init(q);
+		if (ret)
+			goto unreg_dev;
+	}
+
+	if (dev->has_fb) {
+		/* Create framebuffer for testing capture/output overlay */
+		ret = vivid_fb_init(dev);
+		if (ret)
+			goto unreg_dev;
+		v4l2_info(&dev->v4l2_dev, "Framebuffer device registered as fb%d\n",
+				dev->fb_info.node);
+	}
+
+	/* finally start creating the device nodes */
+	if (dev->has_vid_cap) {
+		vfd = &dev->vid_cap_dev;
+		strlcpy(vfd->name, "vivid-vid-cap", sizeof(vfd->name));
+		vfd->fops = &vivid_fops;
+		vfd->ioctl_ops = &vivid_ioctl_ops;
+		vfd->release = video_device_release_empty;
+		vfd->v4l2_dev = &dev->v4l2_dev;
+		vfd->queue = &dev->vb_vid_cap_q;
+		vfd->tvnorms = tvnorms_cap;
+
+		/*
+		 * Provide a mutex to v4l2 core. It will be used to protect
+		 * all fops and v4l2 ioctls.
+		 */
+		vfd->lock = &dev->mutex;
+		video_set_drvdata(vfd, dev);
+
+		ret = video_register_device(vfd, VFL_TYPE_GRABBER, vid_cap_nr[inst]);
+		if (ret < 0)
+			goto unreg_dev;
+		v4l2_info(&dev->v4l2_dev, "V4L2 capture device registered as %s\n",
+					  video_device_node_name(vfd));
+	}
+
+	if (dev->has_vid_out) {
+		vfd = &dev->vid_out_dev;
+		strlcpy(vfd->name, "vivid-vid-out", sizeof(vfd->name));
+		vfd->vfl_dir = VFL_DIR_TX;
+		vfd->fops = &vivid_fops;
+		vfd->ioctl_ops = &vivid_ioctl_ops;
+		vfd->release = video_device_release_empty;
+		vfd->v4l2_dev = &dev->v4l2_dev;
+		vfd->queue = &dev->vb_vid_out_q;
+		vfd->tvnorms = tvnorms_out;
+
+		/*
+		 * Provide a mutex to v4l2 core. It will be used to protect
+		 * all fops and v4l2 ioctls.
+		 */
+		vfd->lock = &dev->mutex;
+		video_set_drvdata(vfd, dev);
+
+		ret = video_register_device(vfd, VFL_TYPE_GRABBER, vid_out_nr[inst]);
+		if (ret < 0)
+			goto unreg_dev;
+		v4l2_info(&dev->v4l2_dev, "V4L2 output device registered as %s\n",
+					  video_device_node_name(vfd));
+	}
+
+	if (dev->has_vbi_cap) {
+		vfd = &dev->vbi_cap_dev;
+		strlcpy(vfd->name, "vivid-vbi-cap", sizeof(vfd->name));
+		vfd->fops = &vivid_fops;
+		vfd->ioctl_ops = &vivid_ioctl_ops;
+		vfd->release = video_device_release_empty;
+		vfd->v4l2_dev = &dev->v4l2_dev;
+		vfd->queue = &dev->vb_vbi_cap_q;
+		vfd->lock = &dev->mutex;
+		vfd->tvnorms = tvnorms_cap;
+		video_set_drvdata(vfd, dev);
+
+		ret = video_register_device(vfd, VFL_TYPE_VBI, vbi_cap_nr[inst]);
+		if (ret < 0)
+			goto unreg_dev;
+		v4l2_info(&dev->v4l2_dev, "V4L2 capture device registered as %s, supports %s VBI\n",
+					  video_device_node_name(vfd),
+					  (dev->has_raw_vbi_cap && dev->has_sliced_vbi_cap) ?
+					  "raw and sliced" :
+					  (dev->has_raw_vbi_cap ? "raw" : "sliced"));
+	}
+
+	if (dev->has_vbi_out) {
+		vfd = &dev->vbi_out_dev;
+		strlcpy(vfd->name, "vivid-vbi-out", sizeof(vfd->name));
+		vfd->vfl_dir = VFL_DIR_TX;
+		vfd->fops = &vivid_fops;
+		vfd->ioctl_ops = &vivid_ioctl_ops;
+		vfd->release = video_device_release_empty;
+		vfd->v4l2_dev = &dev->v4l2_dev;
+		vfd->queue = &dev->vb_vbi_out_q;
+		vfd->lock = &dev->mutex;
+		vfd->tvnorms = tvnorms_out;
+		video_set_drvdata(vfd, dev);
+
+		ret = video_register_device(vfd, VFL_TYPE_VBI, vbi_out_nr[inst]);
+		if (ret < 0)
+			goto unreg_dev;
+		v4l2_info(&dev->v4l2_dev, "V4L2 output device registered as %s, supports %s VBI\n",
+					  video_device_node_name(vfd),
+					  (dev->has_raw_vbi_out && dev->has_sliced_vbi_out) ?
+					  "raw and sliced" :
+					  (dev->has_raw_vbi_out ? "raw" : "sliced"));
+	}
+
+	if (dev->has_sdr_cap) {
+		vfd = &dev->sdr_cap_dev;
+		strlcpy(vfd->name, "vivid-sdr-cap", sizeof(vfd->name));
+		vfd->fops = &vivid_fops;
+		vfd->ioctl_ops = &vivid_ioctl_ops;
+		vfd->release = video_device_release_empty;
+		vfd->v4l2_dev = &dev->v4l2_dev;
+		vfd->queue = &dev->vb_sdr_cap_q;
+		vfd->lock = &dev->mutex;
+		video_set_drvdata(vfd, dev);
+
+		ret = video_register_device(vfd, VFL_TYPE_SDR, sdr_cap_nr[inst]);
+		if (ret < 0)
+			goto unreg_dev;
+		v4l2_info(&dev->v4l2_dev, "V4L2 capture device registered as %s\n",
+					  video_device_node_name(vfd));
+	}
+
+	if (dev->has_radio_rx) {
+		vfd = &dev->radio_rx_dev;
+		strlcpy(vfd->name, "vivid-rad-rx", sizeof(vfd->name));
+		vfd->fops = &vivid_radio_fops;
+		vfd->ioctl_ops = &vivid_ioctl_ops;
+		vfd->release = video_device_release_empty;
+		vfd->v4l2_dev = &dev->v4l2_dev;
+		vfd->lock = &dev->mutex;
+		video_set_drvdata(vfd, dev);
+
+		ret = video_register_device(vfd, VFL_TYPE_RADIO, radio_rx_nr[inst]);
+		if (ret < 0)
+			goto unreg_dev;
+		v4l2_info(&dev->v4l2_dev, "V4L2 receiver device registered as %s\n",
+					  video_device_node_name(vfd));
+	}
+
+	if (dev->has_radio_tx) {
+		vfd = &dev->radio_tx_dev;
+		strlcpy(vfd->name, "vivid-rad-tx", sizeof(vfd->name));
+		vfd->vfl_dir = VFL_DIR_TX;
+		vfd->fops = &vivid_radio_fops;
+		vfd->ioctl_ops = &vivid_ioctl_ops;
+		vfd->release = video_device_release_empty;
+		vfd->v4l2_dev = &dev->v4l2_dev;
+		vfd->lock = &dev->mutex;
+		video_set_drvdata(vfd, dev);
+
+		ret = video_register_device(vfd, VFL_TYPE_RADIO, radio_tx_nr[inst]);
+		if (ret < 0)
+			goto unreg_dev;
+		v4l2_info(&dev->v4l2_dev, "V4L2 transmitter device registered as %s\n",
+					  video_device_node_name(vfd));
+	}
+
+	/* Now that everything is fine, let's add it to device list */
+	vivid_devs[inst] = dev;
+
+	return 0;
+
+unreg_dev:
+	video_unregister_device(&dev->radio_tx_dev);
+	video_unregister_device(&dev->radio_rx_dev);
+	video_unregister_device(&dev->sdr_cap_dev);
+	video_unregister_device(&dev->vbi_out_dev);
+	video_unregister_device(&dev->vbi_cap_dev);
+	video_unregister_device(&dev->vid_out_dev);
+	video_unregister_device(&dev->vid_cap_dev);
+free_dev:
+	v4l2_device_put(&dev->v4l2_dev);
+	return ret;
+}
+
+/* This routine allocates from 1 to n_devs virtual drivers.
+
+   The real maximum number of virtual drivers will depend on how many drivers
+   will succeed. This is limited to the maximum number of devices that
+   videodev supports, which is equal to VIDEO_NUM_DEVICES.
+ */
+static int vivid_probe(struct platform_device *pdev)
+{
+	const struct font_desc *font = find_font("VGA8x16");
+	int ret = 0, i;
+
+	if (font == NULL) {
+		pr_err("vivid: could not find font\n");
+		return -ENODEV;
+	}
+
+	tpg_set_font(font->data);
+
+	n_devs = clamp_t(unsigned, n_devs, 1, VIVID_MAX_DEVS);
+
+	for (i = 0; i < n_devs; i++) {
+		ret = vivid_create_instance(pdev, i);
+		if (ret) {
+			/* If some instantiations succeeded, keep driver */
+			if (i)
+				ret = 0;
+			break;
+		}
+	}
+
+	if (ret < 0) {
+		pr_err("vivid: error %d while loading driver\n", ret);
+		return ret;
+	}
+
+	/* n_devs will reflect the actual number of allocated devices */
+	n_devs = i;
+
+	return ret;
+}
+
+static int vivid_remove(struct platform_device *pdev)
+{
+	struct vivid_dev *dev;
+	unsigned i;
+
+
+	for (i = 0; i < n_devs; i++) {
+		dev = vivid_devs[i];
+		if (!dev)
+			continue;
+
+		if (dev->has_vid_cap) {
+			v4l2_info(&dev->v4l2_dev, "unregistering %s\n",
+				video_device_node_name(&dev->vid_cap_dev));
+			video_unregister_device(&dev->vid_cap_dev);
+		}
+		if (dev->has_vid_out) {
+			v4l2_info(&dev->v4l2_dev, "unregistering %s\n",
+				video_device_node_name(&dev->vid_out_dev));
+			video_unregister_device(&dev->vid_out_dev);
+		}
+		if (dev->has_vbi_cap) {
+			v4l2_info(&dev->v4l2_dev, "unregistering %s\n",
+				video_device_node_name(&dev->vbi_cap_dev));
+			video_unregister_device(&dev->vbi_cap_dev);
+		}
+		if (dev->has_vbi_out) {
+			v4l2_info(&dev->v4l2_dev, "unregistering %s\n",
+				video_device_node_name(&dev->vbi_out_dev));
+			video_unregister_device(&dev->vbi_out_dev);
+		}
+		if (dev->has_sdr_cap) {
+			v4l2_info(&dev->v4l2_dev, "unregistering %s\n",
+				video_device_node_name(&dev->sdr_cap_dev));
+			video_unregister_device(&dev->sdr_cap_dev);
+		}
+		if (dev->has_radio_rx) {
+			v4l2_info(&dev->v4l2_dev, "unregistering %s\n",
+				video_device_node_name(&dev->radio_rx_dev));
+			video_unregister_device(&dev->radio_rx_dev);
+		}
+		if (dev->has_radio_tx) {
+			v4l2_info(&dev->v4l2_dev, "unregistering %s\n",
+				video_device_node_name(&dev->radio_tx_dev));
+			video_unregister_device(&dev->radio_tx_dev);
+		}
+		if (dev->has_fb) {
+			v4l2_info(&dev->v4l2_dev, "unregistering fb%d\n",
+				dev->fb_info.node);
+			unregister_framebuffer(&dev->fb_info);
+			vivid_fb_release_buffers(dev);
+		}
+		v4l2_device_put(&dev->v4l2_dev);
+		vivid_devs[i] = NULL;
+	}
+	return 0;
+}
+
+static void vivid_pdev_release(struct device *dev)
+{
+}
+
+static struct platform_device vivid_pdev = {
+	.name		= "vivid",
+	.dev.release	= vivid_pdev_release,
+};
+
+static struct platform_driver vivid_pdrv = {
+	.probe		= vivid_probe,
+	.remove		= vivid_remove,
+	.driver		= {
+		.name	= "vivid",
+	},
+};
+
+static int __init vivid_init(void)
+{
+	int ret;
+
+	ret = platform_device_register(&vivid_pdev);
+	if (ret)
+		return ret;
+
+	ret = platform_driver_register(&vivid_pdrv);
+	if (ret)
+		platform_device_unregister(&vivid_pdev);
+
+	return ret;
+}
+
+static void __exit vivid_exit(void)
+{
+	platform_driver_unregister(&vivid_pdrv);
+	platform_device_unregister(&vivid_pdev);
+}
+
+module_init(vivid_init);
+module_exit(vivid_exit);
diff --git a/drivers/media/platform/vivid/vivid-core.h b/drivers/media/platform/vivid/vivid-core.h
new file mode 100644
index 0000000..55b304a
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-core.h
@@ -0,0 +1,536 @@
+/*
+ * vivid-core.h - core datastructures
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VIVID_CORE_H_
+#define _VIVID_CORE_H_
+
+#include <linux/fb.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-ctrls.h>
+#include "vivid-tpg.h"
+#include "vivid-rds-gen.h"
+#include "vivid-vbi-gen.h"
+
+#define dprintk(dev, level, fmt, arg...) \
+	v4l2_dbg(level, vivid_debug, &dev->v4l2_dev, fmt, ## arg)
+
+/* Maximum allowed frame rate
+ *
+ * vivid will allow setting timeperframe in [1/FPS_MAX - FPS_MAX/1] range.
+ *
+ * Ideally FPS_MAX should be infinity, i.e. practically UINT_MAX, but that
+ * might hit application errors when they manipulate these values.
+ *
+ * Besides, for tpf < 10ms image-generation logic should be changed, to avoid
+ * producing frames with equal content.
+ */
+#define FPS_MAX 100
+
+/* The maximum number of clip rectangles */
+#define MAX_CLIPS  16
+/* The maximum number of inputs */
+#define MAX_INPUTS 16
+/* The maximum number of outputs */
+#define MAX_OUTPUTS 16
+/* The maximum up or down scaling factor is 4 */
+#define MAX_ZOOM  4
+/* The maximum image width/height are set to 4K DMT */
+#define MAX_WIDTH  4096
+#define MAX_HEIGHT 2160
+/* The minimum image width/height */
+#define MIN_WIDTH  16
+#define MIN_HEIGHT 16
+/* The data_offset of plane 0 for the multiplanar formats */
+#define PLANE0_DATA_OFFSET 128
+
+/* The supported TV frequency range in MHz */
+#define MIN_TV_FREQ (44U * 16U)
+#define MAX_TV_FREQ (958U * 16U)
+
+/* The number of samples returned in every SDR buffer */
+#define SDR_CAP_SAMPLES_PER_BUF 0x4000
+
+/* used by the threads to know when to resync internal counters */
+#define JIFFIES_PER_DAY (3600U * 24U * HZ)
+#define JIFFIES_RESYNC (JIFFIES_PER_DAY * (0xf0000000U / JIFFIES_PER_DAY))
+
+extern const struct v4l2_rect vivid_min_rect;
+extern const struct v4l2_rect vivid_max_rect;
+extern unsigned vivid_debug;
+
+struct vivid_fmt {
+	u32	fourcc;          /* v4l2 format id */
+	bool	is_yuv;
+	bool	can_do_overlay;
+	u8	vdownsampling[TPG_MAX_PLANES];
+	u32	alpha_mask;
+	u8	planes;
+	u8	buffers;
+	u32	data_offset[TPG_MAX_PLANES];
+	u32	bit_depth[TPG_MAX_PLANES];
+};
+
+extern struct vivid_fmt vivid_formats[];
+
+/* buffer for one video frame */
+struct vivid_buffer {
+	/* common v4l buffer stuff -- must be first */
+	struct vb2_v4l2_buffer vb;
+	struct list_head	list;
+};
+
+enum vivid_input {
+	WEBCAM,
+	TV,
+	SVID,
+	HDMI,
+};
+
+enum vivid_signal_mode {
+	CURRENT_DV_TIMINGS,
+	CURRENT_STD = CURRENT_DV_TIMINGS,
+	NO_SIGNAL,
+	NO_LOCK,
+	OUT_OF_RANGE,
+	SELECTED_DV_TIMINGS,
+	SELECTED_STD = SELECTED_DV_TIMINGS,
+	CYCLE_DV_TIMINGS,
+	CYCLE_STD = CYCLE_DV_TIMINGS,
+	CUSTOM_DV_TIMINGS,
+};
+
+enum vivid_colorspace {
+	VIVID_CS_170M,
+	VIVID_CS_709,
+	VIVID_CS_SRGB,
+	VIVID_CS_ADOBERGB,
+	VIVID_CS_2020,
+	VIVID_CS_DCI_P3,
+	VIVID_CS_240M,
+	VIVID_CS_SYS_M,
+	VIVID_CS_SYS_BG,
+};
+
+#define VIVID_INVALID_SIGNAL(mode) \
+	((mode) == NO_SIGNAL || (mode) == NO_LOCK || (mode) == OUT_OF_RANGE)
+
+struct vivid_dev {
+	unsigned			inst;
+	struct v4l2_device		v4l2_dev;
+	struct v4l2_ctrl_handler	ctrl_hdl_user_gen;
+	struct v4l2_ctrl_handler	ctrl_hdl_user_vid;
+	struct v4l2_ctrl_handler	ctrl_hdl_user_aud;
+	struct v4l2_ctrl_handler	ctrl_hdl_streaming;
+	struct v4l2_ctrl_handler	ctrl_hdl_sdtv_cap;
+	struct v4l2_ctrl_handler	ctrl_hdl_loop_cap;
+	struct video_device		vid_cap_dev;
+	struct v4l2_ctrl_handler	ctrl_hdl_vid_cap;
+	struct video_device		vid_out_dev;
+	struct v4l2_ctrl_handler	ctrl_hdl_vid_out;
+	struct video_device		vbi_cap_dev;
+	struct v4l2_ctrl_handler	ctrl_hdl_vbi_cap;
+	struct video_device		vbi_out_dev;
+	struct v4l2_ctrl_handler	ctrl_hdl_vbi_out;
+	struct video_device		radio_rx_dev;
+	struct v4l2_ctrl_handler	ctrl_hdl_radio_rx;
+	struct video_device		radio_tx_dev;
+	struct v4l2_ctrl_handler	ctrl_hdl_radio_tx;
+	struct video_device		sdr_cap_dev;
+	struct v4l2_ctrl_handler	ctrl_hdl_sdr_cap;
+	spinlock_t			slock;
+	struct mutex			mutex;
+
+	/* capabilities */
+	u32				vid_cap_caps;
+	u32				vid_out_caps;
+	u32				vbi_cap_caps;
+	u32				vbi_out_caps;
+	u32				sdr_cap_caps;
+	u32				radio_rx_caps;
+	u32				radio_tx_caps;
+
+	/* supported features */
+	bool				multiplanar;
+	unsigned			num_inputs;
+	u8				input_type[MAX_INPUTS];
+	u8				input_name_counter[MAX_INPUTS];
+	unsigned			num_outputs;
+	u8				output_type[MAX_OUTPUTS];
+	u8				output_name_counter[MAX_OUTPUTS];
+	bool				has_audio_inputs;
+	bool				has_audio_outputs;
+	bool				has_vid_cap;
+	bool				has_vid_out;
+	bool				has_vbi_cap;
+	bool				has_raw_vbi_cap;
+	bool				has_sliced_vbi_cap;
+	bool				has_vbi_out;
+	bool				has_raw_vbi_out;
+	bool				has_sliced_vbi_out;
+	bool				has_radio_rx;
+	bool				has_radio_tx;
+	bool				has_sdr_cap;
+	bool				has_fb;
+
+	bool				can_loop_video;
+
+	/* controls */
+	struct v4l2_ctrl		*brightness;
+	struct v4l2_ctrl		*contrast;
+	struct v4l2_ctrl		*saturation;
+	struct v4l2_ctrl		*hue;
+	struct {
+		/* autogain/gain cluster */
+		struct v4l2_ctrl	*autogain;
+		struct v4l2_ctrl	*gain;
+	};
+	struct v4l2_ctrl		*volume;
+	struct v4l2_ctrl		*mute;
+	struct v4l2_ctrl		*alpha;
+	struct v4l2_ctrl		*button;
+	struct v4l2_ctrl		*boolean;
+	struct v4l2_ctrl		*int32;
+	struct v4l2_ctrl		*int64;
+	struct v4l2_ctrl		*menu;
+	struct v4l2_ctrl		*string;
+	struct v4l2_ctrl		*bitmask;
+	struct v4l2_ctrl		*int_menu;
+	struct v4l2_ctrl		*test_pattern;
+	struct v4l2_ctrl		*colorspace;
+	struct v4l2_ctrl		*rgb_range_cap;
+	struct v4l2_ctrl		*real_rgb_range_cap;
+	struct {
+		/* std_signal_mode/standard cluster */
+		struct v4l2_ctrl	*ctrl_std_signal_mode;
+		struct v4l2_ctrl	*ctrl_standard;
+	};
+	struct {
+		/* dv_timings_signal_mode/timings cluster */
+		struct v4l2_ctrl	*ctrl_dv_timings_signal_mode;
+		struct v4l2_ctrl	*ctrl_dv_timings;
+	};
+	struct v4l2_ctrl		*ctrl_has_crop_cap;
+	struct v4l2_ctrl		*ctrl_has_compose_cap;
+	struct v4l2_ctrl		*ctrl_has_scaler_cap;
+	struct v4l2_ctrl		*ctrl_has_crop_out;
+	struct v4l2_ctrl		*ctrl_has_compose_out;
+	struct v4l2_ctrl		*ctrl_has_scaler_out;
+	struct v4l2_ctrl		*ctrl_tx_mode;
+	struct v4l2_ctrl		*ctrl_tx_rgb_range;
+
+	struct v4l2_ctrl		*radio_tx_rds_pi;
+	struct v4l2_ctrl		*radio_tx_rds_pty;
+	struct v4l2_ctrl		*radio_tx_rds_mono_stereo;
+	struct v4l2_ctrl		*radio_tx_rds_art_head;
+	struct v4l2_ctrl		*radio_tx_rds_compressed;
+	struct v4l2_ctrl		*radio_tx_rds_dyn_pty;
+	struct v4l2_ctrl		*radio_tx_rds_ta;
+	struct v4l2_ctrl		*radio_tx_rds_tp;
+	struct v4l2_ctrl		*radio_tx_rds_ms;
+	struct v4l2_ctrl		*radio_tx_rds_psname;
+	struct v4l2_ctrl		*radio_tx_rds_radiotext;
+
+	struct v4l2_ctrl		*radio_rx_rds_pty;
+	struct v4l2_ctrl		*radio_rx_rds_ta;
+	struct v4l2_ctrl		*radio_rx_rds_tp;
+	struct v4l2_ctrl		*radio_rx_rds_ms;
+	struct v4l2_ctrl		*radio_rx_rds_psname;
+	struct v4l2_ctrl		*radio_rx_rds_radiotext;
+
+	unsigned			input_brightness[MAX_INPUTS];
+	unsigned			osd_mode;
+	unsigned			button_pressed;
+	bool				sensor_hflip;
+	bool				sensor_vflip;
+	bool				hflip;
+	bool				vflip;
+	bool				vbi_cap_interlaced;
+	bool				loop_video;
+
+	/* Framebuffer */
+	unsigned long			video_pbase;
+	void				*video_vbase;
+	u32				video_buffer_size;
+	int				display_width;
+	int				display_height;
+	int				display_byte_stride;
+	int				bits_per_pixel;
+	int				bytes_per_pixel;
+	struct fb_info			fb_info;
+	struct fb_var_screeninfo	fb_defined;
+	struct fb_fix_screeninfo	fb_fix;
+
+	/* Error injection */
+	bool				queue_setup_error;
+	bool				buf_prepare_error;
+	bool				start_streaming_error;
+	bool				dqbuf_error;
+	bool				seq_wrap;
+	bool				time_wrap;
+	__kernel_time_t			time_wrap_offset;
+	unsigned			perc_dropped_buffers;
+	enum vivid_signal_mode		std_signal_mode;
+	unsigned			query_std_last;
+	v4l2_std_id			query_std;
+	enum tpg_video_aspect		std_aspect_ratio;
+
+	enum vivid_signal_mode		dv_timings_signal_mode;
+	char				**query_dv_timings_qmenu;
+	unsigned			query_dv_timings_size;
+	unsigned			query_dv_timings_last;
+	unsigned			query_dv_timings;
+	enum tpg_video_aspect		dv_timings_aspect_ratio;
+
+	/* Input */
+	unsigned			input;
+	v4l2_std_id			std_cap;
+	struct v4l2_dv_timings		dv_timings_cap;
+	u32				service_set_cap;
+	struct vivid_vbi_gen_data	vbi_gen;
+	u8				*edid;
+	unsigned			edid_blocks;
+	unsigned			edid_max_blocks;
+	unsigned			webcam_size_idx;
+	unsigned			webcam_ival_idx;
+	unsigned			tv_freq;
+	unsigned			tv_audmode;
+	unsigned			tv_field_cap;
+	unsigned			tv_audio_input;
+
+	/* Capture Overlay */
+	struct v4l2_framebuffer		fb_cap;
+	struct v4l2_fh			*overlay_cap_owner;
+	void				*fb_vbase_cap;
+	int				overlay_cap_top, overlay_cap_left;
+	enum v4l2_field			overlay_cap_field;
+	void				*bitmap_cap;
+	struct v4l2_clip		clips_cap[MAX_CLIPS];
+	struct v4l2_clip		try_clips_cap[MAX_CLIPS];
+	unsigned			clipcount_cap;
+
+	/* Output */
+	unsigned			output;
+	v4l2_std_id			std_out;
+	struct v4l2_dv_timings		dv_timings_out;
+	u32				colorspace_out;
+	u32				ycbcr_enc_out;
+	u32				quantization_out;
+	u32				xfer_func_out;
+	u32				service_set_out;
+	unsigned			bytesperline_out[TPG_MAX_PLANES];
+	unsigned			tv_field_out;
+	unsigned			tv_audio_output;
+	bool				vbi_out_have_wss;
+	u8				vbi_out_wss[2];
+	bool				vbi_out_have_cc[2];
+	u8				vbi_out_cc[2][2];
+	bool				dvi_d_out;
+	u8				*scaled_line;
+	u8				*blended_line;
+	unsigned			cur_scaled_line;
+
+	/* Output Overlay */
+	void				*fb_vbase_out;
+	bool				overlay_out_enabled;
+	int				overlay_out_top, overlay_out_left;
+	void				*bitmap_out;
+	struct v4l2_clip		clips_out[MAX_CLIPS];
+	struct v4l2_clip		try_clips_out[MAX_CLIPS];
+	unsigned			clipcount_out;
+	unsigned			fbuf_out_flags;
+	u32				chromakey_out;
+	u8				global_alpha_out;
+
+	/* video capture */
+	struct tpg_data			tpg;
+	unsigned			ms_vid_cap;
+	bool				must_blank[VIDEO_MAX_FRAME];
+
+	const struct vivid_fmt		*fmt_cap;
+	struct v4l2_fract		timeperframe_vid_cap;
+	enum v4l2_field			field_cap;
+	struct v4l2_rect		src_rect;
+	struct v4l2_rect		fmt_cap_rect;
+	struct v4l2_rect		crop_cap;
+	struct v4l2_rect		compose_cap;
+	struct v4l2_rect		crop_bounds_cap;
+	struct vb2_queue		vb_vid_cap_q;
+	struct list_head		vid_cap_active;
+	struct vb2_queue		vb_vbi_cap_q;
+	struct list_head		vbi_cap_active;
+
+	/* thread for generating video capture stream */
+	struct task_struct		*kthread_vid_cap;
+	unsigned long			jiffies_vid_cap;
+	u32				cap_seq_offset;
+	u32				cap_seq_count;
+	bool				cap_seq_resync;
+	u32				vid_cap_seq_start;
+	u32				vid_cap_seq_count;
+	bool				vid_cap_streaming;
+	u32				vbi_cap_seq_start;
+	u32				vbi_cap_seq_count;
+	bool				vbi_cap_streaming;
+	bool				stream_sliced_vbi_cap;
+
+	/* video output */
+	const struct vivid_fmt		*fmt_out;
+	struct v4l2_fract		timeperframe_vid_out;
+	enum v4l2_field			field_out;
+	struct v4l2_rect		sink_rect;
+	struct v4l2_rect		fmt_out_rect;
+	struct v4l2_rect		crop_out;
+	struct v4l2_rect		compose_out;
+	struct v4l2_rect		compose_bounds_out;
+	struct vb2_queue		vb_vid_out_q;
+	struct list_head		vid_out_active;
+	struct vb2_queue		vb_vbi_out_q;
+	struct list_head		vbi_out_active;
+
+	/* video loop precalculated rectangles */
+
+	/*
+	 * Intersection between what the output side composes and the capture side
+	 * crops. I.e., what actually needs to be copied from the output buffer to
+	 * the capture buffer.
+	 */
+	struct v4l2_rect		loop_vid_copy;
+	/* The part of the output buffer that (after scaling) corresponds to loop_vid_copy. */
+	struct v4l2_rect		loop_vid_out;
+	/* The part of the capture buffer that (after scaling) corresponds to loop_vid_copy. */
+	struct v4l2_rect		loop_vid_cap;
+	/*
+	 * The intersection of the framebuffer, the overlay output window and
+	 * loop_vid_copy. I.e., the part of the framebuffer that actually should be
+	 * blended with the compose_out rectangle. This uses the framebuffer origin.
+	 */
+	struct v4l2_rect		loop_fb_copy;
+	/* The same as loop_fb_copy but with compose_out origin. */
+	struct v4l2_rect		loop_vid_overlay;
+	/*
+	 * The part of the capture buffer that (after scaling) corresponds
+	 * to loop_vid_overlay.
+	 */
+	struct v4l2_rect		loop_vid_overlay_cap;
+
+	/* thread for generating video output stream */
+	struct task_struct		*kthread_vid_out;
+	unsigned long			jiffies_vid_out;
+	u32				out_seq_offset;
+	u32				out_seq_count;
+	bool				out_seq_resync;
+	u32				vid_out_seq_start;
+	u32				vid_out_seq_count;
+	bool				vid_out_streaming;
+	u32				vbi_out_seq_start;
+	u32				vbi_out_seq_count;
+	bool				vbi_out_streaming;
+	bool				stream_sliced_vbi_out;
+
+	/* SDR capture */
+	struct vb2_queue		vb_sdr_cap_q;
+	struct list_head		sdr_cap_active;
+	u32				sdr_pixelformat; /* v4l2 format id */
+	unsigned			sdr_buffersize;
+	unsigned			sdr_adc_freq;
+	unsigned			sdr_fm_freq;
+	unsigned			sdr_fm_deviation;
+	int				sdr_fixp_src_phase;
+	int				sdr_fixp_mod_phase;
+
+	bool				tstamp_src_is_soe;
+	bool				has_crop_cap;
+	bool				has_compose_cap;
+	bool				has_scaler_cap;
+	bool				has_crop_out;
+	bool				has_compose_out;
+	bool				has_scaler_out;
+
+	/* thread for generating SDR stream */
+	struct task_struct		*kthread_sdr_cap;
+	unsigned long			jiffies_sdr_cap;
+	u32				sdr_cap_seq_offset;
+	u32				sdr_cap_seq_count;
+	bool				sdr_cap_seq_resync;
+
+	/* RDS generator */
+	struct vivid_rds_gen		rds_gen;
+
+	/* Radio receiver */
+	unsigned			radio_rx_freq;
+	unsigned			radio_rx_audmode;
+	int				radio_rx_sig_qual;
+	unsigned			radio_rx_hw_seek_mode;
+	bool				radio_rx_hw_seek_prog_lim;
+	bool				radio_rx_rds_controls;
+	bool				radio_rx_rds_enabled;
+	unsigned			radio_rx_rds_use_alternates;
+	unsigned			radio_rx_rds_last_block;
+	struct v4l2_fh			*radio_rx_rds_owner;
+
+	/* Radio transmitter */
+	unsigned			radio_tx_freq;
+	unsigned			radio_tx_subchans;
+	bool				radio_tx_rds_controls;
+	unsigned			radio_tx_rds_last_block;
+	struct v4l2_fh			*radio_tx_rds_owner;
+
+	/* Shared between radio receiver and transmitter */
+	bool				radio_rds_loop;
+	struct timespec			radio_rds_init_ts;
+};
+
+static inline bool vivid_is_webcam(const struct vivid_dev *dev)
+{
+	return dev->input_type[dev->input] == WEBCAM;
+}
+
+static inline bool vivid_is_tv_cap(const struct vivid_dev *dev)
+{
+	return dev->input_type[dev->input] == TV;
+}
+
+static inline bool vivid_is_svid_cap(const struct vivid_dev *dev)
+{
+	return dev->input_type[dev->input] == SVID;
+}
+
+static inline bool vivid_is_hdmi_cap(const struct vivid_dev *dev)
+{
+	return dev->input_type[dev->input] == HDMI;
+}
+
+static inline bool vivid_is_sdtv_cap(const struct vivid_dev *dev)
+{
+	return vivid_is_tv_cap(dev) || vivid_is_svid_cap(dev);
+}
+
+static inline bool vivid_is_svid_out(const struct vivid_dev *dev)
+{
+	return dev->output_type[dev->output] == SVID;
+}
+
+static inline bool vivid_is_hdmi_out(const struct vivid_dev *dev)
+{
+	return dev->output_type[dev->output] == HDMI;
+}
+
+#endif
diff --git a/drivers/media/platform/vivid/vivid-ctrls.c b/drivers/media/platform/vivid/vivid-ctrls.c
new file mode 100644
index 0000000..f41ac0b
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-ctrls.c
@@ -0,0 +1,1677 @@
+/*
+ * vivid-ctrls.c - control support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-common.h>
+
+#include "vivid-core.h"
+#include "vivid-vid-cap.h"
+#include "vivid-vid-out.h"
+#include "vivid-vid-common.h"
+#include "vivid-radio-common.h"
+#include "vivid-osd.h"
+#include "vivid-ctrls.h"
+
+#define VIVID_CID_CUSTOM_BASE		(V4L2_CID_USER_BASE | 0xf000)
+#define VIVID_CID_BUTTON		(VIVID_CID_CUSTOM_BASE + 0)
+#define VIVID_CID_BOOLEAN		(VIVID_CID_CUSTOM_BASE + 1)
+#define VIVID_CID_INTEGER		(VIVID_CID_CUSTOM_BASE + 2)
+#define VIVID_CID_INTEGER64		(VIVID_CID_CUSTOM_BASE + 3)
+#define VIVID_CID_MENU			(VIVID_CID_CUSTOM_BASE + 4)
+#define VIVID_CID_STRING		(VIVID_CID_CUSTOM_BASE + 5)
+#define VIVID_CID_BITMASK		(VIVID_CID_CUSTOM_BASE + 6)
+#define VIVID_CID_INTMENU		(VIVID_CID_CUSTOM_BASE + 7)
+#define VIVID_CID_U32_ARRAY		(VIVID_CID_CUSTOM_BASE + 8)
+#define VIVID_CID_U16_MATRIX		(VIVID_CID_CUSTOM_BASE + 9)
+#define VIVID_CID_U8_4D_ARRAY		(VIVID_CID_CUSTOM_BASE + 10)
+
+#define VIVID_CID_VIVID_BASE		(0x00f00000 | 0xf000)
+#define VIVID_CID_VIVID_CLASS		(0x00f00000 | 1)
+#define VIVID_CID_TEST_PATTERN		(VIVID_CID_VIVID_BASE + 0)
+#define VIVID_CID_OSD_TEXT_MODE		(VIVID_CID_VIVID_BASE + 1)
+#define VIVID_CID_HOR_MOVEMENT		(VIVID_CID_VIVID_BASE + 2)
+#define VIVID_CID_VERT_MOVEMENT		(VIVID_CID_VIVID_BASE + 3)
+#define VIVID_CID_SHOW_BORDER		(VIVID_CID_VIVID_BASE + 4)
+#define VIVID_CID_SHOW_SQUARE		(VIVID_CID_VIVID_BASE + 5)
+#define VIVID_CID_INSERT_SAV		(VIVID_CID_VIVID_BASE + 6)
+#define VIVID_CID_INSERT_EAV		(VIVID_CID_VIVID_BASE + 7)
+#define VIVID_CID_VBI_CAP_INTERLACED	(VIVID_CID_VIVID_BASE + 8)
+
+#define VIVID_CID_HFLIP			(VIVID_CID_VIVID_BASE + 20)
+#define VIVID_CID_VFLIP			(VIVID_CID_VIVID_BASE + 21)
+#define VIVID_CID_STD_ASPECT_RATIO	(VIVID_CID_VIVID_BASE + 22)
+#define VIVID_CID_DV_TIMINGS_ASPECT_RATIO	(VIVID_CID_VIVID_BASE + 23)
+#define VIVID_CID_TSTAMP_SRC		(VIVID_CID_VIVID_BASE + 24)
+#define VIVID_CID_COLORSPACE		(VIVID_CID_VIVID_BASE + 25)
+#define VIVID_CID_XFER_FUNC		(VIVID_CID_VIVID_BASE + 26)
+#define VIVID_CID_YCBCR_ENC		(VIVID_CID_VIVID_BASE + 27)
+#define VIVID_CID_QUANTIZATION		(VIVID_CID_VIVID_BASE + 28)
+#define VIVID_CID_LIMITED_RGB_RANGE	(VIVID_CID_VIVID_BASE + 29)
+#define VIVID_CID_ALPHA_MODE		(VIVID_CID_VIVID_BASE + 30)
+#define VIVID_CID_HAS_CROP_CAP		(VIVID_CID_VIVID_BASE + 31)
+#define VIVID_CID_HAS_COMPOSE_CAP	(VIVID_CID_VIVID_BASE + 32)
+#define VIVID_CID_HAS_SCALER_CAP	(VIVID_CID_VIVID_BASE + 33)
+#define VIVID_CID_HAS_CROP_OUT		(VIVID_CID_VIVID_BASE + 34)
+#define VIVID_CID_HAS_COMPOSE_OUT	(VIVID_CID_VIVID_BASE + 35)
+#define VIVID_CID_HAS_SCALER_OUT	(VIVID_CID_VIVID_BASE + 36)
+#define VIVID_CID_LOOP_VIDEO		(VIVID_CID_VIVID_BASE + 37)
+#define VIVID_CID_SEQ_WRAP		(VIVID_CID_VIVID_BASE + 38)
+#define VIVID_CID_TIME_WRAP		(VIVID_CID_VIVID_BASE + 39)
+#define VIVID_CID_MAX_EDID_BLOCKS	(VIVID_CID_VIVID_BASE + 40)
+#define VIVID_CID_PERCENTAGE_FILL	(VIVID_CID_VIVID_BASE + 41)
+
+#define VIVID_CID_STD_SIGNAL_MODE	(VIVID_CID_VIVID_BASE + 60)
+#define VIVID_CID_STANDARD		(VIVID_CID_VIVID_BASE + 61)
+#define VIVID_CID_DV_TIMINGS_SIGNAL_MODE	(VIVID_CID_VIVID_BASE + 62)
+#define VIVID_CID_DV_TIMINGS		(VIVID_CID_VIVID_BASE + 63)
+#define VIVID_CID_PERC_DROPPED		(VIVID_CID_VIVID_BASE + 64)
+#define VIVID_CID_DISCONNECT		(VIVID_CID_VIVID_BASE + 65)
+#define VIVID_CID_DQBUF_ERROR		(VIVID_CID_VIVID_BASE + 66)
+#define VIVID_CID_QUEUE_SETUP_ERROR	(VIVID_CID_VIVID_BASE + 67)
+#define VIVID_CID_BUF_PREPARE_ERROR	(VIVID_CID_VIVID_BASE + 68)
+#define VIVID_CID_START_STR_ERROR	(VIVID_CID_VIVID_BASE + 69)
+#define VIVID_CID_QUEUE_ERROR		(VIVID_CID_VIVID_BASE + 70)
+#define VIVID_CID_CLEAR_FB		(VIVID_CID_VIVID_BASE + 71)
+
+#define VIVID_CID_RADIO_SEEK_MODE	(VIVID_CID_VIVID_BASE + 90)
+#define VIVID_CID_RADIO_SEEK_PROG_LIM	(VIVID_CID_VIVID_BASE + 91)
+#define VIVID_CID_RADIO_RX_RDS_RBDS	(VIVID_CID_VIVID_BASE + 92)
+#define VIVID_CID_RADIO_RX_RDS_BLOCKIO	(VIVID_CID_VIVID_BASE + 93)
+
+#define VIVID_CID_RADIO_TX_RDS_BLOCKIO	(VIVID_CID_VIVID_BASE + 94)
+
+#define VIVID_CID_SDR_CAP_FM_DEVIATION	(VIVID_CID_VIVID_BASE + 110)
+
+/* General User Controls */
+
+static int vivid_user_gen_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_user_gen);
+
+	switch (ctrl->id) {
+	case VIVID_CID_DISCONNECT:
+		v4l2_info(&dev->v4l2_dev, "disconnect\n");
+		clear_bit(V4L2_FL_REGISTERED, &dev->vid_cap_dev.flags);
+		clear_bit(V4L2_FL_REGISTERED, &dev->vid_out_dev.flags);
+		clear_bit(V4L2_FL_REGISTERED, &dev->vbi_cap_dev.flags);
+		clear_bit(V4L2_FL_REGISTERED, &dev->vbi_out_dev.flags);
+		clear_bit(V4L2_FL_REGISTERED, &dev->sdr_cap_dev.flags);
+		clear_bit(V4L2_FL_REGISTERED, &dev->radio_rx_dev.flags);
+		clear_bit(V4L2_FL_REGISTERED, &dev->radio_tx_dev.flags);
+		break;
+	case VIVID_CID_CLEAR_FB:
+		vivid_clear_fb(dev);
+		break;
+	case VIVID_CID_BUTTON:
+		dev->button_pressed = 30;
+		break;
+	}
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops vivid_user_gen_ctrl_ops = {
+	.s_ctrl = vivid_user_gen_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_button = {
+	.ops = &vivid_user_gen_ctrl_ops,
+	.id = VIVID_CID_BUTTON,
+	.name = "Button",
+	.type = V4L2_CTRL_TYPE_BUTTON,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_boolean = {
+	.ops = &vivid_user_gen_ctrl_ops,
+	.id = VIVID_CID_BOOLEAN,
+	.name = "Boolean",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.min = 0,
+	.max = 1,
+	.step = 1,
+	.def = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_int32 = {
+	.ops = &vivid_user_gen_ctrl_ops,
+	.id = VIVID_CID_INTEGER,
+	.name = "Integer 32 Bits",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.min = 0xffffffff80000000ULL,
+	.max = 0x7fffffff,
+	.step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_int64 = {
+	.ops = &vivid_user_gen_ctrl_ops,
+	.id = VIVID_CID_INTEGER64,
+	.name = "Integer 64 Bits",
+	.type = V4L2_CTRL_TYPE_INTEGER64,
+	.min = 0x8000000000000000ULL,
+	.max = 0x7fffffffffffffffLL,
+	.step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_u32_array = {
+	.ops = &vivid_user_gen_ctrl_ops,
+	.id = VIVID_CID_U32_ARRAY,
+	.name = "U32 1 Element Array",
+	.type = V4L2_CTRL_TYPE_U32,
+	.def = 0x18,
+	.min = 0x10,
+	.max = 0x20000,
+	.step = 1,
+	.dims = { 1 },
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_u16_matrix = {
+	.ops = &vivid_user_gen_ctrl_ops,
+	.id = VIVID_CID_U16_MATRIX,
+	.name = "U16 8x16 Matrix",
+	.type = V4L2_CTRL_TYPE_U16,
+	.def = 0x18,
+	.min = 0x10,
+	.max = 0x2000,
+	.step = 1,
+	.dims = { 8, 16 },
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_u8_4d_array = {
+	.ops = &vivid_user_gen_ctrl_ops,
+	.id = VIVID_CID_U8_4D_ARRAY,
+	.name = "U8 2x3x4x5 Array",
+	.type = V4L2_CTRL_TYPE_U8,
+	.def = 0x18,
+	.min = 0x10,
+	.max = 0x20,
+	.step = 1,
+	.dims = { 2, 3, 4, 5 },
+};
+
+static const char * const vivid_ctrl_menu_strings[] = {
+	"Menu Item 0 (Skipped)",
+	"Menu Item 1",
+	"Menu Item 2 (Skipped)",
+	"Menu Item 3",
+	"Menu Item 4",
+	"Menu Item 5 (Skipped)",
+	NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_menu = {
+	.ops = &vivid_user_gen_ctrl_ops,
+	.id = VIVID_CID_MENU,
+	.name = "Menu",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.min = 1,
+	.max = 4,
+	.def = 3,
+	.menu_skip_mask = 0x04,
+	.qmenu = vivid_ctrl_menu_strings,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_string = {
+	.ops = &vivid_user_gen_ctrl_ops,
+	.id = VIVID_CID_STRING,
+	.name = "String",
+	.type = V4L2_CTRL_TYPE_STRING,
+	.min = 2,
+	.max = 4,
+	.step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_bitmask = {
+	.ops = &vivid_user_gen_ctrl_ops,
+	.id = VIVID_CID_BITMASK,
+	.name = "Bitmask",
+	.type = V4L2_CTRL_TYPE_BITMASK,
+	.def = 0x80002000,
+	.min = 0,
+	.max = 0x80402010,
+	.step = 0,
+};
+
+static const s64 vivid_ctrl_int_menu_values[] = {
+	1, 1, 2, 3, 5, 8, 13, 21, 42,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_int_menu = {
+	.ops = &vivid_user_gen_ctrl_ops,
+	.id = VIVID_CID_INTMENU,
+	.name = "Integer Menu",
+	.type = V4L2_CTRL_TYPE_INTEGER_MENU,
+	.min = 1,
+	.max = 8,
+	.def = 4,
+	.menu_skip_mask = 0x02,
+	.qmenu_int = vivid_ctrl_int_menu_values,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_disconnect = {
+	.ops = &vivid_user_gen_ctrl_ops,
+	.id = VIVID_CID_DISCONNECT,
+	.name = "Disconnect",
+	.type = V4L2_CTRL_TYPE_BUTTON,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_clear_fb = {
+	.ops = &vivid_user_gen_ctrl_ops,
+	.id = VIVID_CID_CLEAR_FB,
+	.name = "Clear Framebuffer",
+	.type = V4L2_CTRL_TYPE_BUTTON,
+};
+
+
+/* Video User Controls */
+
+static int vivid_user_vid_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_user_vid);
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTOGAIN:
+		dev->gain->val = dev->jiffies_vid_cap & 0xff;
+		break;
+	}
+	return 0;
+}
+
+static int vivid_user_vid_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_user_vid);
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		dev->input_brightness[dev->input] = ctrl->val - dev->input * 128;
+		tpg_s_brightness(&dev->tpg, dev->input_brightness[dev->input]);
+		break;
+	case V4L2_CID_CONTRAST:
+		tpg_s_contrast(&dev->tpg, ctrl->val);
+		break;
+	case V4L2_CID_SATURATION:
+		tpg_s_saturation(&dev->tpg, ctrl->val);
+		break;
+	case V4L2_CID_HUE:
+		tpg_s_hue(&dev->tpg, ctrl->val);
+		break;
+	case V4L2_CID_HFLIP:
+		dev->hflip = ctrl->val;
+		tpg_s_hflip(&dev->tpg, dev->sensor_hflip ^ dev->hflip);
+		break;
+	case V4L2_CID_VFLIP:
+		dev->vflip = ctrl->val;
+		tpg_s_vflip(&dev->tpg, dev->sensor_vflip ^ dev->vflip);
+		break;
+	case V4L2_CID_ALPHA_COMPONENT:
+		tpg_s_alpha_component(&dev->tpg, ctrl->val);
+		break;
+	}
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops vivid_user_vid_ctrl_ops = {
+	.g_volatile_ctrl = vivid_user_vid_g_volatile_ctrl,
+	.s_ctrl = vivid_user_vid_s_ctrl,
+};
+
+
+/* Video Capture Controls */
+
+static int vivid_vid_cap_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	static const u32 colorspaces[] = {
+		V4L2_COLORSPACE_SMPTE170M,
+		V4L2_COLORSPACE_REC709,
+		V4L2_COLORSPACE_SRGB,
+		V4L2_COLORSPACE_ADOBERGB,
+		V4L2_COLORSPACE_BT2020,
+		V4L2_COLORSPACE_DCI_P3,
+		V4L2_COLORSPACE_SMPTE240M,
+		V4L2_COLORSPACE_470_SYSTEM_M,
+		V4L2_COLORSPACE_470_SYSTEM_BG,
+	};
+	struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_vid_cap);
+	unsigned i;
+
+	switch (ctrl->id) {
+	case VIVID_CID_TEST_PATTERN:
+		vivid_update_quality(dev);
+		tpg_s_pattern(&dev->tpg, ctrl->val);
+		break;
+	case VIVID_CID_COLORSPACE:
+		tpg_s_colorspace(&dev->tpg, colorspaces[ctrl->val]);
+		vivid_send_source_change(dev, TV);
+		vivid_send_source_change(dev, SVID);
+		vivid_send_source_change(dev, HDMI);
+		vivid_send_source_change(dev, WEBCAM);
+		break;
+	case VIVID_CID_XFER_FUNC:
+		tpg_s_xfer_func(&dev->tpg, ctrl->val);
+		vivid_send_source_change(dev, TV);
+		vivid_send_source_change(dev, SVID);
+		vivid_send_source_change(dev, HDMI);
+		vivid_send_source_change(dev, WEBCAM);
+		break;
+	case VIVID_CID_YCBCR_ENC:
+		tpg_s_ycbcr_enc(&dev->tpg, ctrl->val);
+		vivid_send_source_change(dev, TV);
+		vivid_send_source_change(dev, SVID);
+		vivid_send_source_change(dev, HDMI);
+		vivid_send_source_change(dev, WEBCAM);
+		break;
+	case VIVID_CID_QUANTIZATION:
+		tpg_s_quantization(&dev->tpg, ctrl->val);
+		vivid_send_source_change(dev, TV);
+		vivid_send_source_change(dev, SVID);
+		vivid_send_source_change(dev, HDMI);
+		vivid_send_source_change(dev, WEBCAM);
+		break;
+	case V4L2_CID_DV_RX_RGB_RANGE:
+		if (!vivid_is_hdmi_cap(dev))
+			break;
+		tpg_s_rgb_range(&dev->tpg, ctrl->val);
+		break;
+	case VIVID_CID_LIMITED_RGB_RANGE:
+		tpg_s_real_rgb_range(&dev->tpg, ctrl->val ?
+				V4L2_DV_RGB_RANGE_LIMITED : V4L2_DV_RGB_RANGE_FULL);
+		break;
+	case VIVID_CID_ALPHA_MODE:
+		tpg_s_alpha_mode(&dev->tpg, ctrl->val);
+		break;
+	case VIVID_CID_HOR_MOVEMENT:
+		tpg_s_mv_hor_mode(&dev->tpg, ctrl->val);
+		break;
+	case VIVID_CID_VERT_MOVEMENT:
+		tpg_s_mv_vert_mode(&dev->tpg, ctrl->val);
+		break;
+	case VIVID_CID_OSD_TEXT_MODE:
+		dev->osd_mode = ctrl->val;
+		break;
+	case VIVID_CID_PERCENTAGE_FILL:
+		tpg_s_perc_fill(&dev->tpg, ctrl->val);
+		for (i = 0; i < VIDEO_MAX_FRAME; i++)
+			dev->must_blank[i] = ctrl->val < 100;
+		break;
+	case VIVID_CID_INSERT_SAV:
+		tpg_s_insert_sav(&dev->tpg, ctrl->val);
+		break;
+	case VIVID_CID_INSERT_EAV:
+		tpg_s_insert_eav(&dev->tpg, ctrl->val);
+		break;
+	case VIVID_CID_HFLIP:
+		dev->sensor_hflip = ctrl->val;
+		tpg_s_hflip(&dev->tpg, dev->sensor_hflip ^ dev->hflip);
+		break;
+	case VIVID_CID_VFLIP:
+		dev->sensor_vflip = ctrl->val;
+		tpg_s_vflip(&dev->tpg, dev->sensor_vflip ^ dev->vflip);
+		break;
+	case VIVID_CID_HAS_CROP_CAP:
+		dev->has_crop_cap = ctrl->val;
+		vivid_update_format_cap(dev, true);
+		break;
+	case VIVID_CID_HAS_COMPOSE_CAP:
+		dev->has_compose_cap = ctrl->val;
+		vivid_update_format_cap(dev, true);
+		break;
+	case VIVID_CID_HAS_SCALER_CAP:
+		dev->has_scaler_cap = ctrl->val;
+		vivid_update_format_cap(dev, true);
+		break;
+	case VIVID_CID_SHOW_BORDER:
+		tpg_s_show_border(&dev->tpg, ctrl->val);
+		break;
+	case VIVID_CID_SHOW_SQUARE:
+		tpg_s_show_square(&dev->tpg, ctrl->val);
+		break;
+	case VIVID_CID_STD_ASPECT_RATIO:
+		dev->std_aspect_ratio = ctrl->val;
+		tpg_s_video_aspect(&dev->tpg, vivid_get_video_aspect(dev));
+		break;
+	case VIVID_CID_DV_TIMINGS_SIGNAL_MODE:
+		dev->dv_timings_signal_mode = dev->ctrl_dv_timings_signal_mode->val;
+		if (dev->dv_timings_signal_mode == SELECTED_DV_TIMINGS)
+			dev->query_dv_timings = dev->ctrl_dv_timings->val;
+		v4l2_ctrl_activate(dev->ctrl_dv_timings,
+				dev->dv_timings_signal_mode == SELECTED_DV_TIMINGS);
+		vivid_update_quality(dev);
+		vivid_send_source_change(dev, HDMI);
+		break;
+	case VIVID_CID_DV_TIMINGS_ASPECT_RATIO:
+		dev->dv_timings_aspect_ratio = ctrl->val;
+		tpg_s_video_aspect(&dev->tpg, vivid_get_video_aspect(dev));
+		break;
+	case VIVID_CID_TSTAMP_SRC:
+		dev->tstamp_src_is_soe = ctrl->val;
+		dev->vb_vid_cap_q.timestamp_flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+		if (dev->tstamp_src_is_soe)
+			dev->vb_vid_cap_q.timestamp_flags |= V4L2_BUF_FLAG_TSTAMP_SRC_SOE;
+		break;
+	case VIVID_CID_MAX_EDID_BLOCKS:
+		dev->edid_max_blocks = ctrl->val;
+		if (dev->edid_blocks > dev->edid_max_blocks)
+			dev->edid_blocks = dev->edid_max_blocks;
+		break;
+	}
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops vivid_vid_cap_ctrl_ops = {
+	.s_ctrl = vivid_vid_cap_s_ctrl,
+};
+
+static const char * const vivid_ctrl_hor_movement_strings[] = {
+	"Move Left Fast",
+	"Move Left",
+	"Move Left Slow",
+	"No Movement",
+	"Move Right Slow",
+	"Move Right",
+	"Move Right Fast",
+	NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_hor_movement = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_HOR_MOVEMENT,
+	.name = "Horizontal Movement",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.max = TPG_MOVE_POS_FAST,
+	.def = TPG_MOVE_NONE,
+	.qmenu = vivid_ctrl_hor_movement_strings,
+};
+
+static const char * const vivid_ctrl_vert_movement_strings[] = {
+	"Move Up Fast",
+	"Move Up",
+	"Move Up Slow",
+	"No Movement",
+	"Move Down Slow",
+	"Move Down",
+	"Move Down Fast",
+	NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_vert_movement = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_VERT_MOVEMENT,
+	.name = "Vertical Movement",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.max = TPG_MOVE_POS_FAST,
+	.def = TPG_MOVE_NONE,
+	.qmenu = vivid_ctrl_vert_movement_strings,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_show_border = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_SHOW_BORDER,
+	.name = "Show Border",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_show_square = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_SHOW_SQUARE,
+	.name = "Show Square",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.step = 1,
+};
+
+static const char * const vivid_ctrl_osd_mode_strings[] = {
+	"All",
+	"Counters Only",
+	"None",
+	NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_osd_mode = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_OSD_TEXT_MODE,
+	.name = "OSD Text Mode",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.max = ARRAY_SIZE(vivid_ctrl_osd_mode_strings) - 2,
+	.qmenu = vivid_ctrl_osd_mode_strings,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_perc_fill = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_PERCENTAGE_FILL,
+	.name = "Fill Percentage of Frame",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.min = 0,
+	.max = 100,
+	.def = 100,
+	.step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_insert_sav = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_INSERT_SAV,
+	.name = "Insert SAV Code in Image",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_insert_eav = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_INSERT_EAV,
+	.name = "Insert EAV Code in Image",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_hflip = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_HFLIP,
+	.name = "Sensor Flipped Horizontally",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_vflip = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_VFLIP,
+	.name = "Sensor Flipped Vertically",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_has_crop_cap = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_HAS_CROP_CAP,
+	.name = "Enable Capture Cropping",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.def = 1,
+	.step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_has_compose_cap = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_HAS_COMPOSE_CAP,
+	.name = "Enable Capture Composing",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.def = 1,
+	.step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_has_scaler_cap = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_HAS_SCALER_CAP,
+	.name = "Enable Capture Scaler",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.def = 1,
+	.step = 1,
+};
+
+static const char * const vivid_ctrl_tstamp_src_strings[] = {
+	"End of Frame",
+	"Start of Exposure",
+	NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_tstamp_src = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_TSTAMP_SRC,
+	.name = "Timestamp Source",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.max = ARRAY_SIZE(vivid_ctrl_tstamp_src_strings) - 2,
+	.qmenu = vivid_ctrl_tstamp_src_strings,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_std_aspect_ratio = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_STD_ASPECT_RATIO,
+	.name = "Standard Aspect Ratio",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.min = 1,
+	.max = 4,
+	.def = 1,
+	.qmenu = tpg_aspect_strings,
+};
+
+static const char * const vivid_ctrl_dv_timings_signal_mode_strings[] = {
+	"Current DV Timings",
+	"No Signal",
+	"No Lock",
+	"Out of Range",
+	"Selected DV Timings",
+	"Cycle Through All DV Timings",
+	"Custom DV Timings",
+	NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_dv_timings_signal_mode = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_DV_TIMINGS_SIGNAL_MODE,
+	.name = "DV Timings Signal Mode",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.max = 5,
+	.qmenu = vivid_ctrl_dv_timings_signal_mode_strings,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_dv_timings_aspect_ratio = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_DV_TIMINGS_ASPECT_RATIO,
+	.name = "DV Timings Aspect Ratio",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.max = 3,
+	.qmenu = tpg_aspect_strings,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_max_edid_blocks = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_MAX_EDID_BLOCKS,
+	.name = "Maximum EDID Blocks",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.min = 1,
+	.max = 256,
+	.def = 2,
+	.step = 1,
+};
+
+static const char * const vivid_ctrl_colorspace_strings[] = {
+	"SMPTE 170M",
+	"Rec. 709",
+	"sRGB",
+	"AdobeRGB",
+	"BT.2020",
+	"DCI-P3",
+	"SMPTE 240M",
+	"470 System M",
+	"470 System BG",
+	NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_colorspace = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_COLORSPACE,
+	.name = "Colorspace",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.max = ARRAY_SIZE(vivid_ctrl_colorspace_strings) - 2,
+	.def = 2,
+	.qmenu = vivid_ctrl_colorspace_strings,
+};
+
+static const char * const vivid_ctrl_xfer_func_strings[] = {
+	"Default",
+	"Rec. 709",
+	"sRGB",
+	"AdobeRGB",
+	"SMPTE 240M",
+	"None",
+	"DCI-P3",
+	"SMPTE 2084",
+	NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_xfer_func = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_XFER_FUNC,
+	.name = "Transfer Function",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.max = ARRAY_SIZE(vivid_ctrl_xfer_func_strings) - 2,
+	.qmenu = vivid_ctrl_xfer_func_strings,
+};
+
+static const char * const vivid_ctrl_ycbcr_enc_strings[] = {
+	"Default",
+	"ITU-R 601",
+	"Rec. 709",
+	"xvYCC 601",
+	"xvYCC 709",
+	"sYCC",
+	"BT.2020",
+	"BT.2020 Constant Luminance",
+	"SMPTE 240M",
+	NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_ycbcr_enc = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_YCBCR_ENC,
+	.name = "Y'CbCr Encoding",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.max = ARRAY_SIZE(vivid_ctrl_ycbcr_enc_strings) - 2,
+	.qmenu = vivid_ctrl_ycbcr_enc_strings,
+};
+
+static const char * const vivid_ctrl_quantization_strings[] = {
+	"Default",
+	"Full Range",
+	"Limited Range",
+	NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_quantization = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_QUANTIZATION,
+	.name = "Quantization",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.max = ARRAY_SIZE(vivid_ctrl_quantization_strings) - 2,
+	.qmenu = vivid_ctrl_quantization_strings,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_alpha_mode = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_ALPHA_MODE,
+	.name = "Apply Alpha To Red Only",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_limited_rgb_range = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_LIMITED_RGB_RANGE,
+	.name = "Limited RGB Range (16-235)",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.step = 1,
+};
+
+
+/* Video Loop Control */
+
+static int vivid_loop_cap_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_loop_cap);
+
+	switch (ctrl->id) {
+	case VIVID_CID_LOOP_VIDEO:
+		dev->loop_video = ctrl->val;
+		vivid_update_quality(dev);
+		vivid_send_source_change(dev, SVID);
+		vivid_send_source_change(dev, HDMI);
+		break;
+	}
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops vivid_loop_cap_ctrl_ops = {
+	.s_ctrl = vivid_loop_cap_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_loop_video = {
+	.ops = &vivid_loop_cap_ctrl_ops,
+	.id = VIVID_CID_LOOP_VIDEO,
+	.name = "Loop Video",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.step = 1,
+};
+
+
+/* VBI Capture Control */
+
+static int vivid_vbi_cap_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_vbi_cap);
+
+	switch (ctrl->id) {
+	case VIVID_CID_VBI_CAP_INTERLACED:
+		dev->vbi_cap_interlaced = ctrl->val;
+		break;
+	}
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops vivid_vbi_cap_ctrl_ops = {
+	.s_ctrl = vivid_vbi_cap_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_vbi_cap_interlaced = {
+	.ops = &vivid_vbi_cap_ctrl_ops,
+	.id = VIVID_CID_VBI_CAP_INTERLACED,
+	.name = "Interlaced VBI Format",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.step = 1,
+};
+
+
+/* Video Output Controls */
+
+static int vivid_vid_out_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_vid_out);
+	struct v4l2_bt_timings *bt = &dev->dv_timings_out.bt;
+
+	switch (ctrl->id) {
+	case VIVID_CID_HAS_CROP_OUT:
+		dev->has_crop_out = ctrl->val;
+		vivid_update_format_out(dev);
+		break;
+	case VIVID_CID_HAS_COMPOSE_OUT:
+		dev->has_compose_out = ctrl->val;
+		vivid_update_format_out(dev);
+		break;
+	case VIVID_CID_HAS_SCALER_OUT:
+		dev->has_scaler_out = ctrl->val;
+		vivid_update_format_out(dev);
+		break;
+	case V4L2_CID_DV_TX_MODE:
+		dev->dvi_d_out = ctrl->val == V4L2_DV_TX_MODE_DVI_D;
+		if (!vivid_is_hdmi_out(dev))
+			break;
+		if (!dev->dvi_d_out && (bt->flags & V4L2_DV_FL_IS_CE_VIDEO)) {
+			if (bt->width == 720 && bt->height <= 576)
+				dev->colorspace_out = V4L2_COLORSPACE_SMPTE170M;
+			else
+				dev->colorspace_out = V4L2_COLORSPACE_REC709;
+			dev->quantization_out = V4L2_QUANTIZATION_DEFAULT;
+		} else {
+			dev->colorspace_out = V4L2_COLORSPACE_SRGB;
+			dev->quantization_out = dev->dvi_d_out ?
+					V4L2_QUANTIZATION_LIM_RANGE :
+					V4L2_QUANTIZATION_DEFAULT;
+		}
+		if (dev->loop_video)
+			vivid_send_source_change(dev, HDMI);
+		break;
+	}
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops vivid_vid_out_ctrl_ops = {
+	.s_ctrl = vivid_vid_out_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_has_crop_out = {
+	.ops = &vivid_vid_out_ctrl_ops,
+	.id = VIVID_CID_HAS_CROP_OUT,
+	.name = "Enable Output Cropping",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.def = 1,
+	.step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_has_compose_out = {
+	.ops = &vivid_vid_out_ctrl_ops,
+	.id = VIVID_CID_HAS_COMPOSE_OUT,
+	.name = "Enable Output Composing",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.def = 1,
+	.step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_has_scaler_out = {
+	.ops = &vivid_vid_out_ctrl_ops,
+	.id = VIVID_CID_HAS_SCALER_OUT,
+	.name = "Enable Output Scaler",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.def = 1,
+	.step = 1,
+};
+
+
+/* Streaming Controls */
+
+static int vivid_streaming_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_streaming);
+	struct timeval tv;
+
+	switch (ctrl->id) {
+	case VIVID_CID_DQBUF_ERROR:
+		dev->dqbuf_error = true;
+		break;
+	case VIVID_CID_PERC_DROPPED:
+		dev->perc_dropped_buffers = ctrl->val;
+		break;
+	case VIVID_CID_QUEUE_SETUP_ERROR:
+		dev->queue_setup_error = true;
+		break;
+	case VIVID_CID_BUF_PREPARE_ERROR:
+		dev->buf_prepare_error = true;
+		break;
+	case VIVID_CID_START_STR_ERROR:
+		dev->start_streaming_error = true;
+		break;
+	case VIVID_CID_QUEUE_ERROR:
+		if (vb2_start_streaming_called(&dev->vb_vid_cap_q))
+			vb2_queue_error(&dev->vb_vid_cap_q);
+		if (vb2_start_streaming_called(&dev->vb_vbi_cap_q))
+			vb2_queue_error(&dev->vb_vbi_cap_q);
+		if (vb2_start_streaming_called(&dev->vb_vid_out_q))
+			vb2_queue_error(&dev->vb_vid_out_q);
+		if (vb2_start_streaming_called(&dev->vb_vbi_out_q))
+			vb2_queue_error(&dev->vb_vbi_out_q);
+		if (vb2_start_streaming_called(&dev->vb_sdr_cap_q))
+			vb2_queue_error(&dev->vb_sdr_cap_q);
+		break;
+	case VIVID_CID_SEQ_WRAP:
+		dev->seq_wrap = ctrl->val;
+		break;
+	case VIVID_CID_TIME_WRAP:
+		dev->time_wrap = ctrl->val;
+		if (ctrl->val == 0) {
+			dev->time_wrap_offset = 0;
+			break;
+		}
+		v4l2_get_timestamp(&tv);
+		dev->time_wrap_offset = -tv.tv_sec - 16;
+		break;
+	}
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops vivid_streaming_ctrl_ops = {
+	.s_ctrl = vivid_streaming_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_dqbuf_error = {
+	.ops = &vivid_streaming_ctrl_ops,
+	.id = VIVID_CID_DQBUF_ERROR,
+	.name = "Inject V4L2_BUF_FLAG_ERROR",
+	.type = V4L2_CTRL_TYPE_BUTTON,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_perc_dropped = {
+	.ops = &vivid_streaming_ctrl_ops,
+	.id = VIVID_CID_PERC_DROPPED,
+	.name = "Percentage of Dropped Buffers",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.min = 0,
+	.max = 100,
+	.step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_queue_setup_error = {
+	.ops = &vivid_streaming_ctrl_ops,
+	.id = VIVID_CID_QUEUE_SETUP_ERROR,
+	.name = "Inject VIDIOC_REQBUFS Error",
+	.type = V4L2_CTRL_TYPE_BUTTON,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_buf_prepare_error = {
+	.ops = &vivid_streaming_ctrl_ops,
+	.id = VIVID_CID_BUF_PREPARE_ERROR,
+	.name = "Inject VIDIOC_QBUF Error",
+	.type = V4L2_CTRL_TYPE_BUTTON,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_start_streaming_error = {
+	.ops = &vivid_streaming_ctrl_ops,
+	.id = VIVID_CID_START_STR_ERROR,
+	.name = "Inject VIDIOC_STREAMON Error",
+	.type = V4L2_CTRL_TYPE_BUTTON,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_queue_error = {
+	.ops = &vivid_streaming_ctrl_ops,
+	.id = VIVID_CID_QUEUE_ERROR,
+	.name = "Inject Fatal Streaming Error",
+	.type = V4L2_CTRL_TYPE_BUTTON,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_seq_wrap = {
+	.ops = &vivid_streaming_ctrl_ops,
+	.id = VIVID_CID_SEQ_WRAP,
+	.name = "Wrap Sequence Number",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_time_wrap = {
+	.ops = &vivid_streaming_ctrl_ops,
+	.id = VIVID_CID_TIME_WRAP,
+	.name = "Wrap Timestamp",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.step = 1,
+};
+
+
+/* SDTV Capture Controls */
+
+static int vivid_sdtv_cap_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_sdtv_cap);
+
+	switch (ctrl->id) {
+	case VIVID_CID_STD_SIGNAL_MODE:
+		dev->std_signal_mode = dev->ctrl_std_signal_mode->val;
+		if (dev->std_signal_mode == SELECTED_STD)
+			dev->query_std = vivid_standard[dev->ctrl_standard->val];
+		v4l2_ctrl_activate(dev->ctrl_standard, dev->std_signal_mode == SELECTED_STD);
+		vivid_update_quality(dev);
+		vivid_send_source_change(dev, TV);
+		vivid_send_source_change(dev, SVID);
+		break;
+	}
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops vivid_sdtv_cap_ctrl_ops = {
+	.s_ctrl = vivid_sdtv_cap_s_ctrl,
+};
+
+static const char * const vivid_ctrl_std_signal_mode_strings[] = {
+	"Current Standard",
+	"No Signal",
+	"No Lock",
+	"",
+	"Selected Standard",
+	"Cycle Through All Standards",
+	NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_std_signal_mode = {
+	.ops = &vivid_sdtv_cap_ctrl_ops,
+	.id = VIVID_CID_STD_SIGNAL_MODE,
+	.name = "Standard Signal Mode",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.max = ARRAY_SIZE(vivid_ctrl_std_signal_mode_strings) - 2,
+	.menu_skip_mask = 1 << 3,
+	.qmenu = vivid_ctrl_std_signal_mode_strings,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_standard = {
+	.ops = &vivid_sdtv_cap_ctrl_ops,
+	.id = VIVID_CID_STANDARD,
+	.name = "Standard",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.max = 14,
+	.qmenu = vivid_ctrl_standard_strings,
+};
+
+
+
+/* Radio Receiver Controls */
+
+static int vivid_radio_rx_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_radio_rx);
+
+	switch (ctrl->id) {
+	case VIVID_CID_RADIO_SEEK_MODE:
+		dev->radio_rx_hw_seek_mode = ctrl->val;
+		break;
+	case VIVID_CID_RADIO_SEEK_PROG_LIM:
+		dev->radio_rx_hw_seek_prog_lim = ctrl->val;
+		break;
+	case VIVID_CID_RADIO_RX_RDS_RBDS:
+		dev->rds_gen.use_rbds = ctrl->val;
+		break;
+	case VIVID_CID_RADIO_RX_RDS_BLOCKIO:
+		dev->radio_rx_rds_controls = ctrl->val;
+		dev->radio_rx_caps &= ~V4L2_CAP_READWRITE;
+		dev->radio_rx_rds_use_alternates = false;
+		if (!dev->radio_rx_rds_controls) {
+			dev->radio_rx_caps |= V4L2_CAP_READWRITE;
+			__v4l2_ctrl_s_ctrl(dev->radio_rx_rds_pty, 0);
+			__v4l2_ctrl_s_ctrl(dev->radio_rx_rds_ta, 0);
+			__v4l2_ctrl_s_ctrl(dev->radio_rx_rds_tp, 0);
+			__v4l2_ctrl_s_ctrl(dev->radio_rx_rds_ms, 0);
+			__v4l2_ctrl_s_ctrl_string(dev->radio_rx_rds_psname, "");
+			__v4l2_ctrl_s_ctrl_string(dev->radio_rx_rds_radiotext, "");
+		}
+		v4l2_ctrl_activate(dev->radio_rx_rds_pty, dev->radio_rx_rds_controls);
+		v4l2_ctrl_activate(dev->radio_rx_rds_psname, dev->radio_rx_rds_controls);
+		v4l2_ctrl_activate(dev->radio_rx_rds_radiotext, dev->radio_rx_rds_controls);
+		v4l2_ctrl_activate(dev->radio_rx_rds_ta, dev->radio_rx_rds_controls);
+		v4l2_ctrl_activate(dev->radio_rx_rds_tp, dev->radio_rx_rds_controls);
+		v4l2_ctrl_activate(dev->radio_rx_rds_ms, dev->radio_rx_rds_controls);
+		break;
+	case V4L2_CID_RDS_RECEPTION:
+		dev->radio_rx_rds_enabled = ctrl->val;
+		break;
+	}
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops vivid_radio_rx_ctrl_ops = {
+	.s_ctrl = vivid_radio_rx_s_ctrl,
+};
+
+static const char * const vivid_ctrl_radio_rds_mode_strings[] = {
+	"Block I/O",
+	"Controls",
+	NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_radio_rx_rds_blockio = {
+	.ops = &vivid_radio_rx_ctrl_ops,
+	.id = VIVID_CID_RADIO_RX_RDS_BLOCKIO,
+	.name = "RDS Rx I/O Mode",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.qmenu = vivid_ctrl_radio_rds_mode_strings,
+	.max = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_radio_rx_rds_rbds = {
+	.ops = &vivid_radio_rx_ctrl_ops,
+	.id = VIVID_CID_RADIO_RX_RDS_RBDS,
+	.name = "Generate RBDS Instead of RDS",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.step = 1,
+};
+
+static const char * const vivid_ctrl_radio_hw_seek_mode_strings[] = {
+	"Bounded",
+	"Wrap Around",
+	"Both",
+	NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_radio_hw_seek_mode = {
+	.ops = &vivid_radio_rx_ctrl_ops,
+	.id = VIVID_CID_RADIO_SEEK_MODE,
+	.name = "Radio HW Seek Mode",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.max = 2,
+	.qmenu = vivid_ctrl_radio_hw_seek_mode_strings,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_radio_hw_seek_prog_lim = {
+	.ops = &vivid_radio_rx_ctrl_ops,
+	.id = VIVID_CID_RADIO_SEEK_PROG_LIM,
+	.name = "Radio Programmable HW Seek",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.step = 1,
+};
+
+
+/* Radio Transmitter Controls */
+
+static int vivid_radio_tx_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_radio_tx);
+
+	switch (ctrl->id) {
+	case VIVID_CID_RADIO_TX_RDS_BLOCKIO:
+		dev->radio_tx_rds_controls = ctrl->val;
+		dev->radio_tx_caps &= ~V4L2_CAP_READWRITE;
+		if (!dev->radio_tx_rds_controls)
+			dev->radio_tx_caps |= V4L2_CAP_READWRITE;
+		break;
+	case V4L2_CID_RDS_TX_PTY:
+		if (dev->radio_rx_rds_controls)
+			v4l2_ctrl_s_ctrl(dev->radio_rx_rds_pty, ctrl->val);
+		break;
+	case V4L2_CID_RDS_TX_PS_NAME:
+		if (dev->radio_rx_rds_controls)
+			v4l2_ctrl_s_ctrl_string(dev->radio_rx_rds_psname, ctrl->p_new.p_char);
+		break;
+	case V4L2_CID_RDS_TX_RADIO_TEXT:
+		if (dev->radio_rx_rds_controls)
+			v4l2_ctrl_s_ctrl_string(dev->radio_rx_rds_radiotext, ctrl->p_new.p_char);
+		break;
+	case V4L2_CID_RDS_TX_TRAFFIC_ANNOUNCEMENT:
+		if (dev->radio_rx_rds_controls)
+			v4l2_ctrl_s_ctrl(dev->radio_rx_rds_ta, ctrl->val);
+		break;
+	case V4L2_CID_RDS_TX_TRAFFIC_PROGRAM:
+		if (dev->radio_rx_rds_controls)
+			v4l2_ctrl_s_ctrl(dev->radio_rx_rds_tp, ctrl->val);
+		break;
+	case V4L2_CID_RDS_TX_MUSIC_SPEECH:
+		if (dev->radio_rx_rds_controls)
+			v4l2_ctrl_s_ctrl(dev->radio_rx_rds_ms, ctrl->val);
+		break;
+	}
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops vivid_radio_tx_ctrl_ops = {
+	.s_ctrl = vivid_radio_tx_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_radio_tx_rds_blockio = {
+	.ops = &vivid_radio_tx_ctrl_ops,
+	.id = VIVID_CID_RADIO_TX_RDS_BLOCKIO,
+	.name = "RDS Tx I/O Mode",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.qmenu = vivid_ctrl_radio_rds_mode_strings,
+	.max = 1,
+	.def = 1,
+};
+
+
+/* SDR Capture Controls */
+
+static int vivid_sdr_cap_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_sdr_cap);
+
+	switch (ctrl->id) {
+	case VIVID_CID_SDR_CAP_FM_DEVIATION:
+		dev->sdr_fm_deviation = ctrl->val;
+		break;
+	}
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops vivid_sdr_cap_ctrl_ops = {
+	.s_ctrl = vivid_sdr_cap_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_sdr_cap_fm_deviation = {
+	.ops = &vivid_sdr_cap_ctrl_ops,
+	.id = VIVID_CID_SDR_CAP_FM_DEVIATION,
+	.name = "FM Deviation",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.min =    100,
+	.max = 200000,
+	.def =  75000,
+	.step =     1,
+};
+
+
+static const struct v4l2_ctrl_config vivid_ctrl_class = {
+	.ops = &vivid_user_gen_ctrl_ops,
+	.flags = V4L2_CTRL_FLAG_READ_ONLY | V4L2_CTRL_FLAG_WRITE_ONLY,
+	.id = VIVID_CID_VIVID_CLASS,
+	.name = "Vivid Controls",
+	.type = V4L2_CTRL_TYPE_CTRL_CLASS,
+};
+
+int vivid_create_controls(struct vivid_dev *dev, bool show_ccs_cap,
+		bool show_ccs_out, bool no_error_inj,
+		bool has_sdtv, bool has_hdmi)
+{
+	struct v4l2_ctrl_handler *hdl_user_gen = &dev->ctrl_hdl_user_gen;
+	struct v4l2_ctrl_handler *hdl_user_vid = &dev->ctrl_hdl_user_vid;
+	struct v4l2_ctrl_handler *hdl_user_aud = &dev->ctrl_hdl_user_aud;
+	struct v4l2_ctrl_handler *hdl_streaming = &dev->ctrl_hdl_streaming;
+	struct v4l2_ctrl_handler *hdl_sdtv_cap = &dev->ctrl_hdl_sdtv_cap;
+	struct v4l2_ctrl_handler *hdl_loop_cap = &dev->ctrl_hdl_loop_cap;
+	struct v4l2_ctrl_handler *hdl_vid_cap = &dev->ctrl_hdl_vid_cap;
+	struct v4l2_ctrl_handler *hdl_vid_out = &dev->ctrl_hdl_vid_out;
+	struct v4l2_ctrl_handler *hdl_vbi_cap = &dev->ctrl_hdl_vbi_cap;
+	struct v4l2_ctrl_handler *hdl_vbi_out = &dev->ctrl_hdl_vbi_out;
+	struct v4l2_ctrl_handler *hdl_radio_rx = &dev->ctrl_hdl_radio_rx;
+	struct v4l2_ctrl_handler *hdl_radio_tx = &dev->ctrl_hdl_radio_tx;
+	struct v4l2_ctrl_handler *hdl_sdr_cap = &dev->ctrl_hdl_sdr_cap;
+	struct v4l2_ctrl_config vivid_ctrl_dv_timings = {
+		.ops = &vivid_vid_cap_ctrl_ops,
+		.id = VIVID_CID_DV_TIMINGS,
+		.name = "DV Timings",
+		.type = V4L2_CTRL_TYPE_MENU,
+	};
+	int i;
+
+	v4l2_ctrl_handler_init(hdl_user_gen, 10);
+	v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_class, NULL);
+	v4l2_ctrl_handler_init(hdl_user_vid, 9);
+	v4l2_ctrl_new_custom(hdl_user_vid, &vivid_ctrl_class, NULL);
+	v4l2_ctrl_handler_init(hdl_user_aud, 2);
+	v4l2_ctrl_new_custom(hdl_user_aud, &vivid_ctrl_class, NULL);
+	v4l2_ctrl_handler_init(hdl_streaming, 8);
+	v4l2_ctrl_new_custom(hdl_streaming, &vivid_ctrl_class, NULL);
+	v4l2_ctrl_handler_init(hdl_sdtv_cap, 2);
+	v4l2_ctrl_new_custom(hdl_sdtv_cap, &vivid_ctrl_class, NULL);
+	v4l2_ctrl_handler_init(hdl_loop_cap, 1);
+	v4l2_ctrl_new_custom(hdl_loop_cap, &vivid_ctrl_class, NULL);
+	v4l2_ctrl_handler_init(hdl_vid_cap, 55);
+	v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_class, NULL);
+	v4l2_ctrl_handler_init(hdl_vid_out, 26);
+	v4l2_ctrl_new_custom(hdl_vid_out, &vivid_ctrl_class, NULL);
+	v4l2_ctrl_handler_init(hdl_vbi_cap, 21);
+	v4l2_ctrl_new_custom(hdl_vbi_cap, &vivid_ctrl_class, NULL);
+	v4l2_ctrl_handler_init(hdl_vbi_out, 19);
+	v4l2_ctrl_new_custom(hdl_vbi_out, &vivid_ctrl_class, NULL);
+	v4l2_ctrl_handler_init(hdl_radio_rx, 17);
+	v4l2_ctrl_new_custom(hdl_radio_rx, &vivid_ctrl_class, NULL);
+	v4l2_ctrl_handler_init(hdl_radio_tx, 17);
+	v4l2_ctrl_new_custom(hdl_radio_tx, &vivid_ctrl_class, NULL);
+	v4l2_ctrl_handler_init(hdl_sdr_cap, 19);
+	v4l2_ctrl_new_custom(hdl_sdr_cap, &vivid_ctrl_class, NULL);
+
+	/* User Controls */
+	dev->volume = v4l2_ctrl_new_std(hdl_user_aud, NULL,
+		V4L2_CID_AUDIO_VOLUME, 0, 255, 1, 200);
+	dev->mute = v4l2_ctrl_new_std(hdl_user_aud, NULL,
+		V4L2_CID_AUDIO_MUTE, 0, 1, 1, 0);
+	if (dev->has_vid_cap) {
+		dev->brightness = v4l2_ctrl_new_std(hdl_user_vid, &vivid_user_vid_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
+		for (i = 0; i < MAX_INPUTS; i++)
+			dev->input_brightness[i] = 128;
+		dev->contrast = v4l2_ctrl_new_std(hdl_user_vid, &vivid_user_vid_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 255, 1, 128);
+		dev->saturation = v4l2_ctrl_new_std(hdl_user_vid, &vivid_user_vid_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 255, 1, 128);
+		dev->hue = v4l2_ctrl_new_std(hdl_user_vid, &vivid_user_vid_ctrl_ops,
+			V4L2_CID_HUE, -128, 128, 1, 0);
+		v4l2_ctrl_new_std(hdl_user_vid, &vivid_user_vid_ctrl_ops,
+			V4L2_CID_HFLIP, 0, 1, 1, 0);
+		v4l2_ctrl_new_std(hdl_user_vid, &vivid_user_vid_ctrl_ops,
+			V4L2_CID_VFLIP, 0, 1, 1, 0);
+		dev->autogain = v4l2_ctrl_new_std(hdl_user_vid, &vivid_user_vid_ctrl_ops,
+			V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+		dev->gain = v4l2_ctrl_new_std(hdl_user_vid, &vivid_user_vid_ctrl_ops,
+			V4L2_CID_GAIN, 0, 255, 1, 100);
+		dev->alpha = v4l2_ctrl_new_std(hdl_user_vid, &vivid_user_vid_ctrl_ops,
+			V4L2_CID_ALPHA_COMPONENT, 0, 255, 1, 0);
+	}
+	dev->button = v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_button, NULL);
+	dev->int32 = v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_int32, NULL);
+	dev->int64 = v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_int64, NULL);
+	dev->boolean = v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_boolean, NULL);
+	dev->menu = v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_menu, NULL);
+	dev->string = v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_string, NULL);
+	dev->bitmask = v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_bitmask, NULL);
+	dev->int_menu = v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_int_menu, NULL);
+	v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_u32_array, NULL);
+	v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_u16_matrix, NULL);
+	v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_u8_4d_array, NULL);
+
+	if (dev->has_vid_cap) {
+		/* Image Processing Controls */
+		struct v4l2_ctrl_config vivid_ctrl_test_pattern = {
+			.ops = &vivid_vid_cap_ctrl_ops,
+			.id = VIVID_CID_TEST_PATTERN,
+			.name = "Test Pattern",
+			.type = V4L2_CTRL_TYPE_MENU,
+			.max = TPG_PAT_NOISE,
+			.qmenu = tpg_pattern_strings,
+		};
+
+		dev->test_pattern = v4l2_ctrl_new_custom(hdl_vid_cap,
+				&vivid_ctrl_test_pattern, NULL);
+		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_perc_fill, NULL);
+		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_hor_movement, NULL);
+		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_vert_movement, NULL);
+		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_osd_mode, NULL);
+		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_show_border, NULL);
+		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_show_square, NULL);
+		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_hflip, NULL);
+		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_vflip, NULL);
+		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_insert_sav, NULL);
+		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_insert_eav, NULL);
+		if (show_ccs_cap) {
+			dev->ctrl_has_crop_cap = v4l2_ctrl_new_custom(hdl_vid_cap,
+				&vivid_ctrl_has_crop_cap, NULL);
+			dev->ctrl_has_compose_cap = v4l2_ctrl_new_custom(hdl_vid_cap,
+				&vivid_ctrl_has_compose_cap, NULL);
+			dev->ctrl_has_scaler_cap = v4l2_ctrl_new_custom(hdl_vid_cap,
+				&vivid_ctrl_has_scaler_cap, NULL);
+		}
+
+		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_tstamp_src, NULL);
+		dev->colorspace = v4l2_ctrl_new_custom(hdl_vid_cap,
+			&vivid_ctrl_colorspace, NULL);
+		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_xfer_func, NULL);
+		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_ycbcr_enc, NULL);
+		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_quantization, NULL);
+		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_alpha_mode, NULL);
+	}
+
+	if (dev->has_vid_out && show_ccs_out) {
+		dev->ctrl_has_crop_out = v4l2_ctrl_new_custom(hdl_vid_out,
+			&vivid_ctrl_has_crop_out, NULL);
+		dev->ctrl_has_compose_out = v4l2_ctrl_new_custom(hdl_vid_out,
+			&vivid_ctrl_has_compose_out, NULL);
+		dev->ctrl_has_scaler_out = v4l2_ctrl_new_custom(hdl_vid_out,
+			&vivid_ctrl_has_scaler_out, NULL);
+	}
+
+	/*
+	 * Testing this driver with v4l2-compliance will trigger the error
+	 * injection controls, and after that nothing will work as expected.
+	 * So we have a module option to drop these error injecting controls
+	 * allowing us to run v4l2_compliance again.
+	 */
+	if (!no_error_inj) {
+		v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_disconnect, NULL);
+		v4l2_ctrl_new_custom(hdl_streaming, &vivid_ctrl_dqbuf_error, NULL);
+		v4l2_ctrl_new_custom(hdl_streaming, &vivid_ctrl_perc_dropped, NULL);
+		v4l2_ctrl_new_custom(hdl_streaming, &vivid_ctrl_queue_setup_error, NULL);
+		v4l2_ctrl_new_custom(hdl_streaming, &vivid_ctrl_buf_prepare_error, NULL);
+		v4l2_ctrl_new_custom(hdl_streaming, &vivid_ctrl_start_streaming_error, NULL);
+		v4l2_ctrl_new_custom(hdl_streaming, &vivid_ctrl_queue_error, NULL);
+		v4l2_ctrl_new_custom(hdl_streaming, &vivid_ctrl_seq_wrap, NULL);
+		v4l2_ctrl_new_custom(hdl_streaming, &vivid_ctrl_time_wrap, NULL);
+	}
+
+	if (has_sdtv && (dev->has_vid_cap || dev->has_vbi_cap)) {
+		if (dev->has_vid_cap)
+			v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_std_aspect_ratio, NULL);
+		dev->ctrl_std_signal_mode = v4l2_ctrl_new_custom(hdl_sdtv_cap,
+			&vivid_ctrl_std_signal_mode, NULL);
+		dev->ctrl_standard = v4l2_ctrl_new_custom(hdl_sdtv_cap,
+			&vivid_ctrl_standard, NULL);
+		if (dev->ctrl_std_signal_mode)
+			v4l2_ctrl_cluster(2, &dev->ctrl_std_signal_mode);
+		if (dev->has_raw_vbi_cap)
+			v4l2_ctrl_new_custom(hdl_vbi_cap, &vivid_ctrl_vbi_cap_interlaced, NULL);
+	}
+
+	if (has_hdmi && dev->has_vid_cap) {
+		dev->ctrl_dv_timings_signal_mode = v4l2_ctrl_new_custom(hdl_vid_cap,
+					&vivid_ctrl_dv_timings_signal_mode, NULL);
+
+		vivid_ctrl_dv_timings.max = dev->query_dv_timings_size - 1;
+		vivid_ctrl_dv_timings.qmenu =
+			(const char * const *)dev->query_dv_timings_qmenu;
+		dev->ctrl_dv_timings = v4l2_ctrl_new_custom(hdl_vid_cap,
+			&vivid_ctrl_dv_timings, NULL);
+		if (dev->ctrl_dv_timings_signal_mode)
+			v4l2_ctrl_cluster(2, &dev->ctrl_dv_timings_signal_mode);
+
+		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_dv_timings_aspect_ratio, NULL);
+		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_max_edid_blocks, NULL);
+		dev->real_rgb_range_cap = v4l2_ctrl_new_custom(hdl_vid_cap,
+			&vivid_ctrl_limited_rgb_range, NULL);
+		dev->rgb_range_cap = v4l2_ctrl_new_std_menu(hdl_vid_cap,
+			&vivid_vid_cap_ctrl_ops,
+			V4L2_CID_DV_RX_RGB_RANGE, V4L2_DV_RGB_RANGE_FULL,
+			0, V4L2_DV_RGB_RANGE_AUTO);
+	}
+	if (has_hdmi && dev->has_vid_out) {
+		/*
+		 * We aren't doing anything with this at the moment, but
+		 * HDMI outputs typically have this controls.
+		 */
+		dev->ctrl_tx_rgb_range = v4l2_ctrl_new_std_menu(hdl_vid_out, NULL,
+			V4L2_CID_DV_TX_RGB_RANGE, V4L2_DV_RGB_RANGE_FULL,
+			0, V4L2_DV_RGB_RANGE_AUTO);
+		dev->ctrl_tx_mode = v4l2_ctrl_new_std_menu(hdl_vid_out, NULL,
+			V4L2_CID_DV_TX_MODE, V4L2_DV_TX_MODE_HDMI,
+			0, V4L2_DV_TX_MODE_HDMI);
+	}
+	if ((dev->has_vid_cap && dev->has_vid_out) ||
+	    (dev->has_vbi_cap && dev->has_vbi_out))
+		v4l2_ctrl_new_custom(hdl_loop_cap, &vivid_ctrl_loop_video, NULL);
+
+	if (dev->has_fb)
+		v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_clear_fb, NULL);
+
+	if (dev->has_radio_rx) {
+		v4l2_ctrl_new_custom(hdl_radio_rx, &vivid_ctrl_radio_hw_seek_mode, NULL);
+		v4l2_ctrl_new_custom(hdl_radio_rx, &vivid_ctrl_radio_hw_seek_prog_lim, NULL);
+		v4l2_ctrl_new_custom(hdl_radio_rx, &vivid_ctrl_radio_rx_rds_blockio, NULL);
+		v4l2_ctrl_new_custom(hdl_radio_rx, &vivid_ctrl_radio_rx_rds_rbds, NULL);
+		v4l2_ctrl_new_std(hdl_radio_rx, &vivid_radio_rx_ctrl_ops,
+			V4L2_CID_RDS_RECEPTION, 0, 1, 1, 1);
+		dev->radio_rx_rds_pty = v4l2_ctrl_new_std(hdl_radio_rx,
+			&vivid_radio_rx_ctrl_ops,
+			V4L2_CID_RDS_RX_PTY, 0, 31, 1, 0);
+		dev->radio_rx_rds_psname = v4l2_ctrl_new_std(hdl_radio_rx,
+			&vivid_radio_rx_ctrl_ops,
+			V4L2_CID_RDS_RX_PS_NAME, 0, 8, 8, 0);
+		dev->radio_rx_rds_radiotext = v4l2_ctrl_new_std(hdl_radio_rx,
+			&vivid_radio_rx_ctrl_ops,
+			V4L2_CID_RDS_RX_RADIO_TEXT, 0, 64, 64, 0);
+		dev->radio_rx_rds_ta = v4l2_ctrl_new_std(hdl_radio_rx,
+			&vivid_radio_rx_ctrl_ops,
+			V4L2_CID_RDS_RX_TRAFFIC_ANNOUNCEMENT, 0, 1, 1, 0);
+		dev->radio_rx_rds_tp = v4l2_ctrl_new_std(hdl_radio_rx,
+			&vivid_radio_rx_ctrl_ops,
+			V4L2_CID_RDS_RX_TRAFFIC_PROGRAM, 0, 1, 1, 0);
+		dev->radio_rx_rds_ms = v4l2_ctrl_new_std(hdl_radio_rx,
+			&vivid_radio_rx_ctrl_ops,
+			V4L2_CID_RDS_RX_MUSIC_SPEECH, 0, 1, 1, 1);
+	}
+	if (dev->has_radio_tx) {
+		v4l2_ctrl_new_custom(hdl_radio_tx,
+			&vivid_ctrl_radio_tx_rds_blockio, NULL);
+		dev->radio_tx_rds_pi = v4l2_ctrl_new_std(hdl_radio_tx,
+			&vivid_radio_tx_ctrl_ops,
+			V4L2_CID_RDS_TX_PI, 0, 0xffff, 1, 0x8088);
+		dev->radio_tx_rds_pty = v4l2_ctrl_new_std(hdl_radio_tx,
+			&vivid_radio_tx_ctrl_ops,
+			V4L2_CID_RDS_TX_PTY, 0, 31, 1, 3);
+		dev->radio_tx_rds_psname = v4l2_ctrl_new_std(hdl_radio_tx,
+			&vivid_radio_tx_ctrl_ops,
+			V4L2_CID_RDS_TX_PS_NAME, 0, 8, 8, 0);
+		if (dev->radio_tx_rds_psname)
+			v4l2_ctrl_s_ctrl_string(dev->radio_tx_rds_psname, "VIVID-TX");
+		dev->radio_tx_rds_radiotext = v4l2_ctrl_new_std(hdl_radio_tx,
+			&vivid_radio_tx_ctrl_ops,
+			V4L2_CID_RDS_TX_RADIO_TEXT, 0, 64 * 2, 64, 0);
+		if (dev->radio_tx_rds_radiotext)
+			v4l2_ctrl_s_ctrl_string(dev->radio_tx_rds_radiotext,
+			       "This is a VIVID default Radio Text template text, change at will");
+		dev->radio_tx_rds_mono_stereo = v4l2_ctrl_new_std(hdl_radio_tx,
+			&vivid_radio_tx_ctrl_ops,
+			V4L2_CID_RDS_TX_MONO_STEREO, 0, 1, 1, 1);
+		dev->radio_tx_rds_art_head = v4l2_ctrl_new_std(hdl_radio_tx,
+			&vivid_radio_tx_ctrl_ops,
+			V4L2_CID_RDS_TX_ARTIFICIAL_HEAD, 0, 1, 1, 0);
+		dev->radio_tx_rds_compressed = v4l2_ctrl_new_std(hdl_radio_tx,
+			&vivid_radio_tx_ctrl_ops,
+			V4L2_CID_RDS_TX_COMPRESSED, 0, 1, 1, 0);
+		dev->radio_tx_rds_dyn_pty = v4l2_ctrl_new_std(hdl_radio_tx,
+			&vivid_radio_tx_ctrl_ops,
+			V4L2_CID_RDS_TX_DYNAMIC_PTY, 0, 1, 1, 0);
+		dev->radio_tx_rds_ta = v4l2_ctrl_new_std(hdl_radio_tx,
+			&vivid_radio_tx_ctrl_ops,
+			V4L2_CID_RDS_TX_TRAFFIC_ANNOUNCEMENT, 0, 1, 1, 0);
+		dev->radio_tx_rds_tp = v4l2_ctrl_new_std(hdl_radio_tx,
+			&vivid_radio_tx_ctrl_ops,
+			V4L2_CID_RDS_TX_TRAFFIC_PROGRAM, 0, 1, 1, 1);
+		dev->radio_tx_rds_ms = v4l2_ctrl_new_std(hdl_radio_tx,
+			&vivid_radio_tx_ctrl_ops,
+			V4L2_CID_RDS_TX_MUSIC_SPEECH, 0, 1, 1, 1);
+	}
+	if (dev->has_sdr_cap) {
+		v4l2_ctrl_new_custom(hdl_sdr_cap,
+			&vivid_ctrl_sdr_cap_fm_deviation, NULL);
+	}
+	if (hdl_user_gen->error)
+		return hdl_user_gen->error;
+	if (hdl_user_vid->error)
+		return hdl_user_vid->error;
+	if (hdl_user_aud->error)
+		return hdl_user_aud->error;
+	if (hdl_streaming->error)
+		return hdl_streaming->error;
+	if (hdl_sdr_cap->error)
+		return hdl_sdr_cap->error;
+	if (hdl_loop_cap->error)
+		return hdl_loop_cap->error;
+
+	if (dev->autogain)
+		v4l2_ctrl_auto_cluster(2, &dev->autogain, 0, true);
+
+	if (dev->has_vid_cap) {
+		v4l2_ctrl_add_handler(hdl_vid_cap, hdl_user_gen, NULL);
+		v4l2_ctrl_add_handler(hdl_vid_cap, hdl_user_vid, NULL);
+		v4l2_ctrl_add_handler(hdl_vid_cap, hdl_user_aud, NULL);
+		v4l2_ctrl_add_handler(hdl_vid_cap, hdl_streaming, NULL);
+		v4l2_ctrl_add_handler(hdl_vid_cap, hdl_sdtv_cap, NULL);
+		v4l2_ctrl_add_handler(hdl_vid_cap, hdl_loop_cap, NULL);
+		if (hdl_vid_cap->error)
+			return hdl_vid_cap->error;
+		dev->vid_cap_dev.ctrl_handler = hdl_vid_cap;
+	}
+	if (dev->has_vid_out) {
+		v4l2_ctrl_add_handler(hdl_vid_out, hdl_user_gen, NULL);
+		v4l2_ctrl_add_handler(hdl_vid_out, hdl_user_aud, NULL);
+		v4l2_ctrl_add_handler(hdl_vid_out, hdl_streaming, NULL);
+		if (hdl_vid_out->error)
+			return hdl_vid_out->error;
+		dev->vid_out_dev.ctrl_handler = hdl_vid_out;
+	}
+	if (dev->has_vbi_cap) {
+		v4l2_ctrl_add_handler(hdl_vbi_cap, hdl_user_gen, NULL);
+		v4l2_ctrl_add_handler(hdl_vbi_cap, hdl_streaming, NULL);
+		v4l2_ctrl_add_handler(hdl_vbi_cap, hdl_sdtv_cap, NULL);
+		v4l2_ctrl_add_handler(hdl_vbi_cap, hdl_loop_cap, NULL);
+		if (hdl_vbi_cap->error)
+			return hdl_vbi_cap->error;
+		dev->vbi_cap_dev.ctrl_handler = hdl_vbi_cap;
+	}
+	if (dev->has_vbi_out) {
+		v4l2_ctrl_add_handler(hdl_vbi_out, hdl_user_gen, NULL);
+		v4l2_ctrl_add_handler(hdl_vbi_out, hdl_streaming, NULL);
+		if (hdl_vbi_out->error)
+			return hdl_vbi_out->error;
+		dev->vbi_out_dev.ctrl_handler = hdl_vbi_out;
+	}
+	if (dev->has_radio_rx) {
+		v4l2_ctrl_add_handler(hdl_radio_rx, hdl_user_gen, NULL);
+		v4l2_ctrl_add_handler(hdl_radio_rx, hdl_user_aud, NULL);
+		if (hdl_radio_rx->error)
+			return hdl_radio_rx->error;
+		dev->radio_rx_dev.ctrl_handler = hdl_radio_rx;
+	}
+	if (dev->has_radio_tx) {
+		v4l2_ctrl_add_handler(hdl_radio_tx, hdl_user_gen, NULL);
+		v4l2_ctrl_add_handler(hdl_radio_tx, hdl_user_aud, NULL);
+		if (hdl_radio_tx->error)
+			return hdl_radio_tx->error;
+		dev->radio_tx_dev.ctrl_handler = hdl_radio_tx;
+	}
+	if (dev->has_sdr_cap) {
+		v4l2_ctrl_add_handler(hdl_sdr_cap, hdl_user_gen, NULL);
+		v4l2_ctrl_add_handler(hdl_sdr_cap, hdl_streaming, NULL);
+		if (hdl_sdr_cap->error)
+			return hdl_sdr_cap->error;
+		dev->sdr_cap_dev.ctrl_handler = hdl_sdr_cap;
+	}
+	return 0;
+}
+
+void vivid_free_controls(struct vivid_dev *dev)
+{
+	v4l2_ctrl_handler_free(&dev->ctrl_hdl_vid_cap);
+	v4l2_ctrl_handler_free(&dev->ctrl_hdl_vid_out);
+	v4l2_ctrl_handler_free(&dev->ctrl_hdl_vbi_cap);
+	v4l2_ctrl_handler_free(&dev->ctrl_hdl_vbi_out);
+	v4l2_ctrl_handler_free(&dev->ctrl_hdl_radio_rx);
+	v4l2_ctrl_handler_free(&dev->ctrl_hdl_radio_tx);
+	v4l2_ctrl_handler_free(&dev->ctrl_hdl_sdr_cap);
+	v4l2_ctrl_handler_free(&dev->ctrl_hdl_user_gen);
+	v4l2_ctrl_handler_free(&dev->ctrl_hdl_user_vid);
+	v4l2_ctrl_handler_free(&dev->ctrl_hdl_user_aud);
+	v4l2_ctrl_handler_free(&dev->ctrl_hdl_streaming);
+	v4l2_ctrl_handler_free(&dev->ctrl_hdl_sdtv_cap);
+	v4l2_ctrl_handler_free(&dev->ctrl_hdl_loop_cap);
+}
diff --git a/drivers/media/platform/vivid/vivid-ctrls.h b/drivers/media/platform/vivid/vivid-ctrls.h
new file mode 100644
index 0000000..9bcca9d
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-ctrls.h
@@ -0,0 +1,34 @@
+/*
+ * vivid-ctrls.h - control support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VIVID_CTRLS_H_
+#define _VIVID_CTRLS_H_
+
+enum vivid_hw_seek_modes {
+	VIVID_HW_SEEK_BOUNDED,
+	VIVID_HW_SEEK_WRAP,
+	VIVID_HW_SEEK_BOTH,
+};
+
+int vivid_create_controls(struct vivid_dev *dev, bool show_ccs_cap,
+		bool show_ccs_out, bool no_error_inj,
+		bool has_sdtv, bool has_hdmi);
+void vivid_free_controls(struct vivid_dev *dev);
+
+#endif
diff --git a/drivers/media/platform/vivid/vivid-kthread-cap.c b/drivers/media/platform/vivid/vivid-kthread-cap.c
new file mode 100644
index 0000000..83cc6d3
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-kthread-cap.c
@@ -0,0 +1,920 @@
+/*
+ * vivid-kthread-cap.h - video/vbi capture thread support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/font.h>
+#include <linux/mutex.h>
+#include <linux/videodev2.h>
+#include <linux/kthread.h>
+#include <linux/freezer.h>
+#include <linux/random.h>
+#include <linux/v4l2-dv-timings.h>
+#include <asm/div64.h>
+#include <media/videobuf2-vmalloc.h>
+#include <media/v4l2-dv-timings.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-event.h>
+
+#include "vivid-core.h"
+#include "vivid-vid-common.h"
+#include "vivid-vid-cap.h"
+#include "vivid-vid-out.h"
+#include "vivid-radio-common.h"
+#include "vivid-radio-rx.h"
+#include "vivid-radio-tx.h"
+#include "vivid-sdr-cap.h"
+#include "vivid-vbi-cap.h"
+#include "vivid-vbi-out.h"
+#include "vivid-osd.h"
+#include "vivid-ctrls.h"
+#include "vivid-kthread-cap.h"
+
+static inline v4l2_std_id vivid_get_std_cap(const struct vivid_dev *dev)
+{
+	if (vivid_is_sdtv_cap(dev))
+		return dev->std_cap;
+	return 0;
+}
+
+static void copy_pix(struct vivid_dev *dev, int win_y, int win_x,
+			u16 *cap, const u16 *osd)
+{
+	u16 out;
+	int left = dev->overlay_out_left;
+	int top = dev->overlay_out_top;
+	int fb_x = win_x + left;
+	int fb_y = win_y + top;
+	int i;
+
+	out = *cap;
+	*cap = *osd;
+	if (dev->bitmap_out) {
+		const u8 *p = dev->bitmap_out;
+		unsigned stride = (dev->compose_out.width + 7) / 8;
+
+		win_x -= dev->compose_out.left;
+		win_y -= dev->compose_out.top;
+		if (!(p[stride * win_y + win_x / 8] & (1 << (win_x & 7))))
+			return;
+	}
+
+	for (i = 0; i < dev->clipcount_out; i++) {
+		struct v4l2_rect *r = &dev->clips_out[i].c;
+
+		if (fb_y >= r->top && fb_y < r->top + r->height &&
+		    fb_x >= r->left && fb_x < r->left + r->width)
+			return;
+	}
+	if ((dev->fbuf_out_flags & V4L2_FBUF_FLAG_CHROMAKEY) &&
+	    *osd != dev->chromakey_out)
+		return;
+	if ((dev->fbuf_out_flags & V4L2_FBUF_FLAG_SRC_CHROMAKEY) &&
+	    out == dev->chromakey_out)
+		return;
+	if (dev->fmt_cap->alpha_mask) {
+		if ((dev->fbuf_out_flags & V4L2_FBUF_FLAG_GLOBAL_ALPHA) &&
+		    dev->global_alpha_out)
+			return;
+		if ((dev->fbuf_out_flags & V4L2_FBUF_FLAG_LOCAL_ALPHA) &&
+		    *cap & dev->fmt_cap->alpha_mask)
+			return;
+		if ((dev->fbuf_out_flags & V4L2_FBUF_FLAG_LOCAL_INV_ALPHA) &&
+		    !(*cap & dev->fmt_cap->alpha_mask))
+			return;
+	}
+	*cap = out;
+}
+
+static void blend_line(struct vivid_dev *dev, unsigned y_offset, unsigned x_offset,
+		u8 *vcapbuf, const u8 *vosdbuf,
+		unsigned width, unsigned pixsize)
+{
+	unsigned x;
+
+	for (x = 0; x < width; x++, vcapbuf += pixsize, vosdbuf += pixsize) {
+		copy_pix(dev, y_offset, x_offset + x,
+			 (u16 *)vcapbuf, (const u16 *)vosdbuf);
+	}
+}
+
+static void scale_line(const u8 *src, u8 *dst, unsigned srcw, unsigned dstw, unsigned twopixsize)
+{
+	/* Coarse scaling with Bresenham */
+	unsigned int_part;
+	unsigned fract_part;
+	unsigned src_x = 0;
+	unsigned error = 0;
+	unsigned x;
+
+	/*
+	 * We always combine two pixels to prevent color bleed in the packed
+	 * yuv case.
+	 */
+	srcw /= 2;
+	dstw /= 2;
+	int_part = srcw / dstw;
+	fract_part = srcw % dstw;
+	for (x = 0; x < dstw; x++, dst += twopixsize) {
+		memcpy(dst, src + src_x * twopixsize, twopixsize);
+		src_x += int_part;
+		error += fract_part;
+		if (error >= dstw) {
+			error -= dstw;
+			src_x++;
+		}
+	}
+}
+
+/*
+ * Precalculate the rectangles needed to perform video looping:
+ *
+ * The nominal pipeline is that the video output buffer is cropped by
+ * crop_out, scaled to compose_out, overlaid with the output overlay,
+ * cropped on the capture side by crop_cap and scaled again to the video
+ * capture buffer using compose_cap.
+ *
+ * To keep things efficient we calculate the intersection of compose_out
+ * and crop_cap (since that's the only part of the video that will
+ * actually end up in the capture buffer), determine which part of the
+ * video output buffer that is and which part of the video capture buffer
+ * so we can scale the video straight from the output buffer to the capture
+ * buffer without any intermediate steps.
+ *
+ * If we need to deal with an output overlay, then there is no choice and
+ * that intermediate step still has to be taken. For the output overlay
+ * support we calculate the intersection of the framebuffer and the overlay
+ * window (which may be partially or wholly outside of the framebuffer
+ * itself) and the intersection of that with loop_vid_copy (i.e. the part of
+ * the actual looped video that will be overlaid). The result is calculated
+ * both in framebuffer coordinates (loop_fb_copy) and compose_out coordinates
+ * (loop_vid_overlay). Finally calculate the part of the capture buffer that
+ * will receive that overlaid video.
+ */
+static void vivid_precalc_copy_rects(struct vivid_dev *dev)
+{
+	/* Framebuffer rectangle */
+	struct v4l2_rect r_fb = {
+		0, 0, dev->display_width, dev->display_height
+	};
+	/* Overlay window rectangle in framebuffer coordinates */
+	struct v4l2_rect r_overlay = {
+		dev->overlay_out_left, dev->overlay_out_top,
+		dev->compose_out.width, dev->compose_out.height
+	};
+
+	dev->loop_vid_copy = rect_intersect(&dev->crop_cap, &dev->compose_out);
+
+	dev->loop_vid_out = dev->loop_vid_copy;
+	rect_scale(&dev->loop_vid_out, &dev->compose_out, &dev->crop_out);
+	dev->loop_vid_out.left += dev->crop_out.left;
+	dev->loop_vid_out.top += dev->crop_out.top;
+
+	dev->loop_vid_cap = dev->loop_vid_copy;
+	rect_scale(&dev->loop_vid_cap, &dev->crop_cap, &dev->compose_cap);
+
+	dprintk(dev, 1,
+		"loop_vid_copy: %dx%d@%dx%d loop_vid_out: %dx%d@%dx%d loop_vid_cap: %dx%d@%dx%d\n",
+		dev->loop_vid_copy.width, dev->loop_vid_copy.height,
+		dev->loop_vid_copy.left, dev->loop_vid_copy.top,
+		dev->loop_vid_out.width, dev->loop_vid_out.height,
+		dev->loop_vid_out.left, dev->loop_vid_out.top,
+		dev->loop_vid_cap.width, dev->loop_vid_cap.height,
+		dev->loop_vid_cap.left, dev->loop_vid_cap.top);
+
+	r_overlay = rect_intersect(&r_fb, &r_overlay);
+
+	/* shift r_overlay to the same origin as compose_out */
+	r_overlay.left += dev->compose_out.left - dev->overlay_out_left;
+	r_overlay.top += dev->compose_out.top - dev->overlay_out_top;
+
+	dev->loop_vid_overlay = rect_intersect(&r_overlay, &dev->loop_vid_copy);
+	dev->loop_fb_copy = dev->loop_vid_overlay;
+
+	/* shift dev->loop_fb_copy back again to the fb origin */
+	dev->loop_fb_copy.left -= dev->compose_out.left - dev->overlay_out_left;
+	dev->loop_fb_copy.top -= dev->compose_out.top - dev->overlay_out_top;
+
+	dev->loop_vid_overlay_cap = dev->loop_vid_overlay;
+	rect_scale(&dev->loop_vid_overlay_cap, &dev->crop_cap, &dev->compose_cap);
+
+	dprintk(dev, 1,
+		"loop_fb_copy: %dx%d@%dx%d loop_vid_overlay: %dx%d@%dx%d loop_vid_overlay_cap: %dx%d@%dx%d\n",
+		dev->loop_fb_copy.width, dev->loop_fb_copy.height,
+		dev->loop_fb_copy.left, dev->loop_fb_copy.top,
+		dev->loop_vid_overlay.width, dev->loop_vid_overlay.height,
+		dev->loop_vid_overlay.left, dev->loop_vid_overlay.top,
+		dev->loop_vid_overlay_cap.width, dev->loop_vid_overlay_cap.height,
+		dev->loop_vid_overlay_cap.left, dev->loop_vid_overlay_cap.top);
+}
+
+static void *plane_vaddr(struct tpg_data *tpg, struct vivid_buffer *buf,
+			 unsigned p, unsigned bpl[TPG_MAX_PLANES], unsigned h)
+{
+	unsigned i;
+	void *vbuf;
+
+	if (p == 0 || tpg_g_buffers(tpg) > 1)
+		return vb2_plane_vaddr(&buf->vb.vb2_buf, p);
+	vbuf = vb2_plane_vaddr(&buf->vb.vb2_buf, 0);
+	for (i = 0; i < p; i++)
+		vbuf += bpl[i] * h / tpg->vdownsampling[i];
+	return vbuf;
+}
+
+static int vivid_copy_buffer(struct vivid_dev *dev, unsigned p, u8 *vcapbuf,
+		struct vivid_buffer *vid_cap_buf)
+{
+	bool blank = dev->must_blank[vid_cap_buf->vb.vb2_buf.index];
+	struct tpg_data *tpg = &dev->tpg;
+	struct vivid_buffer *vid_out_buf = NULL;
+	unsigned vdiv = dev->fmt_out->vdownsampling[p];
+	unsigned twopixsize = tpg_g_twopixelsize(tpg, p);
+	unsigned img_width = tpg_hdiv(tpg, p, dev->compose_cap.width);
+	unsigned img_height = dev->compose_cap.height;
+	unsigned stride_cap = tpg->bytesperline[p];
+	unsigned stride_out = dev->bytesperline_out[p];
+	unsigned stride_osd = dev->display_byte_stride;
+	unsigned hmax = (img_height * tpg->perc_fill) / 100;
+	u8 *voutbuf;
+	u8 *vosdbuf = NULL;
+	unsigned y;
+	bool blend = dev->bitmap_out || dev->clipcount_out || dev->fbuf_out_flags;
+	/* Coarse scaling with Bresenham */
+	unsigned vid_out_int_part;
+	unsigned vid_out_fract_part;
+	unsigned vid_out_y = 0;
+	unsigned vid_out_error = 0;
+	unsigned vid_overlay_int_part = 0;
+	unsigned vid_overlay_fract_part = 0;
+	unsigned vid_overlay_y = 0;
+	unsigned vid_overlay_error = 0;
+	unsigned vid_cap_left = tpg_hdiv(tpg, p, dev->loop_vid_cap.left);
+	unsigned vid_cap_right;
+	bool quick;
+
+	vid_out_int_part = dev->loop_vid_out.height / dev->loop_vid_cap.height;
+	vid_out_fract_part = dev->loop_vid_out.height % dev->loop_vid_cap.height;
+
+	if (!list_empty(&dev->vid_out_active))
+		vid_out_buf = list_entry(dev->vid_out_active.next,
+					 struct vivid_buffer, list);
+	if (vid_out_buf == NULL)
+		return -ENODATA;
+
+	vid_cap_buf->vb.field = vid_out_buf->vb.field;
+
+	voutbuf = plane_vaddr(tpg, vid_out_buf, p,
+			      dev->bytesperline_out, dev->fmt_out_rect.height);
+	if (p < dev->fmt_out->buffers)
+		voutbuf += vid_out_buf->vb.vb2_buf.planes[p].data_offset;
+	voutbuf += tpg_hdiv(tpg, p, dev->loop_vid_out.left) +
+		(dev->loop_vid_out.top / vdiv) * stride_out;
+	vcapbuf += tpg_hdiv(tpg, p, dev->compose_cap.left) +
+		(dev->compose_cap.top / vdiv) * stride_cap;
+
+	if (dev->loop_vid_copy.width == 0 || dev->loop_vid_copy.height == 0) {
+		/*
+		 * If there is nothing to copy, then just fill the capture window
+		 * with black.
+		 */
+		for (y = 0; y < hmax / vdiv; y++, vcapbuf += stride_cap)
+			memcpy(vcapbuf, tpg->black_line[p], img_width);
+		return 0;
+	}
+
+	if (dev->overlay_out_enabled &&
+	    dev->loop_vid_overlay.width && dev->loop_vid_overlay.height) {
+		vosdbuf = dev->video_vbase;
+		vosdbuf += (dev->loop_fb_copy.left * twopixsize) / 2 +
+			   dev->loop_fb_copy.top * stride_osd;
+		vid_overlay_int_part = dev->loop_vid_overlay.height /
+				       dev->loop_vid_overlay_cap.height;
+		vid_overlay_fract_part = dev->loop_vid_overlay.height %
+					 dev->loop_vid_overlay_cap.height;
+	}
+
+	vid_cap_right = tpg_hdiv(tpg, p, dev->loop_vid_cap.left + dev->loop_vid_cap.width);
+	/* quick is true if no video scaling is needed */
+	quick = dev->loop_vid_out.width == dev->loop_vid_cap.width;
+
+	dev->cur_scaled_line = dev->loop_vid_out.height;
+	for (y = 0; y < hmax; y += vdiv, vcapbuf += stride_cap) {
+		/* osdline is true if this line requires overlay blending */
+		bool osdline = vosdbuf && y >= dev->loop_vid_overlay_cap.top &&
+			  y < dev->loop_vid_overlay_cap.top + dev->loop_vid_overlay_cap.height;
+
+		/*
+		 * If this line of the capture buffer doesn't get any video, then
+		 * just fill with black.
+		 */
+		if (y < dev->loop_vid_cap.top ||
+		    y >= dev->loop_vid_cap.top + dev->loop_vid_cap.height) {
+			memcpy(vcapbuf, tpg->black_line[p], img_width);
+			continue;
+		}
+
+		/* fill the left border with black */
+		if (dev->loop_vid_cap.left)
+			memcpy(vcapbuf, tpg->black_line[p], vid_cap_left);
+
+		/* fill the right border with black */
+		if (vid_cap_right < img_width)
+			memcpy(vcapbuf + vid_cap_right, tpg->black_line[p],
+				img_width - vid_cap_right);
+
+		if (quick && !osdline) {
+			memcpy(vcapbuf + vid_cap_left,
+			       voutbuf + vid_out_y * stride_out,
+			       tpg_hdiv(tpg, p, dev->loop_vid_cap.width));
+			goto update_vid_out_y;
+		}
+		if (dev->cur_scaled_line == vid_out_y) {
+			memcpy(vcapbuf + vid_cap_left, dev->scaled_line,
+			       tpg_hdiv(tpg, p, dev->loop_vid_cap.width));
+			goto update_vid_out_y;
+		}
+		if (!osdline) {
+			scale_line(voutbuf + vid_out_y * stride_out, dev->scaled_line,
+				tpg_hdiv(tpg, p, dev->loop_vid_out.width),
+				tpg_hdiv(tpg, p, dev->loop_vid_cap.width),
+				tpg_g_twopixelsize(tpg, p));
+		} else {
+			/*
+			 * Offset in bytes within loop_vid_copy to the start of the
+			 * loop_vid_overlay rectangle.
+			 */
+			unsigned offset =
+				((dev->loop_vid_overlay.left - dev->loop_vid_copy.left) *
+				 twopixsize) / 2;
+			u8 *osd = vosdbuf + vid_overlay_y * stride_osd;
+
+			scale_line(voutbuf + vid_out_y * stride_out, dev->blended_line,
+				dev->loop_vid_out.width, dev->loop_vid_copy.width,
+				tpg_g_twopixelsize(tpg, p));
+			if (blend)
+				blend_line(dev, vid_overlay_y + dev->loop_vid_overlay.top,
+					   dev->loop_vid_overlay.left,
+					   dev->blended_line + offset, osd,
+					   dev->loop_vid_overlay.width, twopixsize / 2);
+			else
+				memcpy(dev->blended_line + offset,
+				       osd, (dev->loop_vid_overlay.width * twopixsize) / 2);
+			scale_line(dev->blended_line, dev->scaled_line,
+					dev->loop_vid_copy.width, dev->loop_vid_cap.width,
+					tpg_g_twopixelsize(tpg, p));
+		}
+		dev->cur_scaled_line = vid_out_y;
+		memcpy(vcapbuf + vid_cap_left, dev->scaled_line,
+		       tpg_hdiv(tpg, p, dev->loop_vid_cap.width));
+
+update_vid_out_y:
+		if (osdline) {
+			vid_overlay_y += vid_overlay_int_part;
+			vid_overlay_error += vid_overlay_fract_part;
+			if (vid_overlay_error >= dev->loop_vid_overlay_cap.height) {
+				vid_overlay_error -= dev->loop_vid_overlay_cap.height;
+				vid_overlay_y++;
+			}
+		}
+		vid_out_y += vid_out_int_part;
+		vid_out_error += vid_out_fract_part;
+		if (vid_out_error >= dev->loop_vid_cap.height / vdiv) {
+			vid_out_error -= dev->loop_vid_cap.height / vdiv;
+			vid_out_y++;
+		}
+	}
+
+	if (!blank)
+		return 0;
+	for (; y < img_height; y += vdiv, vcapbuf += stride_cap)
+		memcpy(vcapbuf, tpg->contrast_line[p], img_width);
+	return 0;
+}
+
+static void vivid_fillbuff(struct vivid_dev *dev, struct vivid_buffer *buf)
+{
+	struct tpg_data *tpg = &dev->tpg;
+	unsigned factor = V4L2_FIELD_HAS_T_OR_B(dev->field_cap) ? 2 : 1;
+	unsigned line_height = 16 / factor;
+	bool is_tv = vivid_is_sdtv_cap(dev);
+	bool is_60hz = is_tv && (dev->std_cap & V4L2_STD_525_60);
+	unsigned p;
+	int line = 1;
+	u8 *basep[TPG_MAX_PLANES][2];
+	unsigned ms;
+	char str[100];
+	s32 gain;
+	bool is_loop = false;
+
+	if (dev->loop_video && dev->can_loop_video &&
+		((vivid_is_svid_cap(dev) &&
+		!VIVID_INVALID_SIGNAL(dev->std_signal_mode)) ||
+		(vivid_is_hdmi_cap(dev) &&
+		!VIVID_INVALID_SIGNAL(dev->dv_timings_signal_mode))))
+		is_loop = true;
+
+	buf->vb.sequence = dev->vid_cap_seq_count;
+	/*
+	 * Take the timestamp now if the timestamp source is set to
+	 * "Start of Exposure".
+	 */
+	if (dev->tstamp_src_is_soe)
+		v4l2_get_timestamp(&buf->vb.timestamp);
+	if (dev->field_cap == V4L2_FIELD_ALTERNATE) {
+		/*
+		 * 60 Hz standards start with the bottom field, 50 Hz standards
+		 * with the top field. So if the 0-based seq_count is even,
+		 * then the field is TOP for 50 Hz and BOTTOM for 60 Hz
+		 * standards.
+		 */
+		buf->vb.field = ((dev->vid_cap_seq_count & 1) ^ is_60hz) ?
+			V4L2_FIELD_BOTTOM : V4L2_FIELD_TOP;
+		/*
+		 * The sequence counter counts frames, not fields. So divide
+		 * by two.
+		 */
+		buf->vb.sequence /= 2;
+	} else {
+		buf->vb.field = dev->field_cap;
+	}
+	tpg_s_field(tpg, buf->vb.field,
+		    dev->field_cap == V4L2_FIELD_ALTERNATE);
+	tpg_s_perc_fill_blank(tpg, dev->must_blank[buf->vb.vb2_buf.index]);
+
+	vivid_precalc_copy_rects(dev);
+
+	for (p = 0; p < tpg_g_planes(tpg); p++) {
+		void *vbuf = plane_vaddr(tpg, buf, p,
+					 tpg->bytesperline, tpg->buf_height);
+
+		/*
+		 * The first plane of a multiplanar format has a non-zero
+		 * data_offset. This helps testing whether the application
+		 * correctly supports non-zero data offsets.
+		 */
+		if (p < tpg_g_buffers(tpg) && dev->fmt_cap->data_offset[p]) {
+			memset(vbuf, dev->fmt_cap->data_offset[p] & 0xff,
+			       dev->fmt_cap->data_offset[p]);
+			vbuf += dev->fmt_cap->data_offset[p];
+		}
+		tpg_calc_text_basep(tpg, basep, p, vbuf);
+		if (!is_loop || vivid_copy_buffer(dev, p, vbuf, buf))
+			tpg_fill_plane_buffer(tpg, vivid_get_std_cap(dev),
+					p, vbuf);
+	}
+	dev->must_blank[buf->vb.vb2_buf.index] = false;
+
+	/* Updates stream time, only update at the start of a new frame. */
+	if (dev->field_cap != V4L2_FIELD_ALTERNATE ||
+			(buf->vb.sequence & 1) == 0)
+		dev->ms_vid_cap =
+			jiffies_to_msecs(jiffies - dev->jiffies_vid_cap);
+
+	ms = dev->ms_vid_cap;
+	if (dev->osd_mode <= 1) {
+		snprintf(str, sizeof(str), " %02d:%02d:%02d:%03d %u%s",
+				(ms / (60 * 60 * 1000)) % 24,
+				(ms / (60 * 1000)) % 60,
+				(ms / 1000) % 60,
+				ms % 1000,
+				buf->vb.sequence,
+				(dev->field_cap == V4L2_FIELD_ALTERNATE) ?
+					(buf->vb.field == V4L2_FIELD_TOP ?
+					 " top" : " bottom") : "");
+		tpg_gen_text(tpg, basep, line++ * line_height, 16, str);
+	}
+	if (dev->osd_mode == 0) {
+		snprintf(str, sizeof(str), " %dx%d, input %d ",
+				dev->src_rect.width, dev->src_rect.height, dev->input);
+		tpg_gen_text(tpg, basep, line++ * line_height, 16, str);
+
+		gain = v4l2_ctrl_g_ctrl(dev->gain);
+		mutex_lock(dev->ctrl_hdl_user_vid.lock);
+		snprintf(str, sizeof(str),
+			" brightness %3d, contrast %3d, saturation %3d, hue %d ",
+			dev->brightness->cur.val,
+			dev->contrast->cur.val,
+			dev->saturation->cur.val,
+			dev->hue->cur.val);
+		tpg_gen_text(tpg, basep, line++ * line_height, 16, str);
+		snprintf(str, sizeof(str),
+			" autogain %d, gain %3d, alpha 0x%02x ",
+			dev->autogain->cur.val, gain, dev->alpha->cur.val);
+		mutex_unlock(dev->ctrl_hdl_user_vid.lock);
+		tpg_gen_text(tpg, basep, line++ * line_height, 16, str);
+		mutex_lock(dev->ctrl_hdl_user_aud.lock);
+		snprintf(str, sizeof(str),
+			" volume %3d, mute %d ",
+			dev->volume->cur.val, dev->mute->cur.val);
+		mutex_unlock(dev->ctrl_hdl_user_aud.lock);
+		tpg_gen_text(tpg, basep, line++ * line_height, 16, str);
+		mutex_lock(dev->ctrl_hdl_user_gen.lock);
+		snprintf(str, sizeof(str), " int32 %d, int64 %lld, bitmask %08x ",
+			dev->int32->cur.val,
+			*dev->int64->p_cur.p_s64,
+			dev->bitmask->cur.val);
+		tpg_gen_text(tpg, basep, line++ * line_height, 16, str);
+		snprintf(str, sizeof(str), " boolean %d, menu %s, string \"%s\" ",
+			dev->boolean->cur.val,
+			dev->menu->qmenu[dev->menu->cur.val],
+			dev->string->p_cur.p_char);
+		tpg_gen_text(tpg, basep, line++ * line_height, 16, str);
+		snprintf(str, sizeof(str), " integer_menu %lld, value %d ",
+			dev->int_menu->qmenu_int[dev->int_menu->cur.val],
+			dev->int_menu->cur.val);
+		mutex_unlock(dev->ctrl_hdl_user_gen.lock);
+		tpg_gen_text(tpg, basep, line++ * line_height, 16, str);
+		if (dev->button_pressed) {
+			dev->button_pressed--;
+			snprintf(str, sizeof(str), " button pressed!");
+			tpg_gen_text(tpg, basep, line++ * line_height, 16, str);
+		}
+	}
+
+	/*
+	 * If "End of Frame" is specified at the timestamp source, then take
+	 * the timestamp now.
+	 */
+	if (!dev->tstamp_src_is_soe)
+		v4l2_get_timestamp(&buf->vb.timestamp);
+	buf->vb.timestamp.tv_sec += dev->time_wrap_offset;
+}
+
+/*
+ * Return true if this pixel coordinate is a valid video pixel.
+ */
+static bool valid_pix(struct vivid_dev *dev, int win_y, int win_x, int fb_y, int fb_x)
+{
+	int i;
+
+	if (dev->bitmap_cap) {
+		/*
+		 * Only if the corresponding bit in the bitmap is set can
+		 * the video pixel be shown. Coordinates are relative to
+		 * the overlay window set by VIDIOC_S_FMT.
+		 */
+		const u8 *p = dev->bitmap_cap;
+		unsigned stride = (dev->compose_cap.width + 7) / 8;
+
+		if (!(p[stride * win_y + win_x / 8] & (1 << (win_x & 7))))
+			return false;
+	}
+
+	for (i = 0; i < dev->clipcount_cap; i++) {
+		/*
+		 * Only if the framebuffer coordinate is not in any of the
+		 * clip rectangles will be video pixel be shown.
+		 */
+		struct v4l2_rect *r = &dev->clips_cap[i].c;
+
+		if (fb_y >= r->top && fb_y < r->top + r->height &&
+		    fb_x >= r->left && fb_x < r->left + r->width)
+			return false;
+	}
+	return true;
+}
+
+/*
+ * Draw the image into the overlay buffer.
+ * Note that the combination of overlay and multiplanar is not supported.
+ */
+static void vivid_overlay(struct vivid_dev *dev, struct vivid_buffer *buf)
+{
+	struct tpg_data *tpg = &dev->tpg;
+	unsigned pixsize = tpg_g_twopixelsize(tpg, 0) / 2;
+	void *vbase = dev->fb_vbase_cap;
+	void *vbuf = vb2_plane_vaddr(&buf->vb.vb2_buf, 0);
+	unsigned img_width = dev->compose_cap.width;
+	unsigned img_height = dev->compose_cap.height;
+	unsigned stride = tpg->bytesperline[0];
+	/* if quick is true, then valid_pix() doesn't have to be called */
+	bool quick = dev->bitmap_cap == NULL && dev->clipcount_cap == 0;
+	int x, y, w, out_x = 0;
+
+	/*
+	 * Overlay support is only supported for formats that have a twopixelsize
+	 * that's >= 2. Warn and bail out if that's not the case.
+	 */
+	if (WARN_ON(pixsize == 0))
+		return;
+	if ((dev->overlay_cap_field == V4L2_FIELD_TOP ||
+	     dev->overlay_cap_field == V4L2_FIELD_BOTTOM) &&
+	    dev->overlay_cap_field != buf->vb.field)
+		return;
+
+	vbuf += dev->compose_cap.left * pixsize + dev->compose_cap.top * stride;
+	x = dev->overlay_cap_left;
+	w = img_width;
+	if (x < 0) {
+		out_x = -x;
+		w = w - out_x;
+		x = 0;
+	} else {
+		w = dev->fb_cap.fmt.width - x;
+		if (w > img_width)
+			w = img_width;
+	}
+	if (w <= 0)
+		return;
+	if (dev->overlay_cap_top >= 0)
+		vbase += dev->overlay_cap_top * dev->fb_cap.fmt.bytesperline;
+	for (y = dev->overlay_cap_top;
+	     y < dev->overlay_cap_top + (int)img_height;
+	     y++, vbuf += stride) {
+		int px;
+
+		if (y < 0 || y > dev->fb_cap.fmt.height)
+			continue;
+		if (quick) {
+			memcpy(vbase + x * pixsize,
+			       vbuf + out_x * pixsize, w * pixsize);
+			vbase += dev->fb_cap.fmt.bytesperline;
+			continue;
+		}
+		for (px = 0; px < w; px++) {
+			if (!valid_pix(dev, y - dev->overlay_cap_top,
+				       px + out_x, y, px + x))
+				continue;
+			memcpy(vbase + (px + x) * pixsize,
+			       vbuf + (px + out_x) * pixsize,
+			       pixsize);
+		}
+		vbase += dev->fb_cap.fmt.bytesperline;
+	}
+}
+
+static void vivid_thread_vid_cap_tick(struct vivid_dev *dev, int dropped_bufs)
+{
+	struct vivid_buffer *vid_cap_buf = NULL;
+	struct vivid_buffer *vbi_cap_buf = NULL;
+
+	dprintk(dev, 1, "Video Capture Thread Tick\n");
+
+	while (dropped_bufs-- > 1)
+		tpg_update_mv_count(&dev->tpg,
+				dev->field_cap == V4L2_FIELD_NONE ||
+				dev->field_cap == V4L2_FIELD_ALTERNATE);
+
+	/* Drop a certain percentage of buffers. */
+	if (dev->perc_dropped_buffers &&
+	    prandom_u32_max(100) < dev->perc_dropped_buffers)
+		goto update_mv;
+
+	spin_lock(&dev->slock);
+	if (!list_empty(&dev->vid_cap_active)) {
+		vid_cap_buf = list_entry(dev->vid_cap_active.next, struct vivid_buffer, list);
+		list_del(&vid_cap_buf->list);
+	}
+	if (!list_empty(&dev->vbi_cap_active)) {
+		if (dev->field_cap != V4L2_FIELD_ALTERNATE ||
+		    (dev->vbi_cap_seq_count & 1)) {
+			vbi_cap_buf = list_entry(dev->vbi_cap_active.next,
+						 struct vivid_buffer, list);
+			list_del(&vbi_cap_buf->list);
+		}
+	}
+	spin_unlock(&dev->slock);
+
+	if (!vid_cap_buf && !vbi_cap_buf)
+		goto update_mv;
+
+	if (vid_cap_buf) {
+		/* Fill buffer */
+		vivid_fillbuff(dev, vid_cap_buf);
+		dprintk(dev, 1, "filled buffer %d\n",
+			vid_cap_buf->vb.vb2_buf.index);
+
+		/* Handle overlay */
+		if (dev->overlay_cap_owner && dev->fb_cap.base &&
+			dev->fb_cap.fmt.pixelformat == dev->fmt_cap->fourcc)
+			vivid_overlay(dev, vid_cap_buf);
+
+		vb2_buffer_done(&vid_cap_buf->vb.vb2_buf, dev->dqbuf_error ?
+				VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+		dprintk(dev, 2, "vid_cap buffer %d done\n",
+				vid_cap_buf->vb.vb2_buf.index);
+	}
+
+	if (vbi_cap_buf) {
+		if (dev->stream_sliced_vbi_cap)
+			vivid_sliced_vbi_cap_process(dev, vbi_cap_buf);
+		else
+			vivid_raw_vbi_cap_process(dev, vbi_cap_buf);
+		vb2_buffer_done(&vbi_cap_buf->vb.vb2_buf, dev->dqbuf_error ?
+				VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+		dprintk(dev, 2, "vbi_cap %d done\n",
+				vbi_cap_buf->vb.vb2_buf.index);
+	}
+	dev->dqbuf_error = false;
+
+update_mv:
+	/* Update the test pattern movement counters */
+	tpg_update_mv_count(&dev->tpg, dev->field_cap == V4L2_FIELD_NONE ||
+				       dev->field_cap == V4L2_FIELD_ALTERNATE);
+}
+
+static int vivid_thread_vid_cap(void *data)
+{
+	struct vivid_dev *dev = data;
+	u64 numerators_since_start;
+	u64 buffers_since_start;
+	u64 next_jiffies_since_start;
+	unsigned long jiffies_since_start;
+	unsigned long cur_jiffies;
+	unsigned wait_jiffies;
+	unsigned numerator;
+	unsigned denominator;
+	int dropped_bufs;
+
+	dprintk(dev, 1, "Video Capture Thread Start\n");
+
+	set_freezable();
+
+	/* Resets frame counters */
+	dev->cap_seq_offset = 0;
+	dev->cap_seq_count = 0;
+	dev->cap_seq_resync = false;
+	dev->jiffies_vid_cap = jiffies;
+
+	for (;;) {
+		try_to_freeze();
+		if (kthread_should_stop())
+			break;
+
+		mutex_lock(&dev->mutex);
+		cur_jiffies = jiffies;
+		if (dev->cap_seq_resync) {
+			dev->jiffies_vid_cap = cur_jiffies;
+			dev->cap_seq_offset = dev->cap_seq_count + 1;
+			dev->cap_seq_count = 0;
+			dev->cap_seq_resync = false;
+		}
+		numerator = dev->timeperframe_vid_cap.numerator;
+		denominator = dev->timeperframe_vid_cap.denominator;
+
+		if (dev->field_cap == V4L2_FIELD_ALTERNATE)
+			denominator *= 2;
+
+		/* Calculate the number of jiffies since we started streaming */
+		jiffies_since_start = cur_jiffies - dev->jiffies_vid_cap;
+		/* Get the number of buffers streamed since the start */
+		buffers_since_start = (u64)jiffies_since_start * denominator +
+				      (HZ * numerator) / 2;
+		do_div(buffers_since_start, HZ * numerator);
+
+		/*
+		 * After more than 0xf0000000 (rounded down to a multiple of
+		 * 'jiffies-per-day' to ease jiffies_to_msecs calculation)
+		 * jiffies have passed since we started streaming reset the
+		 * counters and keep track of the sequence offset.
+		 */
+		if (jiffies_since_start > JIFFIES_RESYNC) {
+			dev->jiffies_vid_cap = cur_jiffies;
+			dev->cap_seq_offset = buffers_since_start;
+			buffers_since_start = 0;
+		}
+		dropped_bufs = buffers_since_start + dev->cap_seq_offset - dev->cap_seq_count;
+		dev->cap_seq_count = buffers_since_start + dev->cap_seq_offset;
+		dev->vid_cap_seq_count = dev->cap_seq_count - dev->vid_cap_seq_start;
+		dev->vbi_cap_seq_count = dev->cap_seq_count - dev->vbi_cap_seq_start;
+
+		vivid_thread_vid_cap_tick(dev, dropped_bufs);
+
+		/*
+		 * Calculate the number of 'numerators' streamed since we started,
+		 * including the current buffer.
+		 */
+		numerators_since_start = ++buffers_since_start * numerator;
+
+		/* And the number of jiffies since we started */
+		jiffies_since_start = jiffies - dev->jiffies_vid_cap;
+
+		mutex_unlock(&dev->mutex);
+
+		/*
+		 * Calculate when that next buffer is supposed to start
+		 * in jiffies since we started streaming.
+		 */
+		next_jiffies_since_start = numerators_since_start * HZ +
+					   denominator / 2;
+		do_div(next_jiffies_since_start, denominator);
+		/* If it is in the past, then just schedule asap */
+		if (next_jiffies_since_start < jiffies_since_start)
+			next_jiffies_since_start = jiffies_since_start;
+
+		wait_jiffies = next_jiffies_since_start - jiffies_since_start;
+		schedule_timeout_interruptible(wait_jiffies ? wait_jiffies : 1);
+	}
+	dprintk(dev, 1, "Video Capture Thread End\n");
+	return 0;
+}
+
+static void vivid_grab_controls(struct vivid_dev *dev, bool grab)
+{
+	v4l2_ctrl_grab(dev->ctrl_has_crop_cap, grab);
+	v4l2_ctrl_grab(dev->ctrl_has_compose_cap, grab);
+	v4l2_ctrl_grab(dev->ctrl_has_scaler_cap, grab);
+}
+
+int vivid_start_generating_vid_cap(struct vivid_dev *dev, bool *pstreaming)
+{
+	dprintk(dev, 1, "%s\n", __func__);
+
+	if (dev->kthread_vid_cap) {
+		u32 seq_count = dev->cap_seq_count + dev->seq_wrap * 128;
+
+		if (pstreaming == &dev->vid_cap_streaming)
+			dev->vid_cap_seq_start = seq_count;
+		else
+			dev->vbi_cap_seq_start = seq_count;
+		*pstreaming = true;
+		return 0;
+	}
+
+	/* Resets frame counters */
+	tpg_init_mv_count(&dev->tpg);
+
+	dev->vid_cap_seq_start = dev->seq_wrap * 128;
+	dev->vbi_cap_seq_start = dev->seq_wrap * 128;
+
+	dev->kthread_vid_cap = kthread_run(vivid_thread_vid_cap, dev,
+			"%s-vid-cap", dev->v4l2_dev.name);
+
+	if (IS_ERR(dev->kthread_vid_cap)) {
+		v4l2_err(&dev->v4l2_dev, "kernel_thread() failed\n");
+		return PTR_ERR(dev->kthread_vid_cap);
+	}
+	*pstreaming = true;
+	vivid_grab_controls(dev, true);
+
+	dprintk(dev, 1, "returning from %s\n", __func__);
+	return 0;
+}
+
+void vivid_stop_generating_vid_cap(struct vivid_dev *dev, bool *pstreaming)
+{
+	dprintk(dev, 1, "%s\n", __func__);
+
+	if (dev->kthread_vid_cap == NULL)
+		return;
+
+	*pstreaming = false;
+	if (pstreaming == &dev->vid_cap_streaming) {
+		/* Release all active buffers */
+		while (!list_empty(&dev->vid_cap_active)) {
+			struct vivid_buffer *buf;
+
+			buf = list_entry(dev->vid_cap_active.next,
+					 struct vivid_buffer, list);
+			list_del(&buf->list);
+			vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+			dprintk(dev, 2, "vid_cap buffer %d done\n",
+				buf->vb.vb2_buf.index);
+		}
+	}
+
+	if (pstreaming == &dev->vbi_cap_streaming) {
+		while (!list_empty(&dev->vbi_cap_active)) {
+			struct vivid_buffer *buf;
+
+			buf = list_entry(dev->vbi_cap_active.next,
+					 struct vivid_buffer, list);
+			list_del(&buf->list);
+			vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+			dprintk(dev, 2, "vbi_cap buffer %d done\n",
+				buf->vb.vb2_buf.index);
+		}
+	}
+
+	if (dev->vid_cap_streaming || dev->vbi_cap_streaming)
+		return;
+
+	/* shutdown control thread */
+	vivid_grab_controls(dev, false);
+	mutex_unlock(&dev->mutex);
+	kthread_stop(dev->kthread_vid_cap);
+	dev->kthread_vid_cap = NULL;
+	mutex_lock(&dev->mutex);
+}
diff --git a/drivers/media/platform/vivid/vivid-kthread-cap.h b/drivers/media/platform/vivid/vivid-kthread-cap.h
new file mode 100644
index 0000000..5b92fc9
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-kthread-cap.h
@@ -0,0 +1,26 @@
+/*
+ * vivid-kthread-cap.h - video/vbi capture thread support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VIVID_KTHREAD_CAP_H_
+#define _VIVID_KTHREAD_CAP_H_
+
+int vivid_start_generating_vid_cap(struct vivid_dev *dev, bool *pstreaming);
+void vivid_stop_generating_vid_cap(struct vivid_dev *dev, bool *pstreaming);
+
+#endif
diff --git a/drivers/media/platform/vivid/vivid-kthread-out.c b/drivers/media/platform/vivid/vivid-kthread-out.c
new file mode 100644
index 0000000..c2c46dc
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-kthread-out.c
@@ -0,0 +1,305 @@
+/*
+ * vivid-kthread-out.h - video/vbi output thread support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/font.h>
+#include <linux/mutex.h>
+#include <linux/videodev2.h>
+#include <linux/kthread.h>
+#include <linux/freezer.h>
+#include <linux/random.h>
+#include <linux/v4l2-dv-timings.h>
+#include <asm/div64.h>
+#include <media/videobuf2-vmalloc.h>
+#include <media/v4l2-dv-timings.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-event.h>
+
+#include "vivid-core.h"
+#include "vivid-vid-common.h"
+#include "vivid-vid-cap.h"
+#include "vivid-vid-out.h"
+#include "vivid-radio-common.h"
+#include "vivid-radio-rx.h"
+#include "vivid-radio-tx.h"
+#include "vivid-sdr-cap.h"
+#include "vivid-vbi-cap.h"
+#include "vivid-vbi-out.h"
+#include "vivid-osd.h"
+#include "vivid-ctrls.h"
+#include "vivid-kthread-out.h"
+
+static void vivid_thread_vid_out_tick(struct vivid_dev *dev)
+{
+	struct vivid_buffer *vid_out_buf = NULL;
+	struct vivid_buffer *vbi_out_buf = NULL;
+
+	dprintk(dev, 1, "Video Output Thread Tick\n");
+
+	/* Drop a certain percentage of buffers. */
+	if (dev->perc_dropped_buffers &&
+	    prandom_u32_max(100) < dev->perc_dropped_buffers)
+		return;
+
+	spin_lock(&dev->slock);
+	/*
+	 * Only dequeue buffer if there is at least one more pending.
+	 * This makes video loopback possible.
+	 */
+	if (!list_empty(&dev->vid_out_active) &&
+	    !list_is_singular(&dev->vid_out_active)) {
+		vid_out_buf = list_entry(dev->vid_out_active.next,
+					 struct vivid_buffer, list);
+		list_del(&vid_out_buf->list);
+	}
+	if (!list_empty(&dev->vbi_out_active) &&
+	    (dev->field_out != V4L2_FIELD_ALTERNATE ||
+	     (dev->vbi_out_seq_count & 1))) {
+		vbi_out_buf = list_entry(dev->vbi_out_active.next,
+					 struct vivid_buffer, list);
+		list_del(&vbi_out_buf->list);
+	}
+	spin_unlock(&dev->slock);
+
+	if (!vid_out_buf && !vbi_out_buf)
+		return;
+
+	if (vid_out_buf) {
+		vid_out_buf->vb.sequence = dev->vid_out_seq_count;
+		if (dev->field_out == V4L2_FIELD_ALTERNATE) {
+			/*
+			 * The sequence counter counts frames, not fields.
+			 * So divide by two.
+			 */
+			vid_out_buf->vb.sequence /= 2;
+		}
+		v4l2_get_timestamp(&vid_out_buf->vb.timestamp);
+		vid_out_buf->vb.timestamp.tv_sec += dev->time_wrap_offset;
+		vb2_buffer_done(&vid_out_buf->vb.vb2_buf, dev->dqbuf_error ?
+				VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+		dprintk(dev, 2, "vid_out buffer %d done\n",
+			vid_out_buf->vb.vb2_buf.index);
+	}
+
+	if (vbi_out_buf) {
+		if (dev->stream_sliced_vbi_out)
+			vivid_sliced_vbi_out_process(dev, vbi_out_buf);
+
+		vbi_out_buf->vb.sequence = dev->vbi_out_seq_count;
+		v4l2_get_timestamp(&vbi_out_buf->vb.timestamp);
+		vbi_out_buf->vb.timestamp.tv_sec += dev->time_wrap_offset;
+		vb2_buffer_done(&vbi_out_buf->vb.vb2_buf, dev->dqbuf_error ?
+				VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+		dprintk(dev, 2, "vbi_out buffer %d done\n",
+			vbi_out_buf->vb.vb2_buf.index);
+	}
+	dev->dqbuf_error = false;
+}
+
+static int vivid_thread_vid_out(void *data)
+{
+	struct vivid_dev *dev = data;
+	u64 numerators_since_start;
+	u64 buffers_since_start;
+	u64 next_jiffies_since_start;
+	unsigned long jiffies_since_start;
+	unsigned long cur_jiffies;
+	unsigned wait_jiffies;
+	unsigned numerator;
+	unsigned denominator;
+
+	dprintk(dev, 1, "Video Output Thread Start\n");
+
+	set_freezable();
+
+	/* Resets frame counters */
+	dev->out_seq_offset = 0;
+	if (dev->seq_wrap)
+		dev->out_seq_count = 0xffffff80U;
+	dev->jiffies_vid_out = jiffies;
+	dev->vid_out_seq_start = dev->vbi_out_seq_start = 0;
+	dev->out_seq_resync = false;
+
+	for (;;) {
+		try_to_freeze();
+		if (kthread_should_stop())
+			break;
+
+		mutex_lock(&dev->mutex);
+		cur_jiffies = jiffies;
+		if (dev->out_seq_resync) {
+			dev->jiffies_vid_out = cur_jiffies;
+			dev->out_seq_offset = dev->out_seq_count + 1;
+			dev->out_seq_count = 0;
+			dev->out_seq_resync = false;
+		}
+		numerator = dev->timeperframe_vid_out.numerator;
+		denominator = dev->timeperframe_vid_out.denominator;
+
+		if (dev->field_out == V4L2_FIELD_ALTERNATE)
+			denominator *= 2;
+
+		/* Calculate the number of jiffies since we started streaming */
+		jiffies_since_start = cur_jiffies - dev->jiffies_vid_out;
+		/* Get the number of buffers streamed since the start */
+		buffers_since_start = (u64)jiffies_since_start * denominator +
+				      (HZ * numerator) / 2;
+		do_div(buffers_since_start, HZ * numerator);
+
+		/*
+		 * After more than 0xf0000000 (rounded down to a multiple of
+		 * 'jiffies-per-day' to ease jiffies_to_msecs calculation)
+		 * jiffies have passed since we started streaming reset the
+		 * counters and keep track of the sequence offset.
+		 */
+		if (jiffies_since_start > JIFFIES_RESYNC) {
+			dev->jiffies_vid_out = cur_jiffies;
+			dev->out_seq_offset = buffers_since_start;
+			buffers_since_start = 0;
+		}
+		dev->out_seq_count = buffers_since_start + dev->out_seq_offset;
+		dev->vid_out_seq_count = dev->out_seq_count - dev->vid_out_seq_start;
+		dev->vbi_out_seq_count = dev->out_seq_count - dev->vbi_out_seq_start;
+
+		vivid_thread_vid_out_tick(dev);
+		mutex_unlock(&dev->mutex);
+
+		/*
+		 * Calculate the number of 'numerators' streamed since we started,
+		 * not including the current buffer.
+		 */
+		numerators_since_start = buffers_since_start * numerator;
+
+		/* And the number of jiffies since we started */
+		jiffies_since_start = jiffies - dev->jiffies_vid_out;
+
+		/* Increase by the 'numerator' of one buffer */
+		numerators_since_start += numerator;
+		/*
+		 * Calculate when that next buffer is supposed to start
+		 * in jiffies since we started streaming.
+		 */
+		next_jiffies_since_start = numerators_since_start * HZ +
+					   denominator / 2;
+		do_div(next_jiffies_since_start, denominator);
+		/* If it is in the past, then just schedule asap */
+		if (next_jiffies_since_start < jiffies_since_start)
+			next_jiffies_since_start = jiffies_since_start;
+
+		wait_jiffies = next_jiffies_since_start - jiffies_since_start;
+		schedule_timeout_interruptible(wait_jiffies ? wait_jiffies : 1);
+	}
+	dprintk(dev, 1, "Video Output Thread End\n");
+	return 0;
+}
+
+static void vivid_grab_controls(struct vivid_dev *dev, bool grab)
+{
+	v4l2_ctrl_grab(dev->ctrl_has_crop_out, grab);
+	v4l2_ctrl_grab(dev->ctrl_has_compose_out, grab);
+	v4l2_ctrl_grab(dev->ctrl_has_scaler_out, grab);
+	v4l2_ctrl_grab(dev->ctrl_tx_mode, grab);
+	v4l2_ctrl_grab(dev->ctrl_tx_rgb_range, grab);
+}
+
+int vivid_start_generating_vid_out(struct vivid_dev *dev, bool *pstreaming)
+{
+	dprintk(dev, 1, "%s\n", __func__);
+
+	if (dev->kthread_vid_out) {
+		u32 seq_count = dev->out_seq_count + dev->seq_wrap * 128;
+
+		if (pstreaming == &dev->vid_out_streaming)
+			dev->vid_out_seq_start = seq_count;
+		else
+			dev->vbi_out_seq_start = seq_count;
+		*pstreaming = true;
+		return 0;
+	}
+
+	/* Resets frame counters */
+	dev->jiffies_vid_out = jiffies;
+	dev->vid_out_seq_start = dev->seq_wrap * 128;
+	dev->vbi_out_seq_start = dev->seq_wrap * 128;
+
+	dev->kthread_vid_out = kthread_run(vivid_thread_vid_out, dev,
+			"%s-vid-out", dev->v4l2_dev.name);
+
+	if (IS_ERR(dev->kthread_vid_out)) {
+		v4l2_err(&dev->v4l2_dev, "kernel_thread() failed\n");
+		return PTR_ERR(dev->kthread_vid_out);
+	}
+	*pstreaming = true;
+	vivid_grab_controls(dev, true);
+
+	dprintk(dev, 1, "returning from %s\n", __func__);
+	return 0;
+}
+
+void vivid_stop_generating_vid_out(struct vivid_dev *dev, bool *pstreaming)
+{
+	dprintk(dev, 1, "%s\n", __func__);
+
+	if (dev->kthread_vid_out == NULL)
+		return;
+
+	*pstreaming = false;
+	if (pstreaming == &dev->vid_out_streaming) {
+		/* Release all active buffers */
+		while (!list_empty(&dev->vid_out_active)) {
+			struct vivid_buffer *buf;
+
+			buf = list_entry(dev->vid_out_active.next,
+					 struct vivid_buffer, list);
+			list_del(&buf->list);
+			vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+			dprintk(dev, 2, "vid_out buffer %d done\n",
+				buf->vb.vb2_buf.index);
+		}
+	}
+
+	if (pstreaming == &dev->vbi_out_streaming) {
+		while (!list_empty(&dev->vbi_out_active)) {
+			struct vivid_buffer *buf;
+
+			buf = list_entry(dev->vbi_out_active.next,
+					 struct vivid_buffer, list);
+			list_del(&buf->list);
+			vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+			dprintk(dev, 2, "vbi_out buffer %d done\n",
+				buf->vb.vb2_buf.index);
+		}
+	}
+
+	if (dev->vid_out_streaming || dev->vbi_out_streaming)
+		return;
+
+	/* shutdown control thread */
+	vivid_grab_controls(dev, false);
+	mutex_unlock(&dev->mutex);
+	kthread_stop(dev->kthread_vid_out);
+	dev->kthread_vid_out = NULL;
+	mutex_lock(&dev->mutex);
+}
diff --git a/drivers/media/platform/vivid/vivid-kthread-out.h b/drivers/media/platform/vivid/vivid-kthread-out.h
new file mode 100644
index 0000000..2bf04a1
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-kthread-out.h
@@ -0,0 +1,26 @@
+/*
+ * vivid-kthread-out.h - video/vbi output thread support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VIVID_KTHREAD_OUT_H_
+#define _VIVID_KTHREAD_OUT_H_
+
+int vivid_start_generating_vid_out(struct vivid_dev *dev, bool *pstreaming);
+void vivid_stop_generating_vid_out(struct vivid_dev *dev, bool *pstreaming);
+
+#endif
diff --git a/drivers/media/platform/vivid/vivid-osd.c b/drivers/media/platform/vivid/vivid-osd.c
new file mode 100644
index 0000000..e15eef6
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-osd.c
@@ -0,0 +1,401 @@
+/*
+ * vivid-osd.c - osd support for testing overlays.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/font.h>
+#include <linux/mutex.h>
+#include <linux/videodev2.h>
+#include <linux/kthread.h>
+#include <linux/freezer.h>
+#include <linux/fb.h>
+#include <media/videobuf2-vmalloc.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-common.h>
+
+#include "vivid-core.h"
+#include "vivid-osd.h"
+
+#define MAX_OSD_WIDTH  720
+#define MAX_OSD_HEIGHT 576
+
+/*
+ * Order: white, yellow, cyan, green, magenta, red, blue, black,
+ * and same again with the alpha bit set (if any)
+ */
+static const u16 rgb555[16] = {
+	0x7fff, 0x7fe0, 0x03ff, 0x03e0, 0x7c1f, 0x7c00, 0x001f, 0x0000,
+	0xffff, 0xffe0, 0x83ff, 0x83e0, 0xfc1f, 0xfc00, 0x801f, 0x8000
+};
+
+static const u16 rgb565[16] = {
+	0xffff, 0xffe0, 0x07ff, 0x07e0, 0xf81f, 0xf800, 0x001f, 0x0000,
+	0xffff, 0xffe0, 0x07ff, 0x07e0, 0xf81f, 0xf800, 0x001f, 0x0000
+};
+
+void vivid_clear_fb(struct vivid_dev *dev)
+{
+	void *p = dev->video_vbase;
+	const u16 *rgb = rgb555;
+	unsigned x, y;
+
+	if (dev->fb_defined.green.length == 6)
+		rgb = rgb565;
+
+	for (y = 0; y < dev->display_height; y++) {
+		u16 *d = p;
+
+		for (x = 0; x < dev->display_width; x++)
+			d[x] = rgb[(y / 16 + x / 16) % 16];
+		p += dev->display_byte_stride;
+	}
+}
+
+/* --------------------------------------------------------------------- */
+
+static int vivid_fb_ioctl(struct fb_info *info, unsigned cmd, unsigned long arg)
+{
+	struct vivid_dev *dev = (struct vivid_dev *)info->par;
+
+	switch (cmd) {
+	case FBIOGET_VBLANK: {
+		struct fb_vblank vblank;
+
+		memset(&vblank, 0, sizeof(vblank));
+		vblank.flags = FB_VBLANK_HAVE_COUNT | FB_VBLANK_HAVE_VCOUNT |
+			FB_VBLANK_HAVE_VSYNC;
+		vblank.count = 0;
+		vblank.vcount = 0;
+		vblank.hcount = 0;
+		if (copy_to_user((void __user *)arg, &vblank, sizeof(vblank)))
+			return -EFAULT;
+		return 0;
+	}
+
+	default:
+		dprintk(dev, 1, "Unknown ioctl %08x\n", cmd);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/* Framebuffer device handling */
+
+static int vivid_fb_set_var(struct vivid_dev *dev, struct fb_var_screeninfo *var)
+{
+	dprintk(dev, 1, "vivid_fb_set_var\n");
+
+	if (var->bits_per_pixel != 16) {
+		dprintk(dev, 1, "vivid_fb_set_var - Invalid bpp\n");
+		return -EINVAL;
+	}
+	dev->display_byte_stride = var->xres * dev->bytes_per_pixel;
+
+	return 0;
+}
+
+static int vivid_fb_get_fix(struct vivid_dev *dev, struct fb_fix_screeninfo *fix)
+{
+	dprintk(dev, 1, "vivid_fb_get_fix\n");
+	memset(fix, 0, sizeof(struct fb_fix_screeninfo));
+	strlcpy(fix->id, "vioverlay fb", sizeof(fix->id));
+	fix->smem_start = dev->video_pbase;
+	fix->smem_len = dev->video_buffer_size;
+	fix->type = FB_TYPE_PACKED_PIXELS;
+	fix->visual = FB_VISUAL_TRUECOLOR;
+	fix->xpanstep = 1;
+	fix->ypanstep = 1;
+	fix->ywrapstep = 0;
+	fix->line_length = dev->display_byte_stride;
+	fix->accel = FB_ACCEL_NONE;
+	return 0;
+}
+
+/* Check the requested display mode, returning -EINVAL if we can't
+   handle it. */
+
+static int _vivid_fb_check_var(struct fb_var_screeninfo *var, struct vivid_dev *dev)
+{
+	dprintk(dev, 1, "vivid_fb_check_var\n");
+
+	var->bits_per_pixel = 16;
+	if (var->green.length == 5) {
+		var->red.offset = 10;
+		var->red.length = 5;
+		var->green.offset = 5;
+		var->green.length = 5;
+		var->blue.offset = 0;
+		var->blue.length = 5;
+		var->transp.offset = 15;
+		var->transp.length = 1;
+	} else {
+		var->red.offset = 11;
+		var->red.length = 5;
+		var->green.offset = 5;
+		var->green.length = 6;
+		var->blue.offset = 0;
+		var->blue.length = 5;
+		var->transp.offset = 0;
+		var->transp.length = 0;
+	}
+	var->xoffset = var->yoffset = 0;
+	var->left_margin = var->upper_margin = 0;
+	var->nonstd = 0;
+
+	var->vmode &= ~FB_VMODE_MASK;
+	var->vmode = FB_VMODE_NONINTERLACED;
+
+	/* Dummy values */
+	var->hsync_len = 24;
+	var->vsync_len = 2;
+	var->pixclock = 84316;
+	var->right_margin = 776;
+	var->lower_margin = 591;
+	return 0;
+}
+
+static int vivid_fb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
+{
+	struct vivid_dev *dev = (struct vivid_dev *) info->par;
+
+	dprintk(dev, 1, "vivid_fb_check_var\n");
+	return _vivid_fb_check_var(var, dev);
+}
+
+static int vivid_fb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
+{
+	return 0;
+}
+
+static int vivid_fb_set_par(struct fb_info *info)
+{
+	int rc = 0;
+	struct vivid_dev *dev = (struct vivid_dev *) info->par;
+
+	dprintk(dev, 1, "vivid_fb_set_par\n");
+
+	rc = vivid_fb_set_var(dev, &info->var);
+	vivid_fb_get_fix(dev, &info->fix);
+	return rc;
+}
+
+static int vivid_fb_setcolreg(unsigned regno, unsigned red, unsigned green,
+				unsigned blue, unsigned transp,
+				struct fb_info *info)
+{
+	u32 color, *palette;
+
+	if (regno >= info->cmap.len)
+		return -EINVAL;
+
+	color = ((transp & 0xFF00) << 16) | ((red & 0xFF00) << 8) |
+		 (green & 0xFF00) | ((blue & 0xFF00) >> 8);
+	if (regno >= 16)
+		return -EINVAL;
+
+	palette = info->pseudo_palette;
+	if (info->var.bits_per_pixel == 16) {
+		switch (info->var.green.length) {
+		case 6:
+			color = (red & 0xf800) |
+				((green & 0xfc00) >> 5) |
+				((blue & 0xf800) >> 11);
+			break;
+		case 5:
+			color = ((red & 0xf800) >> 1) |
+				((green & 0xf800) >> 6) |
+				((blue & 0xf800) >> 11) |
+				(transp ? 0x8000 : 0);
+			break;
+		}
+	}
+	palette[regno] = color;
+	return 0;
+}
+
+/* We don't really support blanking. All this does is enable or
+   disable the OSD. */
+static int vivid_fb_blank(int blank_mode, struct fb_info *info)
+{
+	struct vivid_dev *dev = (struct vivid_dev *)info->par;
+
+	dprintk(dev, 1, "Set blanking mode : %d\n", blank_mode);
+	switch (blank_mode) {
+	case FB_BLANK_UNBLANK:
+		break;
+	case FB_BLANK_NORMAL:
+	case FB_BLANK_HSYNC_SUSPEND:
+	case FB_BLANK_VSYNC_SUSPEND:
+	case FB_BLANK_POWERDOWN:
+		break;
+	}
+	return 0;
+}
+
+static struct fb_ops vivid_fb_ops = {
+	.owner = THIS_MODULE,
+	.fb_check_var   = vivid_fb_check_var,
+	.fb_set_par     = vivid_fb_set_par,
+	.fb_setcolreg   = vivid_fb_setcolreg,
+	.fb_fillrect    = cfb_fillrect,
+	.fb_copyarea    = cfb_copyarea,
+	.fb_imageblit   = cfb_imageblit,
+	.fb_cursor      = NULL,
+	.fb_ioctl       = vivid_fb_ioctl,
+	.fb_pan_display = vivid_fb_pan_display,
+	.fb_blank       = vivid_fb_blank,
+};
+
+/* Initialization */
+
+
+/* Setup our initial video mode */
+static int vivid_fb_init_vidmode(struct vivid_dev *dev)
+{
+	struct v4l2_rect start_window;
+
+	/* Color mode */
+
+	dev->bits_per_pixel = 16;
+	dev->bytes_per_pixel = dev->bits_per_pixel / 8;
+
+	start_window.width = MAX_OSD_WIDTH;
+	start_window.left = 0;
+
+	dev->display_byte_stride = start_window.width * dev->bytes_per_pixel;
+
+	/* Vertical size & position */
+
+	start_window.height = MAX_OSD_HEIGHT;
+	start_window.top = 0;
+
+	dev->display_width = start_window.width;
+	dev->display_height = start_window.height;
+
+	/* Generate a valid fb_var_screeninfo */
+
+	dev->fb_defined.xres = dev->display_width;
+	dev->fb_defined.yres = dev->display_height;
+	dev->fb_defined.xres_virtual = dev->display_width;
+	dev->fb_defined.yres_virtual = dev->display_height;
+	dev->fb_defined.bits_per_pixel = dev->bits_per_pixel;
+	dev->fb_defined.vmode = FB_VMODE_NONINTERLACED;
+	dev->fb_defined.left_margin = start_window.left + 1;
+	dev->fb_defined.upper_margin = start_window.top + 1;
+	dev->fb_defined.accel_flags = FB_ACCEL_NONE;
+	dev->fb_defined.nonstd = 0;
+	/* set default to 1:5:5:5 */
+	dev->fb_defined.green.length = 5;
+
+	/* We've filled in the most data, let the usual mode check
+	   routine fill in the rest. */
+	_vivid_fb_check_var(&dev->fb_defined, dev);
+
+	/* Generate valid fb_fix_screeninfo */
+
+	vivid_fb_get_fix(dev, &dev->fb_fix);
+
+	/* Generate valid fb_info */
+
+	dev->fb_info.node = -1;
+	dev->fb_info.flags = FBINFO_FLAG_DEFAULT;
+	dev->fb_info.fbops = &vivid_fb_ops;
+	dev->fb_info.par = dev;
+	dev->fb_info.var = dev->fb_defined;
+	dev->fb_info.fix = dev->fb_fix;
+	dev->fb_info.screen_base = (u8 __iomem *)dev->video_vbase;
+	dev->fb_info.fbops = &vivid_fb_ops;
+
+	/* Supply some monitor specs. Bogus values will do for now */
+	dev->fb_info.monspecs.hfmin = 8000;
+	dev->fb_info.monspecs.hfmax = 70000;
+	dev->fb_info.monspecs.vfmin = 10;
+	dev->fb_info.monspecs.vfmax = 100;
+
+	/* Allocate color map */
+	if (fb_alloc_cmap(&dev->fb_info.cmap, 256, 1)) {
+		pr_err("abort, unable to alloc cmap\n");
+		return -ENOMEM;
+	}
+
+	/* Allocate the pseudo palette */
+	dev->fb_info.pseudo_palette = kmalloc_array(16, sizeof(u32), GFP_KERNEL);
+
+	return dev->fb_info.pseudo_palette ? 0 : -ENOMEM;
+}
+
+/* Release any memory we've grabbed */
+void vivid_fb_release_buffers(struct vivid_dev *dev)
+{
+	if (dev->video_vbase == NULL)
+		return;
+
+	/* Release cmap */
+	if (dev->fb_info.cmap.len)
+		fb_dealloc_cmap(&dev->fb_info.cmap);
+
+	/* Release pseudo palette */
+	kfree(dev->fb_info.pseudo_palette);
+	kfree((void *)dev->video_vbase);
+}
+
+/* Initialize the specified card */
+
+int vivid_fb_init(struct vivid_dev *dev)
+{
+	int ret;
+
+	dev->video_buffer_size = MAX_OSD_HEIGHT * MAX_OSD_WIDTH * 2;
+	dev->video_vbase = kzalloc(dev->video_buffer_size, GFP_KERNEL | GFP_DMA32);
+	if (dev->video_vbase == NULL)
+		return -ENOMEM;
+	dev->video_pbase = virt_to_phys(dev->video_vbase);
+
+	pr_info("Framebuffer at 0x%lx, mapped to 0x%p, size %dk\n",
+			dev->video_pbase, dev->video_vbase,
+			dev->video_buffer_size / 1024);
+
+	/* Set the startup video mode information */
+	ret = vivid_fb_init_vidmode(dev);
+	if (ret) {
+		vivid_fb_release_buffers(dev);
+		return ret;
+	}
+
+	vivid_clear_fb(dev);
+
+	/* Register the framebuffer */
+	if (register_framebuffer(&dev->fb_info) < 0) {
+		vivid_fb_release_buffers(dev);
+		return -EINVAL;
+	}
+
+	/* Set the card to the requested mode */
+	vivid_fb_set_par(&dev->fb_info);
+	return 0;
+
+}
diff --git a/drivers/media/platform/vivid/vivid-osd.h b/drivers/media/platform/vivid/vivid-osd.h
new file mode 100644
index 0000000..57c9daa
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-osd.h
@@ -0,0 +1,27 @@
+/*
+ * vivid-osd.h - output overlay support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VIVID_OSD_H_
+#define _VIVID_OSD_H_
+
+int vivid_fb_init(struct vivid_dev *dev);
+void vivid_fb_release_buffers(struct vivid_dev *dev);
+void vivid_clear_fb(struct vivid_dev *dev);
+
+#endif
diff --git a/drivers/media/platform/vivid/vivid-radio-common.c b/drivers/media/platform/vivid/vivid-radio-common.c
new file mode 100644
index 0000000..78c1e92
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-radio-common.c
@@ -0,0 +1,189 @@
+/*
+ * vivid-radio-common.c - common radio rx/tx support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/videodev2.h>
+
+#include "vivid-core.h"
+#include "vivid-ctrls.h"
+#include "vivid-radio-common.h"
+#include "vivid-rds-gen.h"
+
+/*
+ * These functions are shared between the vivid receiver and transmitter
+ * since both use the same frequency bands.
+ */
+
+const struct v4l2_frequency_band vivid_radio_bands[TOT_BANDS] = {
+	/* Band FM */
+	{
+		.type = V4L2_TUNER_RADIO,
+		.index = 0,
+		.capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_STEREO |
+			      V4L2_TUNER_CAP_FREQ_BANDS,
+		.rangelow   = FM_FREQ_RANGE_LOW,
+		.rangehigh  = FM_FREQ_RANGE_HIGH,
+		.modulation = V4L2_BAND_MODULATION_FM,
+	},
+	/* Band AM */
+	{
+		.type = V4L2_TUNER_RADIO,
+		.index = 1,
+		.capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_FREQ_BANDS,
+		.rangelow   = AM_FREQ_RANGE_LOW,
+		.rangehigh  = AM_FREQ_RANGE_HIGH,
+		.modulation = V4L2_BAND_MODULATION_AM,
+	},
+	/* Band SW */
+	{
+		.type = V4L2_TUNER_RADIO,
+		.index = 2,
+		.capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_FREQ_BANDS,
+		.rangelow   = SW_FREQ_RANGE_LOW,
+		.rangehigh  = SW_FREQ_RANGE_HIGH,
+		.modulation = V4L2_BAND_MODULATION_AM,
+	},
+};
+
+/*
+ * Initialize the RDS generator. If we can loop, then the RDS generator
+ * is set up with the values from the RDS TX controls, otherwise it
+ * will fill in standard values using one of two alternates.
+ */
+void vivid_radio_rds_init(struct vivid_dev *dev)
+{
+	struct vivid_rds_gen *rds = &dev->rds_gen;
+	bool alt = dev->radio_rx_rds_use_alternates;
+
+	/* Do nothing, blocks will be filled by the transmitter */
+	if (dev->radio_rds_loop && !dev->radio_tx_rds_controls)
+		return;
+
+	if (dev->radio_rds_loop) {
+		v4l2_ctrl_lock(dev->radio_tx_rds_pi);
+		rds->picode = dev->radio_tx_rds_pi->cur.val;
+		rds->pty = dev->radio_tx_rds_pty->cur.val;
+		rds->mono_stereo = dev->radio_tx_rds_mono_stereo->cur.val;
+		rds->art_head = dev->radio_tx_rds_art_head->cur.val;
+		rds->compressed = dev->radio_tx_rds_compressed->cur.val;
+		rds->dyn_pty = dev->radio_tx_rds_dyn_pty->cur.val;
+		rds->ta = dev->radio_tx_rds_ta->cur.val;
+		rds->tp = dev->radio_tx_rds_tp->cur.val;
+		rds->ms = dev->radio_tx_rds_ms->cur.val;
+		strlcpy(rds->psname,
+			dev->radio_tx_rds_psname->p_cur.p_char,
+			sizeof(rds->psname));
+		strlcpy(rds->radiotext,
+			dev->radio_tx_rds_radiotext->p_cur.p_char + alt * 64,
+			sizeof(rds->radiotext));
+		v4l2_ctrl_unlock(dev->radio_tx_rds_pi);
+	} else {
+		vivid_rds_gen_fill(rds, dev->radio_rx_freq, alt);
+	}
+	if (dev->radio_rx_rds_controls) {
+		v4l2_ctrl_s_ctrl(dev->radio_rx_rds_pty, rds->pty);
+		v4l2_ctrl_s_ctrl(dev->radio_rx_rds_ta, rds->ta);
+		v4l2_ctrl_s_ctrl(dev->radio_rx_rds_tp, rds->tp);
+		v4l2_ctrl_s_ctrl(dev->radio_rx_rds_ms, rds->ms);
+		v4l2_ctrl_s_ctrl_string(dev->radio_rx_rds_psname, rds->psname);
+		v4l2_ctrl_s_ctrl_string(dev->radio_rx_rds_radiotext, rds->radiotext);
+		if (!dev->radio_rds_loop)
+			dev->radio_rx_rds_use_alternates = !dev->radio_rx_rds_use_alternates;
+	}
+	vivid_rds_generate(rds);
+}
+
+/*
+ * Calculate the emulated signal quality taking into account the frequency
+ * the transmitter is using.
+ */
+static void vivid_radio_calc_sig_qual(struct vivid_dev *dev)
+{
+	int mod = 16000;
+	int delta = 800;
+	int sig_qual, sig_qual_tx = mod;
+
+	/*
+	 * For SW and FM there is a channel every 1000 kHz, for AM there is one
+	 * every 100 kHz.
+	 */
+	if (dev->radio_rx_freq <= AM_FREQ_RANGE_HIGH) {
+		mod /= 10;
+		delta /= 10;
+	}
+	sig_qual = (dev->radio_rx_freq + delta) % mod - delta;
+	if (dev->has_radio_tx)
+		sig_qual_tx = dev->radio_rx_freq - dev->radio_tx_freq;
+	if (abs(sig_qual_tx) <= abs(sig_qual)) {
+		sig_qual = sig_qual_tx;
+		/*
+		 * Zero the internal rds buffer if we are going to loop
+		 * rds blocks.
+		 */
+		if (!dev->radio_rds_loop && !dev->radio_tx_rds_controls)
+			memset(dev->rds_gen.data, 0,
+			       sizeof(dev->rds_gen.data));
+		dev->radio_rds_loop = dev->radio_rx_freq >= FM_FREQ_RANGE_LOW;
+	} else {
+		dev->radio_rds_loop = false;
+	}
+	if (dev->radio_rx_freq <= AM_FREQ_RANGE_HIGH)
+		sig_qual *= 10;
+	dev->radio_rx_sig_qual = sig_qual;
+}
+
+int vivid_radio_g_frequency(struct file *file, const unsigned *pfreq, struct v4l2_frequency *vf)
+{
+	if (vf->tuner != 0)
+		return -EINVAL;
+	vf->frequency = *pfreq;
+	return 0;
+}
+
+int vivid_radio_s_frequency(struct file *file, unsigned *pfreq, const struct v4l2_frequency *vf)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	unsigned freq;
+	unsigned band;
+
+	if (vf->tuner != 0)
+		return -EINVAL;
+
+	if (vf->frequency >= (FM_FREQ_RANGE_LOW + SW_FREQ_RANGE_HIGH) / 2)
+		band = BAND_FM;
+	else if (vf->frequency <= (AM_FREQ_RANGE_HIGH + SW_FREQ_RANGE_LOW) / 2)
+		band = BAND_AM;
+	else
+		band = BAND_SW;
+
+	freq = clamp_t(u32, vf->frequency, vivid_radio_bands[band].rangelow,
+					   vivid_radio_bands[band].rangehigh);
+	*pfreq = freq;
+
+	/*
+	 * For both receiver and transmitter recalculate the signal quality
+	 * (since that depends on both frequencies) and re-init the rds
+	 * generator.
+	 */
+	vivid_radio_calc_sig_qual(dev);
+	vivid_radio_rds_init(dev);
+	return 0;
+}
diff --git a/drivers/media/platform/vivid/vivid-radio-common.h b/drivers/media/platform/vivid/vivid-radio-common.h
new file mode 100644
index 0000000..92fe589
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-radio-common.h
@@ -0,0 +1,40 @@
+/*
+ * vivid-radio-common.h - common radio rx/tx support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VIVID_RADIO_COMMON_H_
+#define _VIVID_RADIO_COMMON_H_
+
+/* The supported radio frequency ranges in kHz */
+#define FM_FREQ_RANGE_LOW       (64000U * 16U)
+#define FM_FREQ_RANGE_HIGH      (108000U * 16U)
+#define AM_FREQ_RANGE_LOW       (520U * 16U)
+#define AM_FREQ_RANGE_HIGH      (1710U * 16U)
+#define SW_FREQ_RANGE_LOW       (2300U * 16U)
+#define SW_FREQ_RANGE_HIGH      (26100U * 16U)
+
+enum { BAND_FM, BAND_AM, BAND_SW, TOT_BANDS };
+
+extern const struct v4l2_frequency_band vivid_radio_bands[TOT_BANDS];
+
+int vivid_radio_g_frequency(struct file *file, const unsigned *freq, struct v4l2_frequency *vf);
+int vivid_radio_s_frequency(struct file *file, unsigned *freq, const struct v4l2_frequency *vf);
+
+void vivid_radio_rds_init(struct vivid_dev *dev);
+
+#endif
diff --git a/drivers/media/platform/vivid/vivid-radio-rx.c b/drivers/media/platform/vivid/vivid-radio-rx.c
new file mode 100644
index 0000000..f99092c
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-radio-rx.c
@@ -0,0 +1,289 @@
+/*
+ * vivid-radio-rx.c - radio receiver support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/videodev2.h>
+#include <linux/v4l2-dv-timings.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-dv-timings.h>
+
+#include "vivid-core.h"
+#include "vivid-ctrls.h"
+#include "vivid-radio-common.h"
+#include "vivid-rds-gen.h"
+#include "vivid-radio-rx.h"
+
+ssize_t vivid_radio_rx_read(struct file *file, char __user *buf,
+			 size_t size, loff_t *offset)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct timespec ts;
+	struct v4l2_rds_data *data = dev->rds_gen.data;
+	bool use_alternates;
+	unsigned blk;
+	int perc;
+	int i;
+
+	if (dev->radio_rx_rds_controls)
+		return -EINVAL;
+	if (size < sizeof(*data))
+		return 0;
+	size = sizeof(*data) * (size / sizeof(*data));
+
+	if (mutex_lock_interruptible(&dev->mutex))
+		return -ERESTARTSYS;
+	if (dev->radio_rx_rds_owner &&
+	    file->private_data != dev->radio_rx_rds_owner) {
+		mutex_unlock(&dev->mutex);
+		return -EBUSY;
+	}
+	if (dev->radio_rx_rds_owner == NULL) {
+		vivid_radio_rds_init(dev);
+		dev->radio_rx_rds_owner = file->private_data;
+	}
+
+retry:
+	ktime_get_ts(&ts);
+	use_alternates = ts.tv_sec % 10 >= 5;
+	if (dev->radio_rx_rds_last_block == 0 ||
+	    dev->radio_rx_rds_use_alternates != use_alternates) {
+		dev->radio_rx_rds_use_alternates = use_alternates;
+		/* Re-init the RDS generator */
+		vivid_radio_rds_init(dev);
+	}
+	ts = timespec_sub(ts, dev->radio_rds_init_ts);
+	blk = ts.tv_sec * 100 + ts.tv_nsec / 10000000;
+	blk = (blk * VIVID_RDS_GEN_BLOCKS) / 500;
+	if (blk >= dev->radio_rx_rds_last_block + VIVID_RDS_GEN_BLOCKS)
+		dev->radio_rx_rds_last_block = blk - VIVID_RDS_GEN_BLOCKS + 1;
+
+	/*
+	 * No data is available if there hasn't been time to get new data,
+	 * or if the RDS receiver has been disabled, or if we use the data
+	 * from the RDS transmitter and that RDS transmitter has been disabled,
+	 * or if the signal quality is too weak.
+	 */
+	if (blk == dev->radio_rx_rds_last_block || !dev->radio_rx_rds_enabled ||
+	    (dev->radio_rds_loop && !(dev->radio_tx_subchans & V4L2_TUNER_SUB_RDS)) ||
+	    abs(dev->radio_rx_sig_qual) > 200) {
+		mutex_unlock(&dev->mutex);
+		if (file->f_flags & O_NONBLOCK)
+			return -EWOULDBLOCK;
+		if (msleep_interruptible(20) && signal_pending(current))
+			return -EINTR;
+		if (mutex_lock_interruptible(&dev->mutex))
+			return -ERESTARTSYS;
+		goto retry;
+	}
+
+	/* abs(dev->radio_rx_sig_qual) <= 200, map that to a 0-50% range */
+	perc = abs(dev->radio_rx_sig_qual) / 4;
+
+	for (i = 0; i < size && blk > dev->radio_rx_rds_last_block;
+			dev->radio_rx_rds_last_block++) {
+		unsigned data_blk = dev->radio_rx_rds_last_block % VIVID_RDS_GEN_BLOCKS;
+		struct v4l2_rds_data rds = data[data_blk];
+
+		if (data_blk == 0 && dev->radio_rds_loop)
+			vivid_radio_rds_init(dev);
+		if (perc && prandom_u32_max(100) < perc) {
+			switch (prandom_u32_max(4)) {
+			case 0:
+				rds.block |= V4L2_RDS_BLOCK_CORRECTED;
+				break;
+			case 1:
+				rds.block |= V4L2_RDS_BLOCK_INVALID;
+				break;
+			case 2:
+				rds.block |= V4L2_RDS_BLOCK_ERROR;
+				rds.lsb = prandom_u32_max(256);
+				rds.msb = prandom_u32_max(256);
+				break;
+			case 3: /* Skip block altogether */
+				if (i)
+					continue;
+				/*
+				 * Must make sure at least one block is
+				 * returned, otherwise the application
+				 * might think that end-of-file occurred.
+				 */
+				break;
+			}
+		}
+		if (copy_to_user(buf + i, &rds, sizeof(rds))) {
+			i = -EFAULT;
+			break;
+		}
+		i += sizeof(rds);
+	}
+	mutex_unlock(&dev->mutex);
+	return i;
+}
+
+unsigned int vivid_radio_rx_poll(struct file *file, struct poll_table_struct *wait)
+{
+	return POLLIN | POLLRDNORM | v4l2_ctrl_poll(file, wait);
+}
+
+int vivid_radio_rx_enum_freq_bands(struct file *file, void *fh, struct v4l2_frequency_band *band)
+{
+	if (band->tuner != 0)
+		return -EINVAL;
+
+	if (band->index >= TOT_BANDS)
+		return -EINVAL;
+
+	*band = vivid_radio_bands[band->index];
+	return 0;
+}
+
+int vivid_radio_rx_s_hw_freq_seek(struct file *file, void *fh, const struct v4l2_hw_freq_seek *a)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	unsigned low, high;
+	unsigned freq;
+	unsigned spacing;
+	unsigned band;
+
+	if (a->tuner)
+		return -EINVAL;
+	if (a->wrap_around && dev->radio_rx_hw_seek_mode == VIVID_HW_SEEK_BOUNDED)
+		return -EINVAL;
+
+	if (!a->wrap_around && dev->radio_rx_hw_seek_mode == VIVID_HW_SEEK_WRAP)
+		return -EINVAL;
+	if (!a->rangelow ^ !a->rangehigh)
+		return -EINVAL;
+
+	if (file->f_flags & O_NONBLOCK)
+		return -EWOULDBLOCK;
+
+	if (a->rangelow) {
+		for (band = 0; band < TOT_BANDS; band++)
+			if (a->rangelow >= vivid_radio_bands[band].rangelow &&
+			    a->rangehigh <= vivid_radio_bands[band].rangehigh)
+				break;
+		if (band == TOT_BANDS)
+			return -EINVAL;
+		if (!dev->radio_rx_hw_seek_prog_lim &&
+		    (a->rangelow != vivid_radio_bands[band].rangelow ||
+		     a->rangehigh != vivid_radio_bands[band].rangehigh))
+			return -EINVAL;
+		low = a->rangelow;
+		high = a->rangehigh;
+	} else {
+		for (band = 0; band < TOT_BANDS; band++)
+			if (dev->radio_rx_freq >= vivid_radio_bands[band].rangelow &&
+			    dev->radio_rx_freq <= vivid_radio_bands[band].rangehigh)
+				break;
+		if (band == TOT_BANDS)
+			return -EINVAL;
+		low = vivid_radio_bands[band].rangelow;
+		high = vivid_radio_bands[band].rangehigh;
+	}
+	spacing = band == BAND_AM ? 1600 : 16000;
+	freq = clamp(dev->radio_rx_freq, low, high);
+
+	if (a->seek_upward) {
+		freq = spacing * (freq / spacing) + spacing;
+		if (freq > high) {
+			if (!a->wrap_around)
+				return -ENODATA;
+			freq = spacing * (low / spacing) + spacing;
+			if (freq >= dev->radio_rx_freq)
+				return -ENODATA;
+		}
+	} else {
+		freq = spacing * ((freq + spacing - 1) / spacing) - spacing;
+		if (freq < low) {
+			if (!a->wrap_around)
+				return -ENODATA;
+			freq = spacing * ((high + spacing - 1) / spacing) - spacing;
+			if (freq <= dev->radio_rx_freq)
+				return -ENODATA;
+		}
+	}
+	return 0;
+}
+
+int vivid_radio_rx_g_tuner(struct file *file, void *fh, struct v4l2_tuner *vt)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	int delta = 800;
+	int sig_qual;
+
+	if (vt->index > 0)
+		return -EINVAL;
+
+	strlcpy(vt->name, "AM/FM/SW Receiver", sizeof(vt->name));
+	vt->capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_STEREO |
+			 V4L2_TUNER_CAP_FREQ_BANDS | V4L2_TUNER_CAP_RDS |
+			 (dev->radio_rx_rds_controls ?
+				V4L2_TUNER_CAP_RDS_CONTROLS :
+				V4L2_TUNER_CAP_RDS_BLOCK_IO) |
+			 (dev->radio_rx_hw_seek_prog_lim ?
+				V4L2_TUNER_CAP_HWSEEK_PROG_LIM : 0);
+	switch (dev->radio_rx_hw_seek_mode) {
+	case VIVID_HW_SEEK_BOUNDED:
+		vt->capability |= V4L2_TUNER_CAP_HWSEEK_BOUNDED;
+		break;
+	case VIVID_HW_SEEK_WRAP:
+		vt->capability |= V4L2_TUNER_CAP_HWSEEK_WRAP;
+		break;
+	case VIVID_HW_SEEK_BOTH:
+		vt->capability |= V4L2_TUNER_CAP_HWSEEK_WRAP |
+				  V4L2_TUNER_CAP_HWSEEK_BOUNDED;
+		break;
+	}
+	vt->rangelow = AM_FREQ_RANGE_LOW;
+	vt->rangehigh = FM_FREQ_RANGE_HIGH;
+	sig_qual = dev->radio_rx_sig_qual;
+	vt->signal = abs(sig_qual) > delta ? 0 :
+		     0xffff - (abs(sig_qual) * 0xffff) / delta;
+	vt->afc = sig_qual > delta ? 0 : sig_qual;
+	if (abs(sig_qual) > delta)
+		vt->rxsubchans = 0;
+	else if (dev->radio_rx_freq < FM_FREQ_RANGE_LOW || vt->signal < 0x8000)
+		vt->rxsubchans = V4L2_TUNER_SUB_MONO;
+	else if (dev->radio_rds_loop && !(dev->radio_tx_subchans & V4L2_TUNER_SUB_STEREO))
+		vt->rxsubchans = V4L2_TUNER_SUB_MONO;
+	else
+		vt->rxsubchans = V4L2_TUNER_SUB_STEREO;
+	if (dev->radio_rx_rds_enabled &&
+	    (!dev->radio_rds_loop || (dev->radio_tx_subchans & V4L2_TUNER_SUB_RDS)) &&
+	    dev->radio_rx_freq >= FM_FREQ_RANGE_LOW && vt->signal >= 0xc000)
+		vt->rxsubchans |= V4L2_TUNER_SUB_RDS;
+	if (dev->radio_rx_rds_controls)
+		vivid_radio_rds_init(dev);
+	vt->audmode = dev->radio_rx_audmode;
+	return 0;
+}
+
+int vivid_radio_rx_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *vt)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (vt->index)
+		return -EINVAL;
+	dev->radio_rx_audmode = vt->audmode >= V4L2_TUNER_MODE_STEREO;
+	return 0;
+}
diff --git a/drivers/media/platform/vivid/vivid-radio-rx.h b/drivers/media/platform/vivid/vivid-radio-rx.h
new file mode 100644
index 0000000..1077d8f
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-radio-rx.h
@@ -0,0 +1,31 @@
+/*
+ * vivid-radio-rx.h - radio receiver support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VIVID_RADIO_RX_H_
+#define _VIVID_RADIO_RX_H_
+
+ssize_t vivid_radio_rx_read(struct file *, char __user *, size_t, loff_t *);
+unsigned int vivid_radio_rx_poll(struct file *file, struct poll_table_struct *wait);
+
+int vivid_radio_rx_enum_freq_bands(struct file *file, void *fh, struct v4l2_frequency_band *band);
+int vivid_radio_rx_s_hw_freq_seek(struct file *file, void *fh, const struct v4l2_hw_freq_seek *a);
+int vivid_radio_rx_g_tuner(struct file *file, void *fh, struct v4l2_tuner *vt);
+int vivid_radio_rx_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *vt);
+
+#endif
diff --git a/drivers/media/platform/vivid/vivid-radio-tx.c b/drivers/media/platform/vivid/vivid-radio-tx.c
new file mode 100644
index 0000000..8c59d4f
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-radio-tx.c
@@ -0,0 +1,141 @@
+/*
+ * vivid-radio-tx.c - radio transmitter support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/videodev2.h>
+#include <linux/v4l2-dv-timings.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-dv-timings.h>
+
+#include "vivid-core.h"
+#include "vivid-ctrls.h"
+#include "vivid-radio-common.h"
+#include "vivid-radio-tx.h"
+
+ssize_t vivid_radio_tx_write(struct file *file, const char __user *buf,
+			  size_t size, loff_t *offset)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct v4l2_rds_data *data = dev->rds_gen.data;
+	struct timespec ts;
+	unsigned blk;
+	int i;
+
+	if (dev->radio_tx_rds_controls)
+		return -EINVAL;
+
+	if (size < sizeof(*data))
+		return -EINVAL;
+	size = sizeof(*data) * (size / sizeof(*data));
+
+	if (mutex_lock_interruptible(&dev->mutex))
+		return -ERESTARTSYS;
+	if (dev->radio_tx_rds_owner &&
+	    file->private_data != dev->radio_tx_rds_owner) {
+		mutex_unlock(&dev->mutex);
+		return -EBUSY;
+	}
+	dev->radio_tx_rds_owner = file->private_data;
+
+retry:
+	ktime_get_ts(&ts);
+	ts = timespec_sub(ts, dev->radio_rds_init_ts);
+	blk = ts.tv_sec * 100 + ts.tv_nsec / 10000000;
+	blk = (blk * VIVID_RDS_GEN_BLOCKS) / 500;
+	if (blk - VIVID_RDS_GEN_BLOCKS >= dev->radio_tx_rds_last_block)
+		dev->radio_tx_rds_last_block = blk - VIVID_RDS_GEN_BLOCKS + 1;
+
+	/*
+	 * No data is available if there hasn't been time to get new data,
+	 * or if the RDS receiver has been disabled, or if we use the data
+	 * from the RDS transmitter and that RDS transmitter has been disabled,
+	 * or if the signal quality is too weak.
+	 */
+	if (blk == dev->radio_tx_rds_last_block ||
+	    !(dev->radio_tx_subchans & V4L2_TUNER_SUB_RDS)) {
+		mutex_unlock(&dev->mutex);
+		if (file->f_flags & O_NONBLOCK)
+			return -EWOULDBLOCK;
+		if (msleep_interruptible(20) && signal_pending(current))
+			return -EINTR;
+		if (mutex_lock_interruptible(&dev->mutex))
+			return -ERESTARTSYS;
+		goto retry;
+	}
+
+	for (i = 0; i < size && blk > dev->radio_tx_rds_last_block;
+			dev->radio_tx_rds_last_block++) {
+		unsigned data_blk = dev->radio_tx_rds_last_block % VIVID_RDS_GEN_BLOCKS;
+		struct v4l2_rds_data rds;
+
+		if (copy_from_user(&rds, buf + i, sizeof(rds))) {
+			i = -EFAULT;
+			break;
+		}
+		i += sizeof(rds);
+		if (!dev->radio_rds_loop)
+			continue;
+		if ((rds.block & V4L2_RDS_BLOCK_MSK) == V4L2_RDS_BLOCK_INVALID ||
+		    (rds.block & V4L2_RDS_BLOCK_ERROR))
+			continue;
+		rds.block &= V4L2_RDS_BLOCK_MSK;
+		data[data_blk] = rds;
+	}
+	mutex_unlock(&dev->mutex);
+	return i;
+}
+
+unsigned int vivid_radio_tx_poll(struct file *file, struct poll_table_struct *wait)
+{
+	return POLLOUT | POLLWRNORM | v4l2_ctrl_poll(file, wait);
+}
+
+int vidioc_g_modulator(struct file *file, void *fh, struct v4l2_modulator *a)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (a->index > 0)
+		return -EINVAL;
+
+	strlcpy(a->name, "AM/FM/SW Transmitter", sizeof(a->name));
+	a->capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_STEREO |
+			V4L2_TUNER_CAP_FREQ_BANDS | V4L2_TUNER_CAP_RDS |
+			(dev->radio_tx_rds_controls ?
+				V4L2_TUNER_CAP_RDS_CONTROLS :
+				V4L2_TUNER_CAP_RDS_BLOCK_IO);
+	a->rangelow = AM_FREQ_RANGE_LOW;
+	a->rangehigh = FM_FREQ_RANGE_HIGH;
+	a->txsubchans = dev->radio_tx_subchans;
+	return 0;
+}
+
+int vidioc_s_modulator(struct file *file, void *fh, const struct v4l2_modulator *a)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (a->index)
+		return -EINVAL;
+	if (a->txsubchans & ~0x13)
+		return -EINVAL;
+	dev->radio_tx_subchans = a->txsubchans;
+	return 0;
+}
diff --git a/drivers/media/platform/vivid/vivid-radio-tx.h b/drivers/media/platform/vivid/vivid-radio-tx.h
new file mode 100644
index 0000000..7f8ff75
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-radio-tx.h
@@ -0,0 +1,29 @@
+/*
+ * vivid-radio-tx.h - radio transmitter support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VIVID_RADIO_TX_H_
+#define _VIVID_RADIO_TX_H_
+
+ssize_t vivid_radio_tx_write(struct file *, const char __user *, size_t, loff_t *);
+unsigned int vivid_radio_tx_poll(struct file *file, struct poll_table_struct *wait);
+
+int vidioc_g_modulator(struct file *file, void *fh, struct v4l2_modulator *a);
+int vidioc_s_modulator(struct file *file, void *fh, const struct v4l2_modulator *a);
+
+#endif
diff --git a/drivers/media/platform/vivid/vivid-rds-gen.c b/drivers/media/platform/vivid/vivid-rds-gen.c
new file mode 100644
index 0000000..c382343
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-rds-gen.c
@@ -0,0 +1,166 @@
+/*
+ * vivid-rds-gen.c - rds (radio data system) generator support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/kernel.h>
+#include <linux/ktime.h>
+#include <linux/string.h>
+#include <linux/videodev2.h>
+
+#include "vivid-rds-gen.h"
+
+static u8 vivid_get_di(const struct vivid_rds_gen *rds, unsigned grp)
+{
+	switch (grp) {
+	case 0:
+		return (rds->dyn_pty << 2) | (grp & 3);
+	case 1:
+		return (rds->compressed << 2) | (grp & 3);
+	case 2:
+		return (rds->art_head << 2) | (grp & 3);
+	case 3:
+		return (rds->mono_stereo << 2) | (grp & 3);
+	}
+	return 0;
+}
+
+/*
+ * This RDS generator creates 57 RDS groups (one group == four RDS blocks).
+ * Groups 0-3, 22-25 and 44-47 (spaced 22 groups apart) are filled with a
+ * standard 0B group containing the PI code and PS name.
+ *
+ * Groups 4-19 and 26-41 use group 2A for the radio text.
+ *
+ * Group 56 contains the time (group 4A).
+ *
+ * All remaining groups use a filler group 15B block that just repeats
+ * the PI and PTY codes.
+ */
+void vivid_rds_generate(struct vivid_rds_gen *rds)
+{
+	struct v4l2_rds_data *data = rds->data;
+	unsigned grp;
+	struct tm tm;
+	unsigned date;
+	unsigned time;
+	int l;
+
+	for (grp = 0; grp < VIVID_RDS_GEN_GROUPS; grp++, data += VIVID_RDS_GEN_BLKS_PER_GRP) {
+		data[0].lsb = rds->picode & 0xff;
+		data[0].msb = rds->picode >> 8;
+		data[0].block = V4L2_RDS_BLOCK_A | (V4L2_RDS_BLOCK_A << 3);
+		data[1].lsb = rds->pty << 5;
+		data[1].msb = (rds->pty >> 3) | (rds->tp << 2);
+		data[1].block = V4L2_RDS_BLOCK_B | (V4L2_RDS_BLOCK_B << 3);
+		data[3].block = V4L2_RDS_BLOCK_D | (V4L2_RDS_BLOCK_D << 3);
+
+		switch (grp) {
+		case 0 ... 3:
+		case 22 ... 25:
+		case 44 ... 47: /* Group 0B */
+			data[1].lsb |= (rds->ta << 4) | (rds->ms << 3);
+			data[1].lsb |= vivid_get_di(rds, grp % 22);
+			data[1].msb |= 1 << 3;
+			data[2].lsb = rds->picode & 0xff;
+			data[2].msb = rds->picode >> 8;
+			data[2].block = V4L2_RDS_BLOCK_C_ALT | (V4L2_RDS_BLOCK_C_ALT << 3);
+			data[3].lsb = rds->psname[2 * (grp % 22) + 1];
+			data[3].msb = rds->psname[2 * (grp % 22)];
+			break;
+		case 4 ... 19:
+		case 26 ... 41: /* Group 2A */
+			data[1].lsb |= (grp - 4) % 22;
+			data[1].msb |= 4 << 3;
+			data[2].msb = rds->radiotext[4 * ((grp - 4) % 22)];
+			data[2].lsb = rds->radiotext[4 * ((grp - 4) % 22) + 1];
+			data[2].block = V4L2_RDS_BLOCK_C | (V4L2_RDS_BLOCK_C << 3);
+			data[3].msb = rds->radiotext[4 * ((grp - 4) % 22) + 2];
+			data[3].lsb = rds->radiotext[4 * ((grp - 4) % 22) + 3];
+			break;
+		case 56:
+			/*
+			 * Group 4A
+			 *
+			 * Uses the algorithm from Annex G of the RDS standard
+			 * EN 50067:1998 to convert a UTC date to an RDS Modified
+			 * Julian Day.
+			 */
+			time_to_tm(get_seconds(), 0, &tm);
+			l = tm.tm_mon <= 1;
+			date = 14956 + tm.tm_mday + ((tm.tm_year - l) * 1461) / 4 +
+				((tm.tm_mon + 2 + l * 12) * 306001) / 10000;
+			time = (tm.tm_hour << 12) |
+			       (tm.tm_min << 6) |
+			       (sys_tz.tz_minuteswest >= 0 ? 0x20 : 0) |
+			       (abs(sys_tz.tz_minuteswest) / 30);
+			data[1].lsb &= ~3;
+			data[1].lsb |= date >> 15;
+			data[1].msb |= 8 << 3;
+			data[2].lsb = (date << 1) & 0xfe;
+			data[2].lsb |= (time >> 16) & 1;
+			data[2].msb = (date >> 7) & 0xff;
+			data[2].block = V4L2_RDS_BLOCK_C | (V4L2_RDS_BLOCK_C << 3);
+			data[3].lsb = time & 0xff;
+			data[3].msb = (time >> 8) & 0xff;
+			break;
+		default: /* Group 15B */
+			data[1].lsb |= (rds->ta << 4) | (rds->ms << 3);
+			data[1].lsb |= vivid_get_di(rds, grp % 22);
+			data[1].msb |= 0x1f << 3;
+			data[2].lsb = rds->picode & 0xff;
+			data[2].msb = rds->picode >> 8;
+			data[2].block = V4L2_RDS_BLOCK_C_ALT | (V4L2_RDS_BLOCK_C_ALT << 3);
+			data[3].lsb = rds->pty << 5;
+			data[3].lsb |= (rds->ta << 4) | (rds->ms << 3);
+			data[3].lsb |= vivid_get_di(rds, grp % 22);
+			data[3].msb |= rds->pty >> 3;
+			data[3].msb |= 0x1f << 3;
+			break;
+		}
+	}
+}
+
+void vivid_rds_gen_fill(struct vivid_rds_gen *rds, unsigned freq,
+			  bool alt)
+{
+	/* Alternate PTY between Info and Weather */
+	if (rds->use_rbds) {
+		rds->picode = 0x2e75; /* 'KLNX' call sign */
+		rds->pty = alt ? 29 : 2;
+	} else {
+		rds->picode = 0x8088;
+		rds->pty = alt ? 16 : 3;
+	}
+	rds->mono_stereo = true;
+	rds->art_head = false;
+	rds->compressed = false;
+	rds->dyn_pty = false;
+	rds->tp = true;
+	rds->ta = alt;
+	rds->ms = true;
+	snprintf(rds->psname, sizeof(rds->psname), "%6d.%1d",
+		 freq / 16, ((freq & 0xf) * 10) / 16);
+	if (alt)
+		strlcpy(rds->radiotext,
+			" The Radio Data System can switch between different Radio Texts ",
+			sizeof(rds->radiotext));
+	else
+		strlcpy(rds->radiotext,
+			"An example of Radio Text as transmitted by the Radio Data System",
+			sizeof(rds->radiotext));
+}
diff --git a/drivers/media/platform/vivid/vivid-rds-gen.h b/drivers/media/platform/vivid/vivid-rds-gen.h
new file mode 100644
index 0000000..eff4bf5
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-rds-gen.h
@@ -0,0 +1,53 @@
+/*
+ * vivid-rds-gen.h - rds (radio data system) generator support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VIVID_RDS_GEN_H_
+#define _VIVID_RDS_GEN_H_
+
+/*
+ * It takes almost exactly 5 seconds to transmit 57 RDS groups.
+ * Each group has 4 blocks and each block has a payload of 16 bits + a
+ * block identification. The driver will generate the contents of these
+ * 57 groups only when necessary and it will just be played continuously.
+ */
+#define VIVID_RDS_GEN_GROUPS 57
+#define VIVID_RDS_GEN_BLKS_PER_GRP 4
+#define VIVID_RDS_GEN_BLOCKS (VIVID_RDS_GEN_BLKS_PER_GRP * VIVID_RDS_GEN_GROUPS)
+
+struct vivid_rds_gen {
+	struct v4l2_rds_data	data[VIVID_RDS_GEN_BLOCKS];
+	bool			use_rbds;
+	u16			picode;
+	u8			pty;
+	bool			mono_stereo;
+	bool			art_head;
+	bool			compressed;
+	bool			dyn_pty;
+	bool			ta;
+	bool			tp;
+	bool			ms;
+	char			psname[8 + 1];
+	char			radiotext[64 + 1];
+};
+
+void vivid_rds_gen_fill(struct vivid_rds_gen *rds, unsigned freq,
+		    bool use_alternate);
+void vivid_rds_generate(struct vivid_rds_gen *rds);
+
+#endif
diff --git a/drivers/media/platform/vivid/vivid-sdr-cap.c b/drivers/media/platform/vivid/vivid-sdr-cap.c
new file mode 100644
index 0000000..082c401
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-sdr-cap.c
@@ -0,0 +1,568 @@
+/*
+ * vivid-sdr-cap.c - software defined radio support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/kthread.h>
+#include <linux/freezer.h>
+#include <linux/math64.h>
+#include <linux/videodev2.h>
+#include <linux/v4l2-dv-timings.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-dv-timings.h>
+#include <linux/fixp-arith.h>
+
+#include "vivid-core.h"
+#include "vivid-ctrls.h"
+#include "vivid-sdr-cap.h"
+
+/* stream formats */
+struct vivid_format {
+	u32	pixelformat;
+	u32	buffersize;
+};
+
+/* format descriptions for capture and preview */
+static const struct vivid_format formats[] = {
+	{
+		.pixelformat	= V4L2_SDR_FMT_CU8,
+		.buffersize	= SDR_CAP_SAMPLES_PER_BUF * 2,
+	}, {
+		.pixelformat	= V4L2_SDR_FMT_CS8,
+		.buffersize	= SDR_CAP_SAMPLES_PER_BUF * 2,
+	},
+};
+
+static const unsigned int NUM_FORMATS = ARRAY_SIZE(formats);
+
+static const struct v4l2_frequency_band bands_adc[] = {
+	{
+		.tuner = 0,
+		.type = V4L2_TUNER_ADC,
+		.index = 0,
+		.capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS,
+		.rangelow   =  300000,
+		.rangehigh  =  300000,
+	},
+	{
+		.tuner = 0,
+		.type = V4L2_TUNER_ADC,
+		.index = 1,
+		.capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS,
+		.rangelow   =  900001,
+		.rangehigh  = 2800000,
+	},
+	{
+		.tuner = 0,
+		.type = V4L2_TUNER_ADC,
+		.index = 2,
+		.capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS,
+		.rangelow   = 3200000,
+		.rangehigh  = 3200000,
+	},
+};
+
+/* ADC band midpoints */
+#define BAND_ADC_0 ((bands_adc[0].rangehigh + bands_adc[1].rangelow) / 2)
+#define BAND_ADC_1 ((bands_adc[1].rangehigh + bands_adc[2].rangelow) / 2)
+
+static const struct v4l2_frequency_band bands_fm[] = {
+	{
+		.tuner = 1,
+		.type = V4L2_TUNER_RF,
+		.index = 0,
+		.capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS,
+		.rangelow   =    50000000,
+		.rangehigh  =  2000000000,
+	},
+};
+
+static void vivid_thread_sdr_cap_tick(struct vivid_dev *dev)
+{
+	struct vivid_buffer *sdr_cap_buf = NULL;
+
+	dprintk(dev, 1, "SDR Capture Thread Tick\n");
+
+	/* Drop a certain percentage of buffers. */
+	if (dev->perc_dropped_buffers &&
+	    prandom_u32_max(100) < dev->perc_dropped_buffers)
+		return;
+
+	spin_lock(&dev->slock);
+	if (!list_empty(&dev->sdr_cap_active)) {
+		sdr_cap_buf = list_entry(dev->sdr_cap_active.next,
+					 struct vivid_buffer, list);
+		list_del(&sdr_cap_buf->list);
+	}
+	spin_unlock(&dev->slock);
+
+	if (sdr_cap_buf) {
+		sdr_cap_buf->vb.sequence = dev->sdr_cap_seq_count;
+		vivid_sdr_cap_process(dev, sdr_cap_buf);
+		v4l2_get_timestamp(&sdr_cap_buf->vb.timestamp);
+		sdr_cap_buf->vb.timestamp.tv_sec += dev->time_wrap_offset;
+		vb2_buffer_done(&sdr_cap_buf->vb.vb2_buf, dev->dqbuf_error ?
+				VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+		dev->dqbuf_error = false;
+	}
+}
+
+static int vivid_thread_sdr_cap(void *data)
+{
+	struct vivid_dev *dev = data;
+	u64 samples_since_start;
+	u64 buffers_since_start;
+	u64 next_jiffies_since_start;
+	unsigned long jiffies_since_start;
+	unsigned long cur_jiffies;
+	unsigned wait_jiffies;
+
+	dprintk(dev, 1, "SDR Capture Thread Start\n");
+
+	set_freezable();
+
+	/* Resets frame counters */
+	dev->sdr_cap_seq_offset = 0;
+	if (dev->seq_wrap)
+		dev->sdr_cap_seq_offset = 0xffffff80U;
+	dev->jiffies_sdr_cap = jiffies;
+	dev->sdr_cap_seq_resync = false;
+
+	for (;;) {
+		try_to_freeze();
+		if (kthread_should_stop())
+			break;
+
+		mutex_lock(&dev->mutex);
+		cur_jiffies = jiffies;
+		if (dev->sdr_cap_seq_resync) {
+			dev->jiffies_sdr_cap = cur_jiffies;
+			dev->sdr_cap_seq_offset = dev->sdr_cap_seq_count + 1;
+			dev->sdr_cap_seq_count = 0;
+			dev->sdr_cap_seq_resync = false;
+		}
+		/* Calculate the number of jiffies since we started streaming */
+		jiffies_since_start = cur_jiffies - dev->jiffies_sdr_cap;
+		/* Get the number of buffers streamed since the start */
+		buffers_since_start =
+			(u64)jiffies_since_start * dev->sdr_adc_freq +
+				      (HZ * SDR_CAP_SAMPLES_PER_BUF) / 2;
+		do_div(buffers_since_start, HZ * SDR_CAP_SAMPLES_PER_BUF);
+
+		/*
+		 * After more than 0xf0000000 (rounded down to a multiple of
+		 * 'jiffies-per-day' to ease jiffies_to_msecs calculation)
+		 * jiffies have passed since we started streaming reset the
+		 * counters and keep track of the sequence offset.
+		 */
+		if (jiffies_since_start > JIFFIES_RESYNC) {
+			dev->jiffies_sdr_cap = cur_jiffies;
+			dev->sdr_cap_seq_offset = buffers_since_start;
+			buffers_since_start = 0;
+		}
+		dev->sdr_cap_seq_count =
+			buffers_since_start + dev->sdr_cap_seq_offset;
+
+		vivid_thread_sdr_cap_tick(dev);
+		mutex_unlock(&dev->mutex);
+
+		/*
+		 * Calculate the number of samples streamed since we started,
+		 * not including the current buffer.
+		 */
+		samples_since_start = buffers_since_start * SDR_CAP_SAMPLES_PER_BUF;
+
+		/* And the number of jiffies since we started */
+		jiffies_since_start = jiffies - dev->jiffies_sdr_cap;
+
+		/* Increase by the number of samples in one buffer */
+		samples_since_start += SDR_CAP_SAMPLES_PER_BUF;
+		/*
+		 * Calculate when that next buffer is supposed to start
+		 * in jiffies since we started streaming.
+		 */
+		next_jiffies_since_start = samples_since_start * HZ +
+					   dev->sdr_adc_freq / 2;
+		do_div(next_jiffies_since_start, dev->sdr_adc_freq);
+		/* If it is in the past, then just schedule asap */
+		if (next_jiffies_since_start < jiffies_since_start)
+			next_jiffies_since_start = jiffies_since_start;
+
+		wait_jiffies = next_jiffies_since_start - jiffies_since_start;
+		schedule_timeout_interruptible(wait_jiffies ? wait_jiffies : 1);
+	}
+	dprintk(dev, 1, "SDR Capture Thread End\n");
+	return 0;
+}
+
+static int sdr_cap_queue_setup(struct vb2_queue *vq, const void *parg,
+		       unsigned *nbuffers, unsigned *nplanes,
+		       unsigned sizes[], void *alloc_ctxs[])
+{
+	/* 2 = max 16-bit sample returned */
+	sizes[0] = SDR_CAP_SAMPLES_PER_BUF * 2;
+	*nplanes = 1;
+	return 0;
+}
+
+static int sdr_cap_buf_prepare(struct vb2_buffer *vb)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+	unsigned size = SDR_CAP_SAMPLES_PER_BUF * 2;
+
+	dprintk(dev, 1, "%s\n", __func__);
+
+	if (dev->buf_prepare_error) {
+		/*
+		 * Error injection: test what happens if buf_prepare() returns
+		 * an error.
+		 */
+		dev->buf_prepare_error = false;
+		return -EINVAL;
+	}
+	if (vb2_plane_size(vb, 0) < size) {
+		dprintk(dev, 1, "%s data will not fit into plane (%lu < %u)\n",
+				__func__, vb2_plane_size(vb, 0), size);
+		return -EINVAL;
+	}
+	vb2_set_plane_payload(vb, 0, size);
+
+	return 0;
+}
+
+static void sdr_cap_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+	struct vivid_buffer *buf = container_of(vbuf, struct vivid_buffer, vb);
+
+	dprintk(dev, 1, "%s\n", __func__);
+
+	spin_lock(&dev->slock);
+	list_add_tail(&buf->list, &dev->sdr_cap_active);
+	spin_unlock(&dev->slock);
+}
+
+static int sdr_cap_start_streaming(struct vb2_queue *vq, unsigned count)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vq);
+	int err = 0;
+
+	dprintk(dev, 1, "%s\n", __func__);
+	dev->sdr_cap_seq_count = 0;
+	if (dev->start_streaming_error) {
+		dev->start_streaming_error = false;
+		err = -EINVAL;
+	} else if (dev->kthread_sdr_cap == NULL) {
+		dev->kthread_sdr_cap = kthread_run(vivid_thread_sdr_cap, dev,
+				"%s-sdr-cap", dev->v4l2_dev.name);
+
+		if (IS_ERR(dev->kthread_sdr_cap)) {
+			v4l2_err(&dev->v4l2_dev, "kernel_thread() failed\n");
+			err = PTR_ERR(dev->kthread_sdr_cap);
+			dev->kthread_sdr_cap = NULL;
+		}
+	}
+	if (err) {
+		struct vivid_buffer *buf, *tmp;
+
+		list_for_each_entry_safe(buf, tmp, &dev->sdr_cap_active, list) {
+			list_del(&buf->list);
+			vb2_buffer_done(&buf->vb.vb2_buf,
+					VB2_BUF_STATE_QUEUED);
+		}
+	}
+	return err;
+}
+
+/* abort streaming and wait for last buffer */
+static void sdr_cap_stop_streaming(struct vb2_queue *vq)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vq);
+
+	if (dev->kthread_sdr_cap == NULL)
+		return;
+
+	while (!list_empty(&dev->sdr_cap_active)) {
+		struct vivid_buffer *buf;
+
+		buf = list_entry(dev->sdr_cap_active.next,
+				struct vivid_buffer, list);
+		list_del(&buf->list);
+		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+	}
+
+	/* shutdown control thread */
+	mutex_unlock(&dev->mutex);
+	kthread_stop(dev->kthread_sdr_cap);
+	dev->kthread_sdr_cap = NULL;
+	mutex_lock(&dev->mutex);
+}
+
+const struct vb2_ops vivid_sdr_cap_qops = {
+	.queue_setup		= sdr_cap_queue_setup,
+	.buf_prepare		= sdr_cap_buf_prepare,
+	.buf_queue		= sdr_cap_buf_queue,
+	.start_streaming	= sdr_cap_start_streaming,
+	.stop_streaming		= sdr_cap_stop_streaming,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
+};
+
+int vivid_sdr_enum_freq_bands(struct file *file, void *fh,
+		struct v4l2_frequency_band *band)
+{
+	switch (band->tuner) {
+	case 0:
+		if (band->index >= ARRAY_SIZE(bands_adc))
+			return -EINVAL;
+		*band = bands_adc[band->index];
+		return 0;
+	case 1:
+		if (band->index >= ARRAY_SIZE(bands_fm))
+			return -EINVAL;
+		*band = bands_fm[band->index];
+		return 0;
+	default:
+		return -EINVAL;
+	}
+}
+
+int vivid_sdr_g_frequency(struct file *file, void *fh,
+		struct v4l2_frequency *vf)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	switch (vf->tuner) {
+	case 0:
+		vf->frequency = dev->sdr_adc_freq;
+		vf->type = V4L2_TUNER_ADC;
+		return 0;
+	case 1:
+		vf->frequency = dev->sdr_fm_freq;
+		vf->type = V4L2_TUNER_RF;
+		return 0;
+	default:
+		return -EINVAL;
+	}
+}
+
+int vivid_sdr_s_frequency(struct file *file, void *fh,
+		const struct v4l2_frequency *vf)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	unsigned freq = vf->frequency;
+	unsigned band;
+
+	switch (vf->tuner) {
+	case 0:
+		if (vf->type != V4L2_TUNER_ADC)
+			return -EINVAL;
+		if (freq < BAND_ADC_0)
+			band = 0;
+		else if (freq < BAND_ADC_1)
+			band = 1;
+		else
+			band = 2;
+
+		freq = clamp_t(unsigned, freq,
+				bands_adc[band].rangelow,
+				bands_adc[band].rangehigh);
+
+		if (vb2_is_streaming(&dev->vb_sdr_cap_q) &&
+		    freq != dev->sdr_adc_freq) {
+			/* resync the thread's timings */
+			dev->sdr_cap_seq_resync = true;
+		}
+		dev->sdr_adc_freq = freq;
+		return 0;
+	case 1:
+		if (vf->type != V4L2_TUNER_RF)
+			return -EINVAL;
+		dev->sdr_fm_freq = clamp_t(unsigned, freq,
+				bands_fm[0].rangelow,
+				bands_fm[0].rangehigh);
+		return 0;
+	default:
+		return -EINVAL;
+	}
+}
+
+int vivid_sdr_g_tuner(struct file *file, void *fh, struct v4l2_tuner *vt)
+{
+	switch (vt->index) {
+	case 0:
+		strlcpy(vt->name, "ADC", sizeof(vt->name));
+		vt->type = V4L2_TUNER_ADC;
+		vt->capability =
+			V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS;
+		vt->rangelow = bands_adc[0].rangelow;
+		vt->rangehigh = bands_adc[2].rangehigh;
+		return 0;
+	case 1:
+		strlcpy(vt->name, "RF", sizeof(vt->name));
+		vt->type = V4L2_TUNER_RF;
+		vt->capability =
+			V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS;
+		vt->rangelow = bands_fm[0].rangelow;
+		vt->rangehigh = bands_fm[0].rangehigh;
+		return 0;
+	default:
+		return -EINVAL;
+	}
+}
+
+int vivid_sdr_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *vt)
+{
+	if (vt->index > 1)
+		return -EINVAL;
+	return 0;
+}
+
+int vidioc_enum_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_fmtdesc *f)
+{
+	if (f->index >= ARRAY_SIZE(formats))
+		return -EINVAL;
+	f->pixelformat = formats[f->index].pixelformat;
+	return 0;
+}
+
+int vidioc_g_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	f->fmt.sdr.pixelformat = dev->sdr_pixelformat;
+	f->fmt.sdr.buffersize = dev->sdr_buffersize;
+	memset(f->fmt.sdr.reserved, 0, sizeof(f->fmt.sdr.reserved));
+	return 0;
+}
+
+int vidioc_s_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct vb2_queue *q = &dev->vb_sdr_cap_q;
+	int i;
+
+	if (vb2_is_busy(q))
+		return -EBUSY;
+
+	memset(f->fmt.sdr.reserved, 0, sizeof(f->fmt.sdr.reserved));
+	for (i = 0; i < ARRAY_SIZE(formats); i++) {
+		if (formats[i].pixelformat == f->fmt.sdr.pixelformat) {
+			dev->sdr_pixelformat = formats[i].pixelformat;
+			dev->sdr_buffersize = formats[i].buffersize;
+			f->fmt.sdr.buffersize = formats[i].buffersize;
+			return 0;
+		}
+	}
+	dev->sdr_pixelformat = formats[0].pixelformat;
+	dev->sdr_buffersize = formats[0].buffersize;
+	f->fmt.sdr.pixelformat = formats[0].pixelformat;
+	f->fmt.sdr.buffersize = formats[0].buffersize;
+	return 0;
+}
+
+int vidioc_try_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_format *f)
+{
+	int i;
+
+	memset(f->fmt.sdr.reserved, 0, sizeof(f->fmt.sdr.reserved));
+	for (i = 0; i < ARRAY_SIZE(formats); i++) {
+		if (formats[i].pixelformat == f->fmt.sdr.pixelformat) {
+			f->fmt.sdr.buffersize = formats[i].buffersize;
+			return 0;
+		}
+	}
+	f->fmt.sdr.pixelformat = formats[0].pixelformat;
+	f->fmt.sdr.buffersize = formats[0].buffersize;
+	return 0;
+}
+
+#define FIXP_N    (15)
+#define FIXP_FRAC (1 << FIXP_N)
+#define FIXP_2PI  ((int)(2 * 3.141592653589 * FIXP_FRAC))
+#define M_100000PI (3.14159 * 100000)
+
+void vivid_sdr_cap_process(struct vivid_dev *dev, struct vivid_buffer *buf)
+{
+	u8 *vbuf = vb2_plane_vaddr(&buf->vb.vb2_buf, 0);
+	unsigned long i;
+	unsigned long plane_size = vb2_plane_size(&buf->vb.vb2_buf, 0);
+	s64 s64tmp;
+	s32 src_phase_step;
+	s32 mod_phase_step;
+	s32 fixp_i;
+	s32 fixp_q;
+
+	/* calculate phase step */
+	#define BEEP_FREQ 1000 /* 1kHz beep */
+	src_phase_step = DIV_ROUND_CLOSEST(FIXP_2PI * BEEP_FREQ,
+					   dev->sdr_adc_freq);
+
+	for (i = 0; i < plane_size; i += 2) {
+		mod_phase_step = fixp_cos32_rad(dev->sdr_fixp_src_phase,
+						FIXP_2PI) >> (31 - FIXP_N);
+
+		dev->sdr_fixp_src_phase += src_phase_step;
+		s64tmp = (s64) mod_phase_step * dev->sdr_fm_deviation;
+		dev->sdr_fixp_mod_phase += div_s64(s64tmp, M_100000PI);
+
+		/*
+		 * Transfer phase angle to [0, 2xPI] in order to avoid variable
+		 * overflow and make it suitable for cosine implementation
+		 * used, which does not support negative angles.
+		 */
+		dev->sdr_fixp_src_phase %= FIXP_2PI;
+		dev->sdr_fixp_mod_phase %= FIXP_2PI;
+
+		if (dev->sdr_fixp_mod_phase < 0)
+			dev->sdr_fixp_mod_phase += FIXP_2PI;
+
+		fixp_i = fixp_cos32_rad(dev->sdr_fixp_mod_phase, FIXP_2PI);
+		fixp_q = fixp_sin32_rad(dev->sdr_fixp_mod_phase, FIXP_2PI);
+
+		/* Normalize fraction values represented with 32 bit precision
+		 * to fixed point representation with FIXP_N bits */
+		fixp_i >>= (31 - FIXP_N);
+		fixp_q >>= (31 - FIXP_N);
+
+		switch (dev->sdr_pixelformat) {
+		case V4L2_SDR_FMT_CU8:
+			/* convert 'fixp float' to u8 [0, +255] */
+			/* u8 = X * 127.5 + 127.5; X is float [-1.0, +1.0] */
+			fixp_i = fixp_i * 1275 + FIXP_FRAC * 1275;
+			fixp_q = fixp_q * 1275 + FIXP_FRAC * 1275;
+			*vbuf++ = DIV_ROUND_CLOSEST(fixp_i, FIXP_FRAC * 10);
+			*vbuf++ = DIV_ROUND_CLOSEST(fixp_q, FIXP_FRAC * 10);
+			break;
+		case V4L2_SDR_FMT_CS8:
+			/* convert 'fixp float' to s8 [-128, +127] */
+			/* s8 = X * 127.5 - 0.5; X is float [-1.0, +1.0] */
+			fixp_i = fixp_i * 1275 - FIXP_FRAC * 5;
+			fixp_q = fixp_q * 1275 - FIXP_FRAC * 5;
+			*vbuf++ = DIV_ROUND_CLOSEST(fixp_i, FIXP_FRAC * 10);
+			*vbuf++ = DIV_ROUND_CLOSEST(fixp_q, FIXP_FRAC * 10);
+			break;
+		default:
+			break;
+		}
+	}
+}
diff --git a/drivers/media/platform/vivid/vivid-sdr-cap.h b/drivers/media/platform/vivid/vivid-sdr-cap.h
new file mode 100644
index 0000000..43014b2
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-sdr-cap.h
@@ -0,0 +1,36 @@
+/*
+ * vivid-sdr-cap.h - software defined radio support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VIVID_SDR_CAP_H_
+#define _VIVID_SDR_CAP_H_
+
+int vivid_sdr_enum_freq_bands(struct file *file, void *fh, struct v4l2_frequency_band *band);
+int vivid_sdr_g_frequency(struct file *file, void *fh, struct v4l2_frequency *vf);
+int vivid_sdr_s_frequency(struct file *file, void *fh, const struct v4l2_frequency *vf);
+int vivid_sdr_g_tuner(struct file *file, void *fh, struct v4l2_tuner *vt);
+int vivid_sdr_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *vt);
+int vidioc_enum_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_fmtdesc *f);
+int vidioc_g_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_format *f);
+int vidioc_s_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_format *f);
+int vidioc_try_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_format *f);
+void vivid_sdr_cap_process(struct vivid_dev *dev, struct vivid_buffer *buf);
+
+extern const struct vb2_ops vivid_sdr_cap_qops;
+
+#endif
diff --git a/drivers/media/platform/vivid/vivid-tpg-colors.c b/drivers/media/platform/vivid/vivid-tpg-colors.c
new file mode 100644
index 0000000..2299f0c
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-tpg-colors.c
@@ -0,0 +1,1416 @@
+/*
+ * vivid-color.c - A table that converts colors to various colorspaces
+ *
+ * The test pattern generator uses the tpg_colors for its test patterns.
+ * For testing colorspaces the first 8 colors of that table need to be
+ * converted to their equivalent in the target colorspace.
+ *
+ * The tpg_csc_colors[] table is the result of that conversion and since
+ * it is precalculated the colorspace conversion is just a simple table
+ * lookup.
+ *
+ * This source also contains the code used to generate the tpg_csc_colors
+ * table. Run the following command to compile it:
+ *
+ *	gcc vivid-tpg-colors.c -DCOMPILE_APP -o gen-colors -lm
+ *
+ * and run the utility.
+ *
+ * Note that the converted colors are in the range 0x000-0xff0 (so times 16)
+ * in order to preserve precision.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/videodev2.h>
+
+#include "vivid-tpg-colors.h"
+
+/* sRGB colors with range [0-255] */
+const struct color tpg_colors[TPG_COLOR_MAX] = {
+	/*
+	 * Colors to test colorspace conversion: converting these colors
+	 * to other colorspaces will never lead to out-of-gamut colors.
+	 */
+	{ 191, 191, 191 }, /* TPG_COLOR_CSC_WHITE */
+	{ 191, 191,  50 }, /* TPG_COLOR_CSC_YELLOW */
+	{  50, 191, 191 }, /* TPG_COLOR_CSC_CYAN */
+	{  50, 191,  50 }, /* TPG_COLOR_CSC_GREEN */
+	{ 191,  50, 191 }, /* TPG_COLOR_CSC_MAGENTA */
+	{ 191,  50,  50 }, /* TPG_COLOR_CSC_RED */
+	{  50,  50, 191 }, /* TPG_COLOR_CSC_BLUE */
+	{  50,  50,  50 }, /* TPG_COLOR_CSC_BLACK */
+
+	/* 75% colors */
+	{ 191, 191,   0 }, /* TPG_COLOR_75_YELLOW */
+	{   0, 191, 191 }, /* TPG_COLOR_75_CYAN */
+	{   0, 191,   0 }, /* TPG_COLOR_75_GREEN */
+	{ 191,   0, 191 }, /* TPG_COLOR_75_MAGENTA */
+	{ 191,   0,   0 }, /* TPG_COLOR_75_RED */
+	{   0,   0, 191 }, /* TPG_COLOR_75_BLUE */
+
+	/* 100% colors */
+	{ 255, 255, 255 }, /* TPG_COLOR_100_WHITE */
+	{ 255, 255,   0 }, /* TPG_COLOR_100_YELLOW */
+	{   0, 255, 255 }, /* TPG_COLOR_100_CYAN */
+	{   0, 255,   0 }, /* TPG_COLOR_100_GREEN */
+	{ 255,   0, 255 }, /* TPG_COLOR_100_MAGENTA */
+	{ 255,   0,   0 }, /* TPG_COLOR_100_RED */
+	{   0,   0, 255 }, /* TPG_COLOR_100_BLUE */
+	{   0,   0,   0 }, /* TPG_COLOR_100_BLACK */
+
+	{   0,   0,   0 }, /* TPG_COLOR_RANDOM placeholder */
+};
+
+#ifndef COMPILE_APP
+
+/* Generated table */
+const unsigned short tpg_rec709_to_linear[255 * 16 + 1] = {
+	   0,    0,    0,    1,    1,    1,    1,    2,    2,    2,    2,    2,    3,    3,    3,    3,
+	   4,    4,    4,    4,    4,    5,    5,    5,    5,    6,    6,    6,    6,    6,    7,    7,
+	   7,    7,    8,    8,    8,    8,    8,    9,    9,    9,    9,   10,   10,   10,   10,   10,
+	  11,   11,   11,   11,   12,   12,   12,   12,   12,   13,   13,   13,   13,   14,   14,   14,
+	  14,   14,   15,   15,   15,   15,   16,   16,   16,   16,   16,   17,   17,   17,   17,   18,
+	  18,   18,   18,   18,   19,   19,   19,   19,   20,   20,   20,   20,   20,   21,   21,   21,
+	  21,   22,   22,   22,   22,   22,   23,   23,   23,   23,   24,   24,   24,   24,   24,   25,
+	  25,   25,   25,   26,   26,   26,   26,   26,   27,   27,   27,   27,   28,   28,   28,   28,
+	  28,   29,   29,   29,   29,   30,   30,   30,   30,   30,   31,   31,   31,   31,   32,   32,
+	  32,   32,   32,   33,   33,   33,   33,   34,   34,   34,   34,   34,   35,   35,   35,   35,
+	  36,   36,   36,   36,   36,   37,   37,   37,   37,   38,   38,   38,   38,   38,   39,   39,
+	  39,   39,   40,   40,   40,   40,   40,   41,   41,   41,   41,   42,   42,   42,   42,   42,
+	  43,   43,   43,   43,   44,   44,   44,   44,   44,   45,   45,   45,   45,   46,   46,   46,
+	  46,   46,   47,   47,   47,   47,   48,   48,   48,   48,   48,   49,   49,   49,   49,   50,
+	  50,   50,   50,   50,   51,   51,   51,   51,   52,   52,   52,   52,   52,   53,   53,   53,
+	  53,   54,   54,   54,   54,   54,   55,   55,   55,   55,   56,   56,   56,   56,   56,   57,
+	  57,   57,   57,   58,   58,   58,   58,   58,   59,   59,   59,   59,   60,   60,   60,   60,
+	  60,   61,   61,   61,   61,   62,   62,   62,   62,   62,   63,   63,   63,   63,   64,   64,
+	  64,   64,   64,   65,   65,   65,   65,   66,   66,   66,   66,   66,   67,   67,   67,   67,
+	  68,   68,   68,   68,   68,   69,   69,   69,   69,   70,   70,   70,   70,   70,   71,   71,
+	  71,   71,   72,   72,   72,   72,   72,   73,   73,   73,   73,   73,   74,   74,   74,   74,
+	  74,   75,   75,   75,   75,   76,   76,   76,   76,   76,   77,   77,   77,   77,   78,   78,
+	  78,   78,   79,   79,   79,   79,   79,   80,   80,   80,   80,   81,   81,   81,   81,   82,
+	  82,   82,   82,   82,   83,   83,   83,   83,   84,   84,   84,   84,   85,   85,   85,   85,
+	  86,   86,   86,   86,   87,   87,   87,   87,   88,   88,   88,   88,   89,   89,   89,   89,
+	  90,   90,   90,   90,   91,   91,   91,   91,   92,   92,   92,   92,   93,   93,   93,   93,
+	  94,   94,   94,   94,   95,   95,   95,   95,   96,   96,   96,   96,   97,   97,   97,   97,
+	  98,   98,   98,   98,   99,   99,   99,   99,  100,  100,  100,  101,  101,  101,  101,  102,
+	 102,  102,  102,  103,  103,  103,  103,  104,  104,  104,  105,  105,  105,  105,  106,  106,
+	 106,  106,  107,  107,  107,  107,  108,  108,  108,  109,  109,  109,  109,  110,  110,  110,
+	 111,  111,  111,  111,  112,  112,  112,  112,  113,  113,  113,  114,  114,  114,  114,  115,
+	 115,  115,  116,  116,  116,  116,  117,  117,  117,  118,  118,  118,  118,  119,  119,  119,
+	 120,  120,  120,  120,  121,  121,  121,  122,  122,  122,  123,  123,  123,  123,  124,  124,
+	 124,  125,  125,  125,  125,  126,  126,  126,  127,  127,  127,  128,  128,  128,  128,  129,
+	 129,  129,  130,  130,  130,  131,  131,  131,  132,  132,  132,  132,  133,  133,  133,  134,
+	 134,  134,  135,  135,  135,  136,  136,  136,  136,  137,  137,  137,  138,  138,  138,  139,
+	 139,  139,  140,  140,  140,  141,  141,  141,  142,  142,  142,  142,  143,  143,  143,  144,
+	 144,  144,  145,  145,  145,  146,  146,  146,  147,  147,  147,  148,  148,  148,  149,  149,
+	 149,  150,  150,  150,  151,  151,  151,  152,  152,  152,  153,  153,  153,  154,  154,  154,
+	 155,  155,  155,  156,  156,  156,  157,  157,  157,  158,  158,  158,  159,  159,  159,  160,
+	 160,  160,  161,  161,  161,  162,  162,  162,  163,  163,  163,  164,  164,  164,  165,  165,
+	 165,  166,  166,  167,  167,  167,  168,  168,  168,  169,  169,  169,  170,  170,  170,  171,
+	 171,  171,  172,  172,  172,  173,  173,  174,  174,  174,  175,  175,  175,  176,  176,  176,
+	 177,  177,  177,  178,  178,  179,  179,  179,  180,  180,  180,  181,  181,  181,  182,  182,
+	 183,  183,  183,  184,  184,  184,  185,  185,  186,  186,  186,  187,  187,  187,  188,  188,
+	 188,  189,  189,  190,  190,  190,  191,  191,  191,  192,  192,  193,  193,  193,  194,  194,
+	 194,  195,  195,  196,  196,  196,  197,  197,  198,  198,  198,  199,  199,  199,  200,  200,
+	 201,  201,  201,  202,  202,  203,  203,  203,  204,  204,  204,  205,  205,  206,  206,  206,
+	 207,  207,  208,  208,  208,  209,  209,  210,  210,  210,  211,  211,  212,  212,  212,  213,
+	 213,  214,  214,  214,  215,  215,  216,  216,  216,  217,  217,  218,  218,  218,  219,  219,
+	 220,  220,  220,  221,  221,  222,  222,  222,  223,  223,  224,  224,  224,  225,  225,  226,
+	 226,  227,  227,  227,  228,  228,  229,  229,  229,  230,  230,  231,  231,  232,  232,  232,
+	 233,  233,  234,  234,  234,  235,  235,  236,  236,  237,  237,  237,  238,  238,  239,  239,
+	 240,  240,  240,  241,  241,  242,  242,  243,  243,  243,  244,  244,  245,  245,  246,  246,
+	 246,  247,  247,  248,  248,  249,  249,  249,  250,  250,  251,  251,  252,  252,  252,  253,
+	 253,  254,  254,  255,  255,  256,  256,  256,  257,  257,  258,  258,  259,  259,  260,  260,
+	 260,  261,  261,  262,  262,  263,  263,  264,  264,  264,  265,  265,  266,  266,  267,  267,
+	 268,  268,  269,  269,  269,  270,  270,  271,  271,  272,  272,  273,  273,  274,  274,  274,
+	 275,  275,  276,  276,  277,  277,  278,  278,  279,  279,  279,  280,  280,  281,  281,  282,
+	 282,  283,  283,  284,  284,  285,  285,  286,  286,  286,  287,  287,  288,  288,  289,  289,
+	 290,  290,  291,  291,  292,  292,  293,  293,  294,  294,  295,  295,  295,  296,  296,  297,
+	 297,  298,  298,  299,  299,  300,  300,  301,  301,  302,  302,  303,  303,  304,  304,  305,
+	 305,  306,  306,  307,  307,  308,  308,  309,  309,  309,  310,  310,  311,  311,  312,  312,
+	 313,  313,  314,  314,  315,  315,  316,  316,  317,  317,  318,  318,  319,  319,  320,  320,
+	 321,  321,  322,  322,  323,  323,  324,  324,  325,  325,  326,  326,  327,  327,  328,  328,
+	 329,  329,  330,  330,  331,  331,  332,  332,  333,  333,  334,  335,  335,  336,  336,  337,
+	 337,  338,  338,  339,  339,  340,  340,  341,  341,  342,  342,  343,  343,  344,  344,  345,
+	 345,  346,  346,  347,  347,  348,  348,  349,  349,  350,  351,  351,  352,  352,  353,  353,
+	 354,  354,  355,  355,  356,  356,  357,  357,  358,  358,  359,  360,  360,  361,  361,  362,
+	 362,  363,  363,  364,  364,  365,  365,  366,  366,  367,  368,  368,  369,  369,  370,  370,
+	 371,  371,  372,  372,  373,  373,  374,  375,  375,  376,  376,  377,  377,  378,  378,  379,
+	 379,  380,  381,  381,  382,  382,  383,  383,  384,  384,  385,  386,  386,  387,  387,  388,
+	 388,  389,  389,  390,  391,  391,  392,  392,  393,  393,  394,  394,  395,  396,  396,  397,
+	 397,  398,  398,  399,  399,  400,  401,  401,  402,  402,  403,  403,  404,  405,  405,  406,
+	 406,  407,  407,  408,  409,  409,  410,  410,  411,  411,  412,  413,  413,  414,  414,  415,
+	 415,  416,  417,  417,  418,  418,  419,  419,  420,  421,  421,  422,  422,  423,  424,  424,
+	 425,  425,  426,  426,  427,  428,  428,  429,  429,  430,  431,  431,  432,  432,  433,  433,
+	 434,  435,  435,  436,  436,  437,  438,  438,  439,  439,  440,  441,  441,  442,  442,  443,
+	 444,  444,  445,  445,  446,  447,  447,  448,  448,  449,  450,  450,  451,  451,  452,  453,
+	 453,  454,  454,  455,  456,  456,  457,  457,  458,  459,  459,  460,  460,  461,  462,  462,
+	 463,  463,  464,  465,  465,  466,  467,  467,  468,  468,  469,  470,  470,  471,  471,  472,
+	 473,  473,  474,  475,  475,  476,  476,  477,  478,  478,  479,  480,  480,  481,  481,  482,
+	 483,  483,  484,  485,  485,  486,  486,  487,  488,  488,  489,  490,  490,  491,  491,  492,
+	 493,  493,  494,  495,  495,  496,  497,  497,  498,  498,  499,  500,  500,  501,  502,  502,
+	 503,  504,  504,  505,  505,  506,  507,  507,  508,  509,  509,  510,  511,  511,  512,  513,
+	 513,  514,  514,  515,  516,  516,  517,  518,  518,  519,  520,  520,  521,  522,  522,  523,
+	 524,  524,  525,  526,  526,  527,  528,  528,  529,  529,  530,  531,  531,  532,  533,  533,
+	 534,  535,  535,  536,  537,  537,  538,  539,  539,  540,  541,  541,  542,  543,  543,  544,
+	 545,  545,  546,  547,  547,  548,  549,  549,  550,  551,  551,  552,  553,  553,  554,  555,
+	 555,  556,  557,  557,  558,  559,  560,  560,  561,  562,  562,  563,  564,  564,  565,  566,
+	 566,  567,  568,  568,  569,  570,  570,  571,  572,  572,  573,  574,  575,  575,  576,  577,
+	 577,  578,  579,  579,  580,  581,  581,  582,  583,  584,  584,  585,  586,  586,  587,  588,
+	 588,  589,  590,  590,  591,  592,  593,  593,  594,  595,  595,  596,  597,  598,  598,  599,
+	 600,  600,  601,  602,  602,  603,  604,  605,  605,  606,  607,  607,  608,  609,  610,  610,
+	 611,  612,  612,  613,  614,  615,  615,  616,  617,  617,  618,  619,  620,  620,  621,  622,
+	 622,  623,  624,  625,  625,  626,  627,  627,  628,  629,  630,  630,  631,  632,  632,  633,
+	 634,  635,  635,  636,  637,  638,  638,  639,  640,  640,  641,  642,  643,  643,  644,  645,
+	 646,  646,  647,  648,  649,  649,  650,  651,  652,  652,  653,  654,  654,  655,  656,  657,
+	 657,  658,  659,  660,  660,  661,  662,  663,  663,  664,  665,  666,  666,  667,  668,  669,
+	 669,  670,  671,  672,  672,  673,  674,  675,  675,  676,  677,  678,  678,  679,  680,  681,
+	 681,  682,  683,  684,  684,  685,  686,  687,  687,  688,  689,  690,  690,  691,  692,  693,
+	 694,  694,  695,  696,  697,  697,  698,  699,  700,  700,  701,  702,  703,  703,  704,  705,
+	 706,  707,  707,  708,  709,  710,  710,  711,  712,  713,  714,  714,  715,  716,  717,  717,
+	 718,  719,  720,  720,  721,  722,  723,  724,  724,  725,  726,  727,  728,  728,  729,  730,
+	 731,  731,  732,  733,  734,  735,  735,  736,  737,  738,  739,  739,  740,  741,  742,  742,
+	 743,  744,  745,  746,  746,  747,  748,  749,  750,  750,  751,  752,  753,  754,  754,  755,
+	 756,  757,  758,  758,  759,  760,  761,  762,  762,  763,  764,  765,  766,  766,  767,  768,
+	 769,  770,  771,  771,  772,  773,  774,  775,  775,  776,  777,  778,  779,  779,  780,  781,
+	 782,  783,  783,  784,  785,  786,  787,  788,  788,  789,  790,  791,  792,  793,  793,  794,
+	 795,  796,  797,  797,  798,  799,  800,  801,  802,  802,  803,  804,  805,  806,  807,  807,
+	 808,  809,  810,  811,  812,  812,  813,  814,  815,  816,  817,  817,  818,  819,  820,  821,
+	 822,  822,  823,  824,  825,  826,  827,  827,  828,  829,  830,  831,  832,  832,  833,  834,
+	 835,  836,  837,  838,  838,  839,  840,  841,  842,  843,  843,  844,  845,  846,  847,  848,
+	 849,  849,  850,  851,  852,  853,  854,  855,  855,  856,  857,  858,  859,  860,  861,  861,
+	 862,  863,  864,  865,  866,  867,  867,  868,  869,  870,  871,  872,  873,  873,  874,  875,
+	 876,  877,  878,  879,  880,  880,  881,  882,  883,  884,  885,  886,  887,  887,  888,  889,
+	 890,  891,  892,  893,  894,  894,  895,  896,  897,  898,  899,  900,  901,  901,  902,  903,
+	 904,  905,  906,  907,  908,  909,  909,  910,  911,  912,  913,  914,  915,  916,  916,  917,
+	 918,  919,  920,  921,  922,  923,  924,  925,  925,  926,  927,  928,  929,  930,  931,  932,
+	 933,  933,  934,  935,  936,  937,  938,  939,  940,  941,  942,  942,  943,  944,  945,  946,
+	 947,  948,  949,  950,  951,  952,  952,  953,  954,  955,  956,  957,  958,  959,  960,  961,
+	 962,  962,  963,  964,  965,  966,  967,  968,  969,  970,  971,  972,  973,  973,  974,  975,
+	 976,  977,  978,  979,  980,  981,  982,  983,  984,  985,  985,  986,  987,  988,  989,  990,
+	 991,  992,  993,  994,  995,  996,  997,  998,  998,  999, 1000, 1001, 1002, 1003, 1004, 1005,
+	1006, 1007, 1008, 1009, 1010, 1011, 1012, 1013, 1013, 1014, 1015, 1016, 1017, 1018, 1019, 1020,
+	1021, 1022, 1023, 1024, 1025, 1026, 1027, 1028, 1029, 1030, 1030, 1031, 1032, 1033, 1034, 1035,
+	1036, 1037, 1038, 1039, 1040, 1041, 1042, 1043, 1044, 1045, 1046, 1047, 1048, 1049, 1050, 1050,
+	1051, 1052, 1053, 1054, 1055, 1056, 1057, 1058, 1059, 1060, 1061, 1062, 1063, 1064, 1065, 1066,
+	1067, 1068, 1069, 1070, 1071, 1072, 1073, 1074, 1075, 1076, 1077, 1078, 1078, 1079, 1080, 1081,
+	1082, 1083, 1084, 1085, 1086, 1087, 1088, 1089, 1090, 1091, 1092, 1093, 1094, 1095, 1096, 1097,
+	1098, 1099, 1100, 1101, 1102, 1103, 1104, 1105, 1106, 1107, 1108, 1109, 1110, 1111, 1112, 1113,
+	1114, 1115, 1116, 1117, 1118, 1119, 1120, 1121, 1122, 1123, 1124, 1125, 1126, 1127, 1128, 1129,
+	1130, 1131, 1132, 1133, 1134, 1135, 1136, 1137, 1138, 1139, 1140, 1141, 1142, 1143, 1144, 1145,
+	1146, 1147, 1148, 1149, 1150, 1151, 1152, 1153, 1154, 1155, 1156, 1157, 1158, 1159, 1160, 1161,
+	1162, 1163, 1164, 1165, 1166, 1167, 1168, 1169, 1170, 1171, 1172, 1173, 1174, 1175, 1176, 1177,
+	1178, 1179, 1180, 1181, 1182, 1183, 1184, 1185, 1186, 1187, 1188, 1189, 1190, 1191, 1193, 1194,
+	1195, 1196, 1197, 1198, 1199, 1200, 1201, 1202, 1203, 1204, 1205, 1206, 1207, 1208, 1209, 1210,
+	1211, 1212, 1213, 1214, 1215, 1216, 1217, 1218, 1219, 1220, 1221, 1223, 1224, 1225, 1226, 1227,
+	1228, 1229, 1230, 1231, 1232, 1233, 1234, 1235, 1236, 1237, 1238, 1239, 1240, 1241, 1242, 1243,
+	1245, 1246, 1247, 1248, 1249, 1250, 1251, 1252, 1253, 1254, 1255, 1256, 1257, 1258, 1259, 1260,
+	1261, 1262, 1264, 1265, 1266, 1267, 1268, 1269, 1270, 1271, 1272, 1273, 1274, 1275, 1276, 1277,
+	1278, 1280, 1281, 1282, 1283, 1284, 1285, 1286, 1287, 1288, 1289, 1290, 1291, 1292, 1293, 1295,
+	1296, 1297, 1298, 1299, 1300, 1301, 1302, 1303, 1304, 1305, 1306, 1307, 1309, 1310, 1311, 1312,
+	1313, 1314, 1315, 1316, 1317, 1318, 1319, 1320, 1322, 1323, 1324, 1325, 1326, 1327, 1328, 1329,
+	1330, 1331, 1332, 1334, 1335, 1336, 1337, 1338, 1339, 1340, 1341, 1342, 1343, 1345, 1346, 1347,
+	1348, 1349, 1350, 1351, 1352, 1353, 1354, 1356, 1357, 1358, 1359, 1360, 1361, 1362, 1363, 1364,
+	1365, 1367, 1368, 1369, 1370, 1371, 1372, 1373, 1374, 1375, 1377, 1378, 1379, 1380, 1381, 1382,
+	1383, 1384, 1385, 1387, 1388, 1389, 1390, 1391, 1392, 1393, 1394, 1396, 1397, 1398, 1399, 1400,
+	1401, 1402, 1403, 1405, 1406, 1407, 1408, 1409, 1410, 1411, 1412, 1414, 1415, 1416, 1417, 1418,
+	1419, 1420, 1421, 1423, 1424, 1425, 1426, 1427, 1428, 1429, 1431, 1432, 1433, 1434, 1435, 1436,
+	1437, 1439, 1440, 1441, 1442, 1443, 1444, 1445, 1446, 1448, 1449, 1450, 1451, 1452, 1453, 1455,
+	1456, 1457, 1458, 1459, 1460, 1461, 1463, 1464, 1465, 1466, 1467, 1468, 1469, 1471, 1472, 1473,
+	1474, 1475, 1476, 1478, 1479, 1480, 1481, 1482, 1483, 1484, 1486, 1487, 1488, 1489, 1490, 1491,
+	1493, 1494, 1495, 1496, 1497, 1498, 1500, 1501, 1502, 1503, 1504, 1505, 1507, 1508, 1509, 1510,
+	1511, 1512, 1514, 1515, 1516, 1517, 1518, 1519, 1521, 1522, 1523, 1524, 1525, 1527, 1528, 1529,
+	1530, 1531, 1532, 1534, 1535, 1536, 1537, 1538, 1540, 1541, 1542, 1543, 1544, 1545, 1547, 1548,
+	1549, 1550, 1551, 1553, 1554, 1555, 1556, 1557, 1559, 1560, 1561, 1562, 1563, 1564, 1566, 1567,
+	1568, 1569, 1570, 1572, 1573, 1574, 1575, 1576, 1578, 1579, 1580, 1581, 1582, 1584, 1585, 1586,
+	1587, 1588, 1590, 1591, 1592, 1593, 1594, 1596, 1597, 1598, 1599, 1601, 1602, 1603, 1604, 1605,
+	1607, 1608, 1609, 1610, 1611, 1613, 1614, 1615, 1616, 1617, 1619, 1620, 1621, 1622, 1624, 1625,
+	1626, 1627, 1628, 1630, 1631, 1632, 1633, 1635, 1636, 1637, 1638, 1639, 1641, 1642, 1643, 1644,
+	1646, 1647, 1648, 1649, 1650, 1652, 1653, 1654, 1655, 1657, 1658, 1659, 1660, 1662, 1663, 1664,
+	1665, 1667, 1668, 1669, 1670, 1671, 1673, 1674, 1675, 1676, 1678, 1679, 1680, 1681, 1683, 1684,
+	1685, 1686, 1688, 1689, 1690, 1691, 1693, 1694, 1695, 1696, 1698, 1699, 1700, 1701, 1703, 1704,
+	1705, 1706, 1708, 1709, 1710, 1711, 1713, 1714, 1715, 1716, 1718, 1719, 1720, 1721, 1723, 1724,
+	1725, 1726, 1728, 1729, 1730, 1731, 1733, 1734, 1735, 1737, 1738, 1739, 1740, 1742, 1743, 1744,
+	1745, 1747, 1748, 1749, 1750, 1752, 1753, 1754, 1756, 1757, 1758, 1759, 1761, 1762, 1763, 1764,
+	1766, 1767, 1768, 1770, 1771, 1772, 1773, 1775, 1776, 1777, 1778, 1780, 1781, 1782, 1784, 1785,
+	1786, 1787, 1789, 1790, 1791, 1793, 1794, 1795, 1796, 1798, 1799, 1800, 1802, 1803, 1804, 1806,
+	1807, 1808, 1809, 1811, 1812, 1813, 1815, 1816, 1817, 1818, 1820, 1821, 1822, 1824, 1825, 1826,
+	1828, 1829, 1830, 1831, 1833, 1834, 1835, 1837, 1838, 1839, 1841, 1842, 1843, 1844, 1846, 1847,
+	1848, 1850, 1851, 1852, 1854, 1855, 1856, 1858, 1859, 1860, 1862, 1863, 1864, 1865, 1867, 1868,
+	1869, 1871, 1872, 1873, 1875, 1876, 1877, 1879, 1880, 1881, 1883, 1884, 1885, 1887, 1888, 1889,
+	1891, 1892, 1893, 1894, 1896, 1897, 1898, 1900, 1901, 1902, 1904, 1905, 1906, 1908, 1909, 1910,
+	1912, 1913, 1914, 1916, 1917, 1918, 1920, 1921, 1922, 1924, 1925, 1926, 1928, 1929, 1930, 1932,
+	1933, 1935, 1936, 1937, 1939, 1940, 1941, 1943, 1944, 1945, 1947, 1948, 1949, 1951, 1952, 1953,
+	1955, 1956, 1957, 1959, 1960, 1961, 1963, 1964, 1965, 1967, 1968, 1970, 1971, 1972, 1974, 1975,
+	1976, 1978, 1979, 1980, 1982, 1983, 1984, 1986, 1987, 1989, 1990, 1991, 1993, 1994, 1995, 1997,
+	1998, 1999, 2001, 2002, 2004, 2005, 2006, 2008, 2009, 2010, 2012, 2013, 2015, 2016, 2017, 2019,
+	2020, 2021, 2023, 2024, 2026, 2027, 2028, 2030, 2031, 2032, 2034, 2035, 2037, 2038, 2039, 2041,
+	2042, 2043, 2045, 2046, 2048, 2049, 2050, 2052, 2053, 2055, 2056, 2057, 2059, 2060, 2061, 2063,
+	2064, 2066, 2067, 2068, 2070, 2071, 2073, 2074, 2075, 2077, 2078, 2080, 2081, 2082, 2084, 2085,
+	2087, 2088, 2089, 2091, 2092, 2094, 2095, 2096, 2098, 2099, 2101, 2102, 2103, 2105, 2106, 2108,
+	2109, 2110, 2112, 2113, 2115, 2116, 2117, 2119, 2120, 2122, 2123, 2124, 2126, 2127, 2129, 2130,
+	2132, 2133, 2134, 2136, 2137, 2139, 2140, 2141, 2143, 2144, 2146, 2147, 2149, 2150, 2151, 2153,
+	2154, 2156, 2157, 2159, 2160, 2161, 2163, 2164, 2166, 2167, 2169, 2170, 2171, 2173, 2174, 2176,
+	2177, 2179, 2180, 2181, 2183, 2184, 2186, 2187, 2189, 2190, 2191, 2193, 2194, 2196, 2197, 2199,
+	2200, 2202, 2203, 2204, 2206, 2207, 2209, 2210, 2212, 2213, 2214, 2216, 2217, 2219, 2220, 2222,
+	2223, 2225, 2226, 2228, 2229, 2230, 2232, 2233, 2235, 2236, 2238, 2239, 2241, 2242, 2243, 2245,
+	2246, 2248, 2249, 2251, 2252, 2254, 2255, 2257, 2258, 2260, 2261, 2262, 2264, 2265, 2267, 2268,
+	2270, 2271, 2273, 2274, 2276, 2277, 2279, 2280, 2282, 2283, 2284, 2286, 2287, 2289, 2290, 2292,
+	2293, 2295, 2296, 2298, 2299, 2301, 2302, 2304, 2305, 2307, 2308, 2310, 2311, 2312, 2314, 2315,
+	2317, 2318, 2320, 2321, 2323, 2324, 2326, 2327, 2329, 2330, 2332, 2333, 2335, 2336, 2338, 2339,
+	2341, 2342, 2344, 2345, 2347, 2348, 2350, 2351, 2353, 2354, 2356, 2357, 2359, 2360, 2362, 2363,
+	2365, 2366, 2368, 2369, 2371, 2372, 2374, 2375, 2377, 2378, 2380, 2381, 2383, 2384, 2386, 2387,
+	2389, 2390, 2392, 2393, 2395, 2396, 2398, 2399, 2401, 2402, 2404, 2405, 2407, 2408, 2410, 2411,
+	2413, 2414, 2416, 2417, 2419, 2420, 2422, 2423, 2425, 2426, 2428, 2429, 2431, 2433, 2434, 2436,
+	2437, 2439, 2440, 2442, 2443, 2445, 2446, 2448, 2449, 2451, 2452, 2454, 2455, 2457, 2458, 2460,
+	2462, 2463, 2465, 2466, 2468, 2469, 2471, 2472, 2474, 2475, 2477, 2478, 2480, 2481, 2483, 2485,
+	2486, 2488, 2489, 2491, 2492, 2494, 2495, 2497, 2498, 2500, 2502, 2503, 2505, 2506, 2508, 2509,
+	2511, 2512, 2514, 2515, 2517, 2519, 2520, 2522, 2523, 2525, 2526, 2528, 2529, 2531, 2533, 2534,
+	2536, 2537, 2539, 2540, 2542, 2543, 2545, 2547, 2548, 2550, 2551, 2553, 2554, 2556, 2557, 2559,
+	2561, 2562, 2564, 2565, 2567, 2568, 2570, 2572, 2573, 2575, 2576, 2578, 2579, 2581, 2583, 2584,
+	2586, 2587, 2589, 2590, 2592, 2594, 2595, 2597, 2598, 2600, 2601, 2603, 2605, 2606, 2608, 2609,
+	2611, 2613, 2614, 2616, 2617, 2619, 2620, 2622, 2624, 2625, 2627, 2628, 2630, 2632, 2633, 2635,
+	2636, 2638, 2640, 2641, 2643, 2644, 2646, 2647, 2649, 2651, 2652, 2654, 2655, 2657, 2659, 2660,
+	2662, 2663, 2665, 2667, 2668, 2670, 2671, 2673, 2675, 2676, 2678, 2679, 2681, 2683, 2684, 2686,
+	2687, 2689, 2691, 2692, 2694, 2696, 2697, 2699, 2700, 2702, 2704, 2705, 2707, 2708, 2710, 2712,
+	2713, 2715, 2716, 2718, 2720, 2721, 2723, 2725, 2726, 2728, 2729, 2731, 2733, 2734, 2736, 2738,
+	2739, 2741, 2742, 2744, 2746, 2747, 2749, 2751, 2752, 2754, 2755, 2757, 2759, 2760, 2762, 2764,
+	2765, 2767, 2769, 2770, 2772, 2773, 2775, 2777, 2778, 2780, 2782, 2783, 2785, 2787, 2788, 2790,
+	2791, 2793, 2795, 2796, 2798, 2800, 2801, 2803, 2805, 2806, 2808, 2810, 2811, 2813, 2814, 2816,
+	2818, 2819, 2821, 2823, 2824, 2826, 2828, 2829, 2831, 2833, 2834, 2836, 2838, 2839, 2841, 2843,
+	2844, 2846, 2848, 2849, 2851, 2853, 2854, 2856, 2857, 2859, 2861, 2862, 2864, 2866, 2867, 2869,
+	2871, 2872, 2874, 2876, 2877, 2879, 2881, 2882, 2884, 2886, 2888, 2889, 2891, 2893, 2894, 2896,
+	2898, 2899, 2901, 2903, 2904, 2906, 2908, 2909, 2911, 2913, 2914, 2916, 2918, 2919, 2921, 2923,
+	2924, 2926, 2928, 2929, 2931, 2933, 2935, 2936, 2938, 2940, 2941, 2943, 2945, 2946, 2948, 2950,
+	2951, 2953, 2955, 2956, 2958, 2960, 2962, 2963, 2965, 2967, 2968, 2970, 2972, 2973, 2975, 2977,
+	2979, 2980, 2982, 2984, 2985, 2987, 2989, 2990, 2992, 2994, 2996, 2997, 2999, 3001, 3002, 3004,
+	3006, 3008, 3009, 3011, 3013, 3014, 3016, 3018, 3020, 3021, 3023, 3025, 3026, 3028, 3030, 3032,
+	3033, 3035, 3037, 3038, 3040, 3042, 3044, 3045, 3047, 3049, 3050, 3052, 3054, 3056, 3057, 3059,
+	3061, 3063, 3064, 3066, 3068, 3069, 3071, 3073, 3075, 3076, 3078, 3080, 3082, 3083, 3085, 3087,
+	3089, 3090, 3092, 3094, 3095, 3097, 3099, 3101, 3102, 3104, 3106, 3108, 3109, 3111, 3113, 3115,
+	3116, 3118, 3120, 3122, 3123, 3125, 3127, 3129, 3130, 3132, 3134, 3136, 3137, 3139, 3141, 3143,
+	3144, 3146, 3148, 3150, 3151, 3153, 3155, 3157, 3158, 3160, 3162, 3164, 3165, 3167, 3169, 3171,
+	3172, 3174, 3176, 3178, 3179, 3181, 3183, 3185, 3187, 3188, 3190, 3192, 3194, 3195, 3197, 3199,
+	3201, 3202, 3204, 3206, 3208, 3209, 3211, 3213, 3215, 3217, 3218, 3220, 3222, 3224, 3225, 3227,
+	3229, 3231, 3233, 3234, 3236, 3238, 3240, 3241, 3243, 3245, 3247, 3249, 3250, 3252, 3254, 3256,
+	3258, 3259, 3261, 3263, 3265, 3266, 3268, 3270, 3272, 3274, 3275, 3277, 3279, 3281, 3283, 3284,
+	3286, 3288, 3290, 3292, 3293, 3295, 3297, 3299, 3301, 3302, 3304, 3306, 3308, 3310, 3311, 3313,
+	3315, 3317, 3319, 3320, 3322, 3324, 3326, 3328, 3329, 3331, 3333, 3335, 3337, 3338, 3340, 3342,
+	3344, 3346, 3348, 3349, 3351, 3353, 3355, 3357, 3358, 3360, 3362, 3364, 3366, 3368, 3369, 3371,
+	3373, 3375, 3377, 3378, 3380, 3382, 3384, 3386, 3388, 3389, 3391, 3393, 3395, 3397, 3399, 3400,
+	3402, 3404, 3406, 3408, 3410, 3411, 3413, 3415, 3417, 3419, 3421, 3422, 3424, 3426, 3428, 3430,
+	3432, 3433, 3435, 3437, 3439, 3441, 3443, 3444, 3446, 3448, 3450, 3452, 3454, 3455, 3457, 3459,
+	3461, 3463, 3465, 3467, 3468, 3470, 3472, 3474, 3476, 3478, 3480, 3481, 3483, 3485, 3487, 3489,
+	3491, 3492, 3494, 3496, 3498, 3500, 3502, 3504, 3506, 3507, 3509, 3511, 3513, 3515, 3517, 3519,
+	3520, 3522, 3524, 3526, 3528, 3530, 3532, 3533, 3535, 3537, 3539, 3541, 3543, 3545, 3547, 3548,
+	3550, 3552, 3554, 3556, 3558, 3560, 3562, 3563, 3565, 3567, 3569, 3571, 3573, 3575, 3577, 3578,
+	3580, 3582, 3584, 3586, 3588, 3590, 3592, 3594, 3595, 3597, 3599, 3601, 3603, 3605, 3607, 3609,
+	3611, 3612, 3614, 3616, 3618, 3620, 3622, 3624, 3626, 3628, 3629, 3631, 3633, 3635, 3637, 3639,
+	3641, 3643, 3645, 3647, 3648, 3650, 3652, 3654, 3656, 3658, 3660, 3662, 3664, 3666, 3667, 3669,
+	3671, 3673, 3675, 3677, 3679, 3681, 3683, 3685, 3687, 3688, 3690, 3692, 3694, 3696, 3698, 3700,
+	3702, 3704, 3706, 3708, 3710, 3711, 3713, 3715, 3717, 3719, 3721, 3723, 3725, 3727, 3729, 3731,
+	3733, 3735, 3736, 3738, 3740, 3742, 3744, 3746, 3748, 3750, 3752, 3754, 3756, 3758, 3760, 3762,
+	3764, 3765, 3767, 3769, 3771, 3773, 3775, 3777, 3779, 3781, 3783, 3785, 3787, 3789, 3791, 3793,
+	3795, 3796, 3798, 3800, 3802, 3804, 3806, 3808, 3810, 3812, 3814, 3816, 3818, 3820, 3822, 3824,
+	3826, 3828, 3830, 3832, 3833, 3835, 3837, 3839, 3841, 3843, 3845, 3847, 3849, 3851, 3853, 3855,
+	3857, 3859, 3861, 3863, 3865, 3867, 3869, 3871, 3873, 3875, 3877, 3879, 3881, 3883, 3884, 3886,
+	3888, 3890, 3892, 3894, 3896, 3898, 3900, 3902, 3904, 3906, 3908, 3910, 3912, 3914, 3916, 3918,
+	3920, 3922, 3924, 3926, 3928, 3930, 3932, 3934, 3936, 3938, 3940, 3942, 3944, 3946, 3948, 3950,
+	3952, 3954, 3956, 3958, 3960, 3962, 3964, 3966, 3968, 3970, 3972, 3974, 3976, 3978, 3980, 3982,
+	3984, 3986, 3988, 3990, 3992, 3994, 3996, 3998, 4000, 4002, 4004, 4006, 4008, 4010, 4012, 4014,
+	4016, 4018, 4020, 4022, 4024, 4026, 4028, 4030, 4032, 4034, 4036, 4038, 4040, 4042, 4044, 4046,
+	4048, 4050, 4052, 4054, 4056, 4058, 4060, 4062, 4064, 4066, 4068, 4070, 4072, 4074, 4076, 4078,
+	4080,
+};
+
+/* Generated table */
+const unsigned short tpg_linear_to_rec709[255 * 16 + 1] = {
+	   0,    5,    9,   14,   18,   22,   27,   32,   36,   41,   45,   50,   54,   59,   63,   68,
+	  72,   77,   81,   86,   90,   95,   99,  104,  108,  113,  117,  122,  126,  131,  135,  139,
+	 144,  149,  153,  158,  162,  167,  171,  176,  180,  185,  189,  194,  198,  203,  207,  212,
+	 216,  221,  225,  230,  234,  239,  243,  248,  252,  257,  261,  266,  270,  275,  279,  284,
+	 288,  293,  297,  302,  306,  311,  315,  320,  324,  328,  334,  338,  343,  347,  352,  356,
+	 360,  365,  369,  373,  377,  381,  386,  390,  394,  398,  402,  406,  410,  414,  418,  422,
+	 426,  430,  433,  437,  441,  445,  449,  452,  456,  460,  464,  467,  471,  475,  478,  482,
+	 485,  489,  492,  496,  499,  503,  506,  510,  513,  517,  520,  524,  527,  530,  534,  537,
+	 540,  544,  547,  550,  554,  557,  560,  563,  566,  570,  573,  576,  579,  582,  586,  589,
+	 592,  595,  598,  601,  604,  607,  610,  613,  616,  619,  622,  625,  628,  631,  634,  637,
+	 640,  643,  646,  649,  652,  655,  658,  660,  663,  666,  669,  672,  675,  677,  680,  683,
+	 686,  689,  691,  694,  697,  700,  702,  705,  708,  711,  713,  716,  719,  721,  724,  727,
+	 729,  732,  735,  737,  740,  743,  745,  748,  750,  753,  756,  758,  761,  763,  766,  768,
+	 771,  773,  776,  779,  781,  784,  786,  789,  791,  794,  796,  799,  801,  803,  806,  808,
+	 811,  813,  816,  818,  821,  823,  825,  828,  830,  833,  835,  837,  840,  842,  844,  847,
+	 849,  851,  854,  856,  858,  861,  863,  865,  868,  870,  872,  875,  877,  879,  881,  884,
+	 886,  888,  891,  893,  895,  897,  900,  902,  904,  906,  908,  911,  913,  915,  917,  919,
+	 922,  924,  926,  928,  930,  933,  935,  937,  939,  941,  943,  946,  948,  950,  952,  954,
+	 956,  958,  960,  963,  965,  967,  969,  971,  973,  975,  977,  979,  981,  984,  986,  988,
+	 990,  992,  994,  996,  998, 1000, 1002, 1004, 1006, 1008, 1010, 1012, 1014, 1016, 1018, 1020,
+	1022, 1024, 1026, 1028, 1030, 1032, 1034, 1036, 1038, 1040, 1042, 1044, 1046, 1048, 1050, 1052,
+	1054, 1056, 1058, 1060, 1062, 1064, 1066, 1068, 1069, 1071, 1073, 1075, 1077, 1079, 1081, 1083,
+	1085, 1087, 1089, 1090, 1092, 1094, 1096, 1098, 1100, 1102, 1104, 1106, 1107, 1109, 1111, 1113,
+	1115, 1117, 1119, 1120, 1122, 1124, 1126, 1128, 1130, 1131, 1133, 1135, 1137, 1139, 1141, 1142,
+	1144, 1146, 1148, 1150, 1151, 1153, 1155, 1157, 1159, 1160, 1162, 1164, 1166, 1168, 1169, 1171,
+	1173, 1175, 1176, 1178, 1180, 1182, 1184, 1185, 1187, 1189, 1191, 1192, 1194, 1196, 1198, 1199,
+	1201, 1203, 1204, 1206, 1208, 1210, 1211, 1213, 1215, 1217, 1218, 1220, 1222, 1223, 1225, 1227,
+	1228, 1230, 1232, 1234, 1235, 1237, 1239, 1240, 1242, 1244, 1245, 1247, 1249, 1250, 1252, 1254,
+	1255, 1257, 1259, 1260, 1262, 1264, 1265, 1267, 1269, 1270, 1272, 1274, 1275, 1277, 1279, 1280,
+	1282, 1283, 1285, 1287, 1288, 1290, 1292, 1293, 1295, 1296, 1298, 1300, 1301, 1303, 1305, 1306,
+	1308, 1309, 1311, 1313, 1314, 1316, 1317, 1319, 1321, 1322, 1324, 1325, 1327, 1328, 1330, 1332,
+	1333, 1335, 1336, 1338, 1339, 1341, 1343, 1344, 1346, 1347, 1349, 1350, 1352, 1354, 1355, 1357,
+	1358, 1360, 1361, 1363, 1364, 1366, 1367, 1369, 1371, 1372, 1374, 1375, 1377, 1378, 1380, 1381,
+	1383, 1384, 1386, 1387, 1389, 1390, 1392, 1393, 1395, 1396, 1398, 1399, 1401, 1402, 1404, 1405,
+	1407, 1408, 1410, 1411, 1413, 1414, 1416, 1417, 1419, 1420, 1422, 1423, 1425, 1426, 1428, 1429,
+	1431, 1432, 1434, 1435, 1437, 1438, 1440, 1441, 1442, 1444, 1445, 1447, 1448, 1450, 1451, 1453,
+	1454, 1456, 1457, 1458, 1460, 1461, 1463, 1464, 1466, 1467, 1469, 1470, 1471, 1473, 1474, 1476,
+	1477, 1479, 1480, 1481, 1483, 1484, 1486, 1487, 1489, 1490, 1491, 1493, 1494, 1496, 1497, 1498,
+	1500, 1501, 1503, 1504, 1505, 1507, 1508, 1510, 1511, 1512, 1514, 1515, 1517, 1518, 1519, 1521,
+	1522, 1524, 1525, 1526, 1528, 1529, 1531, 1532, 1533, 1535, 1536, 1537, 1539, 1540, 1542, 1543,
+	1544, 1546, 1547, 1548, 1550, 1551, 1553, 1554, 1555, 1557, 1558, 1559, 1561, 1562, 1563, 1565,
+	1566, 1567, 1569, 1570, 1571, 1573, 1574, 1576, 1577, 1578, 1580, 1581, 1582, 1584, 1585, 1586,
+	1588, 1589, 1590, 1592, 1593, 1594, 1596, 1597, 1598, 1600, 1601, 1602, 1603, 1605, 1606, 1607,
+	1609, 1610, 1611, 1613, 1614, 1615, 1617, 1618, 1619, 1621, 1622, 1623, 1624, 1626, 1627, 1628,
+	1630, 1631, 1632, 1634, 1635, 1636, 1637, 1639, 1640, 1641, 1643, 1644, 1645, 1647, 1648, 1649,
+	1650, 1652, 1653, 1654, 1655, 1657, 1658, 1659, 1661, 1662, 1663, 1664, 1666, 1667, 1668, 1670,
+	1671, 1672, 1673, 1675, 1676, 1677, 1678, 1680, 1681, 1682, 1683, 1685, 1686, 1687, 1688, 1690,
+	1691, 1692, 1693, 1695, 1696, 1697, 1698, 1700, 1701, 1702, 1703, 1705, 1706, 1707, 1708, 1710,
+	1711, 1712, 1713, 1715, 1716, 1717, 1718, 1720, 1721, 1722, 1723, 1724, 1726, 1727, 1728, 1729,
+	1731, 1732, 1733, 1734, 1736, 1737, 1738, 1739, 1740, 1742, 1743, 1744, 1745, 1746, 1748, 1749,
+	1750, 1751, 1753, 1754, 1755, 1756, 1757, 1759, 1760, 1761, 1762, 1763, 1765, 1766, 1767, 1768,
+	1769, 1771, 1772, 1773, 1774, 1775, 1777, 1778, 1779, 1780, 1781, 1783, 1784, 1785, 1786, 1787,
+	1788, 1790, 1791, 1792, 1793, 1794, 1796, 1797, 1798, 1799, 1800, 1801, 1803, 1804, 1805, 1806,
+	1807, 1809, 1810, 1811, 1812, 1813, 1814, 1816, 1817, 1818, 1819, 1820, 1821, 1823, 1824, 1825,
+	1826, 1827, 1828, 1829, 1831, 1832, 1833, 1834, 1835, 1836, 1838, 1839, 1840, 1841, 1842, 1843,
+	1845, 1846, 1847, 1848, 1849, 1850, 1851, 1853, 1854, 1855, 1856, 1857, 1858, 1859, 1861, 1862,
+	1863, 1864, 1865, 1866, 1867, 1868, 1870, 1871, 1872, 1873, 1874, 1875, 1876, 1878, 1879, 1880,
+	1881, 1882, 1883, 1884, 1885, 1887, 1888, 1889, 1890, 1891, 1892, 1893, 1894, 1896, 1897, 1898,
+	1899, 1900, 1901, 1902, 1903, 1904, 1906, 1907, 1908, 1909, 1910, 1911, 1912, 1913, 1914, 1916,
+	1917, 1918, 1919, 1920, 1921, 1922, 1923, 1924, 1925, 1927, 1928, 1929, 1930, 1931, 1932, 1933,
+	1934, 1935, 1936, 1938, 1939, 1940, 1941, 1942, 1943, 1944, 1945, 1946, 1947, 1948, 1950, 1951,
+	1952, 1953, 1954, 1955, 1956, 1957, 1958, 1959, 1960, 1961, 1963, 1964, 1965, 1966, 1967, 1968,
+	1969, 1970, 1971, 1972, 1973, 1974, 1975, 1977, 1978, 1979, 1980, 1981, 1982, 1983, 1984, 1985,
+	1986, 1987, 1988, 1989, 1990, 1992, 1993, 1994, 1995, 1996, 1997, 1998, 1999, 2000, 2001, 2002,
+	2003, 2004, 2005, 2006, 2007, 2008, 2010, 2011, 2012, 2013, 2014, 2015, 2016, 2017, 2018, 2019,
+	2020, 2021, 2022, 2023, 2024, 2025, 2026, 2027, 2028, 2029, 2031, 2032, 2033, 2034, 2035, 2036,
+	2037, 2038, 2039, 2040, 2041, 2042, 2043, 2044, 2045, 2046, 2047, 2048, 2049, 2050, 2051, 2052,
+	2053, 2054, 2055, 2056, 2057, 2058, 2060, 2061, 2062, 2063, 2064, 2065, 2066, 2067, 2068, 2069,
+	2070, 2071, 2072, 2073, 2074, 2075, 2076, 2077, 2078, 2079, 2080, 2081, 2082, 2083, 2084, 2085,
+	2086, 2087, 2088, 2089, 2090, 2091, 2092, 2093, 2094, 2095, 2096, 2097, 2098, 2099, 2100, 2101,
+	2102, 2103, 2104, 2105, 2106, 2107, 2108, 2109, 2110, 2111, 2112, 2113, 2114, 2115, 2116, 2117,
+	2118, 2119, 2120, 2121, 2122, 2123, 2124, 2125, 2126, 2127, 2128, 2129, 2130, 2131, 2132, 2133,
+	2134, 2135, 2136, 2137, 2138, 2139, 2140, 2141, 2142, 2143, 2144, 2145, 2146, 2147, 2148, 2149,
+	2150, 2151, 2152, 2153, 2154, 2155, 2156, 2157, 2158, 2159, 2160, 2161, 2162, 2163, 2164, 2165,
+	2166, 2167, 2168, 2169, 2170, 2171, 2172, 2173, 2173, 2174, 2175, 2176, 2177, 2178, 2179, 2180,
+	2181, 2182, 2183, 2184, 2185, 2186, 2187, 2188, 2189, 2190, 2191, 2192, 2193, 2194, 2195, 2196,
+	2197, 2198, 2199, 2200, 2201, 2202, 2202, 2203, 2204, 2205, 2206, 2207, 2208, 2209, 2210, 2211,
+	2212, 2213, 2214, 2215, 2216, 2217, 2218, 2219, 2220, 2221, 2222, 2223, 2224, 2224, 2225, 2226,
+	2227, 2228, 2229, 2230, 2231, 2232, 2233, 2234, 2235, 2236, 2237, 2238, 2239, 2240, 2241, 2241,
+	2242, 2243, 2244, 2245, 2246, 2247, 2248, 2249, 2250, 2251, 2252, 2253, 2254, 2255, 2256, 2257,
+	2257, 2258, 2259, 2260, 2261, 2262, 2263, 2264, 2265, 2266, 2267, 2268, 2269, 2270, 2271, 2271,
+	2272, 2273, 2274, 2275, 2276, 2277, 2278, 2279, 2280, 2281, 2282, 2283, 2283, 2284, 2285, 2286,
+	2287, 2288, 2289, 2290, 2291, 2292, 2293, 2294, 2295, 2295, 2296, 2297, 2298, 2299, 2300, 2301,
+	2302, 2303, 2304, 2305, 2306, 2306, 2307, 2308, 2309, 2310, 2311, 2312, 2313, 2314, 2315, 2316,
+	2317, 2317, 2318, 2319, 2320, 2321, 2322, 2323, 2324, 2325, 2326, 2327, 2327, 2328, 2329, 2330,
+	2331, 2332, 2333, 2334, 2335, 2336, 2336, 2337, 2338, 2339, 2340, 2341, 2342, 2343, 2344, 2345,
+	2345, 2346, 2347, 2348, 2349, 2350, 2351, 2352, 2353, 2354, 2354, 2355, 2356, 2357, 2358, 2359,
+	2360, 2361, 2362, 2363, 2363, 2364, 2365, 2366, 2367, 2368, 2369, 2370, 2371, 2371, 2372, 2373,
+	2374, 2375, 2376, 2377, 2378, 2379, 2379, 2380, 2381, 2382, 2383, 2384, 2385, 2386, 2386, 2387,
+	2388, 2389, 2390, 2391, 2392, 2393, 2394, 2394, 2395, 2396, 2397, 2398, 2399, 2400, 2401, 2401,
+	2402, 2403, 2404, 2405, 2406, 2407, 2408, 2408, 2409, 2410, 2411, 2412, 2413, 2414, 2415, 2415,
+	2416, 2417, 2418, 2419, 2420, 2421, 2422, 2422, 2423, 2424, 2425, 2426, 2427, 2428, 2428, 2429,
+	2430, 2431, 2432, 2433, 2434, 2435, 2435, 2436, 2437, 2438, 2439, 2440, 2441, 2441, 2442, 2443,
+	2444, 2445, 2446, 2447, 2447, 2448, 2449, 2450, 2451, 2452, 2453, 2453, 2454, 2455, 2456, 2457,
+	2458, 2459, 2459, 2460, 2461, 2462, 2463, 2464, 2465, 2465, 2466, 2467, 2468, 2469, 2470, 2471,
+	2471, 2472, 2473, 2474, 2475, 2476, 2477, 2477, 2478, 2479, 2480, 2481, 2482, 2482, 2483, 2484,
+	2485, 2486, 2487, 2488, 2488, 2489, 2490, 2491, 2492, 2493, 2493, 2494, 2495, 2496, 2497, 2498,
+	2499, 2499, 2500, 2501, 2502, 2503, 2504, 2504, 2505, 2506, 2507, 2508, 2509, 2509, 2510, 2511,
+	2512, 2513, 2514, 2514, 2515, 2516, 2517, 2518, 2519, 2519, 2520, 2521, 2522, 2523, 2524, 2524,
+	2525, 2526, 2527, 2528, 2529, 2529, 2530, 2531, 2532, 2533, 2534, 2534, 2535, 2536, 2537, 2538,
+	2539, 2539, 2540, 2541, 2542, 2543, 2544, 2544, 2545, 2546, 2547, 2548, 2548, 2549, 2550, 2551,
+	2552, 2553, 2553, 2554, 2555, 2556, 2557, 2558, 2558, 2559, 2560, 2561, 2562, 2562, 2563, 2564,
+	2565, 2566, 2567, 2567, 2568, 2569, 2570, 2571, 2571, 2572, 2573, 2574, 2575, 2576, 2576, 2577,
+	2578, 2579, 2580, 2580, 2581, 2582, 2583, 2584, 2584, 2585, 2586, 2587, 2588, 2589, 2589, 2590,
+	2591, 2592, 2593, 2593, 2594, 2595, 2596, 2597, 2597, 2598, 2599, 2600, 2601, 2601, 2602, 2603,
+	2604, 2605, 2605, 2606, 2607, 2608, 2609, 2610, 2610, 2611, 2612, 2613, 2614, 2614, 2615, 2616,
+	2617, 2618, 2618, 2619, 2620, 2621, 2622, 2622, 2623, 2624, 2625, 2626, 2626, 2627, 2628, 2629,
+	2630, 2630, 2631, 2632, 2633, 2634, 2634, 2635, 2636, 2637, 2637, 2638, 2639, 2640, 2641, 2641,
+	2642, 2643, 2644, 2645, 2645, 2646, 2647, 2648, 2649, 2649, 2650, 2651, 2652, 2653, 2653, 2654,
+	2655, 2656, 2656, 2657, 2658, 2659, 2660, 2660, 2661, 2662, 2663, 2664, 2664, 2665, 2666, 2667,
+	2668, 2668, 2669, 2670, 2671, 2671, 2672, 2673, 2674, 2675, 2675, 2676, 2677, 2678, 2678, 2679,
+	2680, 2681, 2682, 2682, 2683, 2684, 2685, 2686, 2686, 2687, 2688, 2689, 2689, 2690, 2691, 2692,
+	2693, 2693, 2694, 2695, 2696, 2696, 2697, 2698, 2699, 2700, 2700, 2701, 2702, 2703, 2703, 2704,
+	2705, 2706, 2706, 2707, 2708, 2709, 2710, 2710, 2711, 2712, 2713, 2713, 2714, 2715, 2716, 2717,
+	2717, 2718, 2719, 2720, 2720, 2721, 2722, 2723, 2723, 2724, 2725, 2726, 2727, 2727, 2728, 2729,
+	2730, 2730, 2731, 2732, 2733, 2733, 2734, 2735, 2736, 2736, 2737, 2738, 2739, 2740, 2740, 2741,
+	2742, 2743, 2743, 2744, 2745, 2746, 2746, 2747, 2748, 2749, 2749, 2750, 2751, 2752, 2752, 2753,
+	2754, 2755, 2755, 2756, 2757, 2758, 2759, 2759, 2760, 2761, 2762, 2762, 2763, 2764, 2765, 2765,
+	2766, 2767, 2768, 2768, 2769, 2770, 2771, 2771, 2772, 2773, 2774, 2774, 2775, 2776, 2777, 2777,
+	2778, 2779, 2780, 2780, 2781, 2782, 2783, 2783, 2784, 2785, 2786, 2786, 2787, 2788, 2789, 2789,
+	2790, 2791, 2792, 2792, 2793, 2794, 2795, 2795, 2796, 2797, 2798, 2798, 2799, 2800, 2801, 2801,
+	2802, 2803, 2804, 2804, 2805, 2806, 2807, 2807, 2808, 2809, 2810, 2810, 2811, 2812, 2813, 2813,
+	2814, 2815, 2815, 2816, 2817, 2818, 2818, 2819, 2820, 2821, 2821, 2822, 2823, 2824, 2824, 2825,
+	2826, 2827, 2827, 2828, 2829, 2830, 2830, 2831, 2832, 2832, 2833, 2834, 2835, 2835, 2836, 2837,
+	2838, 2838, 2839, 2840, 2841, 2841, 2842, 2843, 2844, 2844, 2845, 2846, 2846, 2847, 2848, 2849,
+	2849, 2850, 2851, 2852, 2852, 2853, 2854, 2855, 2855, 2856, 2857, 2857, 2858, 2859, 2860, 2860,
+	2861, 2862, 2863, 2863, 2864, 2865, 2865, 2866, 2867, 2868, 2868, 2869, 2870, 2871, 2871, 2872,
+	2873, 2873, 2874, 2875, 2876, 2876, 2877, 2878, 2879, 2879, 2880, 2881, 2881, 2882, 2883, 2884,
+	2884, 2885, 2886, 2886, 2887, 2888, 2889, 2889, 2890, 2891, 2892, 2892, 2893, 2894, 2894, 2895,
+	2896, 2897, 2897, 2898, 2899, 2899, 2900, 2901, 2902, 2902, 2903, 2904, 2904, 2905, 2906, 2907,
+	2907, 2908, 2909, 2909, 2910, 2911, 2912, 2912, 2913, 2914, 2914, 2915, 2916, 2917, 2917, 2918,
+	2919, 2919, 2920, 2921, 2922, 2922, 2923, 2924, 2924, 2925, 2926, 2927, 2927, 2928, 2929, 2929,
+	2930, 2931, 2932, 2932, 2933, 2934, 2934, 2935, 2936, 2937, 2937, 2938, 2939, 2939, 2940, 2941,
+	2941, 2942, 2943, 2944, 2944, 2945, 2946, 2946, 2947, 2948, 2949, 2949, 2950, 2951, 2951, 2952,
+	2953, 2953, 2954, 2955, 2956, 2956, 2957, 2958, 2958, 2959, 2960, 2961, 2961, 2962, 2963, 2963,
+	2964, 2965, 2965, 2966, 2967, 2968, 2968, 2969, 2970, 2970, 2971, 2972, 2972, 2973, 2974, 2975,
+	2975, 2976, 2977, 2977, 2978, 2979, 2979, 2980, 2981, 2982, 2982, 2983, 2984, 2984, 2985, 2986,
+	2986, 2987, 2988, 2988, 2989, 2990, 2991, 2991, 2992, 2993, 2993, 2994, 2995, 2995, 2996, 2997,
+	2998, 2998, 2999, 3000, 3000, 3001, 3002, 3002, 3003, 3004, 3004, 3005, 3006, 3006, 3007, 3008,
+	3009, 3009, 3010, 3011, 3011, 3012, 3013, 3013, 3014, 3015, 3015, 3016, 3017, 3018, 3018, 3019,
+	3020, 3020, 3021, 3022, 3022, 3023, 3024, 3024, 3025, 3026, 3026, 3027, 3028, 3029, 3029, 3030,
+	3031, 3031, 3032, 3033, 3033, 3034, 3035, 3035, 3036, 3037, 3037, 3038, 3039, 3039, 3040, 3041,
+	3042, 3042, 3043, 3044, 3044, 3045, 3046, 3046, 3047, 3048, 3048, 3049, 3050, 3050, 3051, 3052,
+	3052, 3053, 3054, 3054, 3055, 3056, 3056, 3057, 3058, 3059, 3059, 3060, 3061, 3061, 3062, 3063,
+	3063, 3064, 3065, 3065, 3066, 3067, 3067, 3068, 3069, 3069, 3070, 3071, 3071, 3072, 3073, 3073,
+	3074, 3075, 3075, 3076, 3077, 3077, 3078, 3079, 3079, 3080, 3081, 3081, 3082, 3083, 3084, 3084,
+	3085, 3086, 3086, 3087, 3088, 3088, 3089, 3090, 3090, 3091, 3092, 3092, 3093, 3094, 3094, 3095,
+	3096, 3096, 3097, 3098, 3098, 3099, 3100, 3100, 3101, 3102, 3102, 3103, 3104, 3104, 3105, 3106,
+	3106, 3107, 3108, 3108, 3109, 3110, 3110, 3111, 3112, 3112, 3113, 3114, 3114, 3115, 3116, 3116,
+	3117, 3118, 3118, 3119, 3120, 3120, 3121, 3122, 3122, 3123, 3124, 3124, 3125, 3126, 3126, 3127,
+	3128, 3128, 3129, 3130, 3130, 3131, 3132, 3132, 3133, 3134, 3134, 3135, 3135, 3136, 3137, 3137,
+	3138, 3139, 3139, 3140, 3141, 3141, 3142, 3143, 3143, 3144, 3145, 3145, 3146, 3147, 3147, 3148,
+	3149, 3149, 3150, 3151, 3151, 3152, 3153, 3153, 3154, 3155, 3155, 3156, 3157, 3157, 3158, 3159,
+	3159, 3160, 3160, 3161, 3162, 3162, 3163, 3164, 3164, 3165, 3166, 3166, 3167, 3168, 3168, 3169,
+	3170, 3170, 3171, 3172, 3172, 3173, 3174, 3174, 3175, 3175, 3176, 3177, 3177, 3178, 3179, 3179,
+	3180, 3181, 3181, 3182, 3183, 3183, 3184, 3185, 3185, 3186, 3187, 3187, 3188, 3188, 3189, 3190,
+	3190, 3191, 3192, 3192, 3193, 3194, 3194, 3195, 3196, 3196, 3197, 3198, 3198, 3199, 3199, 3200,
+	3201, 3201, 3202, 3203, 3203, 3204, 3205, 3205, 3206, 3207, 3207, 3208, 3209, 3209, 3210, 3210,
+	3211, 3212, 3212, 3213, 3214, 3214, 3215, 3216, 3216, 3217, 3218, 3218, 3219, 3219, 3220, 3221,
+	3221, 3222, 3223, 3223, 3224, 3225, 3225, 3226, 3227, 3227, 3228, 3228, 3229, 3230, 3230, 3231,
+	3232, 3232, 3233, 3234, 3234, 3235, 3235, 3236, 3237, 3237, 3238, 3239, 3239, 3240, 3241, 3241,
+	3242, 3242, 3243, 3244, 3244, 3245, 3246, 3246, 3247, 3248, 3248, 3249, 3249, 3250, 3251, 3251,
+	3252, 3253, 3253, 3254, 3255, 3255, 3256, 3256, 3257, 3258, 3258, 3259, 3260, 3260, 3261, 3262,
+	3262, 3263, 3263, 3264, 3265, 3265, 3266, 3267, 3267, 3268, 3268, 3269, 3270, 3270, 3271, 3272,
+	3272, 3273, 3274, 3274, 3275, 3275, 3276, 3277, 3277, 3278, 3279, 3279, 3280, 3280, 3281, 3282,
+	3282, 3283, 3284, 3284, 3285, 3285, 3286, 3287, 3287, 3288, 3289, 3289, 3290, 3290, 3291, 3292,
+	3292, 3293, 3294, 3294, 3295, 3295, 3296, 3297, 3297, 3298, 3299, 3299, 3300, 3300, 3301, 3302,
+	3302, 3303, 3304, 3304, 3305, 3305, 3306, 3307, 3307, 3308, 3309, 3309, 3310, 3310, 3311, 3312,
+	3312, 3313, 3314, 3314, 3315, 3315, 3316, 3317, 3317, 3318, 3319, 3319, 3320, 3320, 3321, 3322,
+	3322, 3323, 3323, 3324, 3325, 3325, 3326, 3327, 3327, 3328, 3328, 3329, 3330, 3330, 3331, 3332,
+	3332, 3333, 3333, 3334, 3335, 3335, 3336, 3336, 3337, 3338, 3338, 3339, 3340, 3340, 3341, 3341,
+	3342, 3343, 3343, 3344, 3345, 3345, 3346, 3346, 3347, 3348, 3348, 3349, 3349, 3350, 3351, 3351,
+	3352, 3352, 3353, 3354, 3354, 3355, 3356, 3356, 3357, 3357, 3358, 3359, 3359, 3360, 3360, 3361,
+	3362, 3362, 3363, 3364, 3364, 3365, 3365, 3366, 3367, 3367, 3368, 3368, 3369, 3370, 3370, 3371,
+	3371, 3372, 3373, 3373, 3374, 3375, 3375, 3376, 3376, 3377, 3378, 3378, 3379, 3379, 3380, 3381,
+	3381, 3382, 3382, 3383, 3384, 3384, 3385, 3385, 3386, 3387, 3387, 3388, 3389, 3389, 3390, 3390,
+	3391, 3392, 3392, 3393, 3393, 3394, 3395, 3395, 3396, 3396, 3397, 3398, 3398, 3399, 3399, 3400,
+	3401, 3401, 3402, 3402, 3403, 3404, 3404, 3405, 3405, 3406, 3407, 3407, 3408, 3408, 3409, 3410,
+	3410, 3411, 3411, 3412, 3413, 3413, 3414, 3414, 3415, 3416, 3416, 3417, 3418, 3418, 3419, 3419,
+	3420, 3421, 3421, 3422, 3422, 3423, 3424, 3424, 3425, 3425, 3426, 3427, 3427, 3428, 3428, 3429,
+	3430, 3430, 3431, 3431, 3432, 3433, 3433, 3434, 3434, 3435, 3435, 3436, 3437, 3437, 3438, 3438,
+	3439, 3440, 3440, 3441, 3441, 3442, 3443, 3443, 3444, 3444, 3445, 3446, 3446, 3447, 3447, 3448,
+	3449, 3449, 3450, 3450, 3451, 3452, 3452, 3453, 3453, 3454, 3455, 3455, 3456, 3456, 3457, 3458,
+	3458, 3459, 3459, 3460, 3461, 3461, 3462, 3462, 3463, 3463, 3464, 3465, 3465, 3466, 3466, 3467,
+	3468, 3468, 3469, 3469, 3470, 3471, 3471, 3472, 3472, 3473, 3474, 3474, 3475, 3475, 3476, 3476,
+	3477, 3478, 3478, 3479, 3479, 3480, 3481, 3481, 3482, 3482, 3483, 3484, 3484, 3485, 3485, 3486,
+	3486, 3487, 3488, 3488, 3489, 3489, 3490, 3491, 3491, 3492, 3492, 3493, 3494, 3494, 3495, 3495,
+	3496, 3496, 3497, 3498, 3498, 3499, 3499, 3500, 3501, 3501, 3502, 3502, 3503, 3504, 3504, 3505,
+	3505, 3506, 3506, 3507, 3508, 3508, 3509, 3509, 3510, 3511, 3511, 3512, 3512, 3513, 3513, 3514,
+	3515, 3515, 3516, 3516, 3517, 3518, 3518, 3519, 3519, 3520, 3520, 3521, 3522, 3522, 3523, 3523,
+	3524, 3525, 3525, 3526, 3526, 3527, 3527, 3528, 3529, 3529, 3530, 3530, 3531, 3531, 3532, 3533,
+	3533, 3534, 3534, 3535, 3536, 3536, 3537, 3537, 3538, 3538, 3539, 3540, 3540, 3541, 3541, 3542,
+	3542, 3543, 3544, 3544, 3545, 3545, 3546, 3547, 3547, 3548, 3548, 3549, 3549, 3550, 3551, 3551,
+	3552, 3552, 3553, 3553, 3554, 3555, 3555, 3556, 3556, 3557, 3557, 3558, 3559, 3559, 3560, 3560,
+	3561, 3561, 3562, 3563, 3563, 3564, 3564, 3565, 3566, 3566, 3567, 3567, 3568, 3568, 3569, 3570,
+	3570, 3571, 3571, 3572, 3572, 3573, 3574, 3574, 3575, 3575, 3576, 3576, 3577, 3578, 3578, 3579,
+	3579, 3580, 3580, 3581, 3582, 3582, 3583, 3583, 3584, 3584, 3585, 3586, 3586, 3587, 3587, 3588,
+	3588, 3589, 3590, 3590, 3591, 3591, 3592, 3592, 3593, 3594, 3594, 3595, 3595, 3596, 3596, 3597,
+	3597, 3598, 3599, 3599, 3600, 3600, 3601, 3601, 3602, 3603, 3603, 3604, 3604, 3605, 3605, 3606,
+	3607, 3607, 3608, 3608, 3609, 3609, 3610, 3611, 3611, 3612, 3612, 3613, 3613, 3614, 3615, 3615,
+	3616, 3616, 3617, 3617, 3618, 3618, 3619, 3620, 3620, 3621, 3621, 3622, 3622, 3623, 3624, 3624,
+	3625, 3625, 3626, 3626, 3627, 3627, 3628, 3629, 3629, 3630, 3630, 3631, 3631, 3632, 3633, 3633,
+	3634, 3634, 3635, 3635, 3636, 3636, 3637, 3638, 3638, 3639, 3639, 3640, 3640, 3641, 3642, 3642,
+	3643, 3643, 3644, 3644, 3645, 3645, 3646, 3647, 3647, 3648, 3648, 3649, 3649, 3650, 3650, 3651,
+	3652, 3652, 3653, 3653, 3654, 3654, 3655, 3656, 3656, 3657, 3657, 3658, 3658, 3659, 3659, 3660,
+	3661, 3661, 3662, 3662, 3663, 3663, 3664, 3664, 3665, 3666, 3666, 3667, 3667, 3668, 3668, 3669,
+	3669, 3670, 3671, 3671, 3672, 3672, 3673, 3673, 3674, 3674, 3675, 3676, 3676, 3677, 3677, 3678,
+	3678, 3679, 3679, 3680, 3681, 3681, 3682, 3682, 3683, 3683, 3684, 3684, 3685, 3686, 3686, 3687,
+	3687, 3688, 3688, 3689, 3689, 3690, 3691, 3691, 3692, 3692, 3693, 3693, 3694, 3694, 3695, 3695,
+	3696, 3697, 3697, 3698, 3698, 3699, 3699, 3700, 3700, 3701, 3702, 3702, 3703, 3703, 3704, 3704,
+	3705, 3705, 3706, 3707, 3707, 3708, 3708, 3709, 3709, 3710, 3710, 3711, 3711, 3712, 3713, 3713,
+	3714, 3714, 3715, 3715, 3716, 3716, 3717, 3717, 3718, 3719, 3719, 3720, 3720, 3721, 3721, 3722,
+	3722, 3723, 3724, 3724, 3725, 3725, 3726, 3726, 3727, 3727, 3728, 3728, 3729, 3730, 3730, 3731,
+	3731, 3732, 3732, 3733, 3733, 3734, 3734, 3735, 3736, 3736, 3737, 3737, 3738, 3738, 3739, 3739,
+	3740, 3740, 3741, 3742, 3742, 3743, 3743, 3744, 3744, 3745, 3745, 3746, 3746, 3747, 3748, 3748,
+	3749, 3749, 3750, 3750, 3751, 3751, 3752, 3752, 3753, 3753, 3754, 3755, 3755, 3756, 3756, 3757,
+	3757, 3758, 3758, 3759, 3759, 3760, 3761, 3761, 3762, 3762, 3763, 3763, 3764, 3764, 3765, 3765,
+	3766, 3766, 3767, 3768, 3768, 3769, 3769, 3770, 3770, 3771, 3771, 3772, 3772, 3773, 3773, 3774,
+	3775, 3775, 3776, 3776, 3777, 3777, 3778, 3778, 3779, 3779, 3780, 3781, 3781, 3782, 3782, 3783,
+	3783, 3784, 3784, 3785, 3785, 3786, 3786, 3787, 3787, 3788, 3789, 3789, 3790, 3790, 3791, 3791,
+	3792, 3792, 3793, 3793, 3794, 3794, 3795, 3796, 3796, 3797, 3797, 3798, 3798, 3799, 3799, 3800,
+	3800, 3801, 3801, 3802, 3802, 3803, 3804, 3804, 3805, 3805, 3806, 3806, 3807, 3807, 3808, 3808,
+	3809, 3809, 3810, 3811, 3811, 3812, 3812, 3813, 3813, 3814, 3814, 3815, 3815, 3816, 3816, 3817,
+	3817, 3818, 3819, 3819, 3820, 3820, 3821, 3821, 3822, 3822, 3823, 3823, 3824, 3824, 3825, 3825,
+	3826, 3826, 3827, 3828, 3828, 3829, 3829, 3830, 3830, 3831, 3831, 3832, 3832, 3833, 3833, 3834,
+	3834, 3835, 3835, 3836, 3837, 3837, 3838, 3838, 3839, 3839, 3840, 3840, 3841, 3841, 3842, 3842,
+	3843, 3843, 3844, 3844, 3845, 3846, 3846, 3847, 3847, 3848, 3848, 3849, 3849, 3850, 3850, 3851,
+	3851, 3852, 3852, 3853, 3853, 3854, 3855, 3855, 3856, 3856, 3857, 3857, 3858, 3858, 3859, 3859,
+	3860, 3860, 3861, 3861, 3862, 3862, 3863, 3863, 3864, 3864, 3865, 3866, 3866, 3867, 3867, 3868,
+	3868, 3869, 3869, 3870, 3870, 3871, 3871, 3872, 3872, 3873, 3873, 3874, 3874, 3875, 3876, 3876,
+	3877, 3877, 3878, 3878, 3879, 3879, 3880, 3880, 3881, 3881, 3882, 3882, 3883, 3883, 3884, 3884,
+	3885, 3885, 3886, 3886, 3887, 3888, 3888, 3889, 3889, 3890, 3890, 3891, 3891, 3892, 3892, 3893,
+	3893, 3894, 3894, 3895, 3895, 3896, 3896, 3897, 3897, 3898, 3898, 3899, 3900, 3900, 3901, 3901,
+	3902, 3902, 3903, 3903, 3904, 3904, 3905, 3905, 3906, 3906, 3907, 3907, 3908, 3908, 3909, 3909,
+	3910, 3910, 3911, 3911, 3912, 3912, 3913, 3914, 3914, 3915, 3915, 3916, 3916, 3917, 3917, 3918,
+	3918, 3919, 3919, 3920, 3920, 3921, 3921, 3922, 3922, 3923, 3923, 3924, 3924, 3925, 3925, 3926,
+	3926, 3927, 3927, 3928, 3929, 3929, 3930, 3930, 3931, 3931, 3932, 3932, 3933, 3933, 3934, 3934,
+	3935, 3935, 3936, 3936, 3937, 3937, 3938, 3938, 3939, 3939, 3940, 3940, 3941, 3941, 3942, 3942,
+	3943, 3943, 3944, 3944, 3945, 3945, 3946, 3947, 3947, 3948, 3948, 3949, 3949, 3950, 3950, 3951,
+	3951, 3952, 3952, 3953, 3953, 3954, 3954, 3955, 3955, 3956, 3956, 3957, 3957, 3958, 3958, 3959,
+	3959, 3960, 3960, 3961, 3961, 3962, 3962, 3963, 3963, 3964, 3964, 3965, 3965, 3966, 3966, 3967,
+	3967, 3968, 3969, 3969, 3970, 3970, 3971, 3971, 3972, 3972, 3973, 3973, 3974, 3974, 3975, 3975,
+	3976, 3976, 3977, 3977, 3978, 3978, 3979, 3979, 3980, 3980, 3981, 3981, 3982, 3982, 3983, 3983,
+	3984, 3984, 3985, 3985, 3986, 3986, 3987, 3987, 3988, 3988, 3989, 3989, 3990, 3990, 3991, 3991,
+	3992, 3992, 3993, 3993, 3994, 3994, 3995, 3995, 3996, 3996, 3997, 3997, 3998, 3998, 3999, 3999,
+	4000, 4001, 4001, 4002, 4002, 4003, 4003, 4004, 4004, 4005, 4005, 4006, 4006, 4007, 4007, 4008,
+	4008, 4009, 4009, 4010, 4010, 4011, 4011, 4012, 4012, 4013, 4013, 4014, 4014, 4015, 4015, 4016,
+	4016, 4017, 4017, 4018, 4018, 4019, 4019, 4020, 4020, 4021, 4021, 4022, 4022, 4023, 4023, 4024,
+	4024, 4025, 4025, 4026, 4026, 4027, 4027, 4028, 4028, 4029, 4029, 4030, 4030, 4031, 4031, 4032,
+	4032, 4033, 4033, 4034, 4034, 4035, 4035, 4036, 4036, 4037, 4037, 4038, 4038, 4039, 4039, 4040,
+	4040, 4041, 4041, 4042, 4042, 4043, 4043, 4044, 4044, 4045, 4045, 4046, 4046, 4047, 4047, 4048,
+	4048, 4049, 4049, 4050, 4050, 4051, 4051, 4052, 4052, 4053, 4053, 4054, 4054, 4055, 4055, 4056,
+	4056, 4057, 4057, 4058, 4058, 4059, 4059, 4060, 4060, 4061, 4061, 4062, 4062, 4063, 4063, 4064,
+	4064, 4065, 4065, 4066, 4066, 4067, 4067, 4068, 4068, 4069, 4069, 4070, 4070, 4071, 4071, 4072,
+	4072, 4073, 4073, 4074, 4074, 4075, 4075, 4076, 4076, 4077, 4077, 4078, 4078, 4079, 4079, 4080,
+	4080,
+};
+
+/* Generated table */
+const struct color16 tpg_csc_colors[V4L2_COLORSPACE_DCI_P3 + 1][V4L2_XFER_FUNC_SMPTE2084 + 1][TPG_COLOR_CSC_BLACK + 1] = {
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_709][0] = { 2939, 2939, 2939 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_709][1] = { 2953, 2963, 586 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_709][2] = { 0, 2967, 2937 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_709][3] = { 88, 2990, 575 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_709][4] = { 3016, 259, 2933 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_709][5] = { 3030, 405, 558 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_709][6] = { 478, 428, 2931 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_709][7] = { 547, 547, 547 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SRGB][0] = { 3056, 3056, 3056 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SRGB][1] = { 3068, 3077, 838 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SRGB][2] = { 0, 3081, 3053 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SRGB][3] = { 241, 3102, 828 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SRGB][4] = { 3126, 504, 3050 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SRGB][5] = { 3138, 657, 810 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SRGB][6] = { 731, 680, 3048 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SRGB][7] = { 800, 799, 800 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_ADOBERGB][0] = { 3033, 3033, 3033 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_ADOBERGB][1] = { 3046, 3054, 886 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_ADOBERGB][2] = { 0, 3058, 3031 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_ADOBERGB][3] = { 360, 3079, 877 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_ADOBERGB][4] = { 3103, 587, 3027 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_ADOBERGB][5] = { 3116, 723, 861 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_ADOBERGB][6] = { 789, 744, 3025 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_ADOBERGB][7] = { 851, 851, 851 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SMPTE240M][0] = { 2926, 2926, 2926 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SMPTE240M][1] = { 2941, 2950, 546 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SMPTE240M][2] = { 0, 2954, 2924 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SMPTE240M][3] = { 78, 2978, 536 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SMPTE240M][4] = { 3004, 230, 2920 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SMPTE240M][5] = { 3018, 363, 518 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SMPTE240M][6] = { 437, 387, 2918 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SMPTE240M][7] = { 507, 507, 507 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_NONE][0] = { 2125, 2125, 2125 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_NONE][1] = { 2145, 2159, 142 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_NONE][2] = { 0, 2164, 2122 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_NONE][3] = { 19, 2198, 138 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_NONE][4] = { 2236, 57, 2116 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_NONE][5] = { 2256, 90, 133 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_NONE][6] = { 110, 96, 2113 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_NONE][7] = { 130, 130, 130 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_DCI_P3][0] = { 3175, 3175, 3175 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_DCI_P3][1] = { 3186, 3194, 1121 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_DCI_P3][2] = { 0, 3197, 3173 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_DCI_P3][3] = { 523, 3216, 1112 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_DCI_P3][4] = { 3237, 792, 3169 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_DCI_P3][5] = { 3248, 944, 1094 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_DCI_P3][6] = { 1017, 967, 3168 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_DCI_P3][7] = { 1084, 1084, 1084 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SMPTE2084][0] = { 3798, 3798, 3798 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SMPTE2084][1] = { 3802, 3805, 2602 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SMPTE2084][2] = { 0, 3806, 3797 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SMPTE2084][3] = { 1780, 3812, 2592 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SMPTE2084][4] = { 3820, 2215, 3796 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SMPTE2084][5] = { 3824, 2409, 2574 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SMPTE2084][6] = { 2491, 2435, 3795 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SMPTE2084][7] = { 2563, 2563, 2563 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_709][0] = { 2939, 2939, 2939 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_709][1] = { 2953, 2963, 586 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_709][2] = { 0, 2967, 2937 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_709][3] = { 88, 2990, 575 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_709][4] = { 3016, 259, 2933 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_709][5] = { 3030, 405, 558 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_709][6] = { 478, 428, 2931 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_709][7] = { 547, 547, 547 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SRGB][0] = { 3056, 3056, 3056 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SRGB][1] = { 3068, 3077, 838 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SRGB][2] = { 0, 3081, 3053 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SRGB][3] = { 241, 3102, 828 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SRGB][4] = { 3126, 504, 3050 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SRGB][5] = { 3138, 657, 810 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SRGB][6] = { 731, 680, 3048 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SRGB][7] = { 800, 799, 800 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_ADOBERGB][0] = { 3033, 3033, 3033 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_ADOBERGB][1] = { 3046, 3054, 886 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_ADOBERGB][2] = { 0, 3058, 3031 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_ADOBERGB][3] = { 360, 3079, 877 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_ADOBERGB][4] = { 3103, 587, 3027 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_ADOBERGB][5] = { 3116, 723, 861 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_ADOBERGB][6] = { 789, 744, 3025 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_ADOBERGB][7] = { 851, 851, 851 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SMPTE240M][0] = { 2926, 2926, 2926 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SMPTE240M][1] = { 2941, 2950, 546 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SMPTE240M][2] = { 0, 2954, 2924 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SMPTE240M][3] = { 78, 2978, 536 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SMPTE240M][4] = { 3004, 230, 2920 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SMPTE240M][5] = { 3018, 363, 518 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SMPTE240M][6] = { 437, 387, 2918 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SMPTE240M][7] = { 507, 507, 507 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_NONE][0] = { 2125, 2125, 2125 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_NONE][1] = { 2145, 2159, 142 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_NONE][2] = { 0, 2164, 2122 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_NONE][3] = { 19, 2198, 138 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_NONE][4] = { 2236, 57, 2116 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_NONE][5] = { 2256, 90, 133 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_NONE][6] = { 110, 96, 2113 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_NONE][7] = { 130, 130, 130 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_DCI_P3][0] = { 3175, 3175, 3175 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_DCI_P3][1] = { 3186, 3194, 1121 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_DCI_P3][2] = { 0, 3197, 3173 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_DCI_P3][3] = { 523, 3216, 1112 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_DCI_P3][4] = { 3237, 792, 3169 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_DCI_P3][5] = { 3248, 944, 1094 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_DCI_P3][6] = { 1017, 967, 3168 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_DCI_P3][7] = { 1084, 1084, 1084 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SMPTE2084][0] = { 3798, 3798, 3798 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SMPTE2084][1] = { 3802, 3805, 2602 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SMPTE2084][2] = { 0, 3806, 3797 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SMPTE2084][3] = { 1780, 3812, 2592 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SMPTE2084][4] = { 3820, 2215, 3796 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SMPTE2084][5] = { 3824, 2409, 2574 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SMPTE2084][6] = { 2491, 2435, 3795 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SMPTE2084][7] = { 2563, 2563, 2563 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_709][0] = { 2939, 2939, 2939 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_709][1] = { 2939, 2939, 547 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_709][2] = { 547, 2939, 2939 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_709][3] = { 547, 2939, 547 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_709][4] = { 2939, 547, 2939 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_709][5] = { 2939, 547, 547 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_709][6] = { 547, 547, 2939 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_709][7] = { 547, 547, 547 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SRGB][0] = { 3056, 3056, 3056 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SRGB][1] = { 3056, 3056, 800 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SRGB][2] = { 800, 3056, 3056 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SRGB][3] = { 800, 3056, 800 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SRGB][4] = { 3056, 800, 3056 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SRGB][5] = { 3056, 800, 800 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SRGB][6] = { 800, 800, 3056 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SRGB][7] = { 800, 800, 800 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_ADOBERGB][0] = { 3033, 3033, 3033 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_ADOBERGB][1] = { 3033, 3033, 851 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_ADOBERGB][2] = { 851, 3033, 3033 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_ADOBERGB][3] = { 851, 3033, 851 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_ADOBERGB][4] = { 3033, 851, 3033 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_ADOBERGB][5] = { 3033, 851, 851 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_ADOBERGB][6] = { 851, 851, 3033 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_ADOBERGB][7] = { 851, 851, 851 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SMPTE240M][0] = { 2926, 2926, 2926 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SMPTE240M][1] = { 2926, 2926, 507 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SMPTE240M][2] = { 507, 2926, 2926 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SMPTE240M][3] = { 507, 2926, 507 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SMPTE240M][4] = { 2926, 507, 2926 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SMPTE240M][5] = { 2926, 507, 507 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SMPTE240M][6] = { 507, 507, 2926 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SMPTE240M][7] = { 507, 507, 507 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_NONE][0] = { 2125, 2125, 2125 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_NONE][1] = { 2125, 2125, 130 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_NONE][2] = { 130, 2125, 2125 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_NONE][3] = { 130, 2125, 130 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_NONE][4] = { 2125, 130, 2125 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_NONE][5] = { 2125, 130, 130 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_NONE][6] = { 130, 130, 2125 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_NONE][7] = { 130, 130, 130 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_DCI_P3][0] = { 3175, 3175, 3175 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_DCI_P3][1] = { 3175, 3175, 1084 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_DCI_P3][2] = { 1084, 3175, 3175 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_DCI_P3][3] = { 1084, 3175, 1084 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_DCI_P3][4] = { 3175, 1084, 3175 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_DCI_P3][5] = { 3175, 1084, 1084 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_DCI_P3][6] = { 1084, 1084, 3175 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_DCI_P3][7] = { 1084, 1084, 1084 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SMPTE2084][0] = { 3798, 3798, 3798 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SMPTE2084][1] = { 3798, 3798, 2563 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SMPTE2084][2] = { 2563, 3798, 3798 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SMPTE2084][3] = { 2563, 3798, 2563 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SMPTE2084][4] = { 3798, 2563, 3798 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SMPTE2084][5] = { 3798, 2563, 2563 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SMPTE2084][6] = { 2563, 2563, 3798 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SMPTE2084][7] = { 2563, 2563, 2563 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_709][0] = { 2939, 2939, 2939 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_709][1] = { 2892, 3034, 910 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_709][2] = { 1715, 2916, 2914 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_709][3] = { 1631, 3012, 828 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_709][4] = { 2497, 119, 2867 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_709][5] = { 2440, 649, 657 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_709][6] = { 740, 0, 2841 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_709][7] = { 547, 547, 547 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SRGB][0] = { 3056, 3055, 3056 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SRGB][1] = { 3013, 3142, 1157 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SRGB][2] = { 1926, 3034, 3032 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SRGB][3] = { 1847, 3121, 1076 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SRGB][4] = { 2651, 304, 2990 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SRGB][5] = { 2599, 901, 909 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SRGB][6] = { 991, 0, 2966 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SRGB][7] = { 800, 799, 800 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_ADOBERGB][0] = { 3033, 3033, 3033 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_ADOBERGB][1] = { 2989, 3120, 1180 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_ADOBERGB][2] = { 1913, 3011, 3009 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_ADOBERGB][3] = { 1836, 3099, 1105 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_ADOBERGB][4] = { 2627, 413, 2966 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_ADOBERGB][5] = { 2576, 943, 951 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_ADOBERGB][6] = { 1026, 0, 2942 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_ADOBERGB][7] = { 851, 851, 851 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SMPTE240M][0] = { 2926, 2926, 2926 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SMPTE240M][1] = { 2879, 3022, 874 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SMPTE240M][2] = { 1688, 2903, 2901 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SMPTE240M][3] = { 1603, 2999, 791 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SMPTE240M][4] = { 2479, 106, 2853 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SMPTE240M][5] = { 2422, 610, 618 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SMPTE240M][6] = { 702, 0, 2827 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SMPTE240M][7] = { 507, 507, 507 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_NONE][0] = { 2125, 2125, 2125 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_NONE][1] = { 2059, 2262, 266 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_NONE][2] = { 771, 2092, 2089 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_NONE][3] = { 705, 2229, 231 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_NONE][4] = { 1550, 26, 2024 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_NONE][5] = { 1484, 163, 165 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_NONE][6] = { 196, 0, 1988 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_NONE][7] = { 130, 130, 130 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_DCI_P3][0] = { 3175, 3175, 3175 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_DCI_P3][1] = { 3136, 3251, 1429 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_DCI_P3][2] = { 2150, 3156, 3154 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_DCI_P3][3] = { 2077, 3233, 1352 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_DCI_P3][4] = { 2812, 589, 3116 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_DCI_P3][5] = { 2765, 1182, 1190 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_DCI_P3][6] = { 1270, 0, 3094 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_DCI_P3][7] = { 1084, 1084, 1084 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SMPTE2084][0] = { 3798, 3798, 3798 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SMPTE2084][1] = { 3784, 3825, 2879 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SMPTE2084][2] = { 3351, 3791, 3790 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SMPTE2084][3] = { 3311, 3819, 2815 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SMPTE2084][4] = { 3659, 1900, 3777 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SMPTE2084][5] = { 3640, 2662, 2669 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SMPTE2084][6] = { 2743, 0, 3769 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SMPTE2084][7] = { 2563, 2563, 2563 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_709][0] = { 2939, 2939, 2939 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_709][1] = { 2939, 2939, 464 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_709][2] = { 786, 2939, 2939 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_709][3] = { 786, 2939, 464 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_709][4] = { 2879, 547, 2956 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_709][5] = { 2879, 547, 547 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_709][6] = { 547, 547, 2956 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_709][7] = { 547, 547, 547 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SRGB][0] = { 3056, 3056, 3056 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SRGB][1] = { 3056, 3056, 717 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SRGB][2] = { 1036, 3056, 3056 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SRGB][3] = { 1036, 3056, 717 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SRGB][4] = { 3001, 800, 3071 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SRGB][5] = { 3001, 800, 799 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SRGB][6] = { 800, 800, 3071 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SRGB][7] = { 800, 800, 799 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_ADOBERGB][0] = { 3033, 3033, 3033 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_ADOBERGB][1] = { 3033, 3033, 776 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_ADOBERGB][2] = { 1068, 3033, 3033 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_ADOBERGB][3] = { 1068, 3033, 776 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_ADOBERGB][4] = { 2977, 851, 3048 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_ADOBERGB][5] = { 2977, 851, 851 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_ADOBERGB][6] = { 851, 851, 3048 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_ADOBERGB][7] = { 851, 851, 851 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SMPTE240M][0] = { 2926, 2926, 2926 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SMPTE240M][1] = { 2926, 2926, 423 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SMPTE240M][2] = { 749, 2926, 2926 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SMPTE240M][3] = { 749, 2926, 423 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SMPTE240M][4] = { 2865, 507, 2943 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SMPTE240M][5] = { 2865, 507, 507 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SMPTE240M][6] = { 507, 507, 2943 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SMPTE240M][7] = { 507, 507, 507 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_NONE][0] = { 2125, 2125, 2125 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_NONE][1] = { 2125, 2125, 106 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_NONE][2] = { 214, 2125, 2125 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_NONE][3] = { 214, 2125, 106 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_NONE][4] = { 2041, 130, 2149 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_NONE][5] = { 2041, 130, 130 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_NONE][6] = { 130, 130, 2149 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_NONE][7] = { 130, 130, 130 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_DCI_P3][0] = { 3175, 3175, 3175 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_DCI_P3][1] = { 3175, 3175, 1003 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_DCI_P3][2] = { 1313, 3175, 3175 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_DCI_P3][3] = { 1313, 3175, 1003 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_DCI_P3][4] = { 3126, 1084, 3188 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_DCI_P3][5] = { 3126, 1084, 1084 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_DCI_P3][6] = { 1084, 1084, 3188 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_DCI_P3][7] = { 1084, 1084, 1084 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SMPTE2084][0] = { 3798, 3798, 3798 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SMPTE2084][1] = { 3798, 3798, 2476 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SMPTE2084][2] = { 2782, 3798, 3798 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SMPTE2084][3] = { 2782, 3798, 2476 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SMPTE2084][4] = { 3780, 2563, 3803 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SMPTE2084][5] = { 3780, 2563, 2563 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SMPTE2084][6] = { 2563, 2563, 3803 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SMPTE2084][7] = { 2563, 2563, 2563 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_709][0] = { 2939, 2939, 2939 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_709][1] = { 2939, 2939, 547 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_709][2] = { 547, 2939, 2939 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_709][3] = { 547, 2939, 547 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_709][4] = { 2939, 547, 2939 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_709][5] = { 2939, 547, 547 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_709][6] = { 547, 547, 2939 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_709][7] = { 547, 547, 547 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SRGB][0] = { 3056, 3056, 3056 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SRGB][1] = { 3056, 3056, 800 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SRGB][2] = { 800, 3056, 3056 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SRGB][3] = { 800, 3056, 800 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SRGB][4] = { 3056, 800, 3056 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SRGB][5] = { 3056, 800, 800 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SRGB][6] = { 800, 800, 3056 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SRGB][7] = { 800, 800, 800 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_ADOBERGB][0] = { 3033, 3033, 3033 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_ADOBERGB][1] = { 3033, 3033, 851 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_ADOBERGB][2] = { 851, 3033, 3033 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_ADOBERGB][3] = { 851, 3033, 851 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_ADOBERGB][4] = { 3033, 851, 3033 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_ADOBERGB][5] = { 3033, 851, 851 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_ADOBERGB][6] = { 851, 851, 3033 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_ADOBERGB][7] = { 851, 851, 851 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SMPTE240M][0] = { 2926, 2926, 2926 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SMPTE240M][1] = { 2926, 2926, 507 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SMPTE240M][2] = { 507, 2926, 2926 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SMPTE240M][3] = { 507, 2926, 507 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SMPTE240M][4] = { 2926, 507, 2926 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SMPTE240M][5] = { 2926, 507, 507 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SMPTE240M][6] = { 507, 507, 2926 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SMPTE240M][7] = { 507, 507, 507 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_NONE][0] = { 2125, 2125, 2125 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_NONE][1] = { 2125, 2125, 130 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_NONE][2] = { 130, 2125, 2125 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_NONE][3] = { 130, 2125, 130 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_NONE][4] = { 2125, 130, 2125 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_NONE][5] = { 2125, 130, 130 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_NONE][6] = { 130, 130, 2125 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_NONE][7] = { 130, 130, 130 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_DCI_P3][0] = { 3175, 3175, 3175 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_DCI_P3][1] = { 3175, 3175, 1084 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_DCI_P3][2] = { 1084, 3175, 3175 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_DCI_P3][3] = { 1084, 3175, 1084 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_DCI_P3][4] = { 3175, 1084, 3175 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_DCI_P3][5] = { 3175, 1084, 1084 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_DCI_P3][6] = { 1084, 1084, 3175 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_DCI_P3][7] = { 1084, 1084, 1084 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SMPTE2084][0] = { 3798, 3798, 3798 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SMPTE2084][1] = { 3798, 3798, 2563 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SMPTE2084][2] = { 2563, 3798, 3798 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SMPTE2084][3] = { 2563, 3798, 2563 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SMPTE2084][4] = { 3798, 2563, 3798 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SMPTE2084][5] = { 3798, 2563, 2563 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SMPTE2084][6] = { 2563, 2563, 3798 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SMPTE2084][7] = { 2563, 2563, 2563 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_709][0] = { 2939, 2939, 2939 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_709][1] = { 2939, 2939, 781 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_709][2] = { 1622, 2939, 2939 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_709][3] = { 1622, 2939, 781 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_709][4] = { 2502, 547, 2881 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_709][5] = { 2502, 547, 547 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_709][6] = { 547, 547, 2881 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_709][7] = { 547, 547, 547 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SRGB][0] = { 3056, 3056, 3056 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SRGB][1] = { 3056, 3056, 1031 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SRGB][2] = { 1838, 3056, 3056 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SRGB][3] = { 1838, 3056, 1031 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SRGB][4] = { 2657, 800, 3002 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SRGB][5] = { 2657, 800, 800 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SRGB][6] = { 800, 800, 3002 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SRGB][7] = { 800, 800, 800 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_ADOBERGB][0] = { 3033, 3033, 3033 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_ADOBERGB][1] = { 3033, 3033, 1063 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_ADOBERGB][2] = { 1828, 3033, 3033 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_ADOBERGB][3] = { 1828, 3033, 1063 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_ADOBERGB][4] = { 2633, 851, 2979 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_ADOBERGB][5] = { 2633, 851, 851 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_ADOBERGB][6] = { 851, 851, 2979 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_ADOBERGB][7] = { 851, 851, 851 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SMPTE240M][0] = { 2926, 2926, 2926 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SMPTE240M][1] = { 2926, 2926, 744 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SMPTE240M][2] = { 1594, 2926, 2926 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SMPTE240M][3] = { 1594, 2926, 744 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SMPTE240M][4] = { 2484, 507, 2867 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SMPTE240M][5] = { 2484, 507, 507 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SMPTE240M][6] = { 507, 507, 2867 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SMPTE240M][7] = { 507, 507, 507 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_NONE][0] = { 2125, 2125, 2125 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_NONE][1] = { 2125, 2125, 212 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_NONE][2] = { 698, 2125, 2125 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_NONE][3] = { 698, 2125, 212 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_NONE][4] = { 1557, 130, 2043 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_NONE][5] = { 1557, 130, 130 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_NONE][6] = { 130, 130, 2043 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_NONE][7] = { 130, 130, 130 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_DCI_P3][0] = { 3175, 3175, 3175 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_DCI_P3][1] = { 3175, 3175, 1308 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_DCI_P3][2] = { 2069, 3175, 3175 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_DCI_P3][3] = { 2069, 3175, 1308 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_DCI_P3][4] = { 2816, 1084, 3127 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_DCI_P3][5] = { 2816, 1084, 1084 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_DCI_P3][6] = { 1084, 1084, 3127 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_DCI_P3][7] = { 1084, 1084, 1084 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SMPTE2084][0] = { 3798, 3798, 3798 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SMPTE2084][1] = { 3798, 3798, 2778 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SMPTE2084][2] = { 3306, 3798, 3798 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SMPTE2084][3] = { 3306, 3798, 2778 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SMPTE2084][4] = { 3661, 2563, 3781 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SMPTE2084][5] = { 3661, 2563, 2563 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SMPTE2084][6] = { 2563, 2563, 3781 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SMPTE2084][7] = { 2563, 2563, 2563 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_709][0] = { 2939, 2939, 2939 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_709][1] = { 2877, 2923, 1058 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_709][2] = { 1837, 2840, 2916 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_709][3] = { 1734, 2823, 993 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_709][4] = { 2427, 961, 2812 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_709][5] = { 2351, 912, 648 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_709][6] = { 792, 618, 2788 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_709][7] = { 547, 547, 547 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SRGB][0] = { 3056, 3056, 3056 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SRGB][1] = { 2999, 3041, 1301 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SRGB][2] = { 2040, 2965, 3034 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SRGB][3] = { 1944, 2950, 1238 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SRGB][4] = { 2587, 1207, 2940 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SRGB][5] = { 2517, 1159, 900 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SRGB][6] = { 1042, 870, 2917 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SRGB][7] = { 800, 800, 800 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_ADOBERGB][0] = { 3033, 3033, 3033 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_ADOBERGB][1] = { 2976, 3018, 1315 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_ADOBERGB][2] = { 2024, 2942, 3011 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_ADOBERGB][3] = { 1930, 2926, 1256 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_ADOBERGB][4] = { 2563, 1227, 2916 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_ADOBERGB][5] = { 2494, 1183, 943 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_ADOBERGB][6] = { 1073, 916, 2894 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_ADOBERGB][7] = { 851, 851, 851 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SMPTE240M][0] = { 2926, 2926, 2926 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SMPTE240M][1] = { 2864, 2910, 1024 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SMPTE240M][2] = { 1811, 2826, 2903 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SMPTE240M][3] = { 1707, 2809, 958 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SMPTE240M][4] = { 2408, 926, 2798 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SMPTE240M][5] = { 2331, 876, 609 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SMPTE240M][6] = { 755, 579, 2773 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SMPTE240M][7] = { 507, 507, 507 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_NONE][0] = { 2125, 2125, 2125 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_NONE][1] = { 2039, 2102, 338 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_NONE][2] = { 873, 1987, 2092 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_NONE][3] = { 787, 1965, 305 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_NONE][4] = { 1468, 290, 1949 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_NONE][5] = { 1382, 268, 162 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_NONE][6] = { 216, 152, 1917 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_NONE][7] = { 130, 130, 130 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_DCI_P3][0] = { 3175, 3175, 3175 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_DCI_P3][1] = { 3124, 3161, 1566 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_DCI_P3][2] = { 2255, 3094, 3156 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_DCI_P3][3] = { 2166, 3080, 1506 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_DCI_P3][4] = { 2754, 1477, 3071 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_DCI_P3][5] = { 2690, 1431, 1182 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_DCI_P3][6] = { 1318, 1153, 3051 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_DCI_P3][7] = { 1084, 1084, 1084 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SMPTE2084][0] = { 3798, 3798, 3798 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SMPTE2084][1] = { 3780, 3793, 2984 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SMPTE2084][2] = { 3406, 3768, 3791 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SMPTE2084][3] = { 3359, 3763, 2939 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SMPTE2084][4] = { 3636, 2916, 3760 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SMPTE2084][5] = { 3609, 2880, 2661 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SMPTE2084][6] = { 2786, 2633, 3753 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SMPTE2084][7] = { 2563, 2563, 2563 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_709][0] = { 2939, 2939, 2939 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_709][1] = { 2936, 2934, 992 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_709][2] = { 1159, 2890, 2916 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_709][3] = { 1150, 2885, 921 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_709][4] = { 2751, 766, 2837 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_709][5] = { 2747, 747, 650 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_709][6] = { 563, 570, 2812 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_709][7] = { 547, 547, 547 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_SRGB][0] = { 3056, 3056, 3055 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_SRGB][1] = { 3052, 3051, 1237 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_SRGB][2] = { 1397, 3011, 3034 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_SRGB][3] = { 1389, 3006, 1168 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_SRGB][4] = { 2884, 1016, 2962 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_SRGB][5] = { 2880, 998, 902 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_SRGB][6] = { 816, 823, 2940 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_SRGB][7] = { 800, 800, 799 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_ADOBERGB][0] = { 3033, 3033, 3033 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_ADOBERGB][1] = { 3029, 3028, 1255 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_ADOBERGB][2] = { 1406, 2988, 3011 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_ADOBERGB][3] = { 1398, 2983, 1190 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_ADOBERGB][4] = { 2860, 1050, 2939 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_ADOBERGB][5] = { 2857, 1033, 945 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_ADOBERGB][6] = { 866, 873, 2916 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_ADOBERGB][7] = { 851, 851, 851 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_SMPTE240M][0] = { 2926, 2926, 2926 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_SMPTE240M][1] = { 2923, 2921, 957 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_SMPTE240M][2] = { 1125, 2877, 2902 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_SMPTE240M][3] = { 1116, 2871, 885 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_SMPTE240M][4] = { 2736, 729, 2823 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_SMPTE240M][5] = { 2732, 710, 611 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_SMPTE240M][6] = { 523, 531, 2798 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_SMPTE240M][7] = { 507, 507, 507 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_NONE][0] = { 2125, 2125, 2125 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_NONE][1] = { 2120, 2118, 305 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_NONE][2] = { 392, 2056, 2092 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_NONE][3] = { 387, 2049, 271 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_NONE][4] = { 1868, 206, 1983 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_NONE][5] = { 1863, 199, 163 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_NONE][6] = { 135, 137, 1950 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_NONE][7] = { 130, 130, 130 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_DCI_P3][0] = { 3175, 3175, 3175 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_DCI_P3][1] = { 3172, 3170, 1505 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_DCI_P3][2] = { 1657, 3135, 3155 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_DCI_P3][3] = { 1649, 3130, 1439 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_DCI_P3][4] = { 3021, 1294, 3091 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_DCI_P3][5] = { 3018, 1276, 1184 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_DCI_P3][6] = { 1100, 1107, 3071 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_DCI_P3][7] = { 1084, 1084, 1084 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_SMPTE2084][0] = { 3798, 3798, 3798 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_SMPTE2084][1] = { 3797, 3796, 2938 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_SMPTE2084][2] = { 3049, 3783, 3791 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_SMPTE2084][3] = { 3044, 3782, 2887 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_SMPTE2084][4] = { 3741, 2765, 3768 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_SMPTE2084][5] = { 3740, 2749, 2663 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_SMPTE2084][6] = { 2580, 2587, 3760 },
+	[V4L2_COLORSPACE_DCI_P3][V4L2_XFER_FUNC_SMPTE2084][7] = { 2563, 2563, 2563 },
+};
+
+#else
+
+/* This code generates the table above */
+
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+static const double rec709_to_ntsc1953[3][3] = {
+	/*
+	 * This transform uses the Bradford method to compensate for
+	 * the different whitepoints.
+	 */
+	{ 0.6785011, 0.2883441, 0.0331548 },
+	{ 0.0165284, 1.0518725, -0.0684009 },
+	{ 0.0179230, 0.0506096, 0.9314674 }
+};
+
+static const double rec709_to_ebu[3][3] = {
+	{ 0.9578221, 0.0421779, -0.0000000 },
+	{ -0.0000000, 1.0000000, 0.0000000 },
+	{ -0.0000000, -0.0119367, 1.0119367 }
+};
+
+static const double rec709_to_170m[3][3] = {
+	{ 1.0653640, -0.0553900, -0.0099740 },
+	{ -0.0196361, 1.0363630, -0.0167269 },
+	{ 0.0016327, 0.0044133, 0.9939540 },
+};
+
+static const double rec709_to_240m[3][3] = {
+	{ 1.0653640, -0.0553900, -0.0099740 },
+	{ -0.0196361, 1.0363630, -0.0167269 },
+	{ 0.0016327, 0.0044133, 0.9939540 },
+};
+
+static const double rec709_to_adobergb[3][3] = {
+	{ 0.7151627, 0.2848373, -0.0000000 },
+	{ 0.0000000, 1.0000000, 0.0000000 },
+	{ -0.0000000, 0.0411705, 0.9588295 },
+};
+
+static const double rec709_to_bt2020[3][3] = {
+	{ 0.6274524, 0.3292485, 0.0432991 },
+	{ 0.0691092, 0.9195311, 0.0113597 },
+	{ 0.0163976, 0.0880301, 0.8955723 },
+};
+
+static const double rec709_to_dcip3[3][3] = {
+	/*
+	 * This transform uses the Bradford method to compensate for
+	 * the different whitepoints.
+	 */
+	{ 0.8686648, 0.1288456, 0.0024896 },
+	{ 0.0345479, 0.9618084, 0.0036437 },
+	{ 0.0167785, 0.0710559, 0.9121655 }
+};
+
+static void mult_matrix(double *r, double *g, double *b, const double m[3][3])
+{
+	double ir, ig, ib;
+
+	ir = m[0][0] * (*r) + m[0][1] * (*g) + m[0][2] * (*b);
+	ig = m[1][0] * (*r) + m[1][1] * (*g) + m[1][2] * (*b);
+	ib = m[2][0] * (*r) + m[2][1] * (*g) + m[2][2] * (*b);
+	*r = ir;
+	*g = ig;
+	*b = ib;
+}
+
+static double transfer_srgb_to_rgb(double v)
+{
+	if (v < -0.04045)
+		return pow((-v + 0.055) / 1.055, 2.4);
+	return (v <= 0.04045) ? v / 12.92 : pow((v + 0.055) / 1.055, 2.4);
+}
+
+static double transfer_rgb_to_srgb(double v)
+{
+	if (v <= -0.0031308)
+		return -1.055 * pow(-v, 1.0 / 2.4) + 0.055;
+	if (v <= 0.0031308)
+		return v * 12.92;
+	return 1.055 * pow(v, 1.0 / 2.4) - 0.055;
+}
+
+static double transfer_rgb_to_smpte240m(double v)
+{
+	return (v <= 0.0228) ? v * 4.0 : 1.1115 * pow(v, 0.45) - 0.1115;
+}
+
+static double transfer_rgb_to_rec709(double v)
+{
+	if (v <= -0.018)
+		return -1.099 * pow(-v, 0.45) + 0.099;
+	return (v < 0.018) ? v * 4.5 : 1.099 * pow(v, 0.45) - 0.099;
+}
+
+static double transfer_rec709_to_rgb(double v)
+{
+	return (v < 0.081) ? v / 4.5 : pow((v + 0.099) / 1.099, 1.0 / 0.45);
+}
+
+static double transfer_rgb_to_adobergb(double v)
+{
+	return pow(v, 1.0 / 2.19921875);
+}
+
+static double transfer_rgb_to_dcip3(double v)
+{
+	return pow(v, 1.0 / 2.6);
+}
+
+static double transfer_rgb_to_smpte2084(double v)
+{
+	const double m1 = (2610.0 / 4096.0) / 4.0;
+	const double m2 = 128.0 * 2523.0 / 4096.0;
+	const double c1 = 3424.0 / 4096.0;
+	const double c2 = 32.0 * 2413.0 / 4096.0;
+	const double c3 = 32.0 * 2392.0 / 4096.0;
+
+	v = pow(v, m1);
+	return pow((c1 + c2 * v) / (1 + c3 * v), m2);
+}
+
+static double transfer_srgb_to_rec709(double v)
+{
+	return transfer_rgb_to_rec709(transfer_srgb_to_rgb(v));
+}
+
+static void csc(enum v4l2_colorspace colorspace, enum v4l2_xfer_func xfer_func,
+		double *r, double *g, double *b)
+{
+	int clamp = 1;
+
+	*r = transfer_srgb_to_rgb(*r);
+	*g = transfer_srgb_to_rgb(*g);
+	*b = transfer_srgb_to_rgb(*b);
+
+	/* Convert the primaries of Rec. 709 Linear RGB */
+	switch (colorspace) {
+	case V4L2_COLORSPACE_SMPTE240M:
+		mult_matrix(r, g, b, rec709_to_240m);
+		break;
+	case V4L2_COLORSPACE_SMPTE170M:
+		mult_matrix(r, g, b, rec709_to_170m);
+		break;
+	case V4L2_COLORSPACE_470_SYSTEM_BG:
+		mult_matrix(r, g, b, rec709_to_ebu);
+		break;
+	case V4L2_COLORSPACE_470_SYSTEM_M:
+		mult_matrix(r, g, b, rec709_to_ntsc1953);
+		break;
+	case V4L2_COLORSPACE_ADOBERGB:
+		mult_matrix(r, g, b, rec709_to_adobergb);
+		break;
+	case V4L2_COLORSPACE_BT2020:
+		mult_matrix(r, g, b, rec709_to_bt2020);
+		break;
+	case V4L2_COLORSPACE_DCI_P3:
+		mult_matrix(r, g, b, rec709_to_dcip3);
+		break;
+	case V4L2_COLORSPACE_SRGB:
+	case V4L2_COLORSPACE_REC709:
+		break;
+	default:
+		break;
+	}
+
+	if (clamp) {
+		*r = ((*r) < 0) ? 0 : (((*r) > 1) ? 1 : (*r));
+		*g = ((*g) < 0) ? 0 : (((*g) > 1) ? 1 : (*g));
+		*b = ((*b) < 0) ? 0 : (((*b) > 1) ? 1 : (*b));
+	}
+
+	switch (xfer_func) {
+	case V4L2_XFER_FUNC_709:
+		*r = transfer_rgb_to_rec709(*r);
+		*g = transfer_rgb_to_rec709(*g);
+		*b = transfer_rgb_to_rec709(*b);
+		break;
+	case V4L2_XFER_FUNC_SRGB:
+		*r = transfer_rgb_to_srgb(*r);
+		*g = transfer_rgb_to_srgb(*g);
+		*b = transfer_rgb_to_srgb(*b);
+		break;
+	case V4L2_XFER_FUNC_ADOBERGB:
+		*r = transfer_rgb_to_adobergb(*r);
+		*g = transfer_rgb_to_adobergb(*g);
+		*b = transfer_rgb_to_adobergb(*b);
+		break;
+	case V4L2_XFER_FUNC_DCI_P3:
+		*r = transfer_rgb_to_dcip3(*r);
+		*g = transfer_rgb_to_dcip3(*g);
+		*b = transfer_rgb_to_dcip3(*b);
+		break;
+	case V4L2_XFER_FUNC_SMPTE2084:
+		*r = transfer_rgb_to_smpte2084(*r);
+		*g = transfer_rgb_to_smpte2084(*g);
+		*b = transfer_rgb_to_smpte2084(*b);
+		break;
+	case V4L2_XFER_FUNC_SMPTE240M:
+		*r = transfer_rgb_to_smpte240m(*r);
+		*g = transfer_rgb_to_smpte240m(*g);
+		*b = transfer_rgb_to_smpte240m(*b);
+		break;
+	case V4L2_XFER_FUNC_NONE:
+		break;
+	}
+}
+
+int main(int argc, char **argv)
+{
+	static const unsigned colorspaces[] = {
+		0,
+		V4L2_COLORSPACE_SMPTE170M,
+		V4L2_COLORSPACE_SMPTE240M,
+		V4L2_COLORSPACE_REC709,
+		0,
+		V4L2_COLORSPACE_470_SYSTEM_M,
+		V4L2_COLORSPACE_470_SYSTEM_BG,
+		0,
+		V4L2_COLORSPACE_SRGB,
+		V4L2_COLORSPACE_ADOBERGB,
+		V4L2_COLORSPACE_BT2020,
+		0,
+		V4L2_COLORSPACE_DCI_P3,
+	};
+	static const char * const colorspace_names[] = {
+		"",
+		"V4L2_COLORSPACE_SMPTE170M",
+		"V4L2_COLORSPACE_SMPTE240M",
+		"V4L2_COLORSPACE_REC709",
+		"",
+		"V4L2_COLORSPACE_470_SYSTEM_M",
+		"V4L2_COLORSPACE_470_SYSTEM_BG",
+		"",
+		"V4L2_COLORSPACE_SRGB",
+		"V4L2_COLORSPACE_ADOBERGB",
+		"V4L2_COLORSPACE_BT2020",
+		"",
+		"V4L2_COLORSPACE_DCI_P3",
+	};
+	static const char * const xfer_func_names[] = {
+		"",
+		"V4L2_XFER_FUNC_709",
+		"V4L2_XFER_FUNC_SRGB",
+		"V4L2_XFER_FUNC_ADOBERGB",
+		"V4L2_XFER_FUNC_SMPTE240M",
+		"V4L2_XFER_FUNC_NONE",
+		"V4L2_XFER_FUNC_DCI_P3",
+		"V4L2_XFER_FUNC_SMPTE2084",
+	};
+	int i;
+	int x;
+	int c;
+
+	printf("/* Generated table */\n");
+	printf("const unsigned short tpg_rec709_to_linear[255 * 16 + 1] = {");
+	for (i = 0; i <= 255 * 16; i++) {
+		if (i % 16 == 0)
+			printf("\n\t");
+		printf("%4d,%s",
+			(int)(0.5 + 16.0 * 255.0 *
+				transfer_rec709_to_rgb(i / (16.0 * 255.0))),
+			i % 16 == 15 || i == 255 * 16 ? "" : " ");
+	}
+	printf("\n};\n\n");
+
+	printf("/* Generated table */\n");
+	printf("const unsigned short tpg_linear_to_rec709[255 * 16 + 1] = {");
+	for (i = 0; i <= 255 * 16; i++) {
+		if (i % 16 == 0)
+			printf("\n\t");
+		printf("%4d,%s",
+			(int)(0.5 + 16.0 * 255.0 *
+				transfer_rgb_to_rec709(i / (16.0 * 255.0))),
+			i % 16 == 15 || i == 255 * 16 ? "" : " ");
+	}
+	printf("\n};\n\n");
+
+	printf("/* Generated table */\n");
+	printf("const struct color16 tpg_csc_colors[V4L2_COLORSPACE_DCI_P3 + 1][V4L2_XFER_FUNC_SMPTE2084 + 1][TPG_COLOR_CSC_BLACK + 1] = {\n");
+	for (c = 0; c <= V4L2_COLORSPACE_DCI_P3; c++) {
+		for (x = 1; x <= V4L2_XFER_FUNC_SMPTE2084; x++) {
+			for (i = 0; i <= TPG_COLOR_CSC_BLACK; i++) {
+				double r, g, b;
+
+				if (colorspaces[c] == 0)
+					continue;
+
+				r = tpg_colors[i].r / 255.0;
+				g = tpg_colors[i].g / 255.0;
+				b = tpg_colors[i].b / 255.0;
+
+				csc(c, x, &r, &g, &b);
+
+				printf("\t[%s][%s][%d] = { %d, %d, %d },\n",
+					colorspace_names[c],
+					xfer_func_names[x], i,
+					(int)(r * 4080), (int)(g * 4080), (int)(b * 4080));
+			}
+		}
+	}
+	printf("};\n\n");
+	return 0;
+}
+
+#endif
diff --git a/drivers/media/platform/vivid/vivid-tpg-colors.h b/drivers/media/platform/vivid/vivid-tpg-colors.h
new file mode 100644
index 0000000..4e5a76a
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-tpg-colors.h
@@ -0,0 +1,68 @@
+/*
+ * vivid-color.h - Color definitions for the test pattern generator
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VIVID_COLORS_H_
+#define _VIVID_COLORS_H_
+
+struct color {
+	unsigned char r, g, b;
+};
+
+struct color16 {
+	int r, g, b;
+};
+
+enum tpg_color {
+	TPG_COLOR_CSC_WHITE,
+	TPG_COLOR_CSC_YELLOW,
+	TPG_COLOR_CSC_CYAN,
+	TPG_COLOR_CSC_GREEN,
+	TPG_COLOR_CSC_MAGENTA,
+	TPG_COLOR_CSC_RED,
+	TPG_COLOR_CSC_BLUE,
+	TPG_COLOR_CSC_BLACK,
+	TPG_COLOR_75_YELLOW,
+	TPG_COLOR_75_CYAN,
+	TPG_COLOR_75_GREEN,
+	TPG_COLOR_75_MAGENTA,
+	TPG_COLOR_75_RED,
+	TPG_COLOR_75_BLUE,
+	TPG_COLOR_100_WHITE,
+	TPG_COLOR_100_YELLOW,
+	TPG_COLOR_100_CYAN,
+	TPG_COLOR_100_GREEN,
+	TPG_COLOR_100_MAGENTA,
+	TPG_COLOR_100_RED,
+	TPG_COLOR_100_BLUE,
+	TPG_COLOR_100_BLACK,
+	TPG_COLOR_TEXTFG,
+	TPG_COLOR_TEXTBG,
+	TPG_COLOR_RANDOM,
+	TPG_COLOR_RAMP,
+	TPG_COLOR_MAX = TPG_COLOR_RAMP + 256
+};
+
+extern const struct color tpg_colors[TPG_COLOR_MAX];
+extern const unsigned short tpg_rec709_to_linear[255 * 16 + 1];
+extern const unsigned short tpg_linear_to_rec709[255 * 16 + 1];
+extern const struct color16 tpg_csc_colors[V4L2_COLORSPACE_DCI_P3 + 1]
+					  [V4L2_XFER_FUNC_SMPTE2084 + 1]
+					  [TPG_COLOR_CSC_BLACK + 1];
+
+#endif
diff --git a/drivers/media/platform/vivid/vivid-tpg.c b/drivers/media/platform/vivid/vivid-tpg.c
new file mode 100644
index 0000000..1425614
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-tpg.c
@@ -0,0 +1,2282 @@
+/*
+ * vivid-tpg.c - Test Pattern Generator
+ *
+ * Note: gen_twopix and tpg_gen_text are based on code from vivi.c. See the
+ * vivi.c source for the copyright information of those functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include "vivid-tpg.h"
+
+/* Must remain in sync with enum tpg_pattern */
+const char * const tpg_pattern_strings[] = {
+	"75% Colorbar",
+	"100% Colorbar",
+	"CSC Colorbar",
+	"Horizontal 100% Colorbar",
+	"100% Color Squares",
+	"100% Black",
+	"100% White",
+	"100% Red",
+	"100% Green",
+	"100% Blue",
+	"16x16 Checkers",
+	"2x2 Checkers",
+	"1x1 Checkers",
+	"2x2 Red/Green Checkers",
+	"1x1 Red/Green Checkers",
+	"Alternating Hor Lines",
+	"Alternating Vert Lines",
+	"One Pixel Wide Cross",
+	"Two Pixels Wide Cross",
+	"Ten Pixels Wide Cross",
+	"Gray Ramp",
+	"Noise",
+	NULL
+};
+
+/* Must remain in sync with enum tpg_aspect */
+const char * const tpg_aspect_strings[] = {
+	"Source Width x Height",
+	"4x3",
+	"14x9",
+	"16x9",
+	"16x9 Anamorphic",
+	NULL
+};
+
+/*
+ * Sine table: sin[0] = 127 * sin(-180 degrees)
+ *             sin[128] = 127 * sin(0 degrees)
+ *             sin[256] = 127 * sin(180 degrees)
+ */
+static const s8 sin[257] = {
+	   0,   -4,   -7,  -11,  -13,  -18,  -20,  -22,  -26,  -29,  -33,  -35,  -37,  -41,  -43,  -48,
+	 -50,  -52,  -56,  -58,  -62,  -63,  -65,  -69,  -71,  -75,  -76,  -78,  -82,  -83,  -87,  -88,
+	 -90,  -93,  -94,  -97,  -99, -101, -103, -104, -107, -108, -110, -111, -112, -114, -115, -117,
+	-118, -119, -120, -121, -122, -123, -123, -124, -125, -125, -126, -126, -127, -127, -127, -127,
+	-127, -127, -127, -127, -126, -126, -125, -125, -124, -124, -123, -122, -121, -120, -119, -118,
+	-117, -116, -114, -113, -111, -110, -109, -107, -105, -103, -101, -100,  -97,  -96,  -93,  -91,
+	 -90,  -87,  -85,  -82,  -80,  -76,  -75,  -73,  -69,  -67,  -63,  -62,  -60,  -56,  -54,  -50,
+	 -48,  -46,  -41,  -39,  -35,  -33,  -31,  -26,  -24,  -20,  -18,  -15,  -11,   -9,   -4,   -2,
+	   0,    2,    4,    9,   11,   15,   18,   20,   24,   26,   31,   33,   35,   39,   41,   46,
+	  48,   50,   54,   56,   60,   62,   64,   67,   69,   73,   75,   76,   80,   82,   85,   87,
+	  90,   91,   93,   96,   97,  100,  101,  103,  105,  107,  109,  110,  111,  113,  114,  116,
+	 117,  118,  119,  120,  121,  122,  123,  124,  124,  125,  125,  126,  126,  127,  127,  127,
+	 127,  127,  127,  127,  127,  126,  126,  125,  125,  124,  123,  123,  122,  121,  120,  119,
+	 118,  117,  115,  114,  112,  111,  110,  108,  107,  104,  103,  101,   99,   97,   94,   93,
+	  90,   88,   87,   83,   82,   78,   76,   75,   71,   69,   65,   64,   62,   58,   56,   52,
+	  50,   48,   43,   41,   37,   35,   33,   29,   26,   22,   20,   18,   13,   11,    7,    4,
+	   0,
+};
+
+#define cos(idx) sin[((idx) + 64) % sizeof(sin)]
+
+/* Global font descriptor */
+static const u8 *font8x16;
+
+void tpg_set_font(const u8 *f)
+{
+	font8x16 = f;
+}
+
+void tpg_init(struct tpg_data *tpg, unsigned w, unsigned h)
+{
+	memset(tpg, 0, sizeof(*tpg));
+	tpg->scaled_width = tpg->src_width = w;
+	tpg->src_height = tpg->buf_height = h;
+	tpg->crop.width = tpg->compose.width = w;
+	tpg->crop.height = tpg->compose.height = h;
+	tpg->recalc_colors = true;
+	tpg->recalc_square_border = true;
+	tpg->brightness = 128;
+	tpg->contrast = 128;
+	tpg->saturation = 128;
+	tpg->hue = 0;
+	tpg->mv_hor_mode = TPG_MOVE_NONE;
+	tpg->mv_vert_mode = TPG_MOVE_NONE;
+	tpg->field = V4L2_FIELD_NONE;
+	tpg_s_fourcc(tpg, V4L2_PIX_FMT_RGB24);
+	tpg->colorspace = V4L2_COLORSPACE_SRGB;
+	tpg->perc_fill = 100;
+}
+
+int tpg_alloc(struct tpg_data *tpg, unsigned max_w)
+{
+	unsigned pat;
+	unsigned plane;
+
+	tpg->max_line_width = max_w;
+	for (pat = 0; pat < TPG_MAX_PAT_LINES; pat++) {
+		for (plane = 0; plane < TPG_MAX_PLANES; plane++) {
+			unsigned pixelsz = plane ? 2 : 4;
+
+			tpg->lines[pat][plane] = vzalloc(max_w * 2 * pixelsz);
+			if (!tpg->lines[pat][plane])
+				return -ENOMEM;
+			if (plane == 0)
+				continue;
+			tpg->downsampled_lines[pat][plane] = vzalloc(max_w * 2 * pixelsz);
+			if (!tpg->downsampled_lines[pat][plane])
+				return -ENOMEM;
+		}
+	}
+	for (plane = 0; plane < TPG_MAX_PLANES; plane++) {
+		unsigned pixelsz = plane ? 2 : 4;
+
+		tpg->contrast_line[plane] = vzalloc(max_w * pixelsz);
+		if (!tpg->contrast_line[plane])
+			return -ENOMEM;
+		tpg->black_line[plane] = vzalloc(max_w * pixelsz);
+		if (!tpg->black_line[plane])
+			return -ENOMEM;
+		tpg->random_line[plane] = vzalloc(max_w * 2 * pixelsz);
+		if (!tpg->random_line[plane])
+			return -ENOMEM;
+	}
+	return 0;
+}
+
+void tpg_free(struct tpg_data *tpg)
+{
+	unsigned pat;
+	unsigned plane;
+
+	for (pat = 0; pat < TPG_MAX_PAT_LINES; pat++)
+		for (plane = 0; plane < TPG_MAX_PLANES; plane++) {
+			vfree(tpg->lines[pat][plane]);
+			tpg->lines[pat][plane] = NULL;
+			if (plane == 0)
+				continue;
+			vfree(tpg->downsampled_lines[pat][plane]);
+			tpg->downsampled_lines[pat][plane] = NULL;
+		}
+	for (plane = 0; plane < TPG_MAX_PLANES; plane++) {
+		vfree(tpg->contrast_line[plane]);
+		vfree(tpg->black_line[plane]);
+		vfree(tpg->random_line[plane]);
+		tpg->contrast_line[plane] = NULL;
+		tpg->black_line[plane] = NULL;
+		tpg->random_line[plane] = NULL;
+	}
+}
+
+bool tpg_s_fourcc(struct tpg_data *tpg, u32 fourcc)
+{
+	tpg->fourcc = fourcc;
+	tpg->planes = 1;
+	tpg->buffers = 1;
+	tpg->recalc_colors = true;
+	tpg->interleaved = false;
+	tpg->vdownsampling[0] = 1;
+	tpg->hdownsampling[0] = 1;
+	tpg->hmask[0] = ~0;
+	tpg->hmask[1] = ~0;
+	tpg->hmask[2] = ~0;
+
+	switch (fourcc) {
+	case V4L2_PIX_FMT_SBGGR8:
+	case V4L2_PIX_FMT_SGBRG8:
+	case V4L2_PIX_FMT_SGRBG8:
+	case V4L2_PIX_FMT_SRGGB8:
+	case V4L2_PIX_FMT_SBGGR10:
+	case V4L2_PIX_FMT_SGBRG10:
+	case V4L2_PIX_FMT_SGRBG10:
+	case V4L2_PIX_FMT_SRGGB10:
+	case V4L2_PIX_FMT_SBGGR12:
+	case V4L2_PIX_FMT_SGBRG12:
+	case V4L2_PIX_FMT_SGRBG12:
+	case V4L2_PIX_FMT_SRGGB12:
+		tpg->interleaved = true;
+		tpg->vdownsampling[1] = 1;
+		tpg->hdownsampling[1] = 1;
+		tpg->planes = 2;
+		/* fall through */
+	case V4L2_PIX_FMT_RGB332:
+	case V4L2_PIX_FMT_RGB565:
+	case V4L2_PIX_FMT_RGB565X:
+	case V4L2_PIX_FMT_RGB444:
+	case V4L2_PIX_FMT_XRGB444:
+	case V4L2_PIX_FMT_ARGB444:
+	case V4L2_PIX_FMT_RGB555:
+	case V4L2_PIX_FMT_XRGB555:
+	case V4L2_PIX_FMT_ARGB555:
+	case V4L2_PIX_FMT_RGB555X:
+	case V4L2_PIX_FMT_XRGB555X:
+	case V4L2_PIX_FMT_ARGB555X:
+	case V4L2_PIX_FMT_BGR666:
+	case V4L2_PIX_FMT_RGB24:
+	case V4L2_PIX_FMT_BGR24:
+	case V4L2_PIX_FMT_RGB32:
+	case V4L2_PIX_FMT_BGR32:
+	case V4L2_PIX_FMT_XRGB32:
+	case V4L2_PIX_FMT_XBGR32:
+	case V4L2_PIX_FMT_ARGB32:
+	case V4L2_PIX_FMT_ABGR32:
+	case V4L2_PIX_FMT_GREY:
+	case V4L2_PIX_FMT_Y16:
+	case V4L2_PIX_FMT_Y16_BE:
+		tpg->is_yuv = false;
+		break;
+	case V4L2_PIX_FMT_YUV444:
+	case V4L2_PIX_FMT_YUV555:
+	case V4L2_PIX_FMT_YUV565:
+	case V4L2_PIX_FMT_YUV32:
+		tpg->is_yuv = true;
+		break;
+	case V4L2_PIX_FMT_YUV420M:
+	case V4L2_PIX_FMT_YVU420M:
+		tpg->buffers = 3;
+		/* fall through */
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_YVU420:
+		tpg->vdownsampling[1] = 2;
+		tpg->vdownsampling[2] = 2;
+		tpg->hdownsampling[1] = 2;
+		tpg->hdownsampling[2] = 2;
+		tpg->planes = 3;
+		tpg->is_yuv = true;
+		break;
+	case V4L2_PIX_FMT_YUV422P:
+		tpg->vdownsampling[1] = 1;
+		tpg->vdownsampling[2] = 1;
+		tpg->hdownsampling[1] = 2;
+		tpg->hdownsampling[2] = 2;
+		tpg->planes = 3;
+		tpg->is_yuv = true;
+		break;
+	case V4L2_PIX_FMT_NV16M:
+	case V4L2_PIX_FMT_NV61M:
+		tpg->buffers = 2;
+		/* fall through */
+	case V4L2_PIX_FMT_NV16:
+	case V4L2_PIX_FMT_NV61:
+		tpg->vdownsampling[1] = 1;
+		tpg->hdownsampling[1] = 1;
+		tpg->hmask[1] = ~1;
+		tpg->planes = 2;
+		tpg->is_yuv = true;
+		break;
+	case V4L2_PIX_FMT_NV12M:
+	case V4L2_PIX_FMT_NV21M:
+		tpg->buffers = 2;
+		/* fall through */
+	case V4L2_PIX_FMT_NV12:
+	case V4L2_PIX_FMT_NV21:
+		tpg->vdownsampling[1] = 2;
+		tpg->hdownsampling[1] = 1;
+		tpg->hmask[1] = ~1;
+		tpg->planes = 2;
+		tpg->is_yuv = true;
+		break;
+	case V4L2_PIX_FMT_NV24:
+	case V4L2_PIX_FMT_NV42:
+		tpg->vdownsampling[1] = 1;
+		tpg->hdownsampling[1] = 1;
+		tpg->planes = 2;
+		tpg->is_yuv = true;
+		break;
+	case V4L2_PIX_FMT_YUYV:
+	case V4L2_PIX_FMT_UYVY:
+	case V4L2_PIX_FMT_YVYU:
+	case V4L2_PIX_FMT_VYUY:
+		tpg->hmask[0] = ~1;
+		tpg->is_yuv = true;
+		break;
+	default:
+		return false;
+	}
+
+	switch (fourcc) {
+	case V4L2_PIX_FMT_GREY:
+	case V4L2_PIX_FMT_RGB332:
+		tpg->twopixelsize[0] = 2;
+		break;
+	case V4L2_PIX_FMT_RGB565:
+	case V4L2_PIX_FMT_RGB565X:
+	case V4L2_PIX_FMT_RGB444:
+	case V4L2_PIX_FMT_XRGB444:
+	case V4L2_PIX_FMT_ARGB444:
+	case V4L2_PIX_FMT_RGB555:
+	case V4L2_PIX_FMT_XRGB555:
+	case V4L2_PIX_FMT_ARGB555:
+	case V4L2_PIX_FMT_RGB555X:
+	case V4L2_PIX_FMT_XRGB555X:
+	case V4L2_PIX_FMT_ARGB555X:
+	case V4L2_PIX_FMT_YUYV:
+	case V4L2_PIX_FMT_UYVY:
+	case V4L2_PIX_FMT_YVYU:
+	case V4L2_PIX_FMT_VYUY:
+	case V4L2_PIX_FMT_YUV444:
+	case V4L2_PIX_FMT_YUV555:
+	case V4L2_PIX_FMT_YUV565:
+	case V4L2_PIX_FMT_Y16:
+	case V4L2_PIX_FMT_Y16_BE:
+		tpg->twopixelsize[0] = 2 * 2;
+		break;
+	case V4L2_PIX_FMT_RGB24:
+	case V4L2_PIX_FMT_BGR24:
+		tpg->twopixelsize[0] = 2 * 3;
+		break;
+	case V4L2_PIX_FMT_BGR666:
+	case V4L2_PIX_FMT_RGB32:
+	case V4L2_PIX_FMT_BGR32:
+	case V4L2_PIX_FMT_XRGB32:
+	case V4L2_PIX_FMT_XBGR32:
+	case V4L2_PIX_FMT_ARGB32:
+	case V4L2_PIX_FMT_ABGR32:
+	case V4L2_PIX_FMT_YUV32:
+		tpg->twopixelsize[0] = 2 * 4;
+		break;
+	case V4L2_PIX_FMT_NV12:
+	case V4L2_PIX_FMT_NV21:
+	case V4L2_PIX_FMT_NV12M:
+	case V4L2_PIX_FMT_NV21M:
+	case V4L2_PIX_FMT_NV16:
+	case V4L2_PIX_FMT_NV61:
+	case V4L2_PIX_FMT_NV16M:
+	case V4L2_PIX_FMT_NV61M:
+	case V4L2_PIX_FMT_SBGGR8:
+	case V4L2_PIX_FMT_SGBRG8:
+	case V4L2_PIX_FMT_SGRBG8:
+	case V4L2_PIX_FMT_SRGGB8:
+		tpg->twopixelsize[0] = 2;
+		tpg->twopixelsize[1] = 2;
+		break;
+	case V4L2_PIX_FMT_SRGGB10:
+	case V4L2_PIX_FMT_SGRBG10:
+	case V4L2_PIX_FMT_SGBRG10:
+	case V4L2_PIX_FMT_SBGGR10:
+	case V4L2_PIX_FMT_SRGGB12:
+	case V4L2_PIX_FMT_SGRBG12:
+	case V4L2_PIX_FMT_SGBRG12:
+	case V4L2_PIX_FMT_SBGGR12:
+		tpg->twopixelsize[0] = 4;
+		tpg->twopixelsize[1] = 4;
+		break;
+	case V4L2_PIX_FMT_YUV422P:
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_YVU420:
+	case V4L2_PIX_FMT_YUV420M:
+	case V4L2_PIX_FMT_YVU420M:
+		tpg->twopixelsize[0] = 2;
+		tpg->twopixelsize[1] = 2;
+		tpg->twopixelsize[2] = 2;
+		break;
+	case V4L2_PIX_FMT_NV24:
+	case V4L2_PIX_FMT_NV42:
+		tpg->twopixelsize[0] = 2;
+		tpg->twopixelsize[1] = 4;
+		break;
+	}
+	return true;
+}
+
+void tpg_s_crop_compose(struct tpg_data *tpg, const struct v4l2_rect *crop,
+		const struct v4l2_rect *compose)
+{
+	tpg->crop = *crop;
+	tpg->compose = *compose;
+	tpg->scaled_width = (tpg->src_width * tpg->compose.width +
+				 tpg->crop.width - 1) / tpg->crop.width;
+	tpg->scaled_width &= ~1;
+	if (tpg->scaled_width > tpg->max_line_width)
+		tpg->scaled_width = tpg->max_line_width;
+	if (tpg->scaled_width < 2)
+		tpg->scaled_width = 2;
+	tpg->recalc_lines = true;
+}
+
+void tpg_reset_source(struct tpg_data *tpg, unsigned width, unsigned height,
+		       u32 field)
+{
+	unsigned p;
+
+	tpg->src_width = width;
+	tpg->src_height = height;
+	tpg->field = field;
+	tpg->buf_height = height;
+	if (V4L2_FIELD_HAS_T_OR_B(field))
+		tpg->buf_height /= 2;
+	tpg->scaled_width = width;
+	tpg->crop.top = tpg->crop.left = 0;
+	tpg->crop.width = width;
+	tpg->crop.height = height;
+	tpg->compose.top = tpg->compose.left = 0;
+	tpg->compose.width = width;
+	tpg->compose.height = tpg->buf_height;
+	for (p = 0; p < tpg->planes; p++)
+		tpg->bytesperline[p] = (width * tpg->twopixelsize[p]) /
+				       (2 * tpg->hdownsampling[p]);
+	tpg->recalc_square_border = true;
+}
+
+static enum tpg_color tpg_get_textbg_color(struct tpg_data *tpg)
+{
+	switch (tpg->pattern) {
+	case TPG_PAT_BLACK:
+		return TPG_COLOR_100_WHITE;
+	case TPG_PAT_CSC_COLORBAR:
+		return TPG_COLOR_CSC_BLACK;
+	default:
+		return TPG_COLOR_100_BLACK;
+	}
+}
+
+static enum tpg_color tpg_get_textfg_color(struct tpg_data *tpg)
+{
+	switch (tpg->pattern) {
+	case TPG_PAT_75_COLORBAR:
+	case TPG_PAT_CSC_COLORBAR:
+		return TPG_COLOR_CSC_WHITE;
+	case TPG_PAT_BLACK:
+		return TPG_COLOR_100_BLACK;
+	default:
+		return TPG_COLOR_100_WHITE;
+	}
+}
+
+static inline int rec709_to_linear(int v)
+{
+	v = clamp(v, 0, 0xff0);
+	return tpg_rec709_to_linear[v];
+}
+
+static inline int linear_to_rec709(int v)
+{
+	v = clamp(v, 0, 0xff0);
+	return tpg_linear_to_rec709[v];
+}
+
+static void rgb2ycbcr(const int m[3][3], int r, int g, int b,
+			int y_offset, int *y, int *cb, int *cr)
+{
+	*y  = ((m[0][0] * r + m[0][1] * g + m[0][2] * b) >> 16) + (y_offset << 4);
+	*cb = ((m[1][0] * r + m[1][1] * g + m[1][2] * b) >> 16) + (128 << 4);
+	*cr = ((m[2][0] * r + m[2][1] * g + m[2][2] * b) >> 16) + (128 << 4);
+}
+
+static void color_to_ycbcr(struct tpg_data *tpg, int r, int g, int b,
+			   int *y, int *cb, int *cr)
+{
+#define COEFF(v, r) ((int)(0.5 + (v) * (r) * 256.0))
+
+	static const int bt601[3][3] = {
+		{ COEFF(0.299, 219),  COEFF(0.587, 219),  COEFF(0.114, 219)  },
+		{ COEFF(-0.169, 224), COEFF(-0.331, 224), COEFF(0.5, 224)    },
+		{ COEFF(0.5, 224),    COEFF(-0.419, 224), COEFF(-0.081, 224) },
+	};
+	static const int bt601_full[3][3] = {
+		{ COEFF(0.299, 255),  COEFF(0.587, 255),  COEFF(0.114, 255)  },
+		{ COEFF(-0.169, 255), COEFF(-0.331, 255), COEFF(0.5, 255)    },
+		{ COEFF(0.5, 255),    COEFF(-0.419, 255), COEFF(-0.081, 255) },
+	};
+	static const int rec709[3][3] = {
+		{ COEFF(0.2126, 219),  COEFF(0.7152, 219),  COEFF(0.0722, 219)  },
+		{ COEFF(-0.1146, 224), COEFF(-0.3854, 224), COEFF(0.5, 224)     },
+		{ COEFF(0.5, 224),     COEFF(-0.4542, 224), COEFF(-0.0458, 224) },
+	};
+	static const int rec709_full[3][3] = {
+		{ COEFF(0.2126, 255),  COEFF(0.7152, 255),  COEFF(0.0722, 255)  },
+		{ COEFF(-0.1146, 255), COEFF(-0.3854, 255), COEFF(0.5, 255)     },
+		{ COEFF(0.5, 255),     COEFF(-0.4542, 255), COEFF(-0.0458, 255) },
+	};
+	static const int smpte240m[3][3] = {
+		{ COEFF(0.212, 219),  COEFF(0.701, 219),  COEFF(0.087, 219)  },
+		{ COEFF(-0.116, 224), COEFF(-0.384, 224), COEFF(0.5, 224)    },
+		{ COEFF(0.5, 224),    COEFF(-0.445, 224), COEFF(-0.055, 224) },
+	};
+	static const int smpte240m_full[3][3] = {
+		{ COEFF(0.212, 255),  COEFF(0.701, 255),  COEFF(0.087, 255)  },
+		{ COEFF(-0.116, 255), COEFF(-0.384, 255), COEFF(0.5, 255)    },
+		{ COEFF(0.5, 255),    COEFF(-0.445, 255), COEFF(-0.055, 255) },
+	};
+	static const int bt2020[3][3] = {
+		{ COEFF(0.2627, 219),  COEFF(0.6780, 219),  COEFF(0.0593, 219)  },
+		{ COEFF(-0.1396, 224), COEFF(-0.3604, 224), COEFF(0.5, 224)     },
+		{ COEFF(0.5, 224),     COEFF(-0.4598, 224), COEFF(-0.0402, 224) },
+	};
+	static const int bt2020_full[3][3] = {
+		{ COEFF(0.2627, 255),  COEFF(0.6780, 255),  COEFF(0.0593, 255)  },
+		{ COEFF(-0.1396, 255), COEFF(-0.3604, 255), COEFF(0.5, 255)     },
+		{ COEFF(0.5, 255),     COEFF(-0.4698, 255), COEFF(-0.0402, 255) },
+	};
+	static const int bt2020c[4] = {
+		COEFF(1.0 / 1.9404, 224), COEFF(1.0 / 1.5816, 224),
+		COEFF(1.0 / 1.7184, 224), COEFF(1.0 / 0.9936, 224),
+	};
+	static const int bt2020c_full[4] = {
+		COEFF(1.0 / 1.9404, 255), COEFF(1.0 / 1.5816, 255),
+		COEFF(1.0 / 1.7184, 255), COEFF(1.0 / 0.9936, 255),
+	};
+
+	bool full = tpg->real_quantization == V4L2_QUANTIZATION_FULL_RANGE;
+	unsigned y_offset = full ? 0 : 16;
+	int lin_y, yc;
+
+	switch (tpg->real_ycbcr_enc) {
+	case V4L2_YCBCR_ENC_601:
+	case V4L2_YCBCR_ENC_SYCC:
+		rgb2ycbcr(full ? bt601_full : bt601, r, g, b, y_offset, y, cb, cr);
+		break;
+	case V4L2_YCBCR_ENC_XV601:
+		/* Ignore quantization range, there is only one possible
+		 * Y'CbCr encoding. */
+		rgb2ycbcr(bt601, r, g, b, 16, y, cb, cr);
+		break;
+	case V4L2_YCBCR_ENC_XV709:
+		/* Ignore quantization range, there is only one possible
+		 * Y'CbCr encoding. */
+		rgb2ycbcr(rec709, r, g, b, 16, y, cb, cr);
+		break;
+	case V4L2_YCBCR_ENC_BT2020:
+		rgb2ycbcr(full ? bt2020_full : bt2020, r, g, b, y_offset, y, cb, cr);
+		break;
+	case V4L2_YCBCR_ENC_BT2020_CONST_LUM:
+		lin_y = (COEFF(0.2627, 255) * rec709_to_linear(r) +
+			 COEFF(0.6780, 255) * rec709_to_linear(g) +
+			 COEFF(0.0593, 255) * rec709_to_linear(b)) >> 16;
+		yc = linear_to_rec709(lin_y);
+		*y = full ? yc : (yc * 219) / 255 + (16 << 4);
+		if (b <= yc)
+			*cb = (((b - yc) * (full ? bt2020c_full[0] : bt2020c[0])) >> 16) + (128 << 4);
+		else
+			*cb = (((b - yc) * (full ? bt2020c_full[1] : bt2020c[1])) >> 16) + (128 << 4);
+		if (r <= yc)
+			*cr = (((r - yc) * (full ? bt2020c_full[2] : bt2020c[2])) >> 16) + (128 << 4);
+		else
+			*cr = (((r - yc) * (full ? bt2020c_full[3] : bt2020c[3])) >> 16) + (128 << 4);
+		break;
+	case V4L2_YCBCR_ENC_SMPTE240M:
+		rgb2ycbcr(full ? smpte240m_full : smpte240m, r, g, b, y_offset, y, cb, cr);
+		break;
+	case V4L2_YCBCR_ENC_709:
+	default:
+		rgb2ycbcr(full ? rec709_full : rec709, r, g, b, y_offset, y, cb, cr);
+		break;
+	}
+}
+
+static void ycbcr2rgb(const int m[3][3], int y, int cb, int cr,
+			int y_offset, int *r, int *g, int *b)
+{
+	y -= y_offset << 4;
+	cb -= 128 << 4;
+	cr -= 128 << 4;
+	*r = m[0][0] * y + m[0][1] * cb + m[0][2] * cr;
+	*g = m[1][0] * y + m[1][1] * cb + m[1][2] * cr;
+	*b = m[2][0] * y + m[2][1] * cb + m[2][2] * cr;
+	*r = clamp(*r >> 12, 0, 0xff0);
+	*g = clamp(*g >> 12, 0, 0xff0);
+	*b = clamp(*b >> 12, 0, 0xff0);
+}
+
+static void ycbcr_to_color(struct tpg_data *tpg, int y, int cb, int cr,
+			   int *r, int *g, int *b)
+{
+#undef COEFF
+#define COEFF(v, r) ((int)(0.5 + (v) * ((255.0 * 255.0 * 16.0) / (r))))
+	static const int bt601[3][3] = {
+		{ COEFF(1, 219), COEFF(0, 224),       COEFF(1.4020, 224)  },
+		{ COEFF(1, 219), COEFF(-0.3441, 224), COEFF(-0.7141, 224) },
+		{ COEFF(1, 219), COEFF(1.7720, 224),  COEFF(0, 224)       },
+	};
+	static const int bt601_full[3][3] = {
+		{ COEFF(1, 255), COEFF(0, 255),       COEFF(1.4020, 255)  },
+		{ COEFF(1, 255), COEFF(-0.3441, 255), COEFF(-0.7141, 255) },
+		{ COEFF(1, 255), COEFF(1.7720, 255),  COEFF(0, 255)       },
+	};
+	static const int rec709[3][3] = {
+		{ COEFF(1, 219), COEFF(0, 224),       COEFF(1.5748, 224)  },
+		{ COEFF(1, 219), COEFF(-0.1873, 224), COEFF(-0.4681, 224) },
+		{ COEFF(1, 219), COEFF(1.8556, 224),  COEFF(0, 224)       },
+	};
+	static const int rec709_full[3][3] = {
+		{ COEFF(1, 255), COEFF(0, 255),       COEFF(1.5748, 255)  },
+		{ COEFF(1, 255), COEFF(-0.1873, 255), COEFF(-0.4681, 255) },
+		{ COEFF(1, 255), COEFF(1.8556, 255),  COEFF(0, 255)       },
+	};
+	static const int smpte240m[3][3] = {
+		{ COEFF(1, 219), COEFF(0, 224),       COEFF(1.5756, 224)  },
+		{ COEFF(1, 219), COEFF(-0.2253, 224), COEFF(-0.4767, 224) },
+		{ COEFF(1, 219), COEFF(1.8270, 224),  COEFF(0, 224)       },
+	};
+	static const int smpte240m_full[3][3] = {
+		{ COEFF(1, 255), COEFF(0, 255),       COEFF(1.5756, 255)  },
+		{ COEFF(1, 255), COEFF(-0.2253, 255), COEFF(-0.4767, 255) },
+		{ COEFF(1, 255), COEFF(1.8270, 255),  COEFF(0, 255)       },
+	};
+	static const int bt2020[3][3] = {
+		{ COEFF(1, 219), COEFF(0, 224),       COEFF(1.4746, 224)  },
+		{ COEFF(1, 219), COEFF(-0.1646, 224), COEFF(-0.5714, 224) },
+		{ COEFF(1, 219), COEFF(1.8814, 224),  COEFF(0, 224)       },
+	};
+	static const int bt2020_full[3][3] = {
+		{ COEFF(1, 255), COEFF(0, 255),       COEFF(1.4746, 255)  },
+		{ COEFF(1, 255), COEFF(-0.1646, 255), COEFF(-0.5714, 255) },
+		{ COEFF(1, 255), COEFF(1.8814, 255),  COEFF(0, 255)       },
+	};
+	static const int bt2020c[4] = {
+		COEFF(1.9404, 224), COEFF(1.5816, 224),
+		COEFF(1.7184, 224), COEFF(0.9936, 224),
+	};
+	static const int bt2020c_full[4] = {
+		COEFF(1.9404, 255), COEFF(1.5816, 255),
+		COEFF(1.7184, 255), COEFF(0.9936, 255),
+	};
+
+	bool full = tpg->real_quantization == V4L2_QUANTIZATION_FULL_RANGE;
+	unsigned y_offset = full ? 0 : 16;
+	int y_fac = full ? COEFF(1.0, 255) : COEFF(1.0, 219);
+	int lin_r, lin_g, lin_b, lin_y;
+
+	switch (tpg->real_ycbcr_enc) {
+	case V4L2_YCBCR_ENC_601:
+	case V4L2_YCBCR_ENC_SYCC:
+		ycbcr2rgb(full ? bt601_full : bt601, y, cb, cr, y_offset, r, g, b);
+		break;
+	case V4L2_YCBCR_ENC_XV601:
+		/* Ignore quantization range, there is only one possible
+		 * Y'CbCr encoding. */
+		ycbcr2rgb(bt601, y, cb, cr, 16, r, g, b);
+		break;
+	case V4L2_YCBCR_ENC_XV709:
+		/* Ignore quantization range, there is only one possible
+		 * Y'CbCr encoding. */
+		ycbcr2rgb(rec709, y, cb, cr, 16, r, g, b);
+		break;
+	case V4L2_YCBCR_ENC_BT2020:
+		ycbcr2rgb(full ? bt2020_full : bt2020, y, cb, cr, y_offset, r, g, b);
+		break;
+	case V4L2_YCBCR_ENC_BT2020_CONST_LUM:
+		y -= full ? 0 : 16 << 4;
+		cb -= 128 << 4;
+		cr -= 128 << 4;
+
+		if (cb <= 0)
+			*b = y_fac * y + (full ? bt2020c_full[0] : bt2020c[0]) * cb;
+		else
+			*b = y_fac * y + (full ? bt2020c_full[1] : bt2020c[1]) * cb;
+		*b = *b >> 12;
+		if (cr <= 0)
+			*r = y_fac * y + (full ? bt2020c_full[2] : bt2020c[2]) * cr;
+		else
+			*r = y_fac * y + (full ? bt2020c_full[3] : bt2020c[3]) * cr;
+		*r = *r >> 12;
+		lin_r = rec709_to_linear(*r);
+		lin_b = rec709_to_linear(*b);
+		lin_y = rec709_to_linear((y * 255) / (full ? 255 : 219));
+
+		lin_g = COEFF(1.0 / 0.6780, 255) * lin_y -
+			COEFF(0.2627 / 0.6780, 255) * lin_r -
+			COEFF(0.0593 / 0.6780, 255) * lin_b;
+		*g = linear_to_rec709(lin_g >> 12);
+		break;
+	case V4L2_YCBCR_ENC_SMPTE240M:
+		ycbcr2rgb(full ? smpte240m_full : smpte240m, y, cb, cr, y_offset, r, g, b);
+		break;
+	case V4L2_YCBCR_ENC_709:
+	default:
+		ycbcr2rgb(full ? rec709_full : rec709, y, cb, cr, y_offset, r, g, b);
+		break;
+	}
+}
+
+/* precalculate color bar values to speed up rendering */
+static void precalculate_color(struct tpg_data *tpg, int k)
+{
+	int col = k;
+	int r = tpg_colors[col].r;
+	int g = tpg_colors[col].g;
+	int b = tpg_colors[col].b;
+
+	if (k == TPG_COLOR_TEXTBG) {
+		col = tpg_get_textbg_color(tpg);
+
+		r = tpg_colors[col].r;
+		g = tpg_colors[col].g;
+		b = tpg_colors[col].b;
+	} else if (k == TPG_COLOR_TEXTFG) {
+		col = tpg_get_textfg_color(tpg);
+
+		r = tpg_colors[col].r;
+		g = tpg_colors[col].g;
+		b = tpg_colors[col].b;
+	} else if (tpg->pattern == TPG_PAT_NOISE) {
+		r = g = b = prandom_u32_max(256);
+	} else if (k == TPG_COLOR_RANDOM) {
+		r = g = b = tpg->qual_offset + prandom_u32_max(196);
+	} else if (k >= TPG_COLOR_RAMP) {
+		r = g = b = k - TPG_COLOR_RAMP;
+	}
+
+	if (tpg->pattern == TPG_PAT_CSC_COLORBAR && col <= TPG_COLOR_CSC_BLACK) {
+		r = tpg_csc_colors[tpg->colorspace][tpg->real_xfer_func][col].r;
+		g = tpg_csc_colors[tpg->colorspace][tpg->real_xfer_func][col].g;
+		b = tpg_csc_colors[tpg->colorspace][tpg->real_xfer_func][col].b;
+	} else {
+		r <<= 4;
+		g <<= 4;
+		b <<= 4;
+	}
+	if (tpg->qual == TPG_QUAL_GRAY || tpg->fourcc == V4L2_PIX_FMT_GREY ||
+	    tpg->fourcc == V4L2_PIX_FMT_Y16 ||
+	    tpg->fourcc == V4L2_PIX_FMT_Y16_BE) {
+		/* Rec. 709 Luma function */
+		/* (0.2126, 0.7152, 0.0722) * (255 * 256) */
+		r = g = b = (13879 * r + 46688 * g + 4713 * b) >> 16;
+	}
+
+	/*
+	 * The assumption is that the RGB output is always full range,
+	 * so only if the rgb_range overrides the 'real' rgb range do
+	 * we need to convert the RGB values.
+	 *
+	 * Remember that r, g and b are still in the 0 - 0xff0 range.
+	 */
+	if (tpg->real_rgb_range == V4L2_DV_RGB_RANGE_LIMITED &&
+	    tpg->rgb_range == V4L2_DV_RGB_RANGE_FULL) {
+		/*
+		 * Convert from full range (which is what r, g and b are)
+		 * to limited range (which is the 'real' RGB range), which
+		 * is then interpreted as full range.
+		 */
+		r = (r * 219) / 255 + (16 << 4);
+		g = (g * 219) / 255 + (16 << 4);
+		b = (b * 219) / 255 + (16 << 4);
+	} else if (tpg->real_rgb_range != V4L2_DV_RGB_RANGE_LIMITED &&
+		   tpg->rgb_range == V4L2_DV_RGB_RANGE_LIMITED) {
+		/*
+		 * Clamp r, g and b to the limited range and convert to full
+		 * range since that's what we deliver.
+		 */
+		r = clamp(r, 16 << 4, 235 << 4);
+		g = clamp(g, 16 << 4, 235 << 4);
+		b = clamp(b, 16 << 4, 235 << 4);
+		r = (r - (16 << 4)) * 255 / 219;
+		g = (g - (16 << 4)) * 255 / 219;
+		b = (b - (16 << 4)) * 255 / 219;
+	}
+
+	if (tpg->brightness != 128 || tpg->contrast != 128 ||
+	    tpg->saturation != 128 || tpg->hue) {
+		/* Implement these operations */
+		int y, cb, cr;
+		int tmp_cb, tmp_cr;
+
+		/* First convert to YCbCr */
+
+		color_to_ycbcr(tpg, r, g, b, &y, &cb, &cr);
+
+		y = (16 << 4) + ((y - (16 << 4)) * tpg->contrast) / 128;
+		y += (tpg->brightness << 4) - (128 << 4);
+
+		cb -= 128 << 4;
+		cr -= 128 << 4;
+		tmp_cb = (cb * cos(128 + tpg->hue)) / 127 + (cr * sin[128 + tpg->hue]) / 127;
+		tmp_cr = (cr * cos(128 + tpg->hue)) / 127 - (cb * sin[128 + tpg->hue]) / 127;
+
+		cb = (128 << 4) + (tmp_cb * tpg->contrast * tpg->saturation) / (128 * 128);
+		cr = (128 << 4) + (tmp_cr * tpg->contrast * tpg->saturation) / (128 * 128);
+		if (tpg->is_yuv) {
+			tpg->colors[k][0] = clamp(y >> 4, 1, 254);
+			tpg->colors[k][1] = clamp(cb >> 4, 1, 254);
+			tpg->colors[k][2] = clamp(cr >> 4, 1, 254);
+			return;
+		}
+		ycbcr_to_color(tpg, y, cb, cr, &r, &g, &b);
+	}
+
+	if (tpg->is_yuv) {
+		/* Convert to YCbCr */
+		int y, cb, cr;
+
+		color_to_ycbcr(tpg, r, g, b, &y, &cb, &cr);
+
+		if (tpg->real_quantization == V4L2_QUANTIZATION_LIM_RANGE) {
+			y = clamp(y, 16 << 4, 235 << 4);
+			cb = clamp(cb, 16 << 4, 240 << 4);
+			cr = clamp(cr, 16 << 4, 240 << 4);
+		}
+		y = clamp(y >> 4, 1, 254);
+		cb = clamp(cb >> 4, 1, 254);
+		cr = clamp(cr >> 4, 1, 254);
+		switch (tpg->fourcc) {
+		case V4L2_PIX_FMT_YUV444:
+			y >>= 4;
+			cb >>= 4;
+			cr >>= 4;
+			break;
+		case V4L2_PIX_FMT_YUV555:
+			y >>= 3;
+			cb >>= 3;
+			cr >>= 3;
+			break;
+		case V4L2_PIX_FMT_YUV565:
+			y >>= 3;
+			cb >>= 2;
+			cr >>= 3;
+			break;
+		}
+		tpg->colors[k][0] = y;
+		tpg->colors[k][1] = cb;
+		tpg->colors[k][2] = cr;
+	} else {
+		if (tpg->real_quantization == V4L2_QUANTIZATION_LIM_RANGE) {
+			r = (r * 219) / 255 + (16 << 4);
+			g = (g * 219) / 255 + (16 << 4);
+			b = (b * 219) / 255 + (16 << 4);
+		}
+		switch (tpg->fourcc) {
+		case V4L2_PIX_FMT_RGB332:
+			r >>= 9;
+			g >>= 9;
+			b >>= 10;
+			break;
+		case V4L2_PIX_FMT_RGB565:
+		case V4L2_PIX_FMT_RGB565X:
+			r >>= 7;
+			g >>= 6;
+			b >>= 7;
+			break;
+		case V4L2_PIX_FMT_RGB444:
+		case V4L2_PIX_FMT_XRGB444:
+		case V4L2_PIX_FMT_ARGB444:
+			r >>= 8;
+			g >>= 8;
+			b >>= 8;
+			break;
+		case V4L2_PIX_FMT_RGB555:
+		case V4L2_PIX_FMT_XRGB555:
+		case V4L2_PIX_FMT_ARGB555:
+		case V4L2_PIX_FMT_RGB555X:
+		case V4L2_PIX_FMT_XRGB555X:
+		case V4L2_PIX_FMT_ARGB555X:
+			r >>= 7;
+			g >>= 7;
+			b >>= 7;
+			break;
+		case V4L2_PIX_FMT_BGR666:
+			r >>= 6;
+			g >>= 6;
+			b >>= 6;
+			break;
+		default:
+			r >>= 4;
+			g >>= 4;
+			b >>= 4;
+			break;
+		}
+
+		tpg->colors[k][0] = r;
+		tpg->colors[k][1] = g;
+		tpg->colors[k][2] = b;
+	}
+}
+
+static void tpg_precalculate_colors(struct tpg_data *tpg)
+{
+	int k;
+
+	for (k = 0; k < TPG_COLOR_MAX; k++)
+		precalculate_color(tpg, k);
+}
+
+/* 'odd' is true for pixels 1, 3, 5, etc. and false for pixels 0, 2, 4, etc. */
+static void gen_twopix(struct tpg_data *tpg,
+		u8 buf[TPG_MAX_PLANES][8], int color, bool odd)
+{
+	unsigned offset = odd * tpg->twopixelsize[0] / 2;
+	u8 alpha = tpg->alpha_component;
+	u8 r_y, g_u, b_v;
+
+	if (tpg->alpha_red_only && color != TPG_COLOR_CSC_RED &&
+				   color != TPG_COLOR_100_RED &&
+				   color != TPG_COLOR_75_RED)
+		alpha = 0;
+	if (color == TPG_COLOR_RANDOM)
+		precalculate_color(tpg, color);
+	r_y = tpg->colors[color][0]; /* R or precalculated Y */
+	g_u = tpg->colors[color][1]; /* G or precalculated U */
+	b_v = tpg->colors[color][2]; /* B or precalculated V */
+
+	switch (tpg->fourcc) {
+	case V4L2_PIX_FMT_GREY:
+		buf[0][offset] = r_y;
+		break;
+	case V4L2_PIX_FMT_Y16:
+		/*
+		 * Ideally both bytes should be set to r_y, but then you won't
+		 * be able to detect endian problems. So keep it 0 except for
+		 * the corner case where r_y is 0xff so white really will be
+		 * white (0xffff).
+		 */
+		buf[0][offset] = r_y == 0xff ? r_y : 0;
+		buf[0][offset+1] = r_y;
+		break;
+	case V4L2_PIX_FMT_Y16_BE:
+		/* See comment for V4L2_PIX_FMT_Y16 above */
+		buf[0][offset] = r_y;
+		buf[0][offset+1] = r_y == 0xff ? r_y : 0;
+		break;
+	case V4L2_PIX_FMT_YUV422P:
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_YUV420M:
+		buf[0][offset] = r_y;
+		if (odd) {
+			buf[1][0] = (buf[1][0] + g_u) / 2;
+			buf[2][0] = (buf[2][0] + b_v) / 2;
+			buf[1][1] = buf[1][0];
+			buf[2][1] = buf[2][0];
+			break;
+		}
+		buf[1][0] = g_u;
+		buf[2][0] = b_v;
+		break;
+	case V4L2_PIX_FMT_YVU420:
+	case V4L2_PIX_FMT_YVU420M:
+		buf[0][offset] = r_y;
+		if (odd) {
+			buf[1][0] = (buf[1][0] + b_v) / 2;
+			buf[2][0] = (buf[2][0] + g_u) / 2;
+			buf[1][1] = buf[1][0];
+			buf[2][1] = buf[2][0];
+			break;
+		}
+		buf[1][0] = b_v;
+		buf[2][0] = g_u;
+		break;
+
+	case V4L2_PIX_FMT_NV12:
+	case V4L2_PIX_FMT_NV12M:
+	case V4L2_PIX_FMT_NV16:
+	case V4L2_PIX_FMT_NV16M:
+		buf[0][offset] = r_y;
+		if (odd) {
+			buf[1][0] = (buf[1][0] + g_u) / 2;
+			buf[1][1] = (buf[1][1] + b_v) / 2;
+			break;
+		}
+		buf[1][0] = g_u;
+		buf[1][1] = b_v;
+		break;
+	case V4L2_PIX_FMT_NV21:
+	case V4L2_PIX_FMT_NV21M:
+	case V4L2_PIX_FMT_NV61:
+	case V4L2_PIX_FMT_NV61M:
+		buf[0][offset] = r_y;
+		if (odd) {
+			buf[1][0] = (buf[1][0] + b_v) / 2;
+			buf[1][1] = (buf[1][1] + g_u) / 2;
+			break;
+		}
+		buf[1][0] = b_v;
+		buf[1][1] = g_u;
+		break;
+
+	case V4L2_PIX_FMT_NV24:
+		buf[0][offset] = r_y;
+		buf[1][2 * offset] = g_u;
+		buf[1][2 * offset + 1] = b_v;
+		break;
+
+	case V4L2_PIX_FMT_NV42:
+		buf[0][offset] = r_y;
+		buf[1][2 * offset] = b_v;
+		buf[1][2 * offset + 1] = g_u;
+		break;
+
+	case V4L2_PIX_FMT_YUYV:
+		buf[0][offset] = r_y;
+		if (odd) {
+			buf[0][1] = (buf[0][1] + g_u) / 2;
+			buf[0][3] = (buf[0][3] + b_v) / 2;
+			break;
+		}
+		buf[0][1] = g_u;
+		buf[0][3] = b_v;
+		break;
+	case V4L2_PIX_FMT_UYVY:
+		buf[0][offset + 1] = r_y;
+		if (odd) {
+			buf[0][0] = (buf[0][0] + g_u) / 2;
+			buf[0][2] = (buf[0][2] + b_v) / 2;
+			break;
+		}
+		buf[0][0] = g_u;
+		buf[0][2] = b_v;
+		break;
+	case V4L2_PIX_FMT_YVYU:
+		buf[0][offset] = r_y;
+		if (odd) {
+			buf[0][1] = (buf[0][1] + b_v) / 2;
+			buf[0][3] = (buf[0][3] + g_u) / 2;
+			break;
+		}
+		buf[0][1] = b_v;
+		buf[0][3] = g_u;
+		break;
+	case V4L2_PIX_FMT_VYUY:
+		buf[0][offset + 1] = r_y;
+		if (odd) {
+			buf[0][0] = (buf[0][0] + b_v) / 2;
+			buf[0][2] = (buf[0][2] + g_u) / 2;
+			break;
+		}
+		buf[0][0] = b_v;
+		buf[0][2] = g_u;
+		break;
+	case V4L2_PIX_FMT_RGB332:
+		buf[0][offset] = (r_y << 5) | (g_u << 2) | b_v;
+		break;
+	case V4L2_PIX_FMT_YUV565:
+	case V4L2_PIX_FMT_RGB565:
+		buf[0][offset] = (g_u << 5) | b_v;
+		buf[0][offset + 1] = (r_y << 3) | (g_u >> 3);
+		break;
+	case V4L2_PIX_FMT_RGB565X:
+		buf[0][offset] = (r_y << 3) | (g_u >> 3);
+		buf[0][offset + 1] = (g_u << 5) | b_v;
+		break;
+	case V4L2_PIX_FMT_RGB444:
+	case V4L2_PIX_FMT_XRGB444:
+		alpha = 0;
+		/* fall through */
+	case V4L2_PIX_FMT_YUV444:
+	case V4L2_PIX_FMT_ARGB444:
+		buf[0][offset] = (g_u << 4) | b_v;
+		buf[0][offset + 1] = (alpha & 0xf0) | r_y;
+		break;
+	case V4L2_PIX_FMT_RGB555:
+	case V4L2_PIX_FMT_XRGB555:
+		alpha = 0;
+		/* fall through */
+	case V4L2_PIX_FMT_YUV555:
+	case V4L2_PIX_FMT_ARGB555:
+		buf[0][offset] = (g_u << 5) | b_v;
+		buf[0][offset + 1] = (alpha & 0x80) | (r_y << 2) | (g_u >> 3);
+		break;
+	case V4L2_PIX_FMT_RGB555X:
+	case V4L2_PIX_FMT_XRGB555X:
+		alpha = 0;
+		/* fall through */
+	case V4L2_PIX_FMT_ARGB555X:
+		buf[0][offset] = (alpha & 0x80) | (r_y << 2) | (g_u >> 3);
+		buf[0][offset + 1] = (g_u << 5) | b_v;
+		break;
+	case V4L2_PIX_FMT_RGB24:
+		buf[0][offset] = r_y;
+		buf[0][offset + 1] = g_u;
+		buf[0][offset + 2] = b_v;
+		break;
+	case V4L2_PIX_FMT_BGR24:
+		buf[0][offset] = b_v;
+		buf[0][offset + 1] = g_u;
+		buf[0][offset + 2] = r_y;
+		break;
+	case V4L2_PIX_FMT_BGR666:
+		buf[0][offset] = (b_v << 2) | (g_u >> 4);
+		buf[0][offset + 1] = (g_u << 4) | (r_y >> 2);
+		buf[0][offset + 2] = r_y << 6;
+		buf[0][offset + 3] = 0;
+		break;
+	case V4L2_PIX_FMT_RGB32:
+	case V4L2_PIX_FMT_XRGB32:
+		alpha = 0;
+		/* fall through */
+	case V4L2_PIX_FMT_YUV32:
+	case V4L2_PIX_FMT_ARGB32:
+		buf[0][offset] = alpha;
+		buf[0][offset + 1] = r_y;
+		buf[0][offset + 2] = g_u;
+		buf[0][offset + 3] = b_v;
+		break;
+	case V4L2_PIX_FMT_BGR32:
+	case V4L2_PIX_FMT_XBGR32:
+		alpha = 0;
+		/* fall through */
+	case V4L2_PIX_FMT_ABGR32:
+		buf[0][offset] = b_v;
+		buf[0][offset + 1] = g_u;
+		buf[0][offset + 2] = r_y;
+		buf[0][offset + 3] = alpha;
+		break;
+	case V4L2_PIX_FMT_SBGGR8:
+		buf[0][offset] = odd ? g_u : b_v;
+		buf[1][offset] = odd ? r_y : g_u;
+		break;
+	case V4L2_PIX_FMT_SGBRG8:
+		buf[0][offset] = odd ? b_v : g_u;
+		buf[1][offset] = odd ? g_u : r_y;
+		break;
+	case V4L2_PIX_FMT_SGRBG8:
+		buf[0][offset] = odd ? r_y : g_u;
+		buf[1][offset] = odd ? g_u : b_v;
+		break;
+	case V4L2_PIX_FMT_SRGGB8:
+		buf[0][offset] = odd ? g_u : r_y;
+		buf[1][offset] = odd ? b_v : g_u;
+		break;
+	case V4L2_PIX_FMT_SBGGR10:
+		buf[0][offset] = odd ? g_u << 2 : b_v << 2;
+		buf[0][offset + 1] = odd ? g_u >> 6 : b_v >> 6;
+		buf[1][offset] = odd ? r_y << 2 : g_u << 2;
+		buf[1][offset + 1] = odd ? r_y >> 6 : g_u >> 6;
+		buf[0][offset] |= (buf[0][offset] >> 2) & 3;
+		buf[1][offset] |= (buf[1][offset] >> 2) & 3;
+		break;
+	case V4L2_PIX_FMT_SGBRG10:
+		buf[0][offset] = odd ? b_v << 2 : g_u << 2;
+		buf[0][offset + 1] = odd ? b_v >> 6 : g_u >> 6;
+		buf[1][offset] = odd ? g_u << 2 : r_y << 2;
+		buf[1][offset + 1] = odd ? g_u >> 6 : r_y >> 6;
+		buf[0][offset] |= (buf[0][offset] >> 2) & 3;
+		buf[1][offset] |= (buf[1][offset] >> 2) & 3;
+		break;
+	case V4L2_PIX_FMT_SGRBG10:
+		buf[0][offset] = odd ? r_y << 2 : g_u << 2;
+		buf[0][offset + 1] = odd ? r_y >> 6 : g_u >> 6;
+		buf[1][offset] = odd ? g_u << 2 : b_v << 2;
+		buf[1][offset + 1] = odd ? g_u >> 6 : b_v >> 6;
+		buf[0][offset] |= (buf[0][offset] >> 2) & 3;
+		buf[1][offset] |= (buf[1][offset] >> 2) & 3;
+		break;
+	case V4L2_PIX_FMT_SRGGB10:
+		buf[0][offset] = odd ? g_u << 2 : r_y << 2;
+		buf[0][offset + 1] = odd ? g_u >> 6 : r_y >> 6;
+		buf[1][offset] = odd ? b_v << 2 : g_u << 2;
+		buf[1][offset + 1] = odd ? b_v >> 6 : g_u >> 6;
+		buf[0][offset] |= (buf[0][offset] >> 2) & 3;
+		buf[1][offset] |= (buf[1][offset] >> 2) & 3;
+		break;
+	case V4L2_PIX_FMT_SBGGR12:
+		buf[0][offset] = odd ? g_u << 4 : b_v << 4;
+		buf[0][offset + 1] = odd ? g_u >> 4 : b_v >> 4;
+		buf[1][offset] = odd ? r_y << 4 : g_u << 4;
+		buf[1][offset + 1] = odd ? r_y >> 4 : g_u >> 4;
+		buf[0][offset] |= (buf[0][offset] >> 4) & 0xf;
+		buf[1][offset] |= (buf[1][offset] >> 4) & 0xf;
+		break;
+	case V4L2_PIX_FMT_SGBRG12:
+		buf[0][offset] = odd ? b_v << 4 : g_u << 4;
+		buf[0][offset + 1] = odd ? b_v >> 4 : g_u >> 4;
+		buf[1][offset] = odd ? g_u << 4 : r_y << 4;
+		buf[1][offset + 1] = odd ? g_u >> 4 : r_y >> 4;
+		buf[0][offset] |= (buf[0][offset] >> 4) & 0xf;
+		buf[1][offset] |= (buf[1][offset] >> 4) & 0xf;
+		break;
+	case V4L2_PIX_FMT_SGRBG12:
+		buf[0][offset] = odd ? r_y << 4 : g_u << 4;
+		buf[0][offset + 1] = odd ? r_y >> 4 : g_u >> 4;
+		buf[1][offset] = odd ? g_u << 4 : b_v << 4;
+		buf[1][offset + 1] = odd ? g_u >> 4 : b_v >> 4;
+		buf[0][offset] |= (buf[0][offset] >> 4) & 0xf;
+		buf[1][offset] |= (buf[1][offset] >> 4) & 0xf;
+		break;
+	case V4L2_PIX_FMT_SRGGB12:
+		buf[0][offset] = odd ? g_u << 4 : r_y << 4;
+		buf[0][offset + 1] = odd ? g_u >> 4 : r_y >> 4;
+		buf[1][offset] = odd ? b_v << 4 : g_u << 4;
+		buf[1][offset + 1] = odd ? b_v >> 4 : g_u >> 4;
+		buf[0][offset] |= (buf[0][offset] >> 4) & 0xf;
+		buf[1][offset] |= (buf[1][offset] >> 4) & 0xf;
+		break;
+	}
+}
+
+unsigned tpg_g_interleaved_plane(const struct tpg_data *tpg, unsigned buf_line)
+{
+	switch (tpg->fourcc) {
+	case V4L2_PIX_FMT_SBGGR8:
+	case V4L2_PIX_FMT_SGBRG8:
+	case V4L2_PIX_FMT_SGRBG8:
+	case V4L2_PIX_FMT_SRGGB8:
+	case V4L2_PIX_FMT_SBGGR10:
+	case V4L2_PIX_FMT_SGBRG10:
+	case V4L2_PIX_FMT_SGRBG10:
+	case V4L2_PIX_FMT_SRGGB10:
+	case V4L2_PIX_FMT_SBGGR12:
+	case V4L2_PIX_FMT_SGBRG12:
+	case V4L2_PIX_FMT_SGRBG12:
+	case V4L2_PIX_FMT_SRGGB12:
+		return buf_line & 1;
+	default:
+		return 0;
+	}
+}
+
+/* Return how many pattern lines are used by the current pattern. */
+static unsigned tpg_get_pat_lines(const struct tpg_data *tpg)
+{
+	switch (tpg->pattern) {
+	case TPG_PAT_CHECKERS_16X16:
+	case TPG_PAT_CHECKERS_2X2:
+	case TPG_PAT_CHECKERS_1X1:
+	case TPG_PAT_COLOR_CHECKERS_2X2:
+	case TPG_PAT_COLOR_CHECKERS_1X1:
+	case TPG_PAT_ALTERNATING_HLINES:
+	case TPG_PAT_CROSS_1_PIXEL:
+	case TPG_PAT_CROSS_2_PIXELS:
+	case TPG_PAT_CROSS_10_PIXELS:
+		return 2;
+	case TPG_PAT_100_COLORSQUARES:
+	case TPG_PAT_100_HCOLORBAR:
+		return 8;
+	default:
+		return 1;
+	}
+}
+
+/* Which pattern line should be used for the given frame line. */
+static unsigned tpg_get_pat_line(const struct tpg_data *tpg, unsigned line)
+{
+	switch (tpg->pattern) {
+	case TPG_PAT_CHECKERS_16X16:
+		return (line >> 4) & 1;
+	case TPG_PAT_CHECKERS_1X1:
+	case TPG_PAT_COLOR_CHECKERS_1X1:
+	case TPG_PAT_ALTERNATING_HLINES:
+		return line & 1;
+	case TPG_PAT_CHECKERS_2X2:
+	case TPG_PAT_COLOR_CHECKERS_2X2:
+		return (line & 2) >> 1;
+	case TPG_PAT_100_COLORSQUARES:
+	case TPG_PAT_100_HCOLORBAR:
+		return (line * 8) / tpg->src_height;
+	case TPG_PAT_CROSS_1_PIXEL:
+		return line == tpg->src_height / 2;
+	case TPG_PAT_CROSS_2_PIXELS:
+		return (line + 1) / 2 == tpg->src_height / 4;
+	case TPG_PAT_CROSS_10_PIXELS:
+		return (line + 10) / 20 == tpg->src_height / 40;
+	default:
+		return 0;
+	}
+}
+
+/*
+ * Which color should be used for the given pattern line and X coordinate.
+ * Note: x is in the range 0 to 2 * tpg->src_width.
+ */
+static enum tpg_color tpg_get_color(const struct tpg_data *tpg,
+				    unsigned pat_line, unsigned x)
+{
+	/* Maximum number of bars are TPG_COLOR_MAX - otherwise, the input print code
+	   should be modified */
+	static const enum tpg_color bars[3][8] = {
+		/* Standard ITU-R 75% color bar sequence */
+		{ TPG_COLOR_CSC_WHITE,   TPG_COLOR_75_YELLOW,
+		  TPG_COLOR_75_CYAN,     TPG_COLOR_75_GREEN,
+		  TPG_COLOR_75_MAGENTA,  TPG_COLOR_75_RED,
+		  TPG_COLOR_75_BLUE,     TPG_COLOR_100_BLACK, },
+		/* Standard ITU-R 100% color bar sequence */
+		{ TPG_COLOR_100_WHITE,   TPG_COLOR_100_YELLOW,
+		  TPG_COLOR_100_CYAN,    TPG_COLOR_100_GREEN,
+		  TPG_COLOR_100_MAGENTA, TPG_COLOR_100_RED,
+		  TPG_COLOR_100_BLUE,    TPG_COLOR_100_BLACK, },
+		/* Color bar sequence suitable to test CSC */
+		{ TPG_COLOR_CSC_WHITE,   TPG_COLOR_CSC_YELLOW,
+		  TPG_COLOR_CSC_CYAN,    TPG_COLOR_CSC_GREEN,
+		  TPG_COLOR_CSC_MAGENTA, TPG_COLOR_CSC_RED,
+		  TPG_COLOR_CSC_BLUE,    TPG_COLOR_CSC_BLACK, },
+	};
+
+	switch (tpg->pattern) {
+	case TPG_PAT_75_COLORBAR:
+	case TPG_PAT_100_COLORBAR:
+	case TPG_PAT_CSC_COLORBAR:
+		return bars[tpg->pattern][((x * 8) / tpg->src_width) % 8];
+	case TPG_PAT_100_COLORSQUARES:
+		return bars[1][(pat_line + (x * 8) / tpg->src_width) % 8];
+	case TPG_PAT_100_HCOLORBAR:
+		return bars[1][pat_line];
+	case TPG_PAT_BLACK:
+		return TPG_COLOR_100_BLACK;
+	case TPG_PAT_WHITE:
+		return TPG_COLOR_100_WHITE;
+	case TPG_PAT_RED:
+		return TPG_COLOR_100_RED;
+	case TPG_PAT_GREEN:
+		return TPG_COLOR_100_GREEN;
+	case TPG_PAT_BLUE:
+		return TPG_COLOR_100_BLUE;
+	case TPG_PAT_CHECKERS_16X16:
+		return (((x >> 4) & 1) ^ (pat_line & 1)) ?
+			TPG_COLOR_100_BLACK : TPG_COLOR_100_WHITE;
+	case TPG_PAT_CHECKERS_1X1:
+		return ((x & 1) ^ (pat_line & 1)) ?
+			TPG_COLOR_100_WHITE : TPG_COLOR_100_BLACK;
+	case TPG_PAT_COLOR_CHECKERS_1X1:
+		return ((x & 1) ^ (pat_line & 1)) ?
+			TPG_COLOR_100_RED : TPG_COLOR_100_BLUE;
+	case TPG_PAT_CHECKERS_2X2:
+		return (((x >> 1) & 1) ^ (pat_line & 1)) ?
+			TPG_COLOR_100_WHITE : TPG_COLOR_100_BLACK;
+	case TPG_PAT_COLOR_CHECKERS_2X2:
+		return (((x >> 1) & 1) ^ (pat_line & 1)) ?
+			TPG_COLOR_100_RED : TPG_COLOR_100_BLUE;
+	case TPG_PAT_ALTERNATING_HLINES:
+		return pat_line ? TPG_COLOR_100_WHITE : TPG_COLOR_100_BLACK;
+	case TPG_PAT_ALTERNATING_VLINES:
+		return (x & 1) ? TPG_COLOR_100_WHITE : TPG_COLOR_100_BLACK;
+	case TPG_PAT_CROSS_1_PIXEL:
+		if (pat_line || (x % tpg->src_width) == tpg->src_width / 2)
+			return TPG_COLOR_100_BLACK;
+		return TPG_COLOR_100_WHITE;
+	case TPG_PAT_CROSS_2_PIXELS:
+		if (pat_line || ((x % tpg->src_width) + 1) / 2 == tpg->src_width / 4)
+			return TPG_COLOR_100_BLACK;
+		return TPG_COLOR_100_WHITE;
+	case TPG_PAT_CROSS_10_PIXELS:
+		if (pat_line || ((x % tpg->src_width) + 10) / 20 == tpg->src_width / 40)
+			return TPG_COLOR_100_BLACK;
+		return TPG_COLOR_100_WHITE;
+	case TPG_PAT_GRAY_RAMP:
+		return TPG_COLOR_RAMP + ((x % tpg->src_width) * 256) / tpg->src_width;
+	default:
+		return TPG_COLOR_100_RED;
+	}
+}
+
+/*
+ * Given the pixel aspect ratio and video aspect ratio calculate the
+ * coordinates of a centered square and the coordinates of the border of
+ * the active video area. The coordinates are relative to the source
+ * frame rectangle.
+ */
+static void tpg_calculate_square_border(struct tpg_data *tpg)
+{
+	unsigned w = tpg->src_width;
+	unsigned h = tpg->src_height;
+	unsigned sq_w, sq_h;
+
+	sq_w = (w * 2 / 5) & ~1;
+	if (((w - sq_w) / 2) & 1)
+		sq_w += 2;
+	sq_h = sq_w;
+	tpg->square.width = sq_w;
+	if (tpg->vid_aspect == TPG_VIDEO_ASPECT_16X9_ANAMORPHIC) {
+		unsigned ana_sq_w = (sq_w / 4) * 3;
+
+		if (((w - ana_sq_w) / 2) & 1)
+			ana_sq_w += 2;
+		tpg->square.width = ana_sq_w;
+	}
+	tpg->square.left = (w - tpg->square.width) / 2;
+	if (tpg->pix_aspect == TPG_PIXEL_ASPECT_NTSC)
+		sq_h = sq_w * 10 / 11;
+	else if (tpg->pix_aspect == TPG_PIXEL_ASPECT_PAL)
+		sq_h = sq_w * 59 / 54;
+	tpg->square.height = sq_h;
+	tpg->square.top = (h - sq_h) / 2;
+	tpg->border.left = 0;
+	tpg->border.width = w;
+	tpg->border.top = 0;
+	tpg->border.height = h;
+	switch (tpg->vid_aspect) {
+	case TPG_VIDEO_ASPECT_4X3:
+		if (tpg->pix_aspect)
+			return;
+		if (3 * w >= 4 * h) {
+			tpg->border.width = ((4 * h) / 3) & ~1;
+			if (((w - tpg->border.width) / 2) & ~1)
+				tpg->border.width -= 2;
+			tpg->border.left = (w - tpg->border.width) / 2;
+			break;
+		}
+		tpg->border.height = ((3 * w) / 4) & ~1;
+		tpg->border.top = (h - tpg->border.height) / 2;
+		break;
+	case TPG_VIDEO_ASPECT_14X9_CENTRE:
+		if (tpg->pix_aspect) {
+			tpg->border.height = tpg->pix_aspect == TPG_PIXEL_ASPECT_NTSC ? 420 : 506;
+			tpg->border.top = (h - tpg->border.height) / 2;
+			break;
+		}
+		if (9 * w >= 14 * h) {
+			tpg->border.width = ((14 * h) / 9) & ~1;
+			if (((w - tpg->border.width) / 2) & ~1)
+				tpg->border.width -= 2;
+			tpg->border.left = (w - tpg->border.width) / 2;
+			break;
+		}
+		tpg->border.height = ((9 * w) / 14) & ~1;
+		tpg->border.top = (h - tpg->border.height) / 2;
+		break;
+	case TPG_VIDEO_ASPECT_16X9_CENTRE:
+		if (tpg->pix_aspect) {
+			tpg->border.height = tpg->pix_aspect == TPG_PIXEL_ASPECT_NTSC ? 368 : 442;
+			tpg->border.top = (h - tpg->border.height) / 2;
+			break;
+		}
+		if (9 * w >= 16 * h) {
+			tpg->border.width = ((16 * h) / 9) & ~1;
+			if (((w - tpg->border.width) / 2) & ~1)
+				tpg->border.width -= 2;
+			tpg->border.left = (w - tpg->border.width) / 2;
+			break;
+		}
+		tpg->border.height = ((9 * w) / 16) & ~1;
+		tpg->border.top = (h - tpg->border.height) / 2;
+		break;
+	default:
+		break;
+	}
+}
+
+static void tpg_precalculate_line(struct tpg_data *tpg)
+{
+	enum tpg_color contrast;
+	u8 pix[TPG_MAX_PLANES][8];
+	unsigned pat;
+	unsigned p;
+	unsigned x;
+
+	switch (tpg->pattern) {
+	case TPG_PAT_GREEN:
+		contrast = TPG_COLOR_100_RED;
+		break;
+	case TPG_PAT_CSC_COLORBAR:
+		contrast = TPG_COLOR_CSC_GREEN;
+		break;
+	default:
+		contrast = TPG_COLOR_100_GREEN;
+		break;
+	}
+
+	for (pat = 0; pat < tpg_get_pat_lines(tpg); pat++) {
+		/* Coarse scaling with Bresenham */
+		unsigned int_part = tpg->src_width / tpg->scaled_width;
+		unsigned fract_part = tpg->src_width % tpg->scaled_width;
+		unsigned src_x = 0;
+		unsigned error = 0;
+
+		for (x = 0; x < tpg->scaled_width * 2; x += 2) {
+			unsigned real_x = src_x;
+			enum tpg_color color1, color2;
+
+			real_x = tpg->hflip ? tpg->src_width * 2 - real_x - 2 : real_x;
+			color1 = tpg_get_color(tpg, pat, real_x);
+
+			src_x += int_part;
+			error += fract_part;
+			if (error >= tpg->scaled_width) {
+				error -= tpg->scaled_width;
+				src_x++;
+			}
+
+			real_x = src_x;
+			real_x = tpg->hflip ? tpg->src_width * 2 - real_x - 2 : real_x;
+			color2 = tpg_get_color(tpg, pat, real_x);
+
+			src_x += int_part;
+			error += fract_part;
+			if (error >= tpg->scaled_width) {
+				error -= tpg->scaled_width;
+				src_x++;
+			}
+
+			gen_twopix(tpg, pix, tpg->hflip ? color2 : color1, 0);
+			gen_twopix(tpg, pix, tpg->hflip ? color1 : color2, 1);
+			for (p = 0; p < tpg->planes; p++) {
+				unsigned twopixsize = tpg->twopixelsize[p];
+				unsigned hdiv = tpg->hdownsampling[p];
+				u8 *pos = tpg->lines[pat][p] + tpg_hdiv(tpg, p, x);
+
+				memcpy(pos, pix[p], twopixsize / hdiv);
+			}
+		}
+	}
+
+	if (tpg->vdownsampling[tpg->planes - 1] > 1) {
+		unsigned pat_lines = tpg_get_pat_lines(tpg);
+
+		for (pat = 0; pat < pat_lines; pat++) {
+			unsigned next_pat = (pat + 1) % pat_lines;
+
+			for (p = 1; p < tpg->planes; p++) {
+				unsigned w = tpg_hdiv(tpg, p, tpg->scaled_width * 2);
+				u8 *pos1 = tpg->lines[pat][p];
+				u8 *pos2 = tpg->lines[next_pat][p];
+				u8 *dest = tpg->downsampled_lines[pat][p];
+
+				for (x = 0; x < w; x++, pos1++, pos2++, dest++)
+					*dest = ((u16)*pos1 + (u16)*pos2) / 2;
+			}
+		}
+	}
+
+	gen_twopix(tpg, pix, contrast, 0);
+	gen_twopix(tpg, pix, contrast, 1);
+	for (p = 0; p < tpg->planes; p++) {
+		unsigned twopixsize = tpg->twopixelsize[p];
+		u8 *pos = tpg->contrast_line[p];
+
+		for (x = 0; x < tpg->scaled_width; x += 2, pos += twopixsize)
+			memcpy(pos, pix[p], twopixsize);
+	}
+
+	gen_twopix(tpg, pix, TPG_COLOR_100_BLACK, 0);
+	gen_twopix(tpg, pix, TPG_COLOR_100_BLACK, 1);
+	for (p = 0; p < tpg->planes; p++) {
+		unsigned twopixsize = tpg->twopixelsize[p];
+		u8 *pos = tpg->black_line[p];
+
+		for (x = 0; x < tpg->scaled_width; x += 2, pos += twopixsize)
+			memcpy(pos, pix[p], twopixsize);
+	}
+
+	for (x = 0; x < tpg->scaled_width * 2; x += 2) {
+		gen_twopix(tpg, pix, TPG_COLOR_RANDOM, 0);
+		gen_twopix(tpg, pix, TPG_COLOR_RANDOM, 1);
+		for (p = 0; p < tpg->planes; p++) {
+			unsigned twopixsize = tpg->twopixelsize[p];
+			u8 *pos = tpg->random_line[p] + x * twopixsize / 2;
+
+			memcpy(pos, pix[p], twopixsize);
+		}
+	}
+
+	gen_twopix(tpg, tpg->textbg, TPG_COLOR_TEXTBG, 0);
+	gen_twopix(tpg, tpg->textbg, TPG_COLOR_TEXTBG, 1);
+	gen_twopix(tpg, tpg->textfg, TPG_COLOR_TEXTFG, 0);
+	gen_twopix(tpg, tpg->textfg, TPG_COLOR_TEXTFG, 1);
+}
+
+/* need this to do rgb24 rendering */
+typedef struct { u16 __; u8 _; } __packed x24;
+
+#define PRINTSTR(PIXTYPE) do {	\
+	unsigned vdiv = tpg->vdownsampling[p]; \
+	unsigned hdiv = tpg->hdownsampling[p]; \
+	int line;	\
+	PIXTYPE fg;	\
+	PIXTYPE bg;	\
+	memcpy(&fg, tpg->textfg[p], sizeof(PIXTYPE));	\
+	memcpy(&bg, tpg->textbg[p], sizeof(PIXTYPE));	\
+	\
+	for (line = first; line < 16; line += vdiv * step) {	\
+		int l = tpg->vflip ? 15 - line : line; \
+		PIXTYPE *pos = (PIXTYPE *)(basep[p][(line / vdiv) & 1] + \
+			       ((y * step + l) / (vdiv * div)) * tpg->bytesperline[p] + \
+			       (x / hdiv) * sizeof(PIXTYPE));	\
+		unsigned s;	\
+	\
+		for (s = 0; s < len; s++) {	\
+			u8 chr = font8x16[text[s] * 16 + line];	\
+	\
+			if (hdiv == 2 && tpg->hflip) { \
+				pos[3] = (chr & (0x01 << 6) ? fg : bg);	\
+				pos[2] = (chr & (0x01 << 4) ? fg : bg);	\
+				pos[1] = (chr & (0x01 << 2) ? fg : bg);	\
+				pos[0] = (chr & (0x01 << 0) ? fg : bg);	\
+			} else if (hdiv == 2) { \
+				pos[0] = (chr & (0x01 << 7) ? fg : bg);	\
+				pos[1] = (chr & (0x01 << 5) ? fg : bg);	\
+				pos[2] = (chr & (0x01 << 3) ? fg : bg);	\
+				pos[3] = (chr & (0x01 << 1) ? fg : bg);	\
+			} else if (tpg->hflip) { \
+				pos[7] = (chr & (0x01 << 7) ? fg : bg);	\
+				pos[6] = (chr & (0x01 << 6) ? fg : bg);	\
+				pos[5] = (chr & (0x01 << 5) ? fg : bg);	\
+				pos[4] = (chr & (0x01 << 4) ? fg : bg);	\
+				pos[3] = (chr & (0x01 << 3) ? fg : bg);	\
+				pos[2] = (chr & (0x01 << 2) ? fg : bg);	\
+				pos[1] = (chr & (0x01 << 1) ? fg : bg);	\
+				pos[0] = (chr & (0x01 << 0) ? fg : bg);	\
+			} else { \
+				pos[0] = (chr & (0x01 << 7) ? fg : bg);	\
+				pos[1] = (chr & (0x01 << 6) ? fg : bg);	\
+				pos[2] = (chr & (0x01 << 5) ? fg : bg);	\
+				pos[3] = (chr & (0x01 << 4) ? fg : bg);	\
+				pos[4] = (chr & (0x01 << 3) ? fg : bg);	\
+				pos[5] = (chr & (0x01 << 2) ? fg : bg);	\
+				pos[6] = (chr & (0x01 << 1) ? fg : bg);	\
+				pos[7] = (chr & (0x01 << 0) ? fg : bg);	\
+			} \
+	\
+			pos += (tpg->hflip ? -8 : 8) / hdiv;	\
+		}	\
+	}	\
+} while (0)
+
+static noinline void tpg_print_str_2(const struct tpg_data *tpg, u8 *basep[TPG_MAX_PLANES][2],
+			unsigned p, unsigned first, unsigned div, unsigned step,
+			int y, int x, char *text, unsigned len)
+{
+	PRINTSTR(u8);
+}
+
+static noinline void tpg_print_str_4(const struct tpg_data *tpg, u8 *basep[TPG_MAX_PLANES][2],
+			unsigned p, unsigned first, unsigned div, unsigned step,
+			int y, int x, char *text, unsigned len)
+{
+	PRINTSTR(u16);
+}
+
+static noinline void tpg_print_str_6(const struct tpg_data *tpg, u8 *basep[TPG_MAX_PLANES][2],
+			unsigned p, unsigned first, unsigned div, unsigned step,
+			int y, int x, char *text, unsigned len)
+{
+	PRINTSTR(x24);
+}
+
+static noinline void tpg_print_str_8(const struct tpg_data *tpg, u8 *basep[TPG_MAX_PLANES][2],
+			unsigned p, unsigned first, unsigned div, unsigned step,
+			int y, int x, char *text, unsigned len)
+{
+	PRINTSTR(u32);
+}
+
+void tpg_gen_text(const struct tpg_data *tpg, u8 *basep[TPG_MAX_PLANES][2],
+		  int y, int x, char *text)
+{
+	unsigned step = V4L2_FIELD_HAS_T_OR_B(tpg->field) ? 2 : 1;
+	unsigned div = step;
+	unsigned first = 0;
+	unsigned len = strlen(text);
+	unsigned p;
+
+	if (font8x16 == NULL || basep == NULL)
+		return;
+
+	/* Checks if it is possible to show string */
+	if (y + 16 >= tpg->compose.height || x + 8 >= tpg->compose.width)
+		return;
+
+	if (len > (tpg->compose.width - x) / 8)
+		len = (tpg->compose.width - x) / 8;
+	if (tpg->vflip)
+		y = tpg->compose.height - y - 16;
+	if (tpg->hflip)
+		x = tpg->compose.width - x - 8;
+	y += tpg->compose.top;
+	x += tpg->compose.left;
+	if (tpg->field == V4L2_FIELD_BOTTOM)
+		first = 1;
+	else if (tpg->field == V4L2_FIELD_SEQ_TB || tpg->field == V4L2_FIELD_SEQ_BT)
+		div = 2;
+
+	for (p = 0; p < tpg->planes; p++) {
+		/* Print text */
+		switch (tpg->twopixelsize[p]) {
+		case 2:
+			tpg_print_str_2(tpg, basep, p, first, div, step, y, x,
+					text, len);
+			break;
+		case 4:
+			tpg_print_str_4(tpg, basep, p, first, div, step, y, x,
+					text, len);
+			break;
+		case 6:
+			tpg_print_str_6(tpg, basep, p, first, div, step, y, x,
+					text, len);
+			break;
+		case 8:
+			tpg_print_str_8(tpg, basep, p, first, div, step, y, x,
+					text, len);
+			break;
+		}
+	}
+}
+
+void tpg_update_mv_step(struct tpg_data *tpg)
+{
+	int factor = tpg->mv_hor_mode > TPG_MOVE_NONE ? -1 : 1;
+
+	if (tpg->hflip)
+		factor = -factor;
+	switch (tpg->mv_hor_mode) {
+	case TPG_MOVE_NEG_FAST:
+	case TPG_MOVE_POS_FAST:
+		tpg->mv_hor_step = ((tpg->src_width + 319) / 320) * 4;
+		break;
+	case TPG_MOVE_NEG:
+	case TPG_MOVE_POS:
+		tpg->mv_hor_step = ((tpg->src_width + 639) / 640) * 4;
+		break;
+	case TPG_MOVE_NEG_SLOW:
+	case TPG_MOVE_POS_SLOW:
+		tpg->mv_hor_step = 2;
+		break;
+	case TPG_MOVE_NONE:
+		tpg->mv_hor_step = 0;
+		break;
+	}
+	if (factor < 0)
+		tpg->mv_hor_step = tpg->src_width - tpg->mv_hor_step;
+
+	factor = tpg->mv_vert_mode > TPG_MOVE_NONE ? -1 : 1;
+	switch (tpg->mv_vert_mode) {
+	case TPG_MOVE_NEG_FAST:
+	case TPG_MOVE_POS_FAST:
+		tpg->mv_vert_step = ((tpg->src_width + 319) / 320) * 4;
+		break;
+	case TPG_MOVE_NEG:
+	case TPG_MOVE_POS:
+		tpg->mv_vert_step = ((tpg->src_width + 639) / 640) * 4;
+		break;
+	case TPG_MOVE_NEG_SLOW:
+	case TPG_MOVE_POS_SLOW:
+		tpg->mv_vert_step = 1;
+		break;
+	case TPG_MOVE_NONE:
+		tpg->mv_vert_step = 0;
+		break;
+	}
+	if (factor < 0)
+		tpg->mv_vert_step = tpg->src_height - tpg->mv_vert_step;
+}
+
+/* Map the line number relative to the crop rectangle to a frame line number */
+static unsigned tpg_calc_frameline(const struct tpg_data *tpg, unsigned src_y,
+				    unsigned field)
+{
+	switch (field) {
+	case V4L2_FIELD_TOP:
+		return tpg->crop.top + src_y * 2;
+	case V4L2_FIELD_BOTTOM:
+		return tpg->crop.top + src_y * 2 + 1;
+	default:
+		return src_y + tpg->crop.top;
+	}
+}
+
+/*
+ * Map the line number relative to the compose rectangle to a destination
+ * buffer line number.
+ */
+static unsigned tpg_calc_buffer_line(const struct tpg_data *tpg, unsigned y,
+				    unsigned field)
+{
+	y += tpg->compose.top;
+	switch (field) {
+	case V4L2_FIELD_SEQ_TB:
+		if (y & 1)
+			return tpg->buf_height / 2 + y / 2;
+		return y / 2;
+	case V4L2_FIELD_SEQ_BT:
+		if (y & 1)
+			return y / 2;
+		return tpg->buf_height / 2 + y / 2;
+	default:
+		return y;
+	}
+}
+
+static void tpg_recalc(struct tpg_data *tpg)
+{
+	if (tpg->recalc_colors) {
+		tpg->recalc_colors = false;
+		tpg->recalc_lines = true;
+		tpg->real_xfer_func = tpg->xfer_func;
+		tpg->real_ycbcr_enc = tpg->ycbcr_enc;
+		tpg->real_quantization = tpg->quantization;
+
+		if (tpg->xfer_func == V4L2_XFER_FUNC_DEFAULT)
+			tpg->real_xfer_func =
+				V4L2_MAP_XFER_FUNC_DEFAULT(tpg->colorspace);
+
+		if (tpg->ycbcr_enc == V4L2_YCBCR_ENC_DEFAULT)
+			tpg->real_ycbcr_enc =
+				V4L2_MAP_YCBCR_ENC_DEFAULT(tpg->colorspace);
+
+		if (tpg->quantization == V4L2_QUANTIZATION_DEFAULT)
+			tpg->real_quantization =
+				V4L2_MAP_QUANTIZATION_DEFAULT(!tpg->is_yuv,
+					tpg->colorspace, tpg->real_ycbcr_enc);
+
+		tpg_precalculate_colors(tpg);
+	}
+	if (tpg->recalc_square_border) {
+		tpg->recalc_square_border = false;
+		tpg_calculate_square_border(tpg);
+	}
+	if (tpg->recalc_lines) {
+		tpg->recalc_lines = false;
+		tpg_precalculate_line(tpg);
+	}
+}
+
+void tpg_calc_text_basep(struct tpg_data *tpg,
+		u8 *basep[TPG_MAX_PLANES][2], unsigned p, u8 *vbuf)
+{
+	unsigned stride = tpg->bytesperline[p];
+	unsigned h = tpg->buf_height;
+
+	tpg_recalc(tpg);
+
+	basep[p][0] = vbuf;
+	basep[p][1] = vbuf;
+	h /= tpg->vdownsampling[p];
+	if (tpg->field == V4L2_FIELD_SEQ_TB)
+		basep[p][1] += h * stride / 2;
+	else if (tpg->field == V4L2_FIELD_SEQ_BT)
+		basep[p][0] += h * stride / 2;
+	if (p == 0 && tpg->interleaved)
+		tpg_calc_text_basep(tpg, basep, 1, vbuf);
+}
+
+static int tpg_pattern_avg(const struct tpg_data *tpg,
+			   unsigned pat1, unsigned pat2)
+{
+	unsigned pat_lines = tpg_get_pat_lines(tpg);
+
+	if (pat1 == (pat2 + 1) % pat_lines)
+		return pat2;
+	if (pat2 == (pat1 + 1) % pat_lines)
+		return pat1;
+	return -1;
+}
+
+void tpg_log_status(struct tpg_data *tpg)
+{
+	pr_info("tpg source WxH: %ux%u (%s)\n",
+			tpg->src_width, tpg->src_height,
+			tpg->is_yuv ? "YCbCr" : "RGB");
+	pr_info("tpg field: %u\n", tpg->field);
+	pr_info("tpg crop: %ux%u@%dx%d\n", tpg->crop.width, tpg->crop.height,
+			tpg->crop.left, tpg->crop.top);
+	pr_info("tpg compose: %ux%u@%dx%d\n", tpg->compose.width, tpg->compose.height,
+			tpg->compose.left, tpg->compose.top);
+	pr_info("tpg colorspace: %d\n", tpg->colorspace);
+	pr_info("tpg transfer function: %d/%d\n", tpg->xfer_func, tpg->real_xfer_func);
+	pr_info("tpg Y'CbCr encoding: %d/%d\n", tpg->ycbcr_enc, tpg->real_ycbcr_enc);
+	pr_info("tpg quantization: %d/%d\n", tpg->quantization, tpg->real_quantization);
+	pr_info("tpg RGB range: %d/%d\n", tpg->rgb_range, tpg->real_rgb_range);
+}
+
+/*
+ * This struct contains common parameters used by both the drawing of the
+ * test pattern and the drawing of the extras (borders, square, etc.)
+ */
+struct tpg_draw_params {
+	/* common data */
+	bool is_tv;
+	bool is_60hz;
+	unsigned twopixsize;
+	unsigned img_width;
+	unsigned stride;
+	unsigned hmax;
+	unsigned frame_line;
+	unsigned frame_line_next;
+
+	/* test pattern */
+	unsigned mv_hor_old;
+	unsigned mv_hor_new;
+	unsigned mv_vert_old;
+	unsigned mv_vert_new;
+
+	/* extras */
+	unsigned wss_width;
+	unsigned wss_random_offset;
+	unsigned sav_eav_f;
+	unsigned left_pillar_width;
+	unsigned right_pillar_start;
+};
+
+static void tpg_fill_params_pattern(const struct tpg_data *tpg, unsigned p,
+				    struct tpg_draw_params *params)
+{
+	params->mv_hor_old =
+		tpg_hscale_div(tpg, p, tpg->mv_hor_count % tpg->src_width);
+	params->mv_hor_new =
+		tpg_hscale_div(tpg, p, (tpg->mv_hor_count + tpg->mv_hor_step) %
+			       tpg->src_width);
+	params->mv_vert_old = tpg->mv_vert_count % tpg->src_height;
+	params->mv_vert_new =
+		(tpg->mv_vert_count + tpg->mv_vert_step) % tpg->src_height;
+}
+
+static void tpg_fill_params_extras(const struct tpg_data *tpg,
+				   unsigned p,
+				   struct tpg_draw_params *params)
+{
+	unsigned left_pillar_width = 0;
+	unsigned right_pillar_start = params->img_width;
+
+	params->wss_width = tpg->crop.left < tpg->src_width / 2 ?
+		tpg->src_width / 2 - tpg->crop.left : 0;
+	if (params->wss_width > tpg->crop.width)
+		params->wss_width = tpg->crop.width;
+	params->wss_width = tpg_hscale_div(tpg, p, params->wss_width);
+	params->wss_random_offset =
+		params->twopixsize * prandom_u32_max(tpg->src_width / 2);
+
+	if (tpg->crop.left < tpg->border.left) {
+		left_pillar_width = tpg->border.left - tpg->crop.left;
+		if (left_pillar_width > tpg->crop.width)
+			left_pillar_width = tpg->crop.width;
+		left_pillar_width = tpg_hscale_div(tpg, p, left_pillar_width);
+	}
+	params->left_pillar_width = left_pillar_width;
+
+	if (tpg->crop.left + tpg->crop.width >
+	    tpg->border.left + tpg->border.width) {
+		right_pillar_start =
+			tpg->border.left + tpg->border.width - tpg->crop.left;
+		right_pillar_start =
+			tpg_hscale_div(tpg, p, right_pillar_start);
+		if (right_pillar_start > params->img_width)
+			right_pillar_start = params->img_width;
+	}
+	params->right_pillar_start = right_pillar_start;
+
+	params->sav_eav_f = tpg->field ==
+			(params->is_60hz ? V4L2_FIELD_TOP : V4L2_FIELD_BOTTOM);
+}
+
+static void tpg_fill_plane_extras(const struct tpg_data *tpg,
+				  const struct tpg_draw_params *params,
+				  unsigned p, unsigned h, u8 *vbuf)
+{
+	unsigned twopixsize = params->twopixsize;
+	unsigned img_width = params->img_width;
+	unsigned frame_line = params->frame_line;
+	const struct v4l2_rect *sq = &tpg->square;
+	const struct v4l2_rect *b = &tpg->border;
+	const struct v4l2_rect *c = &tpg->crop;
+
+	if (params->is_tv && !params->is_60hz &&
+	    frame_line == 0 && params->wss_width) {
+		/*
+		 * Replace the first half of the top line of a 50 Hz frame
+		 * with random data to simulate a WSS signal.
+		 */
+		u8 *wss = tpg->random_line[p] + params->wss_random_offset;
+
+		memcpy(vbuf, wss, params->wss_width);
+	}
+
+	if (tpg->show_border && frame_line >= b->top &&
+	    frame_line < b->top + b->height) {
+		unsigned bottom = b->top + b->height - 1;
+		unsigned left = params->left_pillar_width;
+		unsigned right = params->right_pillar_start;
+
+		if (frame_line == b->top || frame_line == b->top + 1 ||
+		    frame_line == bottom || frame_line == bottom - 1) {
+			memcpy(vbuf + left, tpg->contrast_line[p],
+					right - left);
+		} else {
+			if (b->left >= c->left &&
+			    b->left < c->left + c->width)
+				memcpy(vbuf + left,
+					tpg->contrast_line[p], twopixsize);
+			if (b->left + b->width > c->left &&
+			    b->left + b->width <= c->left + c->width)
+				memcpy(vbuf + right - twopixsize,
+					tpg->contrast_line[p], twopixsize);
+		}
+	}
+	if (tpg->qual != TPG_QUAL_NOISE && frame_line >= b->top &&
+	    frame_line < b->top + b->height) {
+		memcpy(vbuf, tpg->black_line[p], params->left_pillar_width);
+		memcpy(vbuf + params->right_pillar_start, tpg->black_line[p],
+		       img_width - params->right_pillar_start);
+	}
+	if (tpg->show_square && frame_line >= sq->top &&
+	    frame_line < sq->top + sq->height &&
+	    sq->left < c->left + c->width &&
+	    sq->left + sq->width >= c->left) {
+		unsigned left = sq->left;
+		unsigned width = sq->width;
+
+		if (c->left > left) {
+			width -= c->left - left;
+			left = c->left;
+		}
+		if (c->left + c->width < left + width)
+			width -= left + width - c->left - c->width;
+		left -= c->left;
+		left = tpg_hscale_div(tpg, p, left);
+		width = tpg_hscale_div(tpg, p, width);
+		memcpy(vbuf + left, tpg->contrast_line[p], width);
+	}
+	if (tpg->insert_sav) {
+		unsigned offset = tpg_hdiv(tpg, p, tpg->compose.width / 3);
+		u8 *p = vbuf + offset;
+		unsigned vact = 0, hact = 0;
+
+		p[0] = 0xff;
+		p[1] = 0;
+		p[2] = 0;
+		p[3] = 0x80 | (params->sav_eav_f << 6) |
+			(vact << 5) | (hact << 4) |
+			((hact ^ vact) << 3) |
+			((hact ^ params->sav_eav_f) << 2) |
+			((params->sav_eav_f ^ vact) << 1) |
+			(hact ^ vact ^ params->sav_eav_f);
+	}
+	if (tpg->insert_eav) {
+		unsigned offset = tpg_hdiv(tpg, p, tpg->compose.width * 2 / 3);
+		u8 *p = vbuf + offset;
+		unsigned vact = 0, hact = 1;
+
+		p[0] = 0xff;
+		p[1] = 0;
+		p[2] = 0;
+		p[3] = 0x80 | (params->sav_eav_f << 6) |
+			(vact << 5) | (hact << 4) |
+			((hact ^ vact) << 3) |
+			((hact ^ params->sav_eav_f) << 2) |
+			((params->sav_eav_f ^ vact) << 1) |
+			(hact ^ vact ^ params->sav_eav_f);
+	}
+}
+
+static void tpg_fill_plane_pattern(const struct tpg_data *tpg,
+				   const struct tpg_draw_params *params,
+				   unsigned p, unsigned h, u8 *vbuf)
+{
+	unsigned twopixsize = params->twopixsize;
+	unsigned img_width = params->img_width;
+	unsigned mv_hor_old = params->mv_hor_old;
+	unsigned mv_hor_new = params->mv_hor_new;
+	unsigned mv_vert_old = params->mv_vert_old;
+	unsigned mv_vert_new = params->mv_vert_new;
+	unsigned frame_line = params->frame_line;
+	unsigned frame_line_next = params->frame_line_next;
+	unsigned line_offset = tpg_hscale_div(tpg, p, tpg->crop.left);
+	bool even;
+	bool fill_blank = false;
+	unsigned pat_line_old;
+	unsigned pat_line_new;
+	u8 *linestart_older;
+	u8 *linestart_newer;
+	u8 *linestart_top;
+	u8 *linestart_bottom;
+
+	even = !(frame_line & 1);
+
+	if (h >= params->hmax) {
+		if (params->hmax == tpg->compose.height)
+			return;
+		if (!tpg->perc_fill_blank)
+			return;
+		fill_blank = true;
+	}
+
+	if (tpg->vflip) {
+		frame_line = tpg->src_height - frame_line - 1;
+		frame_line_next = tpg->src_height - frame_line_next - 1;
+	}
+
+	if (fill_blank) {
+		linestart_older = tpg->contrast_line[p];
+		linestart_newer = tpg->contrast_line[p];
+	} else if (tpg->qual != TPG_QUAL_NOISE &&
+		   (frame_line < tpg->border.top ||
+		    frame_line >= tpg->border.top + tpg->border.height)) {
+		linestart_older = tpg->black_line[p];
+		linestart_newer = tpg->black_line[p];
+	} else if (tpg->pattern == TPG_PAT_NOISE || tpg->qual == TPG_QUAL_NOISE) {
+		linestart_older = tpg->random_line[p] +
+				  twopixsize * prandom_u32_max(tpg->src_width / 2);
+		linestart_newer = tpg->random_line[p] +
+				  twopixsize * prandom_u32_max(tpg->src_width / 2);
+	} else {
+		unsigned frame_line_old =
+			(frame_line + mv_vert_old) % tpg->src_height;
+		unsigned frame_line_new =
+			(frame_line + mv_vert_new) % tpg->src_height;
+		unsigned pat_line_next_old;
+		unsigned pat_line_next_new;
+
+		pat_line_old = tpg_get_pat_line(tpg, frame_line_old);
+		pat_line_new = tpg_get_pat_line(tpg, frame_line_new);
+		linestart_older = tpg->lines[pat_line_old][p] + mv_hor_old;
+		linestart_newer = tpg->lines[pat_line_new][p] + mv_hor_new;
+
+		if (tpg->vdownsampling[p] > 1 && frame_line != frame_line_next) {
+			int avg_pat;
+
+			/*
+			 * Now decide whether we need to use downsampled_lines[].
+			 * That's necessary if the two lines use different patterns.
+			 */
+			pat_line_next_old = tpg_get_pat_line(tpg,
+					(frame_line_next + mv_vert_old) % tpg->src_height);
+			pat_line_next_new = tpg_get_pat_line(tpg,
+					(frame_line_next + mv_vert_new) % tpg->src_height);
+
+			switch (tpg->field) {
+			case V4L2_FIELD_INTERLACED:
+			case V4L2_FIELD_INTERLACED_BT:
+			case V4L2_FIELD_INTERLACED_TB:
+				avg_pat = tpg_pattern_avg(tpg, pat_line_old, pat_line_new);
+				if (avg_pat < 0)
+					break;
+				linestart_older = tpg->downsampled_lines[avg_pat][p] + mv_hor_old;
+				linestart_newer = linestart_older;
+				break;
+			case V4L2_FIELD_NONE:
+			case V4L2_FIELD_TOP:
+			case V4L2_FIELD_BOTTOM:
+			case V4L2_FIELD_SEQ_BT:
+			case V4L2_FIELD_SEQ_TB:
+				avg_pat = tpg_pattern_avg(tpg, pat_line_old, pat_line_next_old);
+				if (avg_pat >= 0)
+					linestart_older = tpg->downsampled_lines[avg_pat][p] +
+						mv_hor_old;
+				avg_pat = tpg_pattern_avg(tpg, pat_line_new, pat_line_next_new);
+				if (avg_pat >= 0)
+					linestart_newer = tpg->downsampled_lines[avg_pat][p] +
+						mv_hor_new;
+				break;
+			}
+		}
+		linestart_older += line_offset;
+		linestart_newer += line_offset;
+	}
+	if (tpg->field_alternate) {
+		linestart_top = linestart_bottom = linestart_older;
+	} else if (params->is_60hz) {
+		linestart_top = linestart_newer;
+		linestart_bottom = linestart_older;
+	} else {
+		linestart_top = linestart_older;
+		linestart_bottom = linestart_newer;
+	}
+
+	switch (tpg->field) {
+	case V4L2_FIELD_INTERLACED:
+	case V4L2_FIELD_INTERLACED_TB:
+	case V4L2_FIELD_SEQ_TB:
+	case V4L2_FIELD_SEQ_BT:
+		if (even)
+			memcpy(vbuf, linestart_top, img_width);
+		else
+			memcpy(vbuf, linestart_bottom, img_width);
+		break;
+	case V4L2_FIELD_INTERLACED_BT:
+		if (even)
+			memcpy(vbuf, linestart_bottom, img_width);
+		else
+			memcpy(vbuf, linestart_top, img_width);
+		break;
+	case V4L2_FIELD_TOP:
+		memcpy(vbuf, linestart_top, img_width);
+		break;
+	case V4L2_FIELD_BOTTOM:
+		memcpy(vbuf, linestart_bottom, img_width);
+		break;
+	case V4L2_FIELD_NONE:
+	default:
+		memcpy(vbuf, linestart_older, img_width);
+		break;
+	}
+}
+
+void tpg_fill_plane_buffer(struct tpg_data *tpg, v4l2_std_id std,
+			   unsigned p, u8 *vbuf)
+{
+	struct tpg_draw_params params;
+	unsigned factor = V4L2_FIELD_HAS_T_OR_B(tpg->field) ? 2 : 1;
+
+	/* Coarse scaling with Bresenham */
+	unsigned int_part = (tpg->crop.height / factor) / tpg->compose.height;
+	unsigned fract_part = (tpg->crop.height / factor) % tpg->compose.height;
+	unsigned src_y = 0;
+	unsigned error = 0;
+	unsigned h;
+
+	tpg_recalc(tpg);
+
+	params.is_tv = std;
+	params.is_60hz = std & V4L2_STD_525_60;
+	params.twopixsize = tpg->twopixelsize[p];
+	params.img_width = tpg_hdiv(tpg, p, tpg->compose.width);
+	params.stride = tpg->bytesperline[p];
+	params.hmax = (tpg->compose.height * tpg->perc_fill) / 100;
+
+	tpg_fill_params_pattern(tpg, p, &params);
+	tpg_fill_params_extras(tpg, p, &params);
+
+	vbuf += tpg_hdiv(tpg, p, tpg->compose.left);
+
+	for (h = 0; h < tpg->compose.height; h++) {
+		unsigned buf_line;
+
+		params.frame_line = tpg_calc_frameline(tpg, src_y, tpg->field);
+		params.frame_line_next = params.frame_line;
+		buf_line = tpg_calc_buffer_line(tpg, h, tpg->field);
+		src_y += int_part;
+		error += fract_part;
+		if (error >= tpg->compose.height) {
+			error -= tpg->compose.height;
+			src_y++;
+		}
+
+		/*
+		 * For line-interleaved formats determine the 'plane'
+		 * based on the buffer line.
+		 */
+		if (tpg_g_interleaved(tpg))
+			p = tpg_g_interleaved_plane(tpg, buf_line);
+
+		if (tpg->vdownsampling[p] > 1) {
+			/*
+			 * When doing vertical downsampling the field setting
+			 * matters: for SEQ_BT/TB we downsample each field
+			 * separately (i.e. lines 0+2 are combined, as are
+			 * lines 1+3), for the other field settings we combine
+			 * odd and even lines. Doing that for SEQ_BT/TB would
+			 * be really weird.
+			 */
+			if (tpg->field == V4L2_FIELD_SEQ_BT ||
+			    tpg->field == V4L2_FIELD_SEQ_TB) {
+				unsigned next_src_y = src_y;
+
+				if ((h & 3) >= 2)
+					continue;
+				next_src_y += int_part;
+				if (error + fract_part >= tpg->compose.height)
+					next_src_y++;
+				params.frame_line_next =
+					tpg_calc_frameline(tpg, next_src_y, tpg->field);
+			} else {
+				if (h & 1)
+					continue;
+				params.frame_line_next =
+					tpg_calc_frameline(tpg, src_y, tpg->field);
+			}
+
+			buf_line /= tpg->vdownsampling[p];
+		}
+		tpg_fill_plane_pattern(tpg, &params, p, h,
+				vbuf + buf_line * params.stride);
+		tpg_fill_plane_extras(tpg, &params, p, h,
+				vbuf + buf_line * params.stride);
+	}
+}
+
+void tpg_fillbuffer(struct tpg_data *tpg, v4l2_std_id std, unsigned p, u8 *vbuf)
+{
+	unsigned offset = 0;
+	unsigned i;
+
+	if (tpg->buffers > 1) {
+		tpg_fill_plane_buffer(tpg, std, p, vbuf);
+		return;
+	}
+
+	for (i = 0; i < tpg_g_planes(tpg); i++) {
+		tpg_fill_plane_buffer(tpg, std, i, vbuf + offset);
+		offset += tpg_calc_plane_size(tpg, i);
+	}
+}
diff --git a/drivers/media/platform/vivid/vivid-tpg.h b/drivers/media/platform/vivid/vivid-tpg.h
new file mode 100644
index 0000000..9baed6a
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-tpg.h
@@ -0,0 +1,596 @@
+/*
+ * vivid-tpg.h - Test Pattern Generator
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VIVID_TPG_H_
+#define _VIVID_TPG_H_
+
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/random.h>
+#include <linux/slab.h>
+#include <linux/vmalloc.h>
+#include <linux/videodev2.h>
+
+#include "vivid-tpg-colors.h"
+
+enum tpg_pattern {
+	TPG_PAT_75_COLORBAR,
+	TPG_PAT_100_COLORBAR,
+	TPG_PAT_CSC_COLORBAR,
+	TPG_PAT_100_HCOLORBAR,
+	TPG_PAT_100_COLORSQUARES,
+	TPG_PAT_BLACK,
+	TPG_PAT_WHITE,
+	TPG_PAT_RED,
+	TPG_PAT_GREEN,
+	TPG_PAT_BLUE,
+	TPG_PAT_CHECKERS_16X16,
+	TPG_PAT_CHECKERS_2X2,
+	TPG_PAT_CHECKERS_1X1,
+	TPG_PAT_COLOR_CHECKERS_2X2,
+	TPG_PAT_COLOR_CHECKERS_1X1,
+	TPG_PAT_ALTERNATING_HLINES,
+	TPG_PAT_ALTERNATING_VLINES,
+	TPG_PAT_CROSS_1_PIXEL,
+	TPG_PAT_CROSS_2_PIXELS,
+	TPG_PAT_CROSS_10_PIXELS,
+	TPG_PAT_GRAY_RAMP,
+
+	/* Must be the last pattern */
+	TPG_PAT_NOISE,
+};
+
+extern const char * const tpg_pattern_strings[];
+
+enum tpg_quality {
+	TPG_QUAL_COLOR,
+	TPG_QUAL_GRAY,
+	TPG_QUAL_NOISE
+};
+
+enum tpg_video_aspect {
+	TPG_VIDEO_ASPECT_IMAGE,
+	TPG_VIDEO_ASPECT_4X3,
+	TPG_VIDEO_ASPECT_14X9_CENTRE,
+	TPG_VIDEO_ASPECT_16X9_CENTRE,
+	TPG_VIDEO_ASPECT_16X9_ANAMORPHIC,
+};
+
+enum tpg_pixel_aspect {
+	TPG_PIXEL_ASPECT_SQUARE,
+	TPG_PIXEL_ASPECT_NTSC,
+	TPG_PIXEL_ASPECT_PAL,
+};
+
+enum tpg_move_mode {
+	TPG_MOVE_NEG_FAST,
+	TPG_MOVE_NEG,
+	TPG_MOVE_NEG_SLOW,
+	TPG_MOVE_NONE,
+	TPG_MOVE_POS_SLOW,
+	TPG_MOVE_POS,
+	TPG_MOVE_POS_FAST,
+};
+
+extern const char * const tpg_aspect_strings[];
+
+#define TPG_MAX_PLANES 3
+#define TPG_MAX_PAT_LINES 8
+
+struct tpg_data {
+	/* Source frame size */
+	unsigned			src_width, src_height;
+	/* Buffer height */
+	unsigned			buf_height;
+	/* Scaled output frame size */
+	unsigned			scaled_width;
+	u32				field;
+	bool				field_alternate;
+	/* crop coordinates are frame-based */
+	struct v4l2_rect		crop;
+	/* compose coordinates are format-based */
+	struct v4l2_rect		compose;
+	/* border and square coordinates are frame-based */
+	struct v4l2_rect		border;
+	struct v4l2_rect		square;
+
+	/* Color-related fields */
+	enum tpg_quality		qual;
+	unsigned			qual_offset;
+	u8				alpha_component;
+	bool				alpha_red_only;
+	u8				brightness;
+	u8				contrast;
+	u8				saturation;
+	s16				hue;
+	u32				fourcc;
+	bool				is_yuv;
+	u32				colorspace;
+	u32				xfer_func;
+	u32				ycbcr_enc;
+	/*
+	 * Stores the actual transfer function, i.e. will never be
+	 * V4L2_XFER_FUNC_DEFAULT.
+	 */
+	u32				real_xfer_func;
+	/*
+	 * Stores the actual Y'CbCr encoding, i.e. will never be
+	 * V4L2_YCBCR_ENC_DEFAULT.
+	 */
+	u32				real_ycbcr_enc;
+	u32				quantization;
+	/*
+	 * Stores the actual quantization, i.e. will never be
+	 * V4L2_QUANTIZATION_DEFAULT.
+	 */
+	u32				real_quantization;
+	enum tpg_video_aspect		vid_aspect;
+	enum tpg_pixel_aspect		pix_aspect;
+	unsigned			rgb_range;
+	unsigned			real_rgb_range;
+	unsigned			buffers;
+	unsigned			planes;
+	bool				interleaved;
+	u8				vdownsampling[TPG_MAX_PLANES];
+	u8				hdownsampling[TPG_MAX_PLANES];
+	/*
+	 * horizontal positions must be ANDed with this value to enforce
+	 * correct boundaries for packed YUYV values.
+	 */
+	unsigned			hmask[TPG_MAX_PLANES];
+	/* Used to store the colors in native format, either RGB or YUV */
+	u8				colors[TPG_COLOR_MAX][3];
+	u8				textfg[TPG_MAX_PLANES][8], textbg[TPG_MAX_PLANES][8];
+	/* size in bytes for two pixels in each plane */
+	unsigned			twopixelsize[TPG_MAX_PLANES];
+	unsigned			bytesperline[TPG_MAX_PLANES];
+
+	/* Configuration */
+	enum tpg_pattern		pattern;
+	bool				hflip;
+	bool				vflip;
+	unsigned			perc_fill;
+	bool				perc_fill_blank;
+	bool				show_border;
+	bool				show_square;
+	bool				insert_sav;
+	bool				insert_eav;
+
+	/* Test pattern movement */
+	enum tpg_move_mode		mv_hor_mode;
+	int				mv_hor_count;
+	int				mv_hor_step;
+	enum tpg_move_mode		mv_vert_mode;
+	int				mv_vert_count;
+	int				mv_vert_step;
+
+	bool				recalc_colors;
+	bool				recalc_lines;
+	bool				recalc_square_border;
+
+	/* Used to store TPG_MAX_PAT_LINES lines, each with up to two planes */
+	unsigned			max_line_width;
+	u8				*lines[TPG_MAX_PAT_LINES][TPG_MAX_PLANES];
+	u8				*downsampled_lines[TPG_MAX_PAT_LINES][TPG_MAX_PLANES];
+	u8				*random_line[TPG_MAX_PLANES];
+	u8				*contrast_line[TPG_MAX_PLANES];
+	u8				*black_line[TPG_MAX_PLANES];
+};
+
+void tpg_init(struct tpg_data *tpg, unsigned w, unsigned h);
+int tpg_alloc(struct tpg_data *tpg, unsigned max_w);
+void tpg_free(struct tpg_data *tpg);
+void tpg_reset_source(struct tpg_data *tpg, unsigned width, unsigned height,
+		       u32 field);
+void tpg_log_status(struct tpg_data *tpg);
+
+void tpg_set_font(const u8 *f);
+void tpg_gen_text(const struct tpg_data *tpg,
+		u8 *basep[TPG_MAX_PLANES][2], int y, int x, char *text);
+void tpg_calc_text_basep(struct tpg_data *tpg,
+		u8 *basep[TPG_MAX_PLANES][2], unsigned p, u8 *vbuf);
+unsigned tpg_g_interleaved_plane(const struct tpg_data *tpg, unsigned buf_line);
+void tpg_fill_plane_buffer(struct tpg_data *tpg, v4l2_std_id std,
+			   unsigned p, u8 *vbuf);
+void tpg_fillbuffer(struct tpg_data *tpg, v4l2_std_id std,
+		    unsigned p, u8 *vbuf);
+bool tpg_s_fourcc(struct tpg_data *tpg, u32 fourcc);
+void tpg_s_crop_compose(struct tpg_data *tpg, const struct v4l2_rect *crop,
+		const struct v4l2_rect *compose);
+
+static inline void tpg_s_pattern(struct tpg_data *tpg, enum tpg_pattern pattern)
+{
+	if (tpg->pattern == pattern)
+		return;
+	tpg->pattern = pattern;
+	tpg->recalc_colors = true;
+}
+
+static inline void tpg_s_quality(struct tpg_data *tpg,
+				    enum tpg_quality qual, unsigned qual_offset)
+{
+	if (tpg->qual == qual && tpg->qual_offset == qual_offset)
+		return;
+	tpg->qual = qual;
+	tpg->qual_offset = qual_offset;
+	tpg->recalc_colors = true;
+}
+
+static inline enum tpg_quality tpg_g_quality(const struct tpg_data *tpg)
+{
+	return tpg->qual;
+}
+
+static inline void tpg_s_alpha_component(struct tpg_data *tpg,
+					    u8 alpha_component)
+{
+	if (tpg->alpha_component == alpha_component)
+		return;
+	tpg->alpha_component = alpha_component;
+	tpg->recalc_colors = true;
+}
+
+static inline void tpg_s_alpha_mode(struct tpg_data *tpg,
+					    bool red_only)
+{
+	if (tpg->alpha_red_only == red_only)
+		return;
+	tpg->alpha_red_only = red_only;
+	tpg->recalc_colors = true;
+}
+
+static inline void tpg_s_brightness(struct tpg_data *tpg,
+					u8 brightness)
+{
+	if (tpg->brightness == brightness)
+		return;
+	tpg->brightness = brightness;
+	tpg->recalc_colors = true;
+}
+
+static inline void tpg_s_contrast(struct tpg_data *tpg,
+					u8 contrast)
+{
+	if (tpg->contrast == contrast)
+		return;
+	tpg->contrast = contrast;
+	tpg->recalc_colors = true;
+}
+
+static inline void tpg_s_saturation(struct tpg_data *tpg,
+					u8 saturation)
+{
+	if (tpg->saturation == saturation)
+		return;
+	tpg->saturation = saturation;
+	tpg->recalc_colors = true;
+}
+
+static inline void tpg_s_hue(struct tpg_data *tpg,
+					s16 hue)
+{
+	if (tpg->hue == hue)
+		return;
+	tpg->hue = hue;
+	tpg->recalc_colors = true;
+}
+
+static inline void tpg_s_rgb_range(struct tpg_data *tpg,
+					unsigned rgb_range)
+{
+	if (tpg->rgb_range == rgb_range)
+		return;
+	tpg->rgb_range = rgb_range;
+	tpg->recalc_colors = true;
+}
+
+static inline void tpg_s_real_rgb_range(struct tpg_data *tpg,
+					unsigned rgb_range)
+{
+	if (tpg->real_rgb_range == rgb_range)
+		return;
+	tpg->real_rgb_range = rgb_range;
+	tpg->recalc_colors = true;
+}
+
+static inline void tpg_s_colorspace(struct tpg_data *tpg, u32 colorspace)
+{
+	if (tpg->colorspace == colorspace)
+		return;
+	tpg->colorspace = colorspace;
+	tpg->recalc_colors = true;
+}
+
+static inline u32 tpg_g_colorspace(const struct tpg_data *tpg)
+{
+	return tpg->colorspace;
+}
+
+static inline void tpg_s_ycbcr_enc(struct tpg_data *tpg, u32 ycbcr_enc)
+{
+	if (tpg->ycbcr_enc == ycbcr_enc)
+		return;
+	tpg->ycbcr_enc = ycbcr_enc;
+	tpg->recalc_colors = true;
+}
+
+static inline u32 tpg_g_ycbcr_enc(const struct tpg_data *tpg)
+{
+	return tpg->ycbcr_enc;
+}
+
+static inline void tpg_s_xfer_func(struct tpg_data *tpg, u32 xfer_func)
+{
+	if (tpg->xfer_func == xfer_func)
+		return;
+	tpg->xfer_func = xfer_func;
+	tpg->recalc_colors = true;
+}
+
+static inline u32 tpg_g_xfer_func(const struct tpg_data *tpg)
+{
+	return tpg->xfer_func;
+}
+
+static inline void tpg_s_quantization(struct tpg_data *tpg, u32 quantization)
+{
+	if (tpg->quantization == quantization)
+		return;
+	tpg->quantization = quantization;
+	tpg->recalc_colors = true;
+}
+
+static inline u32 tpg_g_quantization(const struct tpg_data *tpg)
+{
+	return tpg->quantization;
+}
+
+static inline unsigned tpg_g_buffers(const struct tpg_data *tpg)
+{
+	return tpg->buffers;
+}
+
+static inline unsigned tpg_g_planes(const struct tpg_data *tpg)
+{
+	return tpg->interleaved ? 1 : tpg->planes;
+}
+
+static inline bool tpg_g_interleaved(const struct tpg_data *tpg)
+{
+	return tpg->interleaved;
+}
+
+static inline unsigned tpg_g_twopixelsize(const struct tpg_data *tpg, unsigned plane)
+{
+	return tpg->twopixelsize[plane];
+}
+
+static inline unsigned tpg_hdiv(const struct tpg_data *tpg,
+				  unsigned plane, unsigned x)
+{
+	return ((x / tpg->hdownsampling[plane]) & tpg->hmask[plane]) *
+		tpg->twopixelsize[plane] / 2;
+}
+
+static inline unsigned tpg_hscale(const struct tpg_data *tpg, unsigned x)
+{
+	return (x * tpg->scaled_width) / tpg->src_width;
+}
+
+static inline unsigned tpg_hscale_div(const struct tpg_data *tpg,
+				      unsigned plane, unsigned x)
+{
+	return tpg_hdiv(tpg, plane, tpg_hscale(tpg, x));
+}
+
+static inline unsigned tpg_g_bytesperline(const struct tpg_data *tpg, unsigned plane)
+{
+	return tpg->bytesperline[plane];
+}
+
+static inline void tpg_s_bytesperline(struct tpg_data *tpg, unsigned plane, unsigned bpl)
+{
+	unsigned p;
+
+	if (tpg->buffers > 1) {
+		tpg->bytesperline[plane] = bpl;
+		return;
+	}
+
+	for (p = 0; p < tpg_g_planes(tpg); p++) {
+		unsigned plane_w = bpl * tpg->twopixelsize[p] / tpg->twopixelsize[0];
+
+		tpg->bytesperline[p] = plane_w / tpg->hdownsampling[p];
+	}
+}
+
+
+static inline unsigned tpg_g_line_width(const struct tpg_data *tpg, unsigned plane)
+{
+	unsigned w = 0;
+	unsigned p;
+
+	if (tpg->buffers > 1)
+		return tpg_g_bytesperline(tpg, plane);
+	for (p = 0; p < tpg_g_planes(tpg); p++) {
+		unsigned plane_w = tpg_g_bytesperline(tpg, p);
+
+		w += plane_w / tpg->vdownsampling[p];
+	}
+	return w;
+}
+
+static inline unsigned tpg_calc_line_width(const struct tpg_data *tpg,
+					   unsigned plane, unsigned bpl)
+{
+	unsigned w = 0;
+	unsigned p;
+
+	if (tpg->buffers > 1)
+		return bpl;
+	for (p = 0; p < tpg_g_planes(tpg); p++) {
+		unsigned plane_w = bpl * tpg->twopixelsize[p] / tpg->twopixelsize[0];
+
+		plane_w /= tpg->hdownsampling[p];
+		w += plane_w / tpg->vdownsampling[p];
+	}
+	return w;
+}
+
+static inline unsigned tpg_calc_plane_size(const struct tpg_data *tpg, unsigned plane)
+{
+	if (plane >= tpg_g_planes(tpg))
+		return 0;
+
+	return tpg_g_bytesperline(tpg, plane) * tpg->buf_height /
+	       tpg->vdownsampling[plane];
+}
+
+static inline void tpg_s_buf_height(struct tpg_data *tpg, unsigned h)
+{
+	tpg->buf_height = h;
+}
+
+static inline void tpg_s_field(struct tpg_data *tpg, unsigned field, bool alternate)
+{
+	tpg->field = field;
+	tpg->field_alternate = alternate;
+}
+
+static inline void tpg_s_perc_fill(struct tpg_data *tpg,
+				      unsigned perc_fill)
+{
+	tpg->perc_fill = perc_fill;
+}
+
+static inline unsigned tpg_g_perc_fill(const struct tpg_data *tpg)
+{
+	return tpg->perc_fill;
+}
+
+static inline void tpg_s_perc_fill_blank(struct tpg_data *tpg,
+					 bool perc_fill_blank)
+{
+	tpg->perc_fill_blank = perc_fill_blank;
+}
+
+static inline void tpg_s_video_aspect(struct tpg_data *tpg,
+					enum tpg_video_aspect vid_aspect)
+{
+	if (tpg->vid_aspect == vid_aspect)
+		return;
+	tpg->vid_aspect = vid_aspect;
+	tpg->recalc_square_border = true;
+}
+
+static inline enum tpg_video_aspect tpg_g_video_aspect(const struct tpg_data *tpg)
+{
+	return tpg->vid_aspect;
+}
+
+static inline void tpg_s_pixel_aspect(struct tpg_data *tpg,
+					enum tpg_pixel_aspect pix_aspect)
+{
+	if (tpg->pix_aspect == pix_aspect)
+		return;
+	tpg->pix_aspect = pix_aspect;
+	tpg->recalc_square_border = true;
+}
+
+static inline void tpg_s_show_border(struct tpg_data *tpg,
+					bool show_border)
+{
+	tpg->show_border = show_border;
+}
+
+static inline void tpg_s_show_square(struct tpg_data *tpg,
+					bool show_square)
+{
+	tpg->show_square = show_square;
+}
+
+static inline void tpg_s_insert_sav(struct tpg_data *tpg, bool insert_sav)
+{
+	tpg->insert_sav = insert_sav;
+}
+
+static inline void tpg_s_insert_eav(struct tpg_data *tpg, bool insert_eav)
+{
+	tpg->insert_eav = insert_eav;
+}
+
+void tpg_update_mv_step(struct tpg_data *tpg);
+
+static inline void tpg_s_mv_hor_mode(struct tpg_data *tpg,
+				enum tpg_move_mode mv_hor_mode)
+{
+	tpg->mv_hor_mode = mv_hor_mode;
+	tpg_update_mv_step(tpg);
+}
+
+static inline void tpg_s_mv_vert_mode(struct tpg_data *tpg,
+				enum tpg_move_mode mv_vert_mode)
+{
+	tpg->mv_vert_mode = mv_vert_mode;
+	tpg_update_mv_step(tpg);
+}
+
+static inline void tpg_init_mv_count(struct tpg_data *tpg)
+{
+	tpg->mv_hor_count = tpg->mv_vert_count = 0;
+}
+
+static inline void tpg_update_mv_count(struct tpg_data *tpg, bool frame_is_field)
+{
+	tpg->mv_hor_count += tpg->mv_hor_step * (frame_is_field ? 1 : 2);
+	tpg->mv_vert_count += tpg->mv_vert_step * (frame_is_field ? 1 : 2);
+}
+
+static inline void tpg_s_hflip(struct tpg_data *tpg, bool hflip)
+{
+	if (tpg->hflip == hflip)
+		return;
+	tpg->hflip = hflip;
+	tpg_update_mv_step(tpg);
+	tpg->recalc_lines = true;
+}
+
+static inline bool tpg_g_hflip(const struct tpg_data *tpg)
+{
+	return tpg->hflip;
+}
+
+static inline void tpg_s_vflip(struct tpg_data *tpg, bool vflip)
+{
+	tpg->vflip = vflip;
+}
+
+static inline bool tpg_g_vflip(const struct tpg_data *tpg)
+{
+	return tpg->vflip;
+}
+
+static inline bool tpg_pattern_is_static(const struct tpg_data *tpg)
+{
+	return tpg->pattern != TPG_PAT_NOISE &&
+	       tpg->mv_hor_mode == TPG_MOVE_NONE &&
+	       tpg->mv_vert_mode == TPG_MOVE_NONE;
+}
+
+#endif
diff --git a/drivers/media/platform/vivid/vivid-vbi-cap.c b/drivers/media/platform/vivid/vivid-vbi-cap.c
new file mode 100644
index 0000000..e903d02
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-vbi-cap.c
@@ -0,0 +1,375 @@
+/*
+ * vivid-vbi-cap.c - vbi capture support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-common.h>
+
+#include "vivid-core.h"
+#include "vivid-kthread-cap.h"
+#include "vivid-vbi-cap.h"
+#include "vivid-vbi-gen.h"
+
+static void vivid_sliced_vbi_cap_fill(struct vivid_dev *dev, unsigned seqnr)
+{
+	struct vivid_vbi_gen_data *vbi_gen = &dev->vbi_gen;
+	bool is_60hz = dev->std_cap & V4L2_STD_525_60;
+
+	vivid_vbi_gen_sliced(vbi_gen, is_60hz, seqnr);
+
+	if (!is_60hz) {
+		if (dev->loop_video) {
+			if (dev->vbi_out_have_wss) {
+				vbi_gen->data[12].data[0] = dev->vbi_out_wss[0];
+				vbi_gen->data[12].data[1] = dev->vbi_out_wss[1];
+			} else {
+				vbi_gen->data[12].id = 0;
+			}
+		} else {
+			switch (tpg_g_video_aspect(&dev->tpg)) {
+			case TPG_VIDEO_ASPECT_14X9_CENTRE:
+				vbi_gen->data[12].data[0] = 0x01;
+				break;
+			case TPG_VIDEO_ASPECT_16X9_CENTRE:
+				vbi_gen->data[12].data[0] = 0x0b;
+				break;
+			case TPG_VIDEO_ASPECT_16X9_ANAMORPHIC:
+				vbi_gen->data[12].data[0] = 0x07;
+				break;
+			case TPG_VIDEO_ASPECT_4X3:
+			default:
+				vbi_gen->data[12].data[0] = 0x08;
+				break;
+			}
+		}
+	} else if (dev->loop_video && is_60hz) {
+		if (dev->vbi_out_have_cc[0]) {
+			vbi_gen->data[0].data[0] = dev->vbi_out_cc[0][0];
+			vbi_gen->data[0].data[1] = dev->vbi_out_cc[0][1];
+		} else {
+			vbi_gen->data[0].id = 0;
+		}
+		if (dev->vbi_out_have_cc[1]) {
+			vbi_gen->data[1].data[0] = dev->vbi_out_cc[1][0];
+			vbi_gen->data[1].data[1] = dev->vbi_out_cc[1][1];
+		} else {
+			vbi_gen->data[1].id = 0;
+		}
+	}
+}
+
+static void vivid_g_fmt_vbi_cap(struct vivid_dev *dev, struct v4l2_vbi_format *vbi)
+{
+	bool is_60hz = dev->std_cap & V4L2_STD_525_60;
+
+	vbi->sampling_rate = 27000000;
+	vbi->offset = 24;
+	vbi->samples_per_line = 1440;
+	vbi->sample_format = V4L2_PIX_FMT_GREY;
+	vbi->start[0] = is_60hz ? V4L2_VBI_ITU_525_F1_START + 9 : V4L2_VBI_ITU_625_F1_START + 5;
+	vbi->start[1] = is_60hz ? V4L2_VBI_ITU_525_F2_START + 9 : V4L2_VBI_ITU_625_F2_START + 5;
+	vbi->count[0] = vbi->count[1] = is_60hz ? 12 : 18;
+	vbi->flags = dev->vbi_cap_interlaced ? V4L2_VBI_INTERLACED : 0;
+	vbi->reserved[0] = 0;
+	vbi->reserved[1] = 0;
+}
+
+void vivid_raw_vbi_cap_process(struct vivid_dev *dev, struct vivid_buffer *buf)
+{
+	struct v4l2_vbi_format vbi;
+	u8 *vbuf = vb2_plane_vaddr(&buf->vb.vb2_buf, 0);
+
+	vivid_g_fmt_vbi_cap(dev, &vbi);
+	buf->vb.sequence = dev->vbi_cap_seq_count;
+	if (dev->field_cap == V4L2_FIELD_ALTERNATE)
+		buf->vb.sequence /= 2;
+
+	vivid_sliced_vbi_cap_fill(dev, buf->vb.sequence);
+
+	memset(vbuf, 0x10, vb2_plane_size(&buf->vb.vb2_buf, 0));
+
+	if (!VIVID_INVALID_SIGNAL(dev->std_signal_mode))
+		vivid_vbi_gen_raw(&dev->vbi_gen, &vbi, vbuf);
+
+	v4l2_get_timestamp(&buf->vb.timestamp);
+	buf->vb.timestamp.tv_sec += dev->time_wrap_offset;
+}
+
+
+void vivid_sliced_vbi_cap_process(struct vivid_dev *dev,
+			struct vivid_buffer *buf)
+{
+	struct v4l2_sliced_vbi_data *vbuf =
+			vb2_plane_vaddr(&buf->vb.vb2_buf, 0);
+
+	buf->vb.sequence = dev->vbi_cap_seq_count;
+	if (dev->field_cap == V4L2_FIELD_ALTERNATE)
+		buf->vb.sequence /= 2;
+
+	vivid_sliced_vbi_cap_fill(dev, buf->vb.sequence);
+
+	memset(vbuf, 0, vb2_plane_size(&buf->vb.vb2_buf, 0));
+	if (!VIVID_INVALID_SIGNAL(dev->std_signal_mode)) {
+		unsigned i;
+
+		for (i = 0; i < 25; i++)
+			vbuf[i] = dev->vbi_gen.data[i];
+	}
+
+	v4l2_get_timestamp(&buf->vb.timestamp);
+	buf->vb.timestamp.tv_sec += dev->time_wrap_offset;
+}
+
+static int vbi_cap_queue_setup(struct vb2_queue *vq, const void *parg,
+		       unsigned *nbuffers, unsigned *nplanes,
+		       unsigned sizes[], void *alloc_ctxs[])
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vq);
+	bool is_60hz = dev->std_cap & V4L2_STD_525_60;
+	unsigned size = vq->type == V4L2_BUF_TYPE_SLICED_VBI_CAPTURE ?
+		36 * sizeof(struct v4l2_sliced_vbi_data) :
+		1440 * 2 * (is_60hz ? 12 : 18);
+
+	if (!vivid_is_sdtv_cap(dev))
+		return -EINVAL;
+
+	sizes[0] = size;
+
+	if (vq->num_buffers + *nbuffers < 2)
+		*nbuffers = 2 - vq->num_buffers;
+
+	*nplanes = 1;
+	return 0;
+}
+
+static int vbi_cap_buf_prepare(struct vb2_buffer *vb)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+	bool is_60hz = dev->std_cap & V4L2_STD_525_60;
+	unsigned size = vb->vb2_queue->type == V4L2_BUF_TYPE_SLICED_VBI_CAPTURE ?
+		36 * sizeof(struct v4l2_sliced_vbi_data) :
+		1440 * 2 * (is_60hz ? 12 : 18);
+
+	dprintk(dev, 1, "%s\n", __func__);
+
+	if (dev->buf_prepare_error) {
+		/*
+		 * Error injection: test what happens if buf_prepare() returns
+		 * an error.
+		 */
+		dev->buf_prepare_error = false;
+		return -EINVAL;
+	}
+	if (vb2_plane_size(vb, 0) < size) {
+		dprintk(dev, 1, "%s data will not fit into plane (%lu < %u)\n",
+				__func__, vb2_plane_size(vb, 0), size);
+		return -EINVAL;
+	}
+	vb2_set_plane_payload(vb, 0, size);
+
+	return 0;
+}
+
+static void vbi_cap_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+	struct vivid_buffer *buf = container_of(vbuf, struct vivid_buffer, vb);
+
+	dprintk(dev, 1, "%s\n", __func__);
+
+	spin_lock(&dev->slock);
+	list_add_tail(&buf->list, &dev->vbi_cap_active);
+	spin_unlock(&dev->slock);
+}
+
+static int vbi_cap_start_streaming(struct vb2_queue *vq, unsigned count)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vq);
+	int err;
+
+	dprintk(dev, 1, "%s\n", __func__);
+	dev->vbi_cap_seq_count = 0;
+	if (dev->start_streaming_error) {
+		dev->start_streaming_error = false;
+		err = -EINVAL;
+	} else {
+		err = vivid_start_generating_vid_cap(dev, &dev->vbi_cap_streaming);
+	}
+	if (err) {
+		struct vivid_buffer *buf, *tmp;
+
+		list_for_each_entry_safe(buf, tmp, &dev->vbi_cap_active, list) {
+			list_del(&buf->list);
+			vb2_buffer_done(&buf->vb.vb2_buf,
+					VB2_BUF_STATE_QUEUED);
+		}
+	}
+	return err;
+}
+
+/* abort streaming and wait for last buffer */
+static void vbi_cap_stop_streaming(struct vb2_queue *vq)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vq);
+
+	dprintk(dev, 1, "%s\n", __func__);
+	vivid_stop_generating_vid_cap(dev, &dev->vbi_cap_streaming);
+}
+
+const struct vb2_ops vivid_vbi_cap_qops = {
+	.queue_setup		= vbi_cap_queue_setup,
+	.buf_prepare		= vbi_cap_buf_prepare,
+	.buf_queue		= vbi_cap_buf_queue,
+	.start_streaming	= vbi_cap_start_streaming,
+	.stop_streaming		= vbi_cap_stop_streaming,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
+};
+
+int vidioc_g_fmt_vbi_cap(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct v4l2_vbi_format *vbi = &f->fmt.vbi;
+
+	if (!vivid_is_sdtv_cap(dev) || !dev->has_raw_vbi_cap)
+		return -EINVAL;
+
+	vivid_g_fmt_vbi_cap(dev, vbi);
+	return 0;
+}
+
+int vidioc_s_fmt_vbi_cap(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	int ret = vidioc_g_fmt_vbi_cap(file, priv, f);
+
+	if (ret)
+		return ret;
+	if (dev->stream_sliced_vbi_cap && vb2_is_busy(&dev->vb_vbi_cap_q))
+		return -EBUSY;
+	dev->stream_sliced_vbi_cap = false;
+	dev->vbi_cap_dev.queue->type = V4L2_BUF_TYPE_VBI_CAPTURE;
+	return 0;
+}
+
+void vivid_fill_service_lines(struct v4l2_sliced_vbi_format *vbi, u32 service_set)
+{
+	vbi->io_size = sizeof(struct v4l2_sliced_vbi_data) * 36;
+	vbi->service_set = service_set;
+	memset(vbi->service_lines, 0, sizeof(vbi->service_lines));
+	memset(vbi->reserved, 0, sizeof(vbi->reserved));
+
+	if (vbi->service_set == 0)
+		return;
+
+	if (vbi->service_set & V4L2_SLICED_CAPTION_525) {
+		vbi->service_lines[0][21] = V4L2_SLICED_CAPTION_525;
+		vbi->service_lines[1][21] = V4L2_SLICED_CAPTION_525;
+	}
+	if (vbi->service_set & V4L2_SLICED_WSS_625) {
+		unsigned i;
+
+		for (i = 7; i <= 18; i++)
+			vbi->service_lines[0][i] =
+			vbi->service_lines[1][i] = V4L2_SLICED_TELETEXT_B;
+		vbi->service_lines[0][23] = V4L2_SLICED_WSS_625;
+	}
+}
+
+int vidioc_g_fmt_sliced_vbi_cap(struct file *file, void *fh, struct v4l2_format *fmt)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct v4l2_sliced_vbi_format *vbi = &fmt->fmt.sliced;
+
+	if (!vivid_is_sdtv_cap(dev) || !dev->has_sliced_vbi_cap)
+		return -EINVAL;
+
+	vivid_fill_service_lines(vbi, dev->service_set_cap);
+	return 0;
+}
+
+int vidioc_try_fmt_sliced_vbi_cap(struct file *file, void *fh, struct v4l2_format *fmt)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct v4l2_sliced_vbi_format *vbi = &fmt->fmt.sliced;
+	bool is_60hz = dev->std_cap & V4L2_STD_525_60;
+	u32 service_set = vbi->service_set;
+
+	if (!vivid_is_sdtv_cap(dev) || !dev->has_sliced_vbi_cap)
+		return -EINVAL;
+
+	service_set &= is_60hz ? V4L2_SLICED_CAPTION_525 :
+				 V4L2_SLICED_WSS_625 | V4L2_SLICED_TELETEXT_B;
+	vivid_fill_service_lines(vbi, service_set);
+	return 0;
+}
+
+int vidioc_s_fmt_sliced_vbi_cap(struct file *file, void *fh, struct v4l2_format *fmt)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct v4l2_sliced_vbi_format *vbi = &fmt->fmt.sliced;
+	int ret = vidioc_try_fmt_sliced_vbi_cap(file, fh, fmt);
+
+	if (ret)
+		return ret;
+	if (!dev->stream_sliced_vbi_cap && vb2_is_busy(&dev->vb_vbi_cap_q))
+		return -EBUSY;
+	dev->service_set_cap = vbi->service_set;
+	dev->stream_sliced_vbi_cap = true;
+	dev->vbi_cap_dev.queue->type = V4L2_BUF_TYPE_SLICED_VBI_CAPTURE;
+	return 0;
+}
+
+int vidioc_g_sliced_vbi_cap(struct file *file, void *fh, struct v4l2_sliced_vbi_cap *cap)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct video_device *vdev = video_devdata(file);
+	bool is_60hz;
+
+	if (vdev->vfl_dir == VFL_DIR_RX) {
+		is_60hz = dev->std_cap & V4L2_STD_525_60;
+		if (!vivid_is_sdtv_cap(dev) || !dev->has_sliced_vbi_cap ||
+		    cap->type != V4L2_BUF_TYPE_SLICED_VBI_CAPTURE)
+			return -EINVAL;
+	} else {
+		is_60hz = dev->std_out & V4L2_STD_525_60;
+		if (!vivid_is_svid_out(dev) || !dev->has_sliced_vbi_out ||
+		    cap->type != V4L2_BUF_TYPE_SLICED_VBI_OUTPUT)
+			return -EINVAL;
+	}
+
+	cap->service_set = is_60hz ? V4L2_SLICED_CAPTION_525 :
+				     V4L2_SLICED_WSS_625 | V4L2_SLICED_TELETEXT_B;
+	if (is_60hz) {
+		cap->service_lines[0][21] = V4L2_SLICED_CAPTION_525;
+		cap->service_lines[1][21] = V4L2_SLICED_CAPTION_525;
+	} else {
+		unsigned i;
+
+		for (i = 7; i <= 18; i++)
+			cap->service_lines[0][i] =
+			cap->service_lines[1][i] = V4L2_SLICED_TELETEXT_B;
+		cap->service_lines[0][23] = V4L2_SLICED_WSS_625;
+	}
+	return 0;
+}
diff --git a/drivers/media/platform/vivid/vivid-vbi-cap.h b/drivers/media/platform/vivid/vivid-vbi-cap.h
new file mode 100644
index 0000000..2d8ea0b
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-vbi-cap.h
@@ -0,0 +1,40 @@
+/*
+ * vivid-vbi-cap.h - vbi capture support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VIVID_VBI_CAP_H_
+#define _VIVID_VBI_CAP_H_
+
+void vivid_fill_time_of_day_packet(u8 *packet);
+void vivid_raw_vbi_cap_process(struct vivid_dev *dev, struct vivid_buffer *buf);
+void vivid_sliced_vbi_cap_process(struct vivid_dev *dev, struct vivid_buffer *buf);
+void vivid_sliced_vbi_out_process(struct vivid_dev *dev, struct vivid_buffer *buf);
+int vidioc_g_fmt_vbi_cap(struct file *file, void *priv,
+					struct v4l2_format *f);
+int vidioc_s_fmt_vbi_cap(struct file *file, void *priv,
+					struct v4l2_format *f);
+int vidioc_g_fmt_sliced_vbi_cap(struct file *file, void *fh, struct v4l2_format *fmt);
+int vidioc_try_fmt_sliced_vbi_cap(struct file *file, void *fh, struct v4l2_format *fmt);
+int vidioc_s_fmt_sliced_vbi_cap(struct file *file, void *fh, struct v4l2_format *fmt);
+int vidioc_g_sliced_vbi_cap(struct file *file, void *fh, struct v4l2_sliced_vbi_cap *cap);
+
+void vivid_fill_service_lines(struct v4l2_sliced_vbi_format *vbi, u32 service_set);
+
+extern const struct vb2_ops vivid_vbi_cap_qops;
+
+#endif
diff --git a/drivers/media/platform/vivid/vivid-vbi-gen.c b/drivers/media/platform/vivid/vivid-vbi-gen.c
new file mode 100644
index 0000000..a2159de
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-vbi-gen.c
@@ -0,0 +1,323 @@
+/*
+ * vivid-vbi-gen.c - vbi generator support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/ktime.h>
+#include <linux/string.h>
+#include <linux/videodev2.h>
+
+#include "vivid-vbi-gen.h"
+
+static void wss_insert(u8 *wss, u32 val, unsigned size)
+{
+	while (size--)
+		*wss++ = (val & (1 << size)) ? 0xc0 : 0x10;
+}
+
+static void vivid_vbi_gen_wss_raw(const struct v4l2_sliced_vbi_data *data,
+		u8 *buf, unsigned sampling_rate)
+{
+	const unsigned rate = 5000000;	/* WSS has a 5 MHz transmission rate */
+	u8 wss[29 + 24 + 24 + 24 + 18 + 18] = { 0 };
+	const unsigned zero = 0x07;
+	const unsigned one = 0x38;
+	unsigned bit = 0;
+	u16 wss_data;
+	int i;
+
+	wss_insert(wss + bit, 0x1f1c71c7, 29); bit += 29;
+	wss_insert(wss + bit, 0x1e3c1f, 24); bit += 24;
+
+	wss_data = (data->data[1] << 8) | data->data[0];
+	for (i = 0; i <= 13; i++, bit += 6)
+		wss_insert(wss + bit, (wss_data & (1 << i)) ? one : zero, 6);
+
+	for (i = 0, bit = 0; bit < sizeof(wss); bit++) {
+		unsigned n = ((bit + 1) * sampling_rate) / rate;
+
+		while (i < n)
+			buf[i++] = wss[bit];
+	}
+}
+
+static void vivid_vbi_gen_teletext_raw(const struct v4l2_sliced_vbi_data *data,
+		u8 *buf, unsigned sampling_rate)
+{
+	const unsigned rate = 6937500 / 10;	/* Teletext has a 6.9375 MHz transmission rate */
+	u8 teletext[45] = { 0x55, 0x55, 0x27 };
+	unsigned bit = 0;
+	int i;
+
+	memcpy(teletext + 3, data->data, sizeof(teletext) - 3);
+	/* prevents 32 bit overflow */
+	sampling_rate /= 10;
+
+	for (i = 0, bit = 0; bit < sizeof(teletext) * 8; bit++) {
+		unsigned n = ((bit + 1) * sampling_rate) / rate;
+		u8 val = (teletext[bit / 8] & (1 << (bit & 7))) ? 0xc0 : 0x10;
+
+		while (i < n)
+			buf[i++] = val;
+	}
+}
+
+static void cc_insert(u8 *cc, u8 ch)
+{
+	unsigned tot = 0;
+	unsigned i;
+
+	for (i = 0; i < 7; i++) {
+		cc[2 * i] = cc[2 * i + 1] = (ch & (1 << i)) ? 1 : 0;
+		tot += cc[2 * i];
+	}
+	cc[14] = cc[15] = !(tot & 1);
+}
+
+#define CC_PREAMBLE_BITS (14 + 4 + 2)
+
+static void vivid_vbi_gen_cc_raw(const struct v4l2_sliced_vbi_data *data,
+		u8 *buf, unsigned sampling_rate)
+{
+	const unsigned rate = 1000000;	/* CC has a 1 MHz transmission rate */
+
+	u8 cc[CC_PREAMBLE_BITS + 2 * 16] = {
+		/* Clock run-in: 7 cycles */
+		0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1,
+		/* 2 cycles of 0 */
+		0, 0, 0, 0,
+		/* Start bit of 1 (each bit is two cycles) */
+		1, 1
+	};
+	unsigned bit, i;
+
+	cc_insert(cc + CC_PREAMBLE_BITS, data->data[0]);
+	cc_insert(cc + CC_PREAMBLE_BITS + 16, data->data[1]);
+
+	for (i = 0, bit = 0; bit < sizeof(cc); bit++) {
+		unsigned n = ((bit + 1) * sampling_rate) / rate;
+
+		while (i < n)
+			buf[i++] = cc[bit] ? 0xc0 : 0x10;
+	}
+}
+
+void vivid_vbi_gen_raw(const struct vivid_vbi_gen_data *vbi,
+		const struct v4l2_vbi_format *vbi_fmt, u8 *buf)
+{
+	unsigned idx;
+
+	for (idx = 0; idx < 25; idx++) {
+		const struct v4l2_sliced_vbi_data *data = vbi->data + idx;
+		unsigned start_2nd_field;
+		unsigned line = data->line;
+		u8 *linebuf = buf;
+
+		start_2nd_field = (data->id & V4L2_SLICED_VBI_525) ? 263 : 313;
+		if (data->field)
+			line += start_2nd_field;
+		line -= vbi_fmt->start[data->field];
+
+		if (vbi_fmt->flags & V4L2_VBI_INTERLACED)
+			linebuf += (line * 2 + data->field) *
+				vbi_fmt->samples_per_line;
+		else
+			linebuf += (line + data->field * vbi_fmt->count[0]) *
+				vbi_fmt->samples_per_line;
+		if (data->id == V4L2_SLICED_CAPTION_525)
+			vivid_vbi_gen_cc_raw(data, linebuf, vbi_fmt->sampling_rate);
+		else if (data->id == V4L2_SLICED_WSS_625)
+			vivid_vbi_gen_wss_raw(data, linebuf, vbi_fmt->sampling_rate);
+		else if (data->id == V4L2_SLICED_TELETEXT_B)
+			vivid_vbi_gen_teletext_raw(data, linebuf, vbi_fmt->sampling_rate);
+	}
+}
+
+static const u8 vivid_cc_sequence1[30] = {
+	0x14, 0x20,	/* Resume Caption Loading */
+	'H',  'e',
+	'l',  'l',
+	'o',  ' ',
+	'w',  'o',
+	'r',  'l',
+	'd',  '!',
+	0x14, 0x2f,	/* End of Caption */
+};
+
+static const u8 vivid_cc_sequence2[30] = {
+	0x14, 0x20,	/* Resume Caption Loading */
+	'C',  'l',
+	'o',  's',
+	'e',  'd',
+	' ',  'c',
+	'a',  'p',
+	't',  'i',
+	'o',  'n',
+	's',  ' ',
+	't',  'e',
+	's',  't',
+	0x14, 0x2f,	/* End of Caption */
+};
+
+static u8 calc_parity(u8 val)
+{
+	unsigned i;
+	unsigned tot = 0;
+
+	for (i = 0; i < 7; i++)
+		tot += (val & (1 << i)) ? 1 : 0;
+	return val | ((tot & 1) ? 0 : 0x80);
+}
+
+static void vivid_vbi_gen_set_time_of_day(u8 *packet)
+{
+	struct tm tm;
+	u8 checksum, i;
+
+	time_to_tm(get_seconds(), 0, &tm);
+	packet[0] = calc_parity(0x07);
+	packet[1] = calc_parity(0x01);
+	packet[2] = calc_parity(0x40 | tm.tm_min);
+	packet[3] = calc_parity(0x40 | tm.tm_hour);
+	packet[4] = calc_parity(0x40 | tm.tm_mday);
+	if (tm.tm_mday == 1 && tm.tm_mon == 2 &&
+	    sys_tz.tz_minuteswest > tm.tm_min + tm.tm_hour * 60)
+		packet[4] = calc_parity(0x60 | tm.tm_mday);
+	packet[5] = calc_parity(0x40 | (1 + tm.tm_mon));
+	packet[6] = calc_parity(0x40 | (1 + tm.tm_wday));
+	packet[7] = calc_parity(0x40 | ((tm.tm_year - 90) & 0x3f));
+	packet[8] = calc_parity(0x0f);
+	for (checksum = i = 0; i <= 8; i++)
+		checksum += packet[i] & 0x7f;
+	packet[9] = calc_parity(0x100 - checksum);
+	checksum = 0;
+	packet[10] = calc_parity(0x07);
+	packet[11] = calc_parity(0x04);
+	if (sys_tz.tz_minuteswest >= 0)
+		packet[12] = calc_parity(0x40 | ((sys_tz.tz_minuteswest / 60) & 0x1f));
+	else
+		packet[12] = calc_parity(0x40 | ((24 + sys_tz.tz_minuteswest / 60) & 0x1f));
+	packet[13] = calc_parity(0);
+	packet[14] = calc_parity(0x0f);
+	for (checksum = 0, i = 10; i <= 14; i++)
+		checksum += packet[i] & 0x7f;
+	packet[15] = calc_parity(0x100 - checksum);
+}
+
+static const u8 hamming[16] = {
+	0x15, 0x02, 0x49, 0x5e, 0x64, 0x73, 0x38, 0x2f,
+	0xd0, 0xc7, 0x8c, 0x9b, 0xa1, 0xb6, 0xfd, 0xea
+};
+
+static void vivid_vbi_gen_teletext(u8 *packet, unsigned line, unsigned frame)
+{
+	unsigned offset = 2;
+	unsigned i;
+
+	packet[0] = hamming[1 + ((line & 1) << 3)];
+	packet[1] = hamming[line >> 1];
+	memset(packet + 2, 0x20, 40);
+	if (line == 0) {
+		/* subcode */
+		packet[2] = hamming[frame % 10];
+		packet[3] = hamming[frame / 10];
+		packet[4] = hamming[0];
+		packet[5] = hamming[0];
+		packet[6] = hamming[0];
+		packet[7] = hamming[0];
+		packet[8] = hamming[0];
+		packet[9] = hamming[1];
+		offset = 10;
+	}
+	packet += offset;
+	memcpy(packet, "Page: 100 Row: 10", 17);
+	packet[7] = '0' + frame / 10;
+	packet[8] = '0' + frame % 10;
+	packet[15] = '0' + line / 10;
+	packet[16] = '0' + line % 10;
+	for (i = 0; i < 42 - offset; i++)
+		packet[i] = calc_parity(packet[i]);
+}
+
+void vivid_vbi_gen_sliced(struct vivid_vbi_gen_data *vbi,
+		bool is_60hz, unsigned seqnr)
+{
+	struct v4l2_sliced_vbi_data *data0 = vbi->data;
+	struct v4l2_sliced_vbi_data *data1 = vbi->data + 1;
+	unsigned frame = seqnr % 60;
+
+	memset(vbi->data, 0, sizeof(vbi->data));
+
+	if (!is_60hz) {
+		unsigned i;
+
+		for (i = 0; i <= 11; i++) {
+			data0->id = V4L2_SLICED_TELETEXT_B;
+			data0->line = 7 + i;
+			vivid_vbi_gen_teletext(data0->data, i, frame);
+			data0++;
+		}
+		data0->id = V4L2_SLICED_WSS_625;
+		data0->line = 23;
+		/* 4x3 video aspect ratio */
+		data0->data[0] = 0x08;
+		data0++;
+		for (i = 0; i <= 11; i++) {
+			data0->id = V4L2_SLICED_TELETEXT_B;
+			data0->field = 1;
+			data0->line = 7 + i;
+			vivid_vbi_gen_teletext(data0->data, 12 + i, frame);
+			data0++;
+		}
+		return;
+	}
+
+	data0->id = V4L2_SLICED_CAPTION_525;
+	data0->line = 21;
+	data1->id = V4L2_SLICED_CAPTION_525;
+	data1->field = 1;
+	data1->line = 21;
+
+	if (frame < 15) {
+		data0->data[0] = calc_parity(vivid_cc_sequence1[2 * frame]);
+		data0->data[1] = calc_parity(vivid_cc_sequence1[2 * frame + 1]);
+	} else if (frame >= 30 && frame < 45) {
+		frame -= 30;
+		data0->data[0] = calc_parity(vivid_cc_sequence2[2 * frame]);
+		data0->data[1] = calc_parity(vivid_cc_sequence2[2 * frame + 1]);
+	} else {
+		data0->data[0] = calc_parity(0);
+		data0->data[1] = calc_parity(0);
+	}
+
+	frame = seqnr % (30 * 60);
+	switch (frame) {
+	case 0:
+		vivid_vbi_gen_set_time_of_day(vbi->time_of_day_packet);
+		/* fall through */
+	case 1 ... 7:
+		data1->data[0] = vbi->time_of_day_packet[frame * 2];
+		data1->data[1] = vbi->time_of_day_packet[frame * 2 + 1];
+		break;
+	default:
+		data1->data[0] = calc_parity(0);
+		data1->data[1] = calc_parity(0);
+		break;
+	}
+}
diff --git a/drivers/media/platform/vivid/vivid-vbi-gen.h b/drivers/media/platform/vivid/vivid-vbi-gen.h
new file mode 100644
index 0000000..8444abe
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-vbi-gen.h
@@ -0,0 +1,33 @@
+/*
+ * vivid-vbi-gen.h - vbi generator support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VIVID_VBI_GEN_H_
+#define _VIVID_VBI_GEN_H_
+
+struct vivid_vbi_gen_data {
+	struct v4l2_sliced_vbi_data data[25];
+	u8 time_of_day_packet[16];
+};
+
+void vivid_vbi_gen_sliced(struct vivid_vbi_gen_data *vbi,
+		bool is_60hz, unsigned seqnr);
+void vivid_vbi_gen_raw(const struct vivid_vbi_gen_data *vbi,
+		const struct v4l2_vbi_format *vbi_fmt, u8 *buf);
+
+#endif
diff --git a/drivers/media/platform/vivid/vivid-vbi-out.c b/drivers/media/platform/vivid/vivid-vbi-out.c
new file mode 100644
index 0000000..75c5709
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-vbi-out.c
@@ -0,0 +1,254 @@
+/*
+ * vivid-vbi-out.c - vbi output support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-common.h>
+
+#include "vivid-core.h"
+#include "vivid-kthread-out.h"
+#include "vivid-vbi-out.h"
+#include "vivid-vbi-cap.h"
+
+static int vbi_out_queue_setup(struct vb2_queue *vq, const void *parg,
+		       unsigned *nbuffers, unsigned *nplanes,
+		       unsigned sizes[], void *alloc_ctxs[])
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vq);
+	bool is_60hz = dev->std_out & V4L2_STD_525_60;
+	unsigned size = vq->type == V4L2_BUF_TYPE_SLICED_VBI_OUTPUT ?
+		36 * sizeof(struct v4l2_sliced_vbi_data) :
+		1440 * 2 * (is_60hz ? 12 : 18);
+
+	if (!vivid_is_svid_out(dev))
+		return -EINVAL;
+
+	sizes[0] = size;
+
+	if (vq->num_buffers + *nbuffers < 2)
+		*nbuffers = 2 - vq->num_buffers;
+
+	*nplanes = 1;
+	return 0;
+}
+
+static int vbi_out_buf_prepare(struct vb2_buffer *vb)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+	bool is_60hz = dev->std_out & V4L2_STD_525_60;
+	unsigned size = vb->vb2_queue->type == V4L2_BUF_TYPE_SLICED_VBI_OUTPUT ?
+		36 * sizeof(struct v4l2_sliced_vbi_data) :
+		1440 * 2 * (is_60hz ? 12 : 18);
+
+	dprintk(dev, 1, "%s\n", __func__);
+
+	if (dev->buf_prepare_error) {
+		/*
+		 * Error injection: test what happens if buf_prepare() returns
+		 * an error.
+		 */
+		dev->buf_prepare_error = false;
+		return -EINVAL;
+	}
+	if (vb2_plane_size(vb, 0) < size) {
+		dprintk(dev, 1, "%s data will not fit into plane (%lu < %u)\n",
+				__func__, vb2_plane_size(vb, 0), size);
+		return -EINVAL;
+	}
+	vb2_set_plane_payload(vb, 0, size);
+
+	return 0;
+}
+
+static void vbi_out_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+	struct vivid_buffer *buf = container_of(vbuf, struct vivid_buffer, vb);
+
+	dprintk(dev, 1, "%s\n", __func__);
+
+	spin_lock(&dev->slock);
+	list_add_tail(&buf->list, &dev->vbi_out_active);
+	spin_unlock(&dev->slock);
+}
+
+static int vbi_out_start_streaming(struct vb2_queue *vq, unsigned count)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vq);
+	int err;
+
+	dprintk(dev, 1, "%s\n", __func__);
+	dev->vbi_out_seq_count = 0;
+	if (dev->start_streaming_error) {
+		dev->start_streaming_error = false;
+		err = -EINVAL;
+	} else {
+		err = vivid_start_generating_vid_out(dev, &dev->vbi_out_streaming);
+	}
+	if (err) {
+		struct vivid_buffer *buf, *tmp;
+
+		list_for_each_entry_safe(buf, tmp, &dev->vbi_out_active, list) {
+			list_del(&buf->list);
+			vb2_buffer_done(&buf->vb.vb2_buf,
+					VB2_BUF_STATE_QUEUED);
+		}
+	}
+	return err;
+}
+
+/* abort streaming and wait for last buffer */
+static void vbi_out_stop_streaming(struct vb2_queue *vq)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vq);
+
+	dprintk(dev, 1, "%s\n", __func__);
+	vivid_stop_generating_vid_out(dev, &dev->vbi_out_streaming);
+	dev->vbi_out_have_wss = false;
+	dev->vbi_out_have_cc[0] = false;
+	dev->vbi_out_have_cc[1] = false;
+}
+
+const struct vb2_ops vivid_vbi_out_qops = {
+	.queue_setup		= vbi_out_queue_setup,
+	.buf_prepare		= vbi_out_buf_prepare,
+	.buf_queue		= vbi_out_buf_queue,
+	.start_streaming	= vbi_out_start_streaming,
+	.stop_streaming		= vbi_out_stop_streaming,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
+};
+
+int vidioc_g_fmt_vbi_out(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct v4l2_vbi_format *vbi = &f->fmt.vbi;
+	bool is_60hz = dev->std_out & V4L2_STD_525_60;
+
+	if (!vivid_is_svid_out(dev) || !dev->has_raw_vbi_out)
+		return -EINVAL;
+
+	vbi->sampling_rate = 25000000;
+	vbi->offset = 24;
+	vbi->samples_per_line = 1440;
+	vbi->sample_format = V4L2_PIX_FMT_GREY;
+	vbi->start[0] = is_60hz ? V4L2_VBI_ITU_525_F1_START + 9 : V4L2_VBI_ITU_625_F1_START + 5;
+	vbi->start[1] = is_60hz ? V4L2_VBI_ITU_525_F2_START + 9 : V4L2_VBI_ITU_625_F2_START + 5;
+	vbi->count[0] = vbi->count[1] = is_60hz ? 12 : 18;
+	vbi->flags = dev->vbi_cap_interlaced ? V4L2_VBI_INTERLACED : 0;
+	vbi->reserved[0] = 0;
+	vbi->reserved[1] = 0;
+	return 0;
+}
+
+int vidioc_s_fmt_vbi_out(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	int ret = vidioc_g_fmt_vbi_out(file, priv, f);
+
+	if (ret)
+		return ret;
+	if (vb2_is_busy(&dev->vb_vbi_out_q))
+		return -EBUSY;
+	dev->stream_sliced_vbi_out = false;
+	dev->vbi_out_dev.queue->type = V4L2_BUF_TYPE_VBI_OUTPUT;
+	return 0;
+}
+
+int vidioc_g_fmt_sliced_vbi_out(struct file *file, void *fh, struct v4l2_format *fmt)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct v4l2_sliced_vbi_format *vbi = &fmt->fmt.sliced;
+
+	if (!vivid_is_svid_out(dev) || !dev->has_sliced_vbi_out)
+		return -EINVAL;
+
+	vivid_fill_service_lines(vbi, dev->service_set_out);
+	return 0;
+}
+
+int vidioc_try_fmt_sliced_vbi_out(struct file *file, void *fh, struct v4l2_format *fmt)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct v4l2_sliced_vbi_format *vbi = &fmt->fmt.sliced;
+	bool is_60hz = dev->std_out & V4L2_STD_525_60;
+	u32 service_set = vbi->service_set;
+
+	if (!vivid_is_svid_out(dev) || !dev->has_sliced_vbi_out)
+		return -EINVAL;
+
+	service_set &= is_60hz ? V4L2_SLICED_CAPTION_525 :
+				 V4L2_SLICED_WSS_625 | V4L2_SLICED_TELETEXT_B;
+	vivid_fill_service_lines(vbi, service_set);
+	return 0;
+}
+
+int vidioc_s_fmt_sliced_vbi_out(struct file *file, void *fh,
+		struct v4l2_format *fmt)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct v4l2_sliced_vbi_format *vbi = &fmt->fmt.sliced;
+	int ret = vidioc_try_fmt_sliced_vbi_out(file, fh, fmt);
+
+	if (ret)
+		return ret;
+	if (vb2_is_busy(&dev->vb_vbi_out_q))
+		return -EBUSY;
+	dev->service_set_out = vbi->service_set;
+	dev->stream_sliced_vbi_out = true;
+	dev->vbi_out_dev.queue->type = V4L2_BUF_TYPE_SLICED_VBI_OUTPUT;
+	return 0;
+}
+
+void vivid_sliced_vbi_out_process(struct vivid_dev *dev,
+		struct vivid_buffer *buf)
+{
+	struct v4l2_sliced_vbi_data *vbi =
+		vb2_plane_vaddr(&buf->vb.vb2_buf, 0);
+	unsigned elems =
+		vb2_get_plane_payload(&buf->vb.vb2_buf, 0) / sizeof(*vbi);
+
+	dev->vbi_out_have_cc[0] = false;
+	dev->vbi_out_have_cc[1] = false;
+	dev->vbi_out_have_wss = false;
+	while (elems--) {
+		switch (vbi->id) {
+		case V4L2_SLICED_CAPTION_525:
+			if ((dev->std_out & V4L2_STD_525_60) && vbi->line == 21) {
+				dev->vbi_out_have_cc[!!vbi->field] = true;
+				dev->vbi_out_cc[!!vbi->field][0] = vbi->data[0];
+				dev->vbi_out_cc[!!vbi->field][1] = vbi->data[1];
+			}
+			break;
+		case V4L2_SLICED_WSS_625:
+			if ((dev->std_out & V4L2_STD_625_50) &&
+			    vbi->field == 0 && vbi->line == 23) {
+				dev->vbi_out_have_wss = true;
+				dev->vbi_out_wss[0] = vbi->data[0];
+				dev->vbi_out_wss[1] = vbi->data[1];
+			}
+			break;
+		}
+		vbi++;
+	}
+}
diff --git a/drivers/media/platform/vivid/vivid-vbi-out.h b/drivers/media/platform/vivid/vivid-vbi-out.h
new file mode 100644
index 0000000..6555ba9
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-vbi-out.h
@@ -0,0 +1,34 @@
+/*
+ * vivid-vbi-out.h - vbi output support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VIVID_VBI_OUT_H_
+#define _VIVID_VBI_OUT_H_
+
+void vivid_sliced_vbi_out_process(struct vivid_dev *dev, struct vivid_buffer *buf);
+int vidioc_g_fmt_vbi_out(struct file *file, void *priv,
+					struct v4l2_format *f);
+int vidioc_s_fmt_vbi_out(struct file *file, void *priv,
+					struct v4l2_format *f);
+int vidioc_g_fmt_sliced_vbi_out(struct file *file, void *fh, struct v4l2_format *fmt);
+int vidioc_try_fmt_sliced_vbi_out(struct file *file, void *fh, struct v4l2_format *fmt);
+int vidioc_s_fmt_sliced_vbi_out(struct file *file, void *fh, struct v4l2_format *fmt);
+
+extern const struct vb2_ops vivid_vbi_out_qops;
+
+#endif
diff --git a/drivers/media/platform/vivid/vivid-vid-cap.c b/drivers/media/platform/vivid/vivid-vid-cap.c
new file mode 100644
index 0000000..ef54123
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-vid-cap.c
@@ -0,0 +1,1845 @@
+/*
+ * vivid-vid-cap.c - video capture support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/vmalloc.h>
+#include <linux/videodev2.h>
+#include <linux/v4l2-dv-timings.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-dv-timings.h>
+
+#include "vivid-core.h"
+#include "vivid-vid-common.h"
+#include "vivid-kthread-cap.h"
+#include "vivid-vid-cap.h"
+
+/* timeperframe: min/max and default */
+static const struct v4l2_fract
+	tpf_min     = {.numerator = 1,		.denominator = FPS_MAX},
+	tpf_max     = {.numerator = FPS_MAX,	.denominator = 1},
+	tpf_default = {.numerator = 1,		.denominator = 30};
+
+static const struct vivid_fmt formats_ovl[] = {
+	{
+		.fourcc   = V4L2_PIX_FMT_RGB565, /* gggbbbbb rrrrrggg */
+		.vdownsampling = { 1 },
+		.bit_depth = { 16 },
+		.planes   = 1,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_XRGB555, /* gggbbbbb arrrrrgg */
+		.vdownsampling = { 1 },
+		.bit_depth = { 16 },
+		.planes   = 1,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_ARGB555, /* gggbbbbb arrrrrgg */
+		.vdownsampling = { 1 },
+		.bit_depth = { 16 },
+		.planes   = 1,
+		.buffers = 1,
+	},
+};
+
+/* The number of discrete webcam framesizes */
+#define VIVID_WEBCAM_SIZES 4
+/* The number of discrete webcam frameintervals */
+#define VIVID_WEBCAM_IVALS (VIVID_WEBCAM_SIZES * 2)
+
+/* Sizes must be in increasing order */
+static const struct v4l2_frmsize_discrete webcam_sizes[VIVID_WEBCAM_SIZES] = {
+	{  320, 180 },
+	{  640, 360 },
+	{ 1280, 720 },
+	{ 1920, 1080 },
+};
+
+/*
+ * Intervals must be in increasing order and there must be twice as many
+ * elements in this array as there are in webcam_sizes.
+ */
+static const struct v4l2_fract webcam_intervals[VIVID_WEBCAM_IVALS] = {
+	{  1, 2 },
+	{  1, 5 },
+	{  1, 10 },
+	{  1, 15 },
+	{  1, 25 },
+	{  1, 30 },
+	{  1, 50 },
+	{  1, 60 },
+};
+
+static const struct v4l2_discrete_probe webcam_probe = {
+	webcam_sizes,
+	VIVID_WEBCAM_SIZES
+};
+
+static int vid_cap_queue_setup(struct vb2_queue *vq, const void *parg,
+		       unsigned *nbuffers, unsigned *nplanes,
+		       unsigned sizes[], void *alloc_ctxs[])
+{
+	const struct v4l2_format *fmt = parg;
+	struct vivid_dev *dev = vb2_get_drv_priv(vq);
+	unsigned buffers = tpg_g_buffers(&dev->tpg);
+	unsigned h = dev->fmt_cap_rect.height;
+	unsigned p;
+
+	if (dev->field_cap == V4L2_FIELD_ALTERNATE) {
+		/*
+		 * You cannot use read() with FIELD_ALTERNATE since the field
+		 * information (TOP/BOTTOM) cannot be passed back to the user.
+		 */
+		if (vb2_fileio_is_active(vq))
+			return -EINVAL;
+	}
+
+	if (dev->queue_setup_error) {
+		/*
+		 * Error injection: test what happens if queue_setup() returns
+		 * an error.
+		 */
+		dev->queue_setup_error = false;
+		return -EINVAL;
+	}
+	if (fmt) {
+		const struct v4l2_pix_format_mplane *mp;
+		struct v4l2_format mp_fmt;
+		const struct vivid_fmt *vfmt;
+
+		if (!V4L2_TYPE_IS_MULTIPLANAR(fmt->type)) {
+			fmt_sp2mp(fmt, &mp_fmt);
+			fmt = &mp_fmt;
+		}
+		mp = &fmt->fmt.pix_mp;
+		/*
+		 * Check if the number of planes in the specified format match
+		 * the number of buffers in the current format. You can't mix that.
+		 */
+		if (mp->num_planes != buffers)
+			return -EINVAL;
+		vfmt = vivid_get_format(dev, mp->pixelformat);
+		for (p = 0; p < buffers; p++) {
+			sizes[p] = mp->plane_fmt[p].sizeimage;
+			if (sizes[p] < tpg_g_line_width(&dev->tpg, p) * h +
+							vfmt->data_offset[p])
+				return -EINVAL;
+		}
+	} else {
+		for (p = 0; p < buffers; p++)
+			sizes[p] = tpg_g_line_width(&dev->tpg, p) * h +
+					dev->fmt_cap->data_offset[p];
+	}
+
+	if (vq->num_buffers + *nbuffers < 2)
+		*nbuffers = 2 - vq->num_buffers;
+
+	*nplanes = buffers;
+
+	/*
+	 * videobuf2-vmalloc allocator is context-less so no need to set
+	 * alloc_ctxs array.
+	 */
+
+	dprintk(dev, 1, "%s: count=%d\n", __func__, *nbuffers);
+	for (p = 0; p < buffers; p++)
+		dprintk(dev, 1, "%s: size[%u]=%u\n", __func__, p, sizes[p]);
+
+	return 0;
+}
+
+static int vid_cap_buf_prepare(struct vb2_buffer *vb)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+	unsigned long size;
+	unsigned buffers = tpg_g_buffers(&dev->tpg);
+	unsigned p;
+
+	dprintk(dev, 1, "%s\n", __func__);
+
+	if (WARN_ON(NULL == dev->fmt_cap))
+		return -EINVAL;
+
+	if (dev->buf_prepare_error) {
+		/*
+		 * Error injection: test what happens if buf_prepare() returns
+		 * an error.
+		 */
+		dev->buf_prepare_error = false;
+		return -EINVAL;
+	}
+	for (p = 0; p < buffers; p++) {
+		size = tpg_g_line_width(&dev->tpg, p) * dev->fmt_cap_rect.height +
+			dev->fmt_cap->data_offset[p];
+
+		if (vb2_plane_size(vb, p) < size) {
+			dprintk(dev, 1, "%s data will not fit into plane %u (%lu < %lu)\n",
+					__func__, p, vb2_plane_size(vb, p), size);
+			return -EINVAL;
+		}
+
+		vb2_set_plane_payload(vb, p, size);
+		vb->planes[p].data_offset = dev->fmt_cap->data_offset[p];
+	}
+
+	return 0;
+}
+
+static void vid_cap_buf_finish(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+	struct v4l2_timecode *tc = &vbuf->timecode;
+	unsigned fps = 25;
+	unsigned seq = vbuf->sequence;
+
+	if (!vivid_is_sdtv_cap(dev))
+		return;
+
+	/*
+	 * Set the timecode. Rarely used, so it is interesting to
+	 * test this.
+	 */
+	vbuf->flags |= V4L2_BUF_FLAG_TIMECODE;
+	if (dev->std_cap & V4L2_STD_525_60)
+		fps = 30;
+	tc->type = (fps == 30) ? V4L2_TC_TYPE_30FPS : V4L2_TC_TYPE_25FPS;
+	tc->flags = 0;
+	tc->frames = seq % fps;
+	tc->seconds = (seq / fps) % 60;
+	tc->minutes = (seq / (60 * fps)) % 60;
+	tc->hours = (seq / (60 * 60 * fps)) % 24;
+}
+
+static void vid_cap_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+	struct vivid_buffer *buf = container_of(vbuf, struct vivid_buffer, vb);
+
+	dprintk(dev, 1, "%s\n", __func__);
+
+	spin_lock(&dev->slock);
+	list_add_tail(&buf->list, &dev->vid_cap_active);
+	spin_unlock(&dev->slock);
+}
+
+static int vid_cap_start_streaming(struct vb2_queue *vq, unsigned count)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vq);
+	unsigned i;
+	int err;
+
+	if (vb2_is_streaming(&dev->vb_vid_out_q))
+		dev->can_loop_video = vivid_vid_can_loop(dev);
+
+	if (dev->kthread_vid_cap)
+		return 0;
+
+	dev->vid_cap_seq_count = 0;
+	dprintk(dev, 1, "%s\n", __func__);
+	for (i = 0; i < VIDEO_MAX_FRAME; i++)
+		dev->must_blank[i] = tpg_g_perc_fill(&dev->tpg) < 100;
+	if (dev->start_streaming_error) {
+		dev->start_streaming_error = false;
+		err = -EINVAL;
+	} else {
+		err = vivid_start_generating_vid_cap(dev, &dev->vid_cap_streaming);
+	}
+	if (err) {
+		struct vivid_buffer *buf, *tmp;
+
+		list_for_each_entry_safe(buf, tmp, &dev->vid_cap_active, list) {
+			list_del(&buf->list);
+			vb2_buffer_done(&buf->vb.vb2_buf,
+					VB2_BUF_STATE_QUEUED);
+		}
+	}
+	return err;
+}
+
+/* abort streaming and wait for last buffer */
+static void vid_cap_stop_streaming(struct vb2_queue *vq)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vq);
+
+	dprintk(dev, 1, "%s\n", __func__);
+	vivid_stop_generating_vid_cap(dev, &dev->vid_cap_streaming);
+	dev->can_loop_video = false;
+}
+
+const struct vb2_ops vivid_vid_cap_qops = {
+	.queue_setup		= vid_cap_queue_setup,
+	.buf_prepare		= vid_cap_buf_prepare,
+	.buf_finish		= vid_cap_buf_finish,
+	.buf_queue		= vid_cap_buf_queue,
+	.start_streaming	= vid_cap_start_streaming,
+	.stop_streaming		= vid_cap_stop_streaming,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
+};
+
+/*
+ * Determine the 'picture' quality based on the current TV frequency: either
+ * COLOR for a good 'signal', GRAY (grayscale picture) for a slightly off
+ * signal or NOISE for no signal.
+ */
+void vivid_update_quality(struct vivid_dev *dev)
+{
+	unsigned freq_modulus;
+
+	if (dev->loop_video && (vivid_is_svid_cap(dev) || vivid_is_hdmi_cap(dev))) {
+		/*
+		 * The 'noise' will only be replaced by the actual video
+		 * if the output video matches the input video settings.
+		 */
+		tpg_s_quality(&dev->tpg, TPG_QUAL_NOISE, 0);
+		return;
+	}
+	if (vivid_is_hdmi_cap(dev) && VIVID_INVALID_SIGNAL(dev->dv_timings_signal_mode)) {
+		tpg_s_quality(&dev->tpg, TPG_QUAL_NOISE, 0);
+		return;
+	}
+	if (vivid_is_sdtv_cap(dev) && VIVID_INVALID_SIGNAL(dev->std_signal_mode)) {
+		tpg_s_quality(&dev->tpg, TPG_QUAL_NOISE, 0);
+		return;
+	}
+	if (!vivid_is_tv_cap(dev)) {
+		tpg_s_quality(&dev->tpg, TPG_QUAL_COLOR, 0);
+		return;
+	}
+
+	/*
+	 * There is a fake channel every 6 MHz at 49.25, 55.25, etc.
+	 * From +/- 0.25 MHz around the channel there is color, and from
+	 * +/- 1 MHz there is grayscale (chroma is lost).
+	 * Everywhere else it is just noise.
+	 */
+	freq_modulus = (dev->tv_freq - 676 /* (43.25-1) * 16 */) % (6 * 16);
+	if (freq_modulus > 2 * 16) {
+		tpg_s_quality(&dev->tpg, TPG_QUAL_NOISE,
+			next_pseudo_random32(dev->tv_freq ^ 0x55) & 0x3f);
+		return;
+	}
+	if (freq_modulus < 12 /*0.75 * 16*/ || freq_modulus > 20 /*1.25 * 16*/)
+		tpg_s_quality(&dev->tpg, TPG_QUAL_GRAY, 0);
+	else
+		tpg_s_quality(&dev->tpg, TPG_QUAL_COLOR, 0);
+}
+
+/*
+ * Get the current picture quality and the associated afc value.
+ */
+static enum tpg_quality vivid_get_quality(struct vivid_dev *dev, s32 *afc)
+{
+	unsigned freq_modulus;
+
+	if (afc)
+		*afc = 0;
+	if (tpg_g_quality(&dev->tpg) == TPG_QUAL_COLOR ||
+	    tpg_g_quality(&dev->tpg) == TPG_QUAL_NOISE)
+		return tpg_g_quality(&dev->tpg);
+
+	/*
+	 * There is a fake channel every 6 MHz at 49.25, 55.25, etc.
+	 * From +/- 0.25 MHz around the channel there is color, and from
+	 * +/- 1 MHz there is grayscale (chroma is lost).
+	 * Everywhere else it is just gray.
+	 */
+	freq_modulus = (dev->tv_freq - 676 /* (43.25-1) * 16 */) % (6 * 16);
+	if (afc)
+		*afc = freq_modulus - 1 * 16;
+	return TPG_QUAL_GRAY;
+}
+
+enum tpg_video_aspect vivid_get_video_aspect(const struct vivid_dev *dev)
+{
+	if (vivid_is_sdtv_cap(dev))
+		return dev->std_aspect_ratio;
+
+	if (vivid_is_hdmi_cap(dev))
+		return dev->dv_timings_aspect_ratio;
+
+	return TPG_VIDEO_ASPECT_IMAGE;
+}
+
+static enum tpg_pixel_aspect vivid_get_pixel_aspect(const struct vivid_dev *dev)
+{
+	if (vivid_is_sdtv_cap(dev))
+		return (dev->std_cap & V4L2_STD_525_60) ?
+			TPG_PIXEL_ASPECT_NTSC : TPG_PIXEL_ASPECT_PAL;
+
+	if (vivid_is_hdmi_cap(dev) &&
+	    dev->src_rect.width == 720 && dev->src_rect.height <= 576)
+		return dev->src_rect.height == 480 ?
+			TPG_PIXEL_ASPECT_NTSC : TPG_PIXEL_ASPECT_PAL;
+
+	return TPG_PIXEL_ASPECT_SQUARE;
+}
+
+/*
+ * Called whenever the format has to be reset which can occur when
+ * changing inputs, standard, timings, etc.
+ */
+void vivid_update_format_cap(struct vivid_dev *dev, bool keep_controls)
+{
+	struct v4l2_bt_timings *bt = &dev->dv_timings_cap.bt;
+	unsigned size;
+
+	switch (dev->input_type[dev->input]) {
+	case WEBCAM:
+	default:
+		dev->src_rect.width = webcam_sizes[dev->webcam_size_idx].width;
+		dev->src_rect.height = webcam_sizes[dev->webcam_size_idx].height;
+		dev->timeperframe_vid_cap = webcam_intervals[dev->webcam_ival_idx];
+		dev->field_cap = V4L2_FIELD_NONE;
+		tpg_s_rgb_range(&dev->tpg, V4L2_DV_RGB_RANGE_AUTO);
+		break;
+	case TV:
+	case SVID:
+		dev->field_cap = dev->tv_field_cap;
+		dev->src_rect.width = 720;
+		if (dev->std_cap & V4L2_STD_525_60) {
+			dev->src_rect.height = 480;
+			dev->timeperframe_vid_cap = (struct v4l2_fract) { 1001, 30000 };
+			dev->service_set_cap = V4L2_SLICED_CAPTION_525;
+		} else {
+			dev->src_rect.height = 576;
+			dev->timeperframe_vid_cap = (struct v4l2_fract) { 1000, 25000 };
+			dev->service_set_cap = V4L2_SLICED_WSS_625 | V4L2_SLICED_TELETEXT_B;
+		}
+		tpg_s_rgb_range(&dev->tpg, V4L2_DV_RGB_RANGE_AUTO);
+		break;
+	case HDMI:
+		dev->src_rect.width = bt->width;
+		dev->src_rect.height = bt->height;
+		size = V4L2_DV_BT_FRAME_WIDTH(bt) * V4L2_DV_BT_FRAME_HEIGHT(bt);
+		dev->timeperframe_vid_cap = (struct v4l2_fract) {
+			size / 100, (u32)bt->pixelclock / 100
+		};
+		if (bt->interlaced)
+			dev->field_cap = V4L2_FIELD_ALTERNATE;
+		else
+			dev->field_cap = V4L2_FIELD_NONE;
+
+		/*
+		 * We can be called from within s_ctrl, in that case we can't
+		 * set/get controls. Luckily we don't need to in that case.
+		 */
+		if (keep_controls || !dev->colorspace)
+			break;
+		if (bt->flags & V4L2_DV_FL_IS_CE_VIDEO) {
+			if (bt->width == 720 && bt->height <= 576)
+				v4l2_ctrl_s_ctrl(dev->colorspace, VIVID_CS_170M);
+			else
+				v4l2_ctrl_s_ctrl(dev->colorspace, VIVID_CS_709);
+			v4l2_ctrl_s_ctrl(dev->real_rgb_range_cap, 1);
+		} else {
+			v4l2_ctrl_s_ctrl(dev->colorspace, VIVID_CS_SRGB);
+			v4l2_ctrl_s_ctrl(dev->real_rgb_range_cap, 0);
+		}
+		tpg_s_rgb_range(&dev->tpg, v4l2_ctrl_g_ctrl(dev->rgb_range_cap));
+		break;
+	}
+	vivid_update_quality(dev);
+	tpg_reset_source(&dev->tpg, dev->src_rect.width, dev->src_rect.height, dev->field_cap);
+	dev->crop_cap = dev->src_rect;
+	dev->crop_bounds_cap = dev->src_rect;
+	dev->compose_cap = dev->crop_cap;
+	if (V4L2_FIELD_HAS_T_OR_B(dev->field_cap))
+		dev->compose_cap.height /= 2;
+	dev->fmt_cap_rect = dev->compose_cap;
+	tpg_s_video_aspect(&dev->tpg, vivid_get_video_aspect(dev));
+	tpg_s_pixel_aspect(&dev->tpg, vivid_get_pixel_aspect(dev));
+	tpg_update_mv_step(&dev->tpg);
+}
+
+/* Map the field to something that is valid for the current input */
+static enum v4l2_field vivid_field_cap(struct vivid_dev *dev, enum v4l2_field field)
+{
+	if (vivid_is_sdtv_cap(dev)) {
+		switch (field) {
+		case V4L2_FIELD_INTERLACED_TB:
+		case V4L2_FIELD_INTERLACED_BT:
+		case V4L2_FIELD_SEQ_TB:
+		case V4L2_FIELD_SEQ_BT:
+		case V4L2_FIELD_TOP:
+		case V4L2_FIELD_BOTTOM:
+		case V4L2_FIELD_ALTERNATE:
+			return field;
+		case V4L2_FIELD_INTERLACED:
+		default:
+			return V4L2_FIELD_INTERLACED;
+		}
+	}
+	if (vivid_is_hdmi_cap(dev))
+		return dev->dv_timings_cap.bt.interlaced ? V4L2_FIELD_ALTERNATE :
+						       V4L2_FIELD_NONE;
+	return V4L2_FIELD_NONE;
+}
+
+static unsigned vivid_colorspace_cap(struct vivid_dev *dev)
+{
+	if (!dev->loop_video || vivid_is_webcam(dev) || vivid_is_tv_cap(dev))
+		return tpg_g_colorspace(&dev->tpg);
+	return dev->colorspace_out;
+}
+
+static unsigned vivid_xfer_func_cap(struct vivid_dev *dev)
+{
+	if (!dev->loop_video || vivid_is_webcam(dev) || vivid_is_tv_cap(dev))
+		return tpg_g_xfer_func(&dev->tpg);
+	return dev->xfer_func_out;
+}
+
+static unsigned vivid_ycbcr_enc_cap(struct vivid_dev *dev)
+{
+	if (!dev->loop_video || vivid_is_webcam(dev) || vivid_is_tv_cap(dev))
+		return tpg_g_ycbcr_enc(&dev->tpg);
+	return dev->ycbcr_enc_out;
+}
+
+static unsigned vivid_quantization_cap(struct vivid_dev *dev)
+{
+	if (!dev->loop_video || vivid_is_webcam(dev) || vivid_is_tv_cap(dev))
+		return tpg_g_quantization(&dev->tpg);
+	return dev->quantization_out;
+}
+
+int vivid_g_fmt_vid_cap(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct v4l2_pix_format_mplane *mp = &f->fmt.pix_mp;
+	unsigned p;
+
+	mp->width        = dev->fmt_cap_rect.width;
+	mp->height       = dev->fmt_cap_rect.height;
+	mp->field        = dev->field_cap;
+	mp->pixelformat  = dev->fmt_cap->fourcc;
+	mp->colorspace   = vivid_colorspace_cap(dev);
+	mp->xfer_func    = vivid_xfer_func_cap(dev);
+	mp->ycbcr_enc    = vivid_ycbcr_enc_cap(dev);
+	mp->quantization = vivid_quantization_cap(dev);
+	mp->num_planes = dev->fmt_cap->buffers;
+	for (p = 0; p < mp->num_planes; p++) {
+		mp->plane_fmt[p].bytesperline = tpg_g_bytesperline(&dev->tpg, p);
+		mp->plane_fmt[p].sizeimage =
+			tpg_g_line_width(&dev->tpg, p) * mp->height +
+			dev->fmt_cap->data_offset[p];
+	}
+	return 0;
+}
+
+int vivid_try_fmt_vid_cap(struct file *file, void *priv,
+			struct v4l2_format *f)
+{
+	struct v4l2_pix_format_mplane *mp = &f->fmt.pix_mp;
+	struct v4l2_plane_pix_format *pfmt = mp->plane_fmt;
+	struct vivid_dev *dev = video_drvdata(file);
+	const struct vivid_fmt *fmt;
+	unsigned bytesperline, max_bpl;
+	unsigned factor = 1;
+	unsigned w, h;
+	unsigned p;
+
+	fmt = vivid_get_format(dev, mp->pixelformat);
+	if (!fmt) {
+		dprintk(dev, 1, "Fourcc format (0x%08x) unknown.\n",
+			mp->pixelformat);
+		mp->pixelformat = V4L2_PIX_FMT_YUYV;
+		fmt = vivid_get_format(dev, mp->pixelformat);
+	}
+
+	mp->field = vivid_field_cap(dev, mp->field);
+	if (vivid_is_webcam(dev)) {
+		const struct v4l2_frmsize_discrete *sz =
+			v4l2_find_nearest_format(&webcam_probe, mp->width, mp->height);
+
+		w = sz->width;
+		h = sz->height;
+	} else if (vivid_is_sdtv_cap(dev)) {
+		w = 720;
+		h = (dev->std_cap & V4L2_STD_525_60) ? 480 : 576;
+	} else {
+		w = dev->src_rect.width;
+		h = dev->src_rect.height;
+	}
+	if (V4L2_FIELD_HAS_T_OR_B(mp->field))
+		factor = 2;
+	if (vivid_is_webcam(dev) ||
+	    (!dev->has_scaler_cap && !dev->has_crop_cap && !dev->has_compose_cap)) {
+		mp->width = w;
+		mp->height = h / factor;
+	} else {
+		struct v4l2_rect r = { 0, 0, mp->width, mp->height * factor };
+
+		rect_set_min_size(&r, &vivid_min_rect);
+		rect_set_max_size(&r, &vivid_max_rect);
+		if (dev->has_scaler_cap && !dev->has_compose_cap) {
+			struct v4l2_rect max_r = { 0, 0, MAX_ZOOM * w, MAX_ZOOM * h };
+
+			rect_set_max_size(&r, &max_r);
+		} else if (!dev->has_scaler_cap && dev->has_crop_cap && !dev->has_compose_cap) {
+			rect_set_max_size(&r, &dev->src_rect);
+		} else if (!dev->has_scaler_cap && !dev->has_crop_cap) {
+			rect_set_min_size(&r, &dev->src_rect);
+		}
+		mp->width = r.width;
+		mp->height = r.height / factor;
+	}
+
+	/* This driver supports custom bytesperline values */
+
+	mp->num_planes = fmt->buffers;
+	for (p = 0; p < mp->num_planes; p++) {
+		/* Calculate the minimum supported bytesperline value */
+		bytesperline = (mp->width * fmt->bit_depth[p]) >> 3;
+		/* Calculate the maximum supported bytesperline value */
+		max_bpl = (MAX_ZOOM * MAX_WIDTH * fmt->bit_depth[p]) >> 3;
+
+		if (pfmt[p].bytesperline > max_bpl)
+			pfmt[p].bytesperline = max_bpl;
+		if (pfmt[p].bytesperline < bytesperline)
+			pfmt[p].bytesperline = bytesperline;
+		pfmt[p].sizeimage = tpg_calc_line_width(&dev->tpg, p, pfmt[p].bytesperline) *
+			mp->height + fmt->data_offset[p];
+		memset(pfmt[p].reserved, 0, sizeof(pfmt[p].reserved));
+	}
+	mp->colorspace = vivid_colorspace_cap(dev);
+	mp->ycbcr_enc = vivid_ycbcr_enc_cap(dev);
+	mp->xfer_func = vivid_xfer_func_cap(dev);
+	mp->quantization = vivid_quantization_cap(dev);
+	memset(mp->reserved, 0, sizeof(mp->reserved));
+	return 0;
+}
+
+int vivid_s_fmt_vid_cap(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct v4l2_pix_format_mplane *mp = &f->fmt.pix_mp;
+	struct vivid_dev *dev = video_drvdata(file);
+	struct v4l2_rect *crop = &dev->crop_cap;
+	struct v4l2_rect *compose = &dev->compose_cap;
+	struct vb2_queue *q = &dev->vb_vid_cap_q;
+	int ret = vivid_try_fmt_vid_cap(file, priv, f);
+	unsigned factor = 1;
+	unsigned p;
+	unsigned i;
+
+	if (ret < 0)
+		return ret;
+
+	if (vb2_is_busy(q)) {
+		dprintk(dev, 1, "%s device busy\n", __func__);
+		return -EBUSY;
+	}
+
+	if (dev->overlay_cap_owner && dev->fb_cap.fmt.pixelformat != mp->pixelformat) {
+		dprintk(dev, 1, "overlay is active, can't change pixelformat\n");
+		return -EBUSY;
+	}
+
+	dev->fmt_cap = vivid_get_format(dev, mp->pixelformat);
+	if (V4L2_FIELD_HAS_T_OR_B(mp->field))
+		factor = 2;
+
+	/* Note: the webcam input doesn't support scaling, cropping or composing */
+
+	if (!vivid_is_webcam(dev) &&
+	    (dev->has_scaler_cap || dev->has_crop_cap || dev->has_compose_cap)) {
+		struct v4l2_rect r = { 0, 0, mp->width, mp->height };
+
+		if (dev->has_scaler_cap) {
+			if (dev->has_compose_cap)
+				rect_map_inside(compose, &r);
+			else
+				*compose = r;
+			if (dev->has_crop_cap && !dev->has_compose_cap) {
+				struct v4l2_rect min_r = {
+					0, 0,
+					r.width / MAX_ZOOM,
+					factor * r.height / MAX_ZOOM
+				};
+				struct v4l2_rect max_r = {
+					0, 0,
+					r.width * MAX_ZOOM,
+					factor * r.height * MAX_ZOOM
+				};
+
+				rect_set_min_size(crop, &min_r);
+				rect_set_max_size(crop, &max_r);
+				rect_map_inside(crop, &dev->crop_bounds_cap);
+			} else if (dev->has_crop_cap) {
+				struct v4l2_rect min_r = {
+					0, 0,
+					compose->width / MAX_ZOOM,
+					factor * compose->height / MAX_ZOOM
+				};
+				struct v4l2_rect max_r = {
+					0, 0,
+					compose->width * MAX_ZOOM,
+					factor * compose->height * MAX_ZOOM
+				};
+
+				rect_set_min_size(crop, &min_r);
+				rect_set_max_size(crop, &max_r);
+				rect_map_inside(crop, &dev->crop_bounds_cap);
+			}
+		} else if (dev->has_crop_cap && !dev->has_compose_cap) {
+			r.height *= factor;
+			rect_set_size_to(crop, &r);
+			rect_map_inside(crop, &dev->crop_bounds_cap);
+			r = *crop;
+			r.height /= factor;
+			rect_set_size_to(compose, &r);
+		} else if (!dev->has_crop_cap) {
+			rect_map_inside(compose, &r);
+		} else {
+			r.height *= factor;
+			rect_set_max_size(crop, &r);
+			rect_map_inside(crop, &dev->crop_bounds_cap);
+			compose->top *= factor;
+			compose->height *= factor;
+			rect_set_size_to(compose, crop);
+			rect_map_inside(compose, &r);
+			compose->top /= factor;
+			compose->height /= factor;
+		}
+	} else if (vivid_is_webcam(dev)) {
+		/* Guaranteed to be a match */
+		for (i = 0; i < ARRAY_SIZE(webcam_sizes); i++)
+			if (webcam_sizes[i].width == mp->width &&
+					webcam_sizes[i].height == mp->height)
+				break;
+		dev->webcam_size_idx = i;
+		if (dev->webcam_ival_idx >= 2 * (VIVID_WEBCAM_SIZES - i))
+			dev->webcam_ival_idx = 2 * (VIVID_WEBCAM_SIZES - i) - 1;
+		vivid_update_format_cap(dev, false);
+	} else {
+		struct v4l2_rect r = { 0, 0, mp->width, mp->height };
+
+		rect_set_size_to(compose, &r);
+		r.height *= factor;
+		rect_set_size_to(crop, &r);
+	}
+
+	dev->fmt_cap_rect.width = mp->width;
+	dev->fmt_cap_rect.height = mp->height;
+	tpg_s_buf_height(&dev->tpg, mp->height);
+	tpg_s_fourcc(&dev->tpg, dev->fmt_cap->fourcc);
+	for (p = 0; p < tpg_g_buffers(&dev->tpg); p++)
+		tpg_s_bytesperline(&dev->tpg, p, mp->plane_fmt[p].bytesperline);
+	dev->field_cap = mp->field;
+	if (dev->field_cap == V4L2_FIELD_ALTERNATE)
+		tpg_s_field(&dev->tpg, V4L2_FIELD_TOP, true);
+	else
+		tpg_s_field(&dev->tpg, dev->field_cap, false);
+	tpg_s_crop_compose(&dev->tpg, &dev->crop_cap, &dev->compose_cap);
+	if (vivid_is_sdtv_cap(dev))
+		dev->tv_field_cap = mp->field;
+	tpg_update_mv_step(&dev->tpg);
+	return 0;
+}
+
+int vidioc_g_fmt_vid_cap_mplane(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!dev->multiplanar)
+		return -ENOTTY;
+	return vivid_g_fmt_vid_cap(file, priv, f);
+}
+
+int vidioc_try_fmt_vid_cap_mplane(struct file *file, void *priv,
+			struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!dev->multiplanar)
+		return -ENOTTY;
+	return vivid_try_fmt_vid_cap(file, priv, f);
+}
+
+int vidioc_s_fmt_vid_cap_mplane(struct file *file, void *priv,
+			struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!dev->multiplanar)
+		return -ENOTTY;
+	return vivid_s_fmt_vid_cap(file, priv, f);
+}
+
+int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (dev->multiplanar)
+		return -ENOTTY;
+	return fmt_sp2mp_func(file, priv, f, vivid_g_fmt_vid_cap);
+}
+
+int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
+			struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (dev->multiplanar)
+		return -ENOTTY;
+	return fmt_sp2mp_func(file, priv, f, vivid_try_fmt_vid_cap);
+}
+
+int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
+			struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (dev->multiplanar)
+		return -ENOTTY;
+	return fmt_sp2mp_func(file, priv, f, vivid_s_fmt_vid_cap);
+}
+
+int vivid_vid_cap_g_selection(struct file *file, void *priv,
+			      struct v4l2_selection *sel)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!dev->has_crop_cap && !dev->has_compose_cap)
+		return -ENOTTY;
+	if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+	if (vivid_is_webcam(dev))
+		return -EINVAL;
+
+	sel->r.left = sel->r.top = 0;
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		if (!dev->has_crop_cap)
+			return -EINVAL;
+		sel->r = dev->crop_cap;
+		break;
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		if (!dev->has_crop_cap)
+			return -EINVAL;
+		sel->r = dev->src_rect;
+		break;
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+		if (!dev->has_compose_cap)
+			return -EINVAL;
+		sel->r = vivid_max_rect;
+		break;
+	case V4L2_SEL_TGT_COMPOSE:
+		if (!dev->has_compose_cap)
+			return -EINVAL;
+		sel->r = dev->compose_cap;
+		break;
+	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+		if (!dev->has_compose_cap)
+			return -EINVAL;
+		sel->r = dev->fmt_cap_rect;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+int vivid_vid_cap_s_selection(struct file *file, void *fh, struct v4l2_selection *s)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct v4l2_rect *crop = &dev->crop_cap;
+	struct v4l2_rect *compose = &dev->compose_cap;
+	unsigned factor = V4L2_FIELD_HAS_T_OR_B(dev->field_cap) ? 2 : 1;
+	int ret;
+
+	if (!dev->has_crop_cap && !dev->has_compose_cap)
+		return -ENOTTY;
+	if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+	if (vivid_is_webcam(dev))
+		return -EINVAL;
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_CROP:
+		if (!dev->has_crop_cap)
+			return -EINVAL;
+		ret = vivid_vid_adjust_sel(s->flags, &s->r);
+		if (ret)
+			return ret;
+		rect_set_min_size(&s->r, &vivid_min_rect);
+		rect_set_max_size(&s->r, &dev->src_rect);
+		rect_map_inside(&s->r, &dev->crop_bounds_cap);
+		s->r.top /= factor;
+		s->r.height /= factor;
+		if (dev->has_scaler_cap) {
+			struct v4l2_rect fmt = dev->fmt_cap_rect;
+			struct v4l2_rect max_rect = {
+				0, 0,
+				s->r.width * MAX_ZOOM,
+				s->r.height * MAX_ZOOM
+			};
+			struct v4l2_rect min_rect = {
+				0, 0,
+				s->r.width / MAX_ZOOM,
+				s->r.height / MAX_ZOOM
+			};
+
+			rect_set_min_size(&fmt, &min_rect);
+			if (!dev->has_compose_cap)
+				rect_set_max_size(&fmt, &max_rect);
+			if (!rect_same_size(&dev->fmt_cap_rect, &fmt) &&
+			    vb2_is_busy(&dev->vb_vid_cap_q))
+				return -EBUSY;
+			if (dev->has_compose_cap) {
+				rect_set_min_size(compose, &min_rect);
+				rect_set_max_size(compose, &max_rect);
+			}
+			dev->fmt_cap_rect = fmt;
+			tpg_s_buf_height(&dev->tpg, fmt.height);
+		} else if (dev->has_compose_cap) {
+			struct v4l2_rect fmt = dev->fmt_cap_rect;
+
+			rect_set_min_size(&fmt, &s->r);
+			if (!rect_same_size(&dev->fmt_cap_rect, &fmt) &&
+			    vb2_is_busy(&dev->vb_vid_cap_q))
+				return -EBUSY;
+			dev->fmt_cap_rect = fmt;
+			tpg_s_buf_height(&dev->tpg, fmt.height);
+			rect_set_size_to(compose, &s->r);
+			rect_map_inside(compose, &dev->fmt_cap_rect);
+		} else {
+			if (!rect_same_size(&s->r, &dev->fmt_cap_rect) &&
+			    vb2_is_busy(&dev->vb_vid_cap_q))
+				return -EBUSY;
+			rect_set_size_to(&dev->fmt_cap_rect, &s->r);
+			rect_set_size_to(compose, &s->r);
+			rect_map_inside(compose, &dev->fmt_cap_rect);
+			tpg_s_buf_height(&dev->tpg, dev->fmt_cap_rect.height);
+		}
+		s->r.top *= factor;
+		s->r.height *= factor;
+		*crop = s->r;
+		break;
+	case V4L2_SEL_TGT_COMPOSE:
+		if (!dev->has_compose_cap)
+			return -EINVAL;
+		ret = vivid_vid_adjust_sel(s->flags, &s->r);
+		if (ret)
+			return ret;
+		rect_set_min_size(&s->r, &vivid_min_rect);
+		rect_set_max_size(&s->r, &dev->fmt_cap_rect);
+		if (dev->has_scaler_cap) {
+			struct v4l2_rect max_rect = {
+				0, 0,
+				dev->src_rect.width * MAX_ZOOM,
+				(dev->src_rect.height / factor) * MAX_ZOOM
+			};
+
+			rect_set_max_size(&s->r, &max_rect);
+			if (dev->has_crop_cap) {
+				struct v4l2_rect min_rect = {
+					0, 0,
+					s->r.width / MAX_ZOOM,
+					(s->r.height * factor) / MAX_ZOOM
+				};
+				struct v4l2_rect max_rect = {
+					0, 0,
+					s->r.width * MAX_ZOOM,
+					(s->r.height * factor) * MAX_ZOOM
+				};
+
+				rect_set_min_size(crop, &min_rect);
+				rect_set_max_size(crop, &max_rect);
+				rect_map_inside(crop, &dev->crop_bounds_cap);
+			}
+		} else if (dev->has_crop_cap) {
+			s->r.top *= factor;
+			s->r.height *= factor;
+			rect_set_max_size(&s->r, &dev->src_rect);
+			rect_set_size_to(crop, &s->r);
+			rect_map_inside(crop, &dev->crop_bounds_cap);
+			s->r.top /= factor;
+			s->r.height /= factor;
+		} else {
+			rect_set_size_to(&s->r, &dev->src_rect);
+			s->r.height /= factor;
+		}
+		rect_map_inside(&s->r, &dev->fmt_cap_rect);
+		if (dev->bitmap_cap && (compose->width != s->r.width ||
+					compose->height != s->r.height)) {
+			kfree(dev->bitmap_cap);
+			dev->bitmap_cap = NULL;
+		}
+		*compose = s->r;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	tpg_s_crop_compose(&dev->tpg, crop, compose);
+	return 0;
+}
+
+int vivid_vid_cap_cropcap(struct file *file, void *priv,
+			      struct v4l2_cropcap *cap)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (cap->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	switch (vivid_get_pixel_aspect(dev)) {
+	case TPG_PIXEL_ASPECT_NTSC:
+		cap->pixelaspect.numerator = 11;
+		cap->pixelaspect.denominator = 10;
+		break;
+	case TPG_PIXEL_ASPECT_PAL:
+		cap->pixelaspect.numerator = 54;
+		cap->pixelaspect.denominator = 59;
+		break;
+	case TPG_PIXEL_ASPECT_SQUARE:
+		cap->pixelaspect.numerator = 1;
+		cap->pixelaspect.denominator = 1;
+		break;
+	}
+	return 0;
+}
+
+int vidioc_enum_fmt_vid_overlay(struct file *file, void  *priv,
+					struct v4l2_fmtdesc *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	const struct vivid_fmt *fmt;
+
+	if (dev->multiplanar)
+		return -ENOTTY;
+
+	if (f->index >= ARRAY_SIZE(formats_ovl))
+		return -EINVAL;
+
+	fmt = &formats_ovl[f->index];
+
+	f->pixelformat = fmt->fourcc;
+	return 0;
+}
+
+int vidioc_g_fmt_vid_overlay(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	const struct v4l2_rect *compose = &dev->compose_cap;
+	struct v4l2_window *win = &f->fmt.win;
+	unsigned clipcount = win->clipcount;
+
+	if (dev->multiplanar)
+		return -ENOTTY;
+
+	win->w.top = dev->overlay_cap_top;
+	win->w.left = dev->overlay_cap_left;
+	win->w.width = compose->width;
+	win->w.height = compose->height;
+	win->field = dev->overlay_cap_field;
+	win->clipcount = dev->clipcount_cap;
+	if (clipcount > dev->clipcount_cap)
+		clipcount = dev->clipcount_cap;
+	if (dev->bitmap_cap == NULL)
+		win->bitmap = NULL;
+	else if (win->bitmap) {
+		if (copy_to_user(win->bitmap, dev->bitmap_cap,
+		    ((compose->width + 7) / 8) * compose->height))
+			return -EFAULT;
+	}
+	if (clipcount && win->clips) {
+		if (copy_to_user(win->clips, dev->clips_cap,
+				 clipcount * sizeof(dev->clips_cap[0])))
+			return -EFAULT;
+	}
+	return 0;
+}
+
+int vidioc_try_fmt_vid_overlay(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	const struct v4l2_rect *compose = &dev->compose_cap;
+	struct v4l2_window *win = &f->fmt.win;
+	int i, j;
+
+	if (dev->multiplanar)
+		return -ENOTTY;
+
+	win->w.left = clamp_t(int, win->w.left,
+			      -dev->fb_cap.fmt.width, dev->fb_cap.fmt.width);
+	win->w.top = clamp_t(int, win->w.top,
+			     -dev->fb_cap.fmt.height, dev->fb_cap.fmt.height);
+	win->w.width = compose->width;
+	win->w.height = compose->height;
+	if (win->field != V4L2_FIELD_BOTTOM && win->field != V4L2_FIELD_TOP)
+		win->field = V4L2_FIELD_ANY;
+	win->chromakey = 0;
+	win->global_alpha = 0;
+	if (win->clipcount && !win->clips)
+		win->clipcount = 0;
+	if (win->clipcount > MAX_CLIPS)
+		win->clipcount = MAX_CLIPS;
+	if (win->clipcount) {
+		if (copy_from_user(dev->try_clips_cap, win->clips,
+				   win->clipcount * sizeof(dev->clips_cap[0])))
+			return -EFAULT;
+		for (i = 0; i < win->clipcount; i++) {
+			struct v4l2_rect *r = &dev->try_clips_cap[i].c;
+
+			r->top = clamp_t(s32, r->top, 0, dev->fb_cap.fmt.height - 1);
+			r->height = clamp_t(s32, r->height, 1, dev->fb_cap.fmt.height - r->top);
+			r->left = clamp_t(u32, r->left, 0, dev->fb_cap.fmt.width - 1);
+			r->width = clamp_t(u32, r->width, 1, dev->fb_cap.fmt.width - r->left);
+		}
+		/*
+		 * Yeah, so sue me, it's an O(n^2) algorithm. But n is a small
+		 * number and it's typically a one-time deal.
+		 */
+		for (i = 0; i < win->clipcount - 1; i++) {
+			struct v4l2_rect *r1 = &dev->try_clips_cap[i].c;
+
+			for (j = i + 1; j < win->clipcount; j++) {
+				struct v4l2_rect *r2 = &dev->try_clips_cap[j].c;
+
+				if (rect_overlap(r1, r2))
+					return -EINVAL;
+			}
+		}
+		if (copy_to_user(win->clips, dev->try_clips_cap,
+				 win->clipcount * sizeof(dev->clips_cap[0])))
+			return -EFAULT;
+	}
+	return 0;
+}
+
+int vidioc_s_fmt_vid_overlay(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	const struct v4l2_rect *compose = &dev->compose_cap;
+	struct v4l2_window *win = &f->fmt.win;
+	int ret = vidioc_try_fmt_vid_overlay(file, priv, f);
+	unsigned bitmap_size = ((compose->width + 7) / 8) * compose->height;
+	unsigned clips_size = win->clipcount * sizeof(dev->clips_cap[0]);
+	void *new_bitmap = NULL;
+
+	if (ret)
+		return ret;
+
+	if (win->bitmap) {
+		new_bitmap = vzalloc(bitmap_size);
+
+		if (new_bitmap == NULL)
+			return -ENOMEM;
+		if (copy_from_user(new_bitmap, win->bitmap, bitmap_size)) {
+			vfree(new_bitmap);
+			return -EFAULT;
+		}
+	}
+
+	dev->overlay_cap_top = win->w.top;
+	dev->overlay_cap_left = win->w.left;
+	dev->overlay_cap_field = win->field;
+	vfree(dev->bitmap_cap);
+	dev->bitmap_cap = new_bitmap;
+	dev->clipcount_cap = win->clipcount;
+	if (dev->clipcount_cap)
+		memcpy(dev->clips_cap, dev->try_clips_cap, clips_size);
+	return 0;
+}
+
+int vivid_vid_cap_overlay(struct file *file, void *fh, unsigned i)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (dev->multiplanar)
+		return -ENOTTY;
+
+	if (i && dev->fb_vbase_cap == NULL)
+		return -EINVAL;
+
+	if (i && dev->fb_cap.fmt.pixelformat != dev->fmt_cap->fourcc) {
+		dprintk(dev, 1, "mismatch between overlay and video capture pixelformats\n");
+		return -EINVAL;
+	}
+
+	if (dev->overlay_cap_owner && dev->overlay_cap_owner != fh)
+		return -EBUSY;
+	dev->overlay_cap_owner = i ? fh : NULL;
+	return 0;
+}
+
+int vivid_vid_cap_g_fbuf(struct file *file, void *fh,
+				struct v4l2_framebuffer *a)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (dev->multiplanar)
+		return -ENOTTY;
+
+	*a = dev->fb_cap;
+	a->capability = V4L2_FBUF_CAP_BITMAP_CLIPPING |
+			V4L2_FBUF_CAP_LIST_CLIPPING;
+	a->flags = V4L2_FBUF_FLAG_PRIMARY;
+	a->fmt.field = V4L2_FIELD_NONE;
+	a->fmt.colorspace = V4L2_COLORSPACE_SRGB;
+	a->fmt.priv = 0;
+	return 0;
+}
+
+int vivid_vid_cap_s_fbuf(struct file *file, void *fh,
+				const struct v4l2_framebuffer *a)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	const struct vivid_fmt *fmt;
+
+	if (dev->multiplanar)
+		return -ENOTTY;
+
+	if (!capable(CAP_SYS_ADMIN) && !capable(CAP_SYS_RAWIO))
+		return -EPERM;
+
+	if (dev->overlay_cap_owner)
+		return -EBUSY;
+
+	if (a->base == NULL) {
+		dev->fb_cap.base = NULL;
+		dev->fb_vbase_cap = NULL;
+		return 0;
+	}
+
+	if (a->fmt.width < 48 || a->fmt.height < 32)
+		return -EINVAL;
+	fmt = vivid_get_format(dev, a->fmt.pixelformat);
+	if (!fmt || !fmt->can_do_overlay)
+		return -EINVAL;
+	if (a->fmt.bytesperline < (a->fmt.width * fmt->bit_depth[0]) / 8)
+		return -EINVAL;
+	if (a->fmt.height * a->fmt.bytesperline < a->fmt.sizeimage)
+		return -EINVAL;
+
+	dev->fb_vbase_cap = phys_to_virt((unsigned long)a->base);
+	dev->fb_cap = *a;
+	dev->overlay_cap_left = clamp_t(int, dev->overlay_cap_left,
+				    -dev->fb_cap.fmt.width, dev->fb_cap.fmt.width);
+	dev->overlay_cap_top = clamp_t(int, dev->overlay_cap_top,
+				   -dev->fb_cap.fmt.height, dev->fb_cap.fmt.height);
+	return 0;
+}
+
+static const struct v4l2_audio vivid_audio_inputs[] = {
+	{ 0, "TV", V4L2_AUDCAP_STEREO },
+	{ 1, "Line-In", V4L2_AUDCAP_STEREO },
+};
+
+int vidioc_enum_input(struct file *file, void *priv,
+				struct v4l2_input *inp)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (inp->index >= dev->num_inputs)
+		return -EINVAL;
+
+	inp->type = V4L2_INPUT_TYPE_CAMERA;
+	switch (dev->input_type[inp->index]) {
+	case WEBCAM:
+		snprintf(inp->name, sizeof(inp->name), "Webcam %u",
+				dev->input_name_counter[inp->index]);
+		inp->capabilities = 0;
+		break;
+	case TV:
+		snprintf(inp->name, sizeof(inp->name), "TV %u",
+				dev->input_name_counter[inp->index]);
+		inp->type = V4L2_INPUT_TYPE_TUNER;
+		inp->std = V4L2_STD_ALL;
+		if (dev->has_audio_inputs)
+			inp->audioset = (1 << ARRAY_SIZE(vivid_audio_inputs)) - 1;
+		inp->capabilities = V4L2_IN_CAP_STD;
+		break;
+	case SVID:
+		snprintf(inp->name, sizeof(inp->name), "S-Video %u",
+				dev->input_name_counter[inp->index]);
+		inp->std = V4L2_STD_ALL;
+		if (dev->has_audio_inputs)
+			inp->audioset = (1 << ARRAY_SIZE(vivid_audio_inputs)) - 1;
+		inp->capabilities = V4L2_IN_CAP_STD;
+		break;
+	case HDMI:
+		snprintf(inp->name, sizeof(inp->name), "HDMI %u",
+				dev->input_name_counter[inp->index]);
+		inp->capabilities = V4L2_IN_CAP_DV_TIMINGS;
+		if (dev->edid_blocks == 0 ||
+		    dev->dv_timings_signal_mode == NO_SIGNAL)
+			inp->status |= V4L2_IN_ST_NO_SIGNAL;
+		else if (dev->dv_timings_signal_mode == NO_LOCK ||
+			 dev->dv_timings_signal_mode == OUT_OF_RANGE)
+			inp->status |= V4L2_IN_ST_NO_H_LOCK;
+		break;
+	}
+	if (dev->sensor_hflip)
+		inp->status |= V4L2_IN_ST_HFLIP;
+	if (dev->sensor_vflip)
+		inp->status |= V4L2_IN_ST_VFLIP;
+	if (dev->input == inp->index && vivid_is_sdtv_cap(dev)) {
+		if (dev->std_signal_mode == NO_SIGNAL) {
+			inp->status |= V4L2_IN_ST_NO_SIGNAL;
+		} else if (dev->std_signal_mode == NO_LOCK) {
+			inp->status |= V4L2_IN_ST_NO_H_LOCK;
+		} else if (vivid_is_tv_cap(dev)) {
+			switch (tpg_g_quality(&dev->tpg)) {
+			case TPG_QUAL_GRAY:
+				inp->status |= V4L2_IN_ST_COLOR_KILL;
+				break;
+			case TPG_QUAL_NOISE:
+				inp->status |= V4L2_IN_ST_NO_H_LOCK;
+				break;
+			default:
+				break;
+			}
+		}
+	}
+	return 0;
+}
+
+int vidioc_g_input(struct file *file, void *priv, unsigned *i)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	*i = dev->input;
+	return 0;
+}
+
+int vidioc_s_input(struct file *file, void *priv, unsigned i)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct v4l2_bt_timings *bt = &dev->dv_timings_cap.bt;
+	unsigned brightness;
+
+	if (i >= dev->num_inputs)
+		return -EINVAL;
+
+	if (i == dev->input)
+		return 0;
+
+	if (vb2_is_busy(&dev->vb_vid_cap_q) || vb2_is_busy(&dev->vb_vbi_cap_q))
+		return -EBUSY;
+
+	dev->input = i;
+	dev->vid_cap_dev.tvnorms = 0;
+	if (dev->input_type[i] == TV || dev->input_type[i] == SVID) {
+		dev->tv_audio_input = (dev->input_type[i] == TV) ? 0 : 1;
+		dev->vid_cap_dev.tvnorms = V4L2_STD_ALL;
+	}
+	dev->vbi_cap_dev.tvnorms = dev->vid_cap_dev.tvnorms;
+	vivid_update_format_cap(dev, false);
+
+	if (dev->colorspace) {
+		switch (dev->input_type[i]) {
+		case WEBCAM:
+			v4l2_ctrl_s_ctrl(dev->colorspace, VIVID_CS_SRGB);
+			break;
+		case TV:
+		case SVID:
+			v4l2_ctrl_s_ctrl(dev->colorspace, VIVID_CS_170M);
+			break;
+		case HDMI:
+			if (bt->flags & V4L2_DV_FL_IS_CE_VIDEO) {
+				if (dev->src_rect.width == 720 && dev->src_rect.height <= 576)
+					v4l2_ctrl_s_ctrl(dev->colorspace, VIVID_CS_170M);
+				else
+					v4l2_ctrl_s_ctrl(dev->colorspace, VIVID_CS_709);
+			} else {
+				v4l2_ctrl_s_ctrl(dev->colorspace, VIVID_CS_SRGB);
+			}
+			break;
+		}
+	}
+
+	/*
+	 * Modify the brightness range depending on the input.
+	 * This makes it easy to use vivid to test if applications can
+	 * handle control range modifications and is also how this is
+	 * typically used in practice as different inputs may be hooked
+	 * up to different receivers with different control ranges.
+	 */
+	brightness = 128 * i + dev->input_brightness[i];
+	v4l2_ctrl_modify_range(dev->brightness,
+			128 * i, 255 + 128 * i, 1, 128 + 128 * i);
+	v4l2_ctrl_s_ctrl(dev->brightness, brightness);
+	return 0;
+}
+
+int vidioc_enumaudio(struct file *file, void *fh, struct v4l2_audio *vin)
+{
+	if (vin->index >= ARRAY_SIZE(vivid_audio_inputs))
+		return -EINVAL;
+	*vin = vivid_audio_inputs[vin->index];
+	return 0;
+}
+
+int vidioc_g_audio(struct file *file, void *fh, struct v4l2_audio *vin)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!vivid_is_sdtv_cap(dev))
+		return -EINVAL;
+	*vin = vivid_audio_inputs[dev->tv_audio_input];
+	return 0;
+}
+
+int vidioc_s_audio(struct file *file, void *fh, const struct v4l2_audio *vin)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!vivid_is_sdtv_cap(dev))
+		return -EINVAL;
+	if (vin->index >= ARRAY_SIZE(vivid_audio_inputs))
+		return -EINVAL;
+	dev->tv_audio_input = vin->index;
+	return 0;
+}
+
+int vivid_video_g_frequency(struct file *file, void *fh, struct v4l2_frequency *vf)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (vf->tuner != 0)
+		return -EINVAL;
+	vf->frequency = dev->tv_freq;
+	return 0;
+}
+
+int vivid_video_s_frequency(struct file *file, void *fh, const struct v4l2_frequency *vf)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (vf->tuner != 0)
+		return -EINVAL;
+	dev->tv_freq = clamp_t(unsigned, vf->frequency, MIN_TV_FREQ, MAX_TV_FREQ);
+	if (vivid_is_tv_cap(dev))
+		vivid_update_quality(dev);
+	return 0;
+}
+
+int vivid_video_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *vt)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (vt->index != 0)
+		return -EINVAL;
+	if (vt->audmode > V4L2_TUNER_MODE_LANG1_LANG2)
+		return -EINVAL;
+	dev->tv_audmode = vt->audmode;
+	return 0;
+}
+
+int vivid_video_g_tuner(struct file *file, void *fh, struct v4l2_tuner *vt)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	enum tpg_quality qual;
+
+	if (vt->index != 0)
+		return -EINVAL;
+
+	vt->capability = V4L2_TUNER_CAP_NORM | V4L2_TUNER_CAP_STEREO |
+			 V4L2_TUNER_CAP_LANG1 | V4L2_TUNER_CAP_LANG2;
+	vt->audmode = dev->tv_audmode;
+	vt->rangelow = MIN_TV_FREQ;
+	vt->rangehigh = MAX_TV_FREQ;
+	qual = vivid_get_quality(dev, &vt->afc);
+	if (qual == TPG_QUAL_COLOR)
+		vt->signal = 0xffff;
+	else if (qual == TPG_QUAL_GRAY)
+		vt->signal = 0x8000;
+	else
+		vt->signal = 0;
+	if (qual == TPG_QUAL_NOISE) {
+		vt->rxsubchans = 0;
+	} else if (qual == TPG_QUAL_GRAY) {
+		vt->rxsubchans = V4L2_TUNER_SUB_MONO;
+	} else {
+		unsigned channel_nr = dev->tv_freq / (6 * 16);
+		unsigned options = (dev->std_cap & V4L2_STD_NTSC_M) ? 4 : 3;
+
+		switch (channel_nr % options) {
+		case 0:
+			vt->rxsubchans = V4L2_TUNER_SUB_MONO;
+			break;
+		case 1:
+			vt->rxsubchans = V4L2_TUNER_SUB_STEREO;
+			break;
+		case 2:
+			if (dev->std_cap & V4L2_STD_NTSC_M)
+				vt->rxsubchans = V4L2_TUNER_SUB_MONO | V4L2_TUNER_SUB_SAP;
+			else
+				vt->rxsubchans = V4L2_TUNER_SUB_LANG1 | V4L2_TUNER_SUB_LANG2;
+			break;
+		case 3:
+			vt->rxsubchans = V4L2_TUNER_SUB_STEREO | V4L2_TUNER_SUB_SAP;
+			break;
+		}
+	}
+	strlcpy(vt->name, "TV Tuner", sizeof(vt->name));
+	return 0;
+}
+
+/* Must remain in sync with the vivid_ctrl_standard_strings array */
+const v4l2_std_id vivid_standard[] = {
+	V4L2_STD_NTSC_M,
+	V4L2_STD_NTSC_M_JP,
+	V4L2_STD_NTSC_M_KR,
+	V4L2_STD_NTSC_443,
+	V4L2_STD_PAL_BG | V4L2_STD_PAL_H,
+	V4L2_STD_PAL_I,
+	V4L2_STD_PAL_DK,
+	V4L2_STD_PAL_M,
+	V4L2_STD_PAL_N,
+	V4L2_STD_PAL_Nc,
+	V4L2_STD_PAL_60,
+	V4L2_STD_SECAM_B | V4L2_STD_SECAM_G | V4L2_STD_SECAM_H,
+	V4L2_STD_SECAM_DK,
+	V4L2_STD_SECAM_L,
+	V4L2_STD_SECAM_LC,
+	V4L2_STD_UNKNOWN
+};
+
+/* Must remain in sync with the vivid_standard array */
+const char * const vivid_ctrl_standard_strings[] = {
+	"NTSC-M",
+	"NTSC-M-JP",
+	"NTSC-M-KR",
+	"NTSC-443",
+	"PAL-BGH",
+	"PAL-I",
+	"PAL-DK",
+	"PAL-M",
+	"PAL-N",
+	"PAL-Nc",
+	"PAL-60",
+	"SECAM-BGH",
+	"SECAM-DK",
+	"SECAM-L",
+	"SECAM-Lc",
+	NULL,
+};
+
+int vidioc_querystd(struct file *file, void *priv, v4l2_std_id *id)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!vivid_is_sdtv_cap(dev))
+		return -ENODATA;
+	if (dev->std_signal_mode == NO_SIGNAL ||
+	    dev->std_signal_mode == NO_LOCK) {
+		*id = V4L2_STD_UNKNOWN;
+		return 0;
+	}
+	if (vivid_is_tv_cap(dev) && tpg_g_quality(&dev->tpg) == TPG_QUAL_NOISE) {
+		*id = V4L2_STD_UNKNOWN;
+	} else if (dev->std_signal_mode == CURRENT_STD) {
+		*id = dev->std_cap;
+	} else if (dev->std_signal_mode == SELECTED_STD) {
+		*id = dev->query_std;
+	} else {
+		*id = vivid_standard[dev->query_std_last];
+		dev->query_std_last = (dev->query_std_last + 1) % ARRAY_SIZE(vivid_standard);
+	}
+
+	return 0;
+}
+
+int vivid_vid_cap_s_std(struct file *file, void *priv, v4l2_std_id id)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!vivid_is_sdtv_cap(dev))
+		return -ENODATA;
+	if (dev->std_cap == id)
+		return 0;
+	if (vb2_is_busy(&dev->vb_vid_cap_q) || vb2_is_busy(&dev->vb_vbi_cap_q))
+		return -EBUSY;
+	dev->std_cap = id;
+	vivid_update_format_cap(dev, false);
+	return 0;
+}
+
+static void find_aspect_ratio(u32 width, u32 height,
+			       u32 *num, u32 *denom)
+{
+	if (!(height % 3) && ((height * 4 / 3) == width)) {
+		*num = 4;
+		*denom = 3;
+	} else if (!(height % 9) && ((height * 16 / 9) == width)) {
+		*num = 16;
+		*denom = 9;
+	} else if (!(height % 10) && ((height * 16 / 10) == width)) {
+		*num = 16;
+		*denom = 10;
+	} else if (!(height % 4) && ((height * 5 / 4) == width)) {
+		*num = 5;
+		*denom = 4;
+	} else if (!(height % 9) && ((height * 15 / 9) == width)) {
+		*num = 15;
+		*denom = 9;
+	} else { /* default to 16:9 */
+		*num = 16;
+		*denom = 9;
+	}
+}
+
+static bool valid_cvt_gtf_timings(struct v4l2_dv_timings *timings)
+{
+	struct v4l2_bt_timings *bt = &timings->bt;
+	u32 total_h_pixel;
+	u32 total_v_lines;
+	u32 h_freq;
+
+	if (!v4l2_valid_dv_timings(timings, &vivid_dv_timings_cap,
+				NULL, NULL))
+		return false;
+
+	total_h_pixel = V4L2_DV_BT_FRAME_WIDTH(bt);
+	total_v_lines = V4L2_DV_BT_FRAME_HEIGHT(bt);
+
+	h_freq = (u32)bt->pixelclock / total_h_pixel;
+
+	if (bt->standards == 0 || (bt->standards & V4L2_DV_BT_STD_CVT)) {
+		if (v4l2_detect_cvt(total_v_lines, h_freq, bt->vsync, bt->width,
+				    bt->polarities, bt->interlaced, timings))
+			return true;
+	}
+
+	if (bt->standards == 0 || (bt->standards & V4L2_DV_BT_STD_GTF)) {
+		struct v4l2_fract aspect_ratio;
+
+		find_aspect_ratio(bt->width, bt->height,
+				  &aspect_ratio.numerator,
+				  &aspect_ratio.denominator);
+		if (v4l2_detect_gtf(total_v_lines, h_freq, bt->vsync,
+				    bt->polarities, bt->interlaced,
+				    aspect_ratio, timings))
+			return true;
+	}
+	return false;
+}
+
+int vivid_vid_cap_s_dv_timings(struct file *file, void *_fh,
+				    struct v4l2_dv_timings *timings)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!vivid_is_hdmi_cap(dev))
+		return -ENODATA;
+	if (!v4l2_find_dv_timings_cap(timings, &vivid_dv_timings_cap,
+				      0, NULL, NULL) &&
+	    !valid_cvt_gtf_timings(timings))
+		return -EINVAL;
+
+	if (v4l2_match_dv_timings(timings, &dev->dv_timings_cap, 0))
+		return 0;
+	if (vb2_is_busy(&dev->vb_vid_cap_q))
+		return -EBUSY;
+
+	dev->dv_timings_cap = *timings;
+	vivid_update_format_cap(dev, false);
+	return 0;
+}
+
+int vidioc_query_dv_timings(struct file *file, void *_fh,
+				    struct v4l2_dv_timings *timings)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!vivid_is_hdmi_cap(dev))
+		return -ENODATA;
+	if (dev->dv_timings_signal_mode == NO_SIGNAL ||
+	    dev->edid_blocks == 0)
+		return -ENOLINK;
+	if (dev->dv_timings_signal_mode == NO_LOCK)
+		return -ENOLCK;
+	if (dev->dv_timings_signal_mode == OUT_OF_RANGE) {
+		timings->bt.pixelclock = vivid_dv_timings_cap.bt.max_pixelclock * 2;
+		return -ERANGE;
+	}
+	if (dev->dv_timings_signal_mode == CURRENT_DV_TIMINGS) {
+		*timings = dev->dv_timings_cap;
+	} else if (dev->dv_timings_signal_mode == SELECTED_DV_TIMINGS) {
+		*timings = v4l2_dv_timings_presets[dev->query_dv_timings];
+	} else {
+		*timings = v4l2_dv_timings_presets[dev->query_dv_timings_last];
+		dev->query_dv_timings_last = (dev->query_dv_timings_last + 1) %
+						dev->query_dv_timings_size;
+	}
+	return 0;
+}
+
+int vidioc_s_edid(struct file *file, void *_fh,
+			 struct v4l2_edid *edid)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	memset(edid->reserved, 0, sizeof(edid->reserved));
+	if (edid->pad >= dev->num_inputs)
+		return -EINVAL;
+	if (dev->input_type[edid->pad] != HDMI || edid->start_block)
+		return -EINVAL;
+	if (edid->blocks == 0) {
+		dev->edid_blocks = 0;
+		return 0;
+	}
+	if (edid->blocks > dev->edid_max_blocks) {
+		edid->blocks = dev->edid_max_blocks;
+		return -E2BIG;
+	}
+	dev->edid_blocks = edid->blocks;
+	memcpy(dev->edid, edid->edid, edid->blocks * 128);
+	return 0;
+}
+
+int vidioc_enum_framesizes(struct file *file, void *fh,
+					 struct v4l2_frmsizeenum *fsize)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!vivid_is_webcam(dev) && !dev->has_scaler_cap)
+		return -EINVAL;
+	if (vivid_get_format(dev, fsize->pixel_format) == NULL)
+		return -EINVAL;
+	if (vivid_is_webcam(dev)) {
+		if (fsize->index >= ARRAY_SIZE(webcam_sizes))
+			return -EINVAL;
+		fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+		fsize->discrete = webcam_sizes[fsize->index];
+		return 0;
+	}
+	if (fsize->index)
+		return -EINVAL;
+	fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
+	fsize->stepwise.min_width = MIN_WIDTH;
+	fsize->stepwise.max_width = MAX_WIDTH * MAX_ZOOM;
+	fsize->stepwise.step_width = 2;
+	fsize->stepwise.min_height = MIN_HEIGHT;
+	fsize->stepwise.max_height = MAX_HEIGHT * MAX_ZOOM;
+	fsize->stepwise.step_height = 2;
+	return 0;
+}
+
+/* timeperframe is arbitrary and continuous */
+int vidioc_enum_frameintervals(struct file *file, void *priv,
+					     struct v4l2_frmivalenum *fival)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	const struct vivid_fmt *fmt;
+	int i;
+
+	fmt = vivid_get_format(dev, fival->pixel_format);
+	if (!fmt)
+		return -EINVAL;
+
+	if (!vivid_is_webcam(dev)) {
+		if (fival->index)
+			return -EINVAL;
+		if (fival->width < MIN_WIDTH || fival->width > MAX_WIDTH * MAX_ZOOM)
+			return -EINVAL;
+		if (fival->height < MIN_HEIGHT || fival->height > MAX_HEIGHT * MAX_ZOOM)
+			return -EINVAL;
+		fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
+		fival->discrete = dev->timeperframe_vid_cap;
+		return 0;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(webcam_sizes); i++)
+		if (fival->width == webcam_sizes[i].width &&
+		    fival->height == webcam_sizes[i].height)
+			break;
+	if (i == ARRAY_SIZE(webcam_sizes))
+		return -EINVAL;
+	if (fival->index >= 2 * (VIVID_WEBCAM_SIZES - i))
+		return -EINVAL;
+	fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
+	fival->discrete = webcam_intervals[fival->index];
+	return 0;
+}
+
+int vivid_vid_cap_g_parm(struct file *file, void *priv,
+			  struct v4l2_streamparm *parm)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (parm->type != (dev->multiplanar ?
+			   V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE :
+			   V4L2_BUF_TYPE_VIDEO_CAPTURE))
+		return -EINVAL;
+
+	parm->parm.capture.capability   = V4L2_CAP_TIMEPERFRAME;
+	parm->parm.capture.timeperframe = dev->timeperframe_vid_cap;
+	parm->parm.capture.readbuffers  = 1;
+	return 0;
+}
+
+#define FRACT_CMP(a, OP, b)	\
+	((u64)(a).numerator * (b).denominator  OP  (u64)(b).numerator * (a).denominator)
+
+int vivid_vid_cap_s_parm(struct file *file, void *priv,
+			  struct v4l2_streamparm *parm)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	unsigned ival_sz = 2 * (VIVID_WEBCAM_SIZES - dev->webcam_size_idx);
+	struct v4l2_fract tpf;
+	unsigned i;
+
+	if (parm->type != (dev->multiplanar ?
+			   V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE :
+			   V4L2_BUF_TYPE_VIDEO_CAPTURE))
+		return -EINVAL;
+	if (!vivid_is_webcam(dev))
+		return vivid_vid_cap_g_parm(file, priv, parm);
+
+	tpf = parm->parm.capture.timeperframe;
+
+	if (tpf.denominator == 0)
+		tpf = webcam_intervals[ival_sz - 1];
+	for (i = 0; i < ival_sz; i++)
+		if (FRACT_CMP(tpf, >=, webcam_intervals[i]))
+			break;
+	if (i == ival_sz)
+		i = ival_sz - 1;
+	dev->webcam_ival_idx = i;
+	tpf = webcam_intervals[dev->webcam_ival_idx];
+	tpf = FRACT_CMP(tpf, <, tpf_min) ? tpf_min : tpf;
+	tpf = FRACT_CMP(tpf, >, tpf_max) ? tpf_max : tpf;
+
+	/* resync the thread's timings */
+	dev->cap_seq_resync = true;
+	dev->timeperframe_vid_cap = tpf;
+	parm->parm.capture.timeperframe = tpf;
+	parm->parm.capture.readbuffers  = 1;
+	return 0;
+}
diff --git a/drivers/media/platform/vivid/vivid-vid-cap.h b/drivers/media/platform/vivid/vivid-vid-cap.h
new file mode 100644
index 0000000..9407981
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-vid-cap.h
@@ -0,0 +1,71 @@
+/*
+ * vivid-vid-cap.h - video capture support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VIVID_VID_CAP_H_
+#define _VIVID_VID_CAP_H_
+
+void vivid_update_quality(struct vivid_dev *dev);
+void vivid_update_format_cap(struct vivid_dev *dev, bool keep_controls);
+enum tpg_video_aspect vivid_get_video_aspect(const struct vivid_dev *dev);
+
+extern const v4l2_std_id vivid_standard[];
+extern const char * const vivid_ctrl_standard_strings[];
+
+extern const struct vb2_ops vivid_vid_cap_qops;
+
+int vivid_g_fmt_vid_cap(struct file *file, void *priv, struct v4l2_format *f);
+int vivid_try_fmt_vid_cap(struct file *file, void *priv, struct v4l2_format *f);
+int vivid_s_fmt_vid_cap(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_g_fmt_vid_cap_mplane(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_try_fmt_vid_cap_mplane(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_s_fmt_vid_cap_mplane(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_g_fmt_vid_cap(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_try_fmt_vid_cap(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_s_fmt_vid_cap(struct file *file, void *priv, struct v4l2_format *f);
+int vivid_vid_cap_g_selection(struct file *file, void *priv, struct v4l2_selection *sel);
+int vivid_vid_cap_s_selection(struct file *file, void *fh, struct v4l2_selection *s);
+int vivid_vid_cap_cropcap(struct file *file, void *priv, struct v4l2_cropcap *cap);
+int vidioc_enum_fmt_vid_overlay(struct file *file, void  *priv, struct v4l2_fmtdesc *f);
+int vidioc_g_fmt_vid_overlay(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_try_fmt_vid_overlay(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_s_fmt_vid_overlay(struct file *file, void *priv, struct v4l2_format *f);
+int vivid_vid_cap_overlay(struct file *file, void *fh, unsigned i);
+int vivid_vid_cap_g_fbuf(struct file *file, void *fh, struct v4l2_framebuffer *a);
+int vivid_vid_cap_s_fbuf(struct file *file, void *fh, const struct v4l2_framebuffer *a);
+int vidioc_enum_input(struct file *file, void *priv, struct v4l2_input *inp);
+int vidioc_g_input(struct file *file, void *priv, unsigned *i);
+int vidioc_s_input(struct file *file, void *priv, unsigned i);
+int vidioc_enumaudio(struct file *file, void *fh, struct v4l2_audio *vin);
+int vidioc_g_audio(struct file *file, void *fh, struct v4l2_audio *vin);
+int vidioc_s_audio(struct file *file, void *fh, const struct v4l2_audio *vin);
+int vivid_video_g_frequency(struct file *file, void *fh, struct v4l2_frequency *vf);
+int vivid_video_s_frequency(struct file *file, void *fh, const struct v4l2_frequency *vf);
+int vivid_video_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *vt);
+int vivid_video_g_tuner(struct file *file, void *fh, struct v4l2_tuner *vt);
+int vidioc_querystd(struct file *file, void *priv, v4l2_std_id *id);
+int vivid_vid_cap_s_std(struct file *file, void *priv, v4l2_std_id id);
+int vivid_vid_cap_s_dv_timings(struct file *file, void *_fh, struct v4l2_dv_timings *timings);
+int vidioc_query_dv_timings(struct file *file, void *_fh, struct v4l2_dv_timings *timings);
+int vidioc_s_edid(struct file *file, void *_fh, struct v4l2_edid *edid);
+int vidioc_enum_framesizes(struct file *file, void *fh, struct v4l2_frmsizeenum *fsize);
+int vidioc_enum_frameintervals(struct file *file, void *priv, struct v4l2_frmivalenum *fival);
+int vivid_vid_cap_g_parm(struct file *file, void *priv, struct v4l2_streamparm *parm);
+int vivid_vid_cap_s_parm(struct file *file, void *priv, struct v4l2_streamparm *parm);
+
+#endif
diff --git a/drivers/media/platform/vivid/vivid-vid-common.c b/drivers/media/platform/vivid/vivid-vid-common.c
new file mode 100644
index 0000000..1678b73
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-vid-common.c
@@ -0,0 +1,901 @@
+/*
+ * vivid-vid-common.c - common video support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/videodev2.h>
+#include <linux/v4l2-dv-timings.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-dv-timings.h>
+
+#include "vivid-core.h"
+#include "vivid-vid-common.h"
+
+const struct v4l2_dv_timings_cap vivid_dv_timings_cap = {
+	.type = V4L2_DV_BT_656_1120,
+	/* keep this initialization for compatibility with GCC < 4.4.6 */
+	.reserved = { 0 },
+	V4L2_INIT_BT_TIMINGS(0, MAX_WIDTH, 0, MAX_HEIGHT, 14000000, 775000000,
+		V4L2_DV_BT_STD_CEA861 | V4L2_DV_BT_STD_DMT |
+		V4L2_DV_BT_STD_CVT | V4L2_DV_BT_STD_GTF,
+		V4L2_DV_BT_CAP_PROGRESSIVE | V4L2_DV_BT_CAP_INTERLACED)
+};
+
+/* ------------------------------------------------------------------
+	Basic structures
+   ------------------------------------------------------------------*/
+
+struct vivid_fmt vivid_formats[] = {
+	{
+		.fourcc   = V4L2_PIX_FMT_YUYV,
+		.vdownsampling = { 1 },
+		.bit_depth = { 16 },
+		.is_yuv   = true,
+		.planes   = 1,
+		.buffers = 1,
+		.data_offset = { PLANE0_DATA_OFFSET },
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_UYVY,
+		.vdownsampling = { 1 },
+		.bit_depth = { 16 },
+		.is_yuv   = true,
+		.planes   = 1,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_YVYU,
+		.vdownsampling = { 1 },
+		.bit_depth = { 16 },
+		.is_yuv   = true,
+		.planes   = 1,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_VYUY,
+		.vdownsampling = { 1 },
+		.bit_depth = { 16 },
+		.is_yuv   = true,
+		.planes   = 1,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_YUV422P,
+		.vdownsampling = { 1, 1, 1 },
+		.bit_depth = { 8, 4, 4 },
+		.is_yuv   = true,
+		.planes   = 3,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_YUV420,
+		.vdownsampling = { 1, 2, 2 },
+		.bit_depth = { 8, 4, 4 },
+		.is_yuv   = true,
+		.planes   = 3,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_YVU420,
+		.vdownsampling = { 1, 2, 2 },
+		.bit_depth = { 8, 4, 4 },
+		.is_yuv   = true,
+		.planes   = 3,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_NV12,
+		.vdownsampling = { 1, 2 },
+		.bit_depth = { 8, 8 },
+		.is_yuv   = true,
+		.planes   = 2,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_NV21,
+		.vdownsampling = { 1, 2 },
+		.bit_depth = { 8, 8 },
+		.is_yuv   = true,
+		.planes   = 2,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_NV16,
+		.vdownsampling = { 1, 1 },
+		.bit_depth = { 8, 8 },
+		.is_yuv   = true,
+		.planes   = 2,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_NV61,
+		.vdownsampling = { 1, 1 },
+		.bit_depth = { 8, 8 },
+		.is_yuv   = true,
+		.planes   = 2,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_NV24,
+		.vdownsampling = { 1, 1 },
+		.bit_depth = { 8, 16 },
+		.is_yuv   = true,
+		.planes   = 2,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_NV42,
+		.vdownsampling = { 1, 1 },
+		.bit_depth = { 8, 16 },
+		.is_yuv   = true,
+		.planes   = 2,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_YUV555, /* uuuvvvvv ayyyyyuu */
+		.vdownsampling = { 1 },
+		.bit_depth = { 16 },
+		.planes   = 1,
+		.buffers = 1,
+		.alpha_mask = 0x8000,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_YUV565, /* uuuvvvvv yyyyyuuu */
+		.vdownsampling = { 1 },
+		.bit_depth = { 16 },
+		.planes   = 1,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_YUV444, /* uuuuvvvv aaaayyyy */
+		.vdownsampling = { 1 },
+		.bit_depth = { 16 },
+		.planes   = 1,
+		.buffers = 1,
+		.alpha_mask = 0xf000,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_YUV32, /* ayuv */
+		.vdownsampling = { 1 },
+		.bit_depth = { 32 },
+		.planes   = 1,
+		.buffers = 1,
+		.alpha_mask = 0x000000ff,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_GREY,
+		.vdownsampling = { 1 },
+		.bit_depth = { 8 },
+		.is_yuv   = true,
+		.planes   = 1,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_Y16,
+		.vdownsampling = { 1 },
+		.bit_depth = { 16 },
+		.is_yuv   = true,
+		.planes   = 1,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_Y16_BE,
+		.vdownsampling = { 1 },
+		.bit_depth = { 16 },
+		.is_yuv   = true,
+		.planes   = 1,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_RGB332, /* rrrgggbb */
+		.vdownsampling = { 1 },
+		.bit_depth = { 8 },
+		.planes   = 1,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_RGB565, /* gggbbbbb rrrrrggg */
+		.vdownsampling = { 1 },
+		.bit_depth = { 16 },
+		.planes   = 1,
+		.buffers = 1,
+		.can_do_overlay = true,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_RGB565X, /* rrrrrggg gggbbbbb */
+		.vdownsampling = { 1 },
+		.bit_depth = { 16 },
+		.planes   = 1,
+		.buffers = 1,
+		.can_do_overlay = true,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_RGB444, /* xxxxrrrr ggggbbbb */
+		.vdownsampling = { 1 },
+		.bit_depth = { 16 },
+		.planes   = 1,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_XRGB444, /* xxxxrrrr ggggbbbb */
+		.vdownsampling = { 1 },
+		.bit_depth = { 16 },
+		.planes   = 1,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_ARGB444, /* aaaarrrr ggggbbbb */
+		.vdownsampling = { 1 },
+		.bit_depth = { 16 },
+		.planes   = 1,
+		.buffers = 1,
+		.alpha_mask = 0x00f0,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_RGB555, /* gggbbbbb xrrrrrgg */
+		.vdownsampling = { 1 },
+		.bit_depth = { 16 },
+		.planes   = 1,
+		.buffers = 1,
+		.can_do_overlay = true,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_XRGB555, /* gggbbbbb xrrrrrgg */
+		.vdownsampling = { 1 },
+		.bit_depth = { 16 },
+		.planes   = 1,
+		.buffers = 1,
+		.can_do_overlay = true,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_ARGB555, /* gggbbbbb arrrrrgg */
+		.vdownsampling = { 1 },
+		.bit_depth = { 16 },
+		.planes   = 1,
+		.buffers = 1,
+		.can_do_overlay = true,
+		.alpha_mask = 0x8000,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_RGB555X, /* xrrrrrgg gggbbbbb */
+		.vdownsampling = { 1 },
+		.bit_depth = { 16 },
+		.planes   = 1,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_XRGB555X, /* xrrrrrgg gggbbbbb */
+		.vdownsampling = { 1 },
+		.bit_depth = { 16 },
+		.planes   = 1,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_ARGB555X, /* arrrrrgg gggbbbbb */
+		.vdownsampling = { 1 },
+		.bit_depth = { 16 },
+		.planes   = 1,
+		.buffers = 1,
+		.alpha_mask = 0x0080,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_RGB24, /* rgb */
+		.vdownsampling = { 1 },
+		.bit_depth = { 24 },
+		.planes   = 1,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_BGR24, /* bgr */
+		.vdownsampling = { 1 },
+		.bit_depth = { 24 },
+		.planes   = 1,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_BGR666, /* bbbbbbgg ggggrrrr rrxxxxxx */
+		.vdownsampling = { 1 },
+		.bit_depth = { 32 },
+		.planes   = 1,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_RGB32, /* xrgb */
+		.vdownsampling = { 1 },
+		.bit_depth = { 32 },
+		.planes   = 1,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_BGR32, /* bgrx */
+		.vdownsampling = { 1 },
+		.bit_depth = { 32 },
+		.planes   = 1,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_XRGB32, /* xrgb */
+		.vdownsampling = { 1 },
+		.bit_depth = { 32 },
+		.planes   = 1,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_XBGR32, /* bgrx */
+		.vdownsampling = { 1 },
+		.bit_depth = { 32 },
+		.planes   = 1,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_ARGB32, /* argb */
+		.vdownsampling = { 1 },
+		.bit_depth = { 32 },
+		.planes   = 1,
+		.buffers = 1,
+		.alpha_mask = 0x000000ff,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_ABGR32, /* bgra */
+		.vdownsampling = { 1 },
+		.bit_depth = { 32 },
+		.planes   = 1,
+		.buffers = 1,
+		.alpha_mask = 0xff000000,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_SBGGR8, /* Bayer BG/GR */
+		.vdownsampling = { 1 },
+		.bit_depth = { 8 },
+		.planes   = 1,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_SGBRG8, /* Bayer GB/RG */
+		.vdownsampling = { 1 },
+		.bit_depth = { 8 },
+		.planes   = 1,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_SGRBG8, /* Bayer GR/BG */
+		.vdownsampling = { 1 },
+		.bit_depth = { 8 },
+		.planes   = 1,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_SRGGB8, /* Bayer RG/GB */
+		.vdownsampling = { 1 },
+		.bit_depth = { 8 },
+		.planes   = 1,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_SBGGR10, /* Bayer BG/GR */
+		.vdownsampling = { 1 },
+		.bit_depth = { 16 },
+		.planes   = 1,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_SGBRG10, /* Bayer GB/RG */
+		.vdownsampling = { 1 },
+		.bit_depth = { 16 },
+		.planes   = 1,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_SGRBG10, /* Bayer GR/BG */
+		.vdownsampling = { 1 },
+		.bit_depth = { 16 },
+		.planes   = 1,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_SRGGB10, /* Bayer RG/GB */
+		.vdownsampling = { 1 },
+		.bit_depth = { 16 },
+		.planes   = 1,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_SBGGR12, /* Bayer BG/GR */
+		.vdownsampling = { 1 },
+		.bit_depth = { 16 },
+		.planes   = 1,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_SGBRG12, /* Bayer GB/RG */
+		.vdownsampling = { 1 },
+		.bit_depth = { 16 },
+		.planes   = 1,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_SGRBG12, /* Bayer GR/BG */
+		.vdownsampling = { 1 },
+		.bit_depth = { 16 },
+		.planes   = 1,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_SRGGB12, /* Bayer RG/GB */
+		.vdownsampling = { 1 },
+		.bit_depth = { 16 },
+		.planes   = 1,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_NV16M,
+		.vdownsampling = { 1, 1 },
+		.bit_depth = { 8, 8 },
+		.is_yuv   = true,
+		.planes   = 2,
+		.buffers = 2,
+		.data_offset = { PLANE0_DATA_OFFSET, 0 },
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_NV61M,
+		.vdownsampling = { 1, 1 },
+		.bit_depth = { 8, 8 },
+		.is_yuv   = true,
+		.planes   = 2,
+		.buffers = 2,
+		.data_offset = { 0, PLANE0_DATA_OFFSET },
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_YUV420M,
+		.vdownsampling = { 1, 2, 2 },
+		.bit_depth = { 8, 4, 4 },
+		.is_yuv   = true,
+		.planes   = 3,
+		.buffers = 3,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_YVU420M,
+		.vdownsampling = { 1, 2, 2 },
+		.bit_depth = { 8, 4, 4 },
+		.is_yuv   = true,
+		.planes   = 3,
+		.buffers = 3,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_NV12M,
+		.vdownsampling = { 1, 2 },
+		.bit_depth = { 8, 8 },
+		.is_yuv   = true,
+		.planes   = 2,
+		.buffers = 2,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_NV21M,
+		.vdownsampling = { 1, 2 },
+		.bit_depth = { 8, 8 },
+		.is_yuv   = true,
+		.planes   = 2,
+		.buffers = 2,
+	},
+};
+
+/* There are 6 multiplanar formats in the list */
+#define VIVID_MPLANAR_FORMATS 6
+
+const struct vivid_fmt *vivid_get_format(struct vivid_dev *dev, u32 pixelformat)
+{
+	const struct vivid_fmt *fmt;
+	unsigned k;
+
+	for (k = 0; k < ARRAY_SIZE(vivid_formats); k++) {
+		fmt = &vivid_formats[k];
+		if (fmt->fourcc == pixelformat)
+			if (fmt->buffers == 1 || dev->multiplanar)
+				return fmt;
+	}
+
+	return NULL;
+}
+
+bool vivid_vid_can_loop(struct vivid_dev *dev)
+{
+	if (dev->src_rect.width != dev->sink_rect.width ||
+	    dev->src_rect.height != dev->sink_rect.height)
+		return false;
+	if (dev->fmt_cap->fourcc != dev->fmt_out->fourcc)
+		return false;
+	if (dev->field_cap != dev->field_out)
+		return false;
+	/*
+	 * While this can be supported, it is just too much work
+	 * to actually implement.
+	 */
+	if (dev->field_cap == V4L2_FIELD_SEQ_TB ||
+	    dev->field_cap == V4L2_FIELD_SEQ_BT)
+		return false;
+	if (vivid_is_svid_cap(dev) && vivid_is_svid_out(dev)) {
+		if (!(dev->std_cap & V4L2_STD_525_60) !=
+		    !(dev->std_out & V4L2_STD_525_60))
+			return false;
+		return true;
+	}
+	if (vivid_is_hdmi_cap(dev) && vivid_is_hdmi_out(dev))
+		return true;
+	return false;
+}
+
+void vivid_send_source_change(struct vivid_dev *dev, unsigned type)
+{
+	struct v4l2_event ev = {
+		.type = V4L2_EVENT_SOURCE_CHANGE,
+		.u.src_change.changes = V4L2_EVENT_SRC_CH_RESOLUTION,
+	};
+	unsigned i;
+
+	for (i = 0; i < dev->num_inputs; i++) {
+		ev.id = i;
+		if (dev->input_type[i] == type) {
+			if (video_is_registered(&dev->vid_cap_dev) && dev->has_vid_cap)
+				v4l2_event_queue(&dev->vid_cap_dev, &ev);
+			if (video_is_registered(&dev->vbi_cap_dev) && dev->has_vbi_cap)
+				v4l2_event_queue(&dev->vbi_cap_dev, &ev);
+		}
+	}
+}
+
+/*
+ * Conversion function that converts a single-planar format to a
+ * single-plane multiplanar format.
+ */
+void fmt_sp2mp(const struct v4l2_format *sp_fmt, struct v4l2_format *mp_fmt)
+{
+	struct v4l2_pix_format_mplane *mp = &mp_fmt->fmt.pix_mp;
+	struct v4l2_plane_pix_format *ppix = &mp->plane_fmt[0];
+	const struct v4l2_pix_format *pix = &sp_fmt->fmt.pix;
+	bool is_out = sp_fmt->type == V4L2_BUF_TYPE_VIDEO_OUTPUT;
+
+	memset(mp->reserved, 0, sizeof(mp->reserved));
+	mp_fmt->type = is_out ? V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE :
+			   V4L2_CAP_VIDEO_CAPTURE_MPLANE;
+	mp->width = pix->width;
+	mp->height = pix->height;
+	mp->pixelformat = pix->pixelformat;
+	mp->field = pix->field;
+	mp->colorspace = pix->colorspace;
+	mp->xfer_func = pix->xfer_func;
+	mp->ycbcr_enc = pix->ycbcr_enc;
+	mp->quantization = pix->quantization;
+	mp->num_planes = 1;
+	mp->flags = pix->flags;
+	ppix->sizeimage = pix->sizeimage;
+	ppix->bytesperline = pix->bytesperline;
+	memset(ppix->reserved, 0, sizeof(ppix->reserved));
+}
+
+int fmt_sp2mp_func(struct file *file, void *priv,
+		struct v4l2_format *f, fmtfunc func)
+{
+	struct v4l2_format fmt;
+	struct v4l2_pix_format_mplane *mp = &fmt.fmt.pix_mp;
+	struct v4l2_plane_pix_format *ppix = &mp->plane_fmt[0];
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	int ret;
+
+	/* Converts to a mplane format */
+	fmt_sp2mp(f, &fmt);
+	/* Passes it to the generic mplane format function */
+	ret = func(file, priv, &fmt);
+	/* Copies back the mplane data to the single plane format */
+	pix->width = mp->width;
+	pix->height = mp->height;
+	pix->pixelformat = mp->pixelformat;
+	pix->field = mp->field;
+	pix->colorspace = mp->colorspace;
+	pix->xfer_func = mp->xfer_func;
+	pix->ycbcr_enc = mp->ycbcr_enc;
+	pix->quantization = mp->quantization;
+	pix->sizeimage = ppix->sizeimage;
+	pix->bytesperline = ppix->bytesperline;
+	pix->flags = mp->flags;
+	return ret;
+}
+
+/* v4l2_rect helper function: copy the width/height values */
+void rect_set_size_to(struct v4l2_rect *r, const struct v4l2_rect *size)
+{
+	r->width = size->width;
+	r->height = size->height;
+}
+
+/* v4l2_rect helper function: width and height of r should be >= min_size */
+void rect_set_min_size(struct v4l2_rect *r, const struct v4l2_rect *min_size)
+{
+	if (r->width < min_size->width)
+		r->width = min_size->width;
+	if (r->height < min_size->height)
+		r->height = min_size->height;
+}
+
+/* v4l2_rect helper function: width and height of r should be <= max_size */
+void rect_set_max_size(struct v4l2_rect *r, const struct v4l2_rect *max_size)
+{
+	if (r->width > max_size->width)
+		r->width = max_size->width;
+	if (r->height > max_size->height)
+		r->height = max_size->height;
+}
+
+/* v4l2_rect helper function: r should be inside boundary */
+void rect_map_inside(struct v4l2_rect *r, const struct v4l2_rect *boundary)
+{
+	rect_set_max_size(r, boundary);
+	if (r->left < boundary->left)
+		r->left = boundary->left;
+	if (r->top < boundary->top)
+		r->top = boundary->top;
+	if (r->left + r->width > boundary->width)
+		r->left = boundary->width - r->width;
+	if (r->top + r->height > boundary->height)
+		r->top = boundary->height - r->height;
+}
+
+/* v4l2_rect helper function: return true if r1 has the same size as r2 */
+bool rect_same_size(const struct v4l2_rect *r1, const struct v4l2_rect *r2)
+{
+	return r1->width == r2->width && r1->height == r2->height;
+}
+
+/* v4l2_rect helper function: calculate the intersection of two rects */
+struct v4l2_rect rect_intersect(const struct v4l2_rect *a, const struct v4l2_rect *b)
+{
+	struct v4l2_rect r;
+	int right, bottom;
+
+	r.top = max(a->top, b->top);
+	r.left = max(a->left, b->left);
+	bottom = min(a->top + a->height, b->top + b->height);
+	right = min(a->left + a->width, b->left + b->width);
+	r.height = max(0, bottom - r.top);
+	r.width = max(0, right - r.left);
+	return r;
+}
+
+/*
+ * v4l2_rect helper function: scale rect r by to->width / from->width and
+ * to->height / from->height.
+ */
+void rect_scale(struct v4l2_rect *r, const struct v4l2_rect *from,
+				     const struct v4l2_rect *to)
+{
+	if (from->width == 0 || from->height == 0) {
+		r->left = r->top = r->width = r->height = 0;
+		return;
+	}
+	r->left = (((r->left - from->left) * to->width) / from->width) & ~1;
+	r->width = ((r->width * to->width) / from->width) & ~1;
+	r->top = ((r->top - from->top) * to->height) / from->height;
+	r->height = (r->height * to->height) / from->height;
+}
+
+bool rect_overlap(const struct v4l2_rect *r1, const struct v4l2_rect *r2)
+{
+	/*
+	 * IF the left side of r1 is to the right of the right side of r2 OR
+	 *    the left side of r2 is to the right of the right side of r1 THEN
+	 * they do not overlap.
+	 */
+	if (r1->left >= r2->left + r2->width ||
+	    r2->left >= r1->left + r1->width)
+		return false;
+	/*
+	 * IF the top side of r1 is below the bottom of r2 OR
+	 *    the top side of r2 is below the bottom of r1 THEN
+	 * they do not overlap.
+	 */
+	if (r1->top >= r2->top + r2->height ||
+	    r2->top >= r1->top + r1->height)
+		return false;
+	return true;
+}
+int vivid_vid_adjust_sel(unsigned flags, struct v4l2_rect *r)
+{
+	unsigned w = r->width;
+	unsigned h = r->height;
+
+	/* sanitize w and h in case someone passes ~0 as the value */
+	w &= 0xffff;
+	h &= 0xffff;
+	if (!(flags & V4L2_SEL_FLAG_LE)) {
+		w++;
+		h++;
+		if (w < 2)
+			w = 2;
+		if (h < 2)
+			h = 2;
+	}
+	if (!(flags & V4L2_SEL_FLAG_GE)) {
+		if (w > MAX_WIDTH)
+			w = MAX_WIDTH;
+		if (h > MAX_HEIGHT)
+			h = MAX_HEIGHT;
+	}
+	w = w & ~1;
+	h = h & ~1;
+	if (w < 2 || h < 2)
+		return -ERANGE;
+	if (w > MAX_WIDTH || h > MAX_HEIGHT)
+		return -ERANGE;
+	if (r->top < 0)
+		r->top = 0;
+	if (r->left < 0)
+		r->left = 0;
+	/* sanitize left and top in case someone passes ~0 as the value */
+	r->left &= 0xfffe;
+	r->top &= 0xfffe;
+	if (r->left + w > MAX_WIDTH)
+		r->left = MAX_WIDTH - w;
+	if (r->top + h > MAX_HEIGHT)
+		r->top = MAX_HEIGHT - h;
+	if ((flags & (V4L2_SEL_FLAG_GE | V4L2_SEL_FLAG_LE)) ==
+			(V4L2_SEL_FLAG_GE | V4L2_SEL_FLAG_LE) &&
+	    (r->width != w || r->height != h))
+		return -ERANGE;
+	r->width = w;
+	r->height = h;
+	return 0;
+}
+
+int vivid_enum_fmt_vid(struct file *file, void  *priv,
+					struct v4l2_fmtdesc *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	const struct vivid_fmt *fmt;
+
+	if (f->index >= ARRAY_SIZE(vivid_formats) -
+	    (dev->multiplanar ? 0 : VIVID_MPLANAR_FORMATS))
+		return -EINVAL;
+
+	fmt = &vivid_formats[f->index];
+
+	f->pixelformat = fmt->fourcc;
+	return 0;
+}
+
+int vidioc_enum_fmt_vid_mplane(struct file *file, void  *priv,
+					struct v4l2_fmtdesc *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!dev->multiplanar)
+		return -ENOTTY;
+	return vivid_enum_fmt_vid(file, priv, f);
+}
+
+int vidioc_enum_fmt_vid(struct file *file, void  *priv,
+					struct v4l2_fmtdesc *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (dev->multiplanar)
+		return -ENOTTY;
+	return vivid_enum_fmt_vid(file, priv, f);
+}
+
+int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *id)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_dir == VFL_DIR_RX) {
+		if (!vivid_is_sdtv_cap(dev))
+			return -ENODATA;
+		*id = dev->std_cap;
+	} else {
+		if (!vivid_is_svid_out(dev))
+			return -ENODATA;
+		*id = dev->std_out;
+	}
+	return 0;
+}
+
+int vidioc_g_dv_timings(struct file *file, void *_fh,
+				    struct v4l2_dv_timings *timings)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_dir == VFL_DIR_RX) {
+		if (!vivid_is_hdmi_cap(dev))
+			return -ENODATA;
+		*timings = dev->dv_timings_cap;
+	} else {
+		if (!vivid_is_hdmi_out(dev))
+			return -ENODATA;
+		*timings = dev->dv_timings_out;
+	}
+	return 0;
+}
+
+int vidioc_enum_dv_timings(struct file *file, void *_fh,
+				    struct v4l2_enum_dv_timings *timings)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_dir == VFL_DIR_RX) {
+		if (!vivid_is_hdmi_cap(dev))
+			return -ENODATA;
+	} else {
+		if (!vivid_is_hdmi_out(dev))
+			return -ENODATA;
+	}
+	return v4l2_enum_dv_timings_cap(timings, &vivid_dv_timings_cap,
+			NULL, NULL);
+}
+
+int vidioc_dv_timings_cap(struct file *file, void *_fh,
+				    struct v4l2_dv_timings_cap *cap)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_dir == VFL_DIR_RX) {
+		if (!vivid_is_hdmi_cap(dev))
+			return -ENODATA;
+	} else {
+		if (!vivid_is_hdmi_out(dev))
+			return -ENODATA;
+	}
+	*cap = vivid_dv_timings_cap;
+	return 0;
+}
+
+int vidioc_g_edid(struct file *file, void *_fh,
+			 struct v4l2_edid *edid)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct video_device *vdev = video_devdata(file);
+
+	memset(edid->reserved, 0, sizeof(edid->reserved));
+	if (vdev->vfl_dir == VFL_DIR_RX) {
+		if (edid->pad >= dev->num_inputs)
+			return -EINVAL;
+		if (dev->input_type[edid->pad] != HDMI)
+			return -EINVAL;
+	} else {
+		if (edid->pad >= dev->num_outputs)
+			return -EINVAL;
+		if (dev->output_type[edid->pad] != HDMI)
+			return -EINVAL;
+	}
+	if (edid->start_block == 0 && edid->blocks == 0) {
+		edid->blocks = dev->edid_blocks;
+		return 0;
+	}
+	if (dev->edid_blocks == 0)
+		return -ENODATA;
+	if (edid->start_block >= dev->edid_blocks)
+		return -EINVAL;
+	if (edid->start_block + edid->blocks > dev->edid_blocks)
+		edid->blocks = dev->edid_blocks - edid->start_block;
+	memcpy(edid->edid, dev->edid, edid->blocks * 128);
+	return 0;
+}
diff --git a/drivers/media/platform/vivid/vivid-vid-common.h b/drivers/media/platform/vivid/vivid-vid-common.h
new file mode 100644
index 0000000..3ec4fa8
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-vid-common.h
@@ -0,0 +1,61 @@
+/*
+ * vivid-vid-common.h - common video support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VIVID_VID_COMMON_H_
+#define _VIVID_VID_COMMON_H_
+
+typedef int (*fmtfunc)(struct file *file, void *priv, struct v4l2_format *f);
+
+/*
+ * Conversion function that converts a single-planar format to a
+ * single-plane multiplanar format.
+ */
+void fmt_sp2mp(const struct v4l2_format *sp_fmt, struct v4l2_format *mp_fmt);
+int fmt_sp2mp_func(struct file *file, void *priv,
+		struct v4l2_format *f, fmtfunc func);
+
+extern const struct v4l2_dv_timings_cap vivid_dv_timings_cap;
+
+const struct vivid_fmt *vivid_get_format(struct vivid_dev *dev, u32 pixelformat);
+
+bool vivid_vid_can_loop(struct vivid_dev *dev);
+void vivid_send_source_change(struct vivid_dev *dev, unsigned type);
+
+bool rect_overlap(const struct v4l2_rect *r1, const struct v4l2_rect *r2);
+void rect_set_size_to(struct v4l2_rect *r, const struct v4l2_rect *size);
+void rect_set_min_size(struct v4l2_rect *r, const struct v4l2_rect *min_size);
+void rect_set_max_size(struct v4l2_rect *r, const struct v4l2_rect *max_size);
+void rect_map_inside(struct v4l2_rect *r, const struct v4l2_rect *boundary);
+bool rect_same_size(const struct v4l2_rect *r1, const struct v4l2_rect *r2);
+struct v4l2_rect rect_intersect(const struct v4l2_rect *a, const struct v4l2_rect *b);
+void rect_scale(struct v4l2_rect *r, const struct v4l2_rect *from,
+				     const struct v4l2_rect *to);
+int vivid_vid_adjust_sel(unsigned flags, struct v4l2_rect *r);
+
+int vivid_enum_fmt_vid(struct file *file, void  *priv, struct v4l2_fmtdesc *f);
+int vidioc_enum_fmt_vid_mplane(struct file *file, void  *priv, struct v4l2_fmtdesc *f);
+int vidioc_enum_fmt_vid(struct file *file, void  *priv, struct v4l2_fmtdesc *f);
+int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *id);
+int vidioc_g_dv_timings(struct file *file, void *_fh, struct v4l2_dv_timings *timings);
+int vidioc_enum_dv_timings(struct file *file, void *_fh, struct v4l2_enum_dv_timings *timings);
+int vidioc_dv_timings_cap(struct file *file, void *_fh, struct v4l2_dv_timings_cap *cap);
+int vidioc_g_edid(struct file *file, void *_fh, struct v4l2_edid *edid);
+int vidioc_subscribe_event(struct v4l2_fh *fh, const struct v4l2_event_subscription *sub);
+
+#endif
diff --git a/drivers/media/platform/vivid/vivid-vid-out.c b/drivers/media/platform/vivid/vivid-vid-out.c
new file mode 100644
index 0000000..b77acb6
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-vid-out.c
@@ -0,0 +1,1192 @@
+/*
+ * vivid-vid-out.c - video output support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/videodev2.h>
+#include <linux/v4l2-dv-timings.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-dv-timings.h>
+
+#include "vivid-core.h"
+#include "vivid-vid-common.h"
+#include "vivid-kthread-out.h"
+#include "vivid-vid-out.h"
+
+static int vid_out_queue_setup(struct vb2_queue *vq, const void *parg,
+		       unsigned *nbuffers, unsigned *nplanes,
+		       unsigned sizes[], void *alloc_ctxs[])
+{
+	const struct v4l2_format *fmt = parg;
+	struct vivid_dev *dev = vb2_get_drv_priv(vq);
+	const struct vivid_fmt *vfmt = dev->fmt_out;
+	unsigned planes = vfmt->buffers;
+	unsigned h = dev->fmt_out_rect.height;
+	unsigned size = dev->bytesperline_out[0] * h;
+	unsigned p;
+
+	for (p = vfmt->buffers; p < vfmt->planes; p++)
+		size += dev->bytesperline_out[p] * h / vfmt->vdownsampling[p];
+
+	if (dev->field_out == V4L2_FIELD_ALTERNATE) {
+		/*
+		 * You cannot use write() with FIELD_ALTERNATE since the field
+		 * information (TOP/BOTTOM) cannot be passed to the kernel.
+		 */
+		if (vb2_fileio_is_active(vq))
+			return -EINVAL;
+	}
+
+	if (dev->queue_setup_error) {
+		/*
+		 * Error injection: test what happens if queue_setup() returns
+		 * an error.
+		 */
+		dev->queue_setup_error = false;
+		return -EINVAL;
+	}
+
+	if (fmt) {
+		const struct v4l2_pix_format_mplane *mp;
+		struct v4l2_format mp_fmt;
+
+		if (!V4L2_TYPE_IS_MULTIPLANAR(fmt->type)) {
+			fmt_sp2mp(fmt, &mp_fmt);
+			fmt = &mp_fmt;
+		}
+		mp = &fmt->fmt.pix_mp;
+		/*
+		 * Check if the number of planes in the specified format match
+		 * the number of planes in the current format. You can't mix that.
+		 */
+		if (mp->num_planes != planes)
+			return -EINVAL;
+		sizes[0] = mp->plane_fmt[0].sizeimage;
+		if (sizes[0] < size)
+			return -EINVAL;
+		for (p = 1; p < planes; p++) {
+			sizes[p] = mp->plane_fmt[p].sizeimage;
+			if (sizes[p] < dev->bytesperline_out[p] * h)
+				return -EINVAL;
+		}
+	} else {
+		for (p = 0; p < planes; p++)
+			sizes[p] = p ? dev->bytesperline_out[p] * h : size;
+	}
+
+	if (vq->num_buffers + *nbuffers < 2)
+		*nbuffers = 2 - vq->num_buffers;
+
+	*nplanes = planes;
+
+	/*
+	 * videobuf2-vmalloc allocator is context-less so no need to set
+	 * alloc_ctxs array.
+	 */
+
+	dprintk(dev, 1, "%s: count=%d\n", __func__, *nbuffers);
+	for (p = 0; p < planes; p++)
+		dprintk(dev, 1, "%s: size[%u]=%u\n", __func__, p, sizes[p]);
+	return 0;
+}
+
+static int vid_out_buf_prepare(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+	unsigned long size;
+	unsigned planes;
+	unsigned p;
+
+	dprintk(dev, 1, "%s\n", __func__);
+
+	if (WARN_ON(NULL == dev->fmt_out))
+		return -EINVAL;
+
+	planes = dev->fmt_out->planes;
+
+	if (dev->buf_prepare_error) {
+		/*
+		 * Error injection: test what happens if buf_prepare() returns
+		 * an error.
+		 */
+		dev->buf_prepare_error = false;
+		return -EINVAL;
+	}
+
+	if (dev->field_out != V4L2_FIELD_ALTERNATE)
+		vbuf->field = dev->field_out;
+	else if (vbuf->field != V4L2_FIELD_TOP &&
+		 vbuf->field != V4L2_FIELD_BOTTOM)
+		return -EINVAL;
+
+	for (p = 0; p < planes; p++) {
+		size = dev->bytesperline_out[p] * dev->fmt_out_rect.height +
+			vb->planes[p].data_offset;
+
+		if (vb2_get_plane_payload(vb, p) < size) {
+			dprintk(dev, 1, "%s the payload is too small for plane %u (%lu < %lu)\n",
+					__func__, p, vb2_get_plane_payload(vb, p), size);
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+static void vid_out_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+	struct vivid_buffer *buf = container_of(vbuf, struct vivid_buffer, vb);
+
+	dprintk(dev, 1, "%s\n", __func__);
+
+	spin_lock(&dev->slock);
+	list_add_tail(&buf->list, &dev->vid_out_active);
+	spin_unlock(&dev->slock);
+}
+
+static int vid_out_start_streaming(struct vb2_queue *vq, unsigned count)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vq);
+	int err;
+
+	if (vb2_is_streaming(&dev->vb_vid_cap_q))
+		dev->can_loop_video = vivid_vid_can_loop(dev);
+
+	if (dev->kthread_vid_out)
+		return 0;
+
+	dev->vid_out_seq_count = 0;
+	dprintk(dev, 1, "%s\n", __func__);
+	if (dev->start_streaming_error) {
+		dev->start_streaming_error = false;
+		err = -EINVAL;
+	} else {
+		err = vivid_start_generating_vid_out(dev, &dev->vid_out_streaming);
+	}
+	if (err) {
+		struct vivid_buffer *buf, *tmp;
+
+		list_for_each_entry_safe(buf, tmp, &dev->vid_out_active, list) {
+			list_del(&buf->list);
+			vb2_buffer_done(&buf->vb.vb2_buf,
+					VB2_BUF_STATE_QUEUED);
+		}
+	}
+	return err;
+}
+
+/* abort streaming and wait for last buffer */
+static void vid_out_stop_streaming(struct vb2_queue *vq)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vq);
+
+	dprintk(dev, 1, "%s\n", __func__);
+	vivid_stop_generating_vid_out(dev, &dev->vid_out_streaming);
+	dev->can_loop_video = false;
+}
+
+const struct vb2_ops vivid_vid_out_qops = {
+	.queue_setup		= vid_out_queue_setup,
+	.buf_prepare		= vid_out_buf_prepare,
+	.buf_queue		= vid_out_buf_queue,
+	.start_streaming	= vid_out_start_streaming,
+	.stop_streaming		= vid_out_stop_streaming,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
+};
+
+/*
+ * Called whenever the format has to be reset which can occur when
+ * changing outputs, standard, timings, etc.
+ */
+void vivid_update_format_out(struct vivid_dev *dev)
+{
+	struct v4l2_bt_timings *bt = &dev->dv_timings_out.bt;
+	unsigned size, p;
+
+	switch (dev->output_type[dev->output]) {
+	case SVID:
+	default:
+		dev->field_out = dev->tv_field_out;
+		dev->sink_rect.width = 720;
+		if (dev->std_out & V4L2_STD_525_60) {
+			dev->sink_rect.height = 480;
+			dev->timeperframe_vid_out = (struct v4l2_fract) { 1001, 30000 };
+			dev->service_set_out = V4L2_SLICED_CAPTION_525;
+		} else {
+			dev->sink_rect.height = 576;
+			dev->timeperframe_vid_out = (struct v4l2_fract) { 1000, 25000 };
+			dev->service_set_out = V4L2_SLICED_WSS_625 | V4L2_SLICED_TELETEXT_B;
+		}
+		dev->colorspace_out = V4L2_COLORSPACE_SMPTE170M;
+		break;
+	case HDMI:
+		dev->sink_rect.width = bt->width;
+		dev->sink_rect.height = bt->height;
+		size = V4L2_DV_BT_FRAME_WIDTH(bt) * V4L2_DV_BT_FRAME_HEIGHT(bt);
+		dev->timeperframe_vid_out = (struct v4l2_fract) {
+			size / 100, (u32)bt->pixelclock / 100
+		};
+		if (bt->interlaced)
+			dev->field_out = V4L2_FIELD_ALTERNATE;
+		else
+			dev->field_out = V4L2_FIELD_NONE;
+		if (!dev->dvi_d_out && (bt->flags & V4L2_DV_FL_IS_CE_VIDEO)) {
+			if (bt->width == 720 && bt->height <= 576)
+				dev->colorspace_out = V4L2_COLORSPACE_SMPTE170M;
+			else
+				dev->colorspace_out = V4L2_COLORSPACE_REC709;
+		} else {
+			dev->colorspace_out = V4L2_COLORSPACE_SRGB;
+		}
+		break;
+	}
+	dev->xfer_func_out = V4L2_XFER_FUNC_DEFAULT;
+	dev->ycbcr_enc_out = V4L2_YCBCR_ENC_DEFAULT;
+	dev->quantization_out = V4L2_QUANTIZATION_DEFAULT;
+	dev->compose_out = dev->sink_rect;
+	dev->compose_bounds_out = dev->sink_rect;
+	dev->crop_out = dev->compose_out;
+	if (V4L2_FIELD_HAS_T_OR_B(dev->field_out))
+		dev->crop_out.height /= 2;
+	dev->fmt_out_rect = dev->crop_out;
+	for (p = 0; p < dev->fmt_out->planes; p++)
+		dev->bytesperline_out[p] =
+			(dev->sink_rect.width * dev->fmt_out->bit_depth[p]) / 8;
+}
+
+/* Map the field to something that is valid for the current output */
+static enum v4l2_field vivid_field_out(struct vivid_dev *dev, enum v4l2_field field)
+{
+	if (vivid_is_svid_out(dev)) {
+		switch (field) {
+		case V4L2_FIELD_INTERLACED_TB:
+		case V4L2_FIELD_INTERLACED_BT:
+		case V4L2_FIELD_SEQ_TB:
+		case V4L2_FIELD_SEQ_BT:
+		case V4L2_FIELD_ALTERNATE:
+			return field;
+		case V4L2_FIELD_INTERLACED:
+		default:
+			return V4L2_FIELD_INTERLACED;
+		}
+	}
+	if (vivid_is_hdmi_out(dev))
+		return dev->dv_timings_out.bt.interlaced ? V4L2_FIELD_ALTERNATE :
+						       V4L2_FIELD_NONE;
+	return V4L2_FIELD_NONE;
+}
+
+static enum tpg_pixel_aspect vivid_get_pixel_aspect(const struct vivid_dev *dev)
+{
+	if (vivid_is_svid_out(dev))
+		return (dev->std_out & V4L2_STD_525_60) ?
+			TPG_PIXEL_ASPECT_NTSC : TPG_PIXEL_ASPECT_PAL;
+
+	if (vivid_is_hdmi_out(dev) &&
+	    dev->sink_rect.width == 720 && dev->sink_rect.height <= 576)
+		return dev->sink_rect.height == 480 ?
+			TPG_PIXEL_ASPECT_NTSC : TPG_PIXEL_ASPECT_PAL;
+
+	return TPG_PIXEL_ASPECT_SQUARE;
+}
+
+int vivid_g_fmt_vid_out(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct v4l2_pix_format_mplane *mp = &f->fmt.pix_mp;
+	const struct vivid_fmt *fmt = dev->fmt_out;
+	unsigned p;
+
+	mp->width        = dev->fmt_out_rect.width;
+	mp->height       = dev->fmt_out_rect.height;
+	mp->field        = dev->field_out;
+	mp->pixelformat  = fmt->fourcc;
+	mp->colorspace   = dev->colorspace_out;
+	mp->xfer_func    = dev->xfer_func_out;
+	mp->ycbcr_enc    = dev->ycbcr_enc_out;
+	mp->quantization = dev->quantization_out;
+	mp->num_planes = fmt->buffers;
+	for (p = 0; p < mp->num_planes; p++) {
+		mp->plane_fmt[p].bytesperline = dev->bytesperline_out[p];
+		mp->plane_fmt[p].sizeimage =
+			mp->plane_fmt[p].bytesperline * mp->height;
+	}
+	for (p = fmt->buffers; p < fmt->planes; p++) {
+		unsigned stride = dev->bytesperline_out[p];
+
+		mp->plane_fmt[0].sizeimage +=
+			(stride * mp->height) / fmt->vdownsampling[p];
+	}
+	return 0;
+}
+
+int vivid_try_fmt_vid_out(struct file *file, void *priv,
+			struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct v4l2_bt_timings *bt = &dev->dv_timings_out.bt;
+	struct v4l2_pix_format_mplane *mp = &f->fmt.pix_mp;
+	struct v4l2_plane_pix_format *pfmt = mp->plane_fmt;
+	const struct vivid_fmt *fmt;
+	unsigned bytesperline, max_bpl;
+	unsigned factor = 1;
+	unsigned w, h;
+	unsigned p;
+
+	fmt = vivid_get_format(dev, mp->pixelformat);
+	if (!fmt) {
+		dprintk(dev, 1, "Fourcc format (0x%08x) unknown.\n",
+			mp->pixelformat);
+		mp->pixelformat = V4L2_PIX_FMT_YUYV;
+		fmt = vivid_get_format(dev, mp->pixelformat);
+	}
+
+	mp->field = vivid_field_out(dev, mp->field);
+	if (vivid_is_svid_out(dev)) {
+		w = 720;
+		h = (dev->std_out & V4L2_STD_525_60) ? 480 : 576;
+	} else {
+		w = dev->sink_rect.width;
+		h = dev->sink_rect.height;
+	}
+	if (V4L2_FIELD_HAS_T_OR_B(mp->field))
+		factor = 2;
+	if (!dev->has_scaler_out && !dev->has_crop_out && !dev->has_compose_out) {
+		mp->width = w;
+		mp->height = h / factor;
+	} else {
+		struct v4l2_rect r = { 0, 0, mp->width, mp->height * factor };
+
+		rect_set_min_size(&r, &vivid_min_rect);
+		rect_set_max_size(&r, &vivid_max_rect);
+		if (dev->has_scaler_out && !dev->has_crop_out) {
+			struct v4l2_rect max_r = { 0, 0, MAX_ZOOM * w, MAX_ZOOM * h };
+
+			rect_set_max_size(&r, &max_r);
+		} else if (!dev->has_scaler_out && dev->has_compose_out && !dev->has_crop_out) {
+			rect_set_max_size(&r, &dev->sink_rect);
+		} else if (!dev->has_scaler_out && !dev->has_compose_out) {
+			rect_set_min_size(&r, &dev->sink_rect);
+		}
+		mp->width = r.width;
+		mp->height = r.height / factor;
+	}
+
+	/* This driver supports custom bytesperline values */
+
+	/* Calculate the minimum supported bytesperline value */
+	bytesperline = (mp->width * fmt->bit_depth[0]) >> 3;
+	/* Calculate the maximum supported bytesperline value */
+	max_bpl = (MAX_ZOOM * MAX_WIDTH * fmt->bit_depth[0]) >> 3;
+	mp->num_planes = fmt->buffers;
+	for (p = 0; p < mp->num_planes; p++) {
+		if (pfmt[p].bytesperline > max_bpl)
+			pfmt[p].bytesperline = max_bpl;
+		if (pfmt[p].bytesperline < bytesperline)
+			pfmt[p].bytesperline = bytesperline;
+		pfmt[p].sizeimage = pfmt[p].bytesperline * mp->height;
+		memset(pfmt[p].reserved, 0, sizeof(pfmt[p].reserved));
+	}
+	for (p = fmt->buffers; p < fmt->planes; p++)
+		pfmt[0].sizeimage += (pfmt[0].bytesperline * fmt->bit_depth[p]) /
+				     (fmt->bit_depth[0] * fmt->vdownsampling[p]);
+	mp->xfer_func = V4L2_XFER_FUNC_DEFAULT;
+	mp->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+	mp->quantization = V4L2_QUANTIZATION_DEFAULT;
+	if (vivid_is_svid_out(dev)) {
+		mp->colorspace = V4L2_COLORSPACE_SMPTE170M;
+	} else if (dev->dvi_d_out || !(bt->flags & V4L2_DV_FL_IS_CE_VIDEO)) {
+		mp->colorspace = V4L2_COLORSPACE_SRGB;
+		if (dev->dvi_d_out)
+			mp->quantization = V4L2_QUANTIZATION_LIM_RANGE;
+	} else if (bt->width == 720 && bt->height <= 576) {
+		mp->colorspace = V4L2_COLORSPACE_SMPTE170M;
+	} else if (mp->colorspace != V4L2_COLORSPACE_SMPTE170M &&
+		   mp->colorspace != V4L2_COLORSPACE_REC709 &&
+		   mp->colorspace != V4L2_COLORSPACE_ADOBERGB &&
+		   mp->colorspace != V4L2_COLORSPACE_BT2020 &&
+		   mp->colorspace != V4L2_COLORSPACE_SRGB) {
+		mp->colorspace = V4L2_COLORSPACE_REC709;
+	}
+	memset(mp->reserved, 0, sizeof(mp->reserved));
+	return 0;
+}
+
+int vivid_s_fmt_vid_out(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct v4l2_pix_format_mplane *mp = &f->fmt.pix_mp;
+	struct vivid_dev *dev = video_drvdata(file);
+	struct v4l2_rect *crop = &dev->crop_out;
+	struct v4l2_rect *compose = &dev->compose_out;
+	struct vb2_queue *q = &dev->vb_vid_out_q;
+	int ret = vivid_try_fmt_vid_out(file, priv, f);
+	unsigned factor = 1;
+	unsigned p;
+
+	if (ret < 0)
+		return ret;
+
+	if (vb2_is_busy(q) &&
+	    (vivid_is_svid_out(dev) ||
+	     mp->width != dev->fmt_out_rect.width ||
+	     mp->height != dev->fmt_out_rect.height ||
+	     mp->pixelformat != dev->fmt_out->fourcc ||
+	     mp->field != dev->field_out)) {
+		dprintk(dev, 1, "%s device busy\n", __func__);
+		return -EBUSY;
+	}
+
+	/*
+	 * Allow for changing the colorspace on the fly. Useful for testing
+	 * purposes, and it is something that HDMI transmitters are able
+	 * to do.
+	 */
+	if (vb2_is_busy(q))
+		goto set_colorspace;
+
+	dev->fmt_out = vivid_get_format(dev, mp->pixelformat);
+	if (V4L2_FIELD_HAS_T_OR_B(mp->field))
+		factor = 2;
+
+	if (dev->has_scaler_out || dev->has_crop_out || dev->has_compose_out) {
+		struct v4l2_rect r = { 0, 0, mp->width, mp->height };
+
+		if (dev->has_scaler_out) {
+			if (dev->has_crop_out)
+				rect_map_inside(crop, &r);
+			else
+				*crop = r;
+			if (dev->has_compose_out && !dev->has_crop_out) {
+				struct v4l2_rect min_r = {
+					0, 0,
+					r.width / MAX_ZOOM,
+					factor * r.height / MAX_ZOOM
+				};
+				struct v4l2_rect max_r = {
+					0, 0,
+					r.width * MAX_ZOOM,
+					factor * r.height * MAX_ZOOM
+				};
+
+				rect_set_min_size(compose, &min_r);
+				rect_set_max_size(compose, &max_r);
+				rect_map_inside(compose, &dev->compose_bounds_out);
+			} else if (dev->has_compose_out) {
+				struct v4l2_rect min_r = {
+					0, 0,
+					crop->width / MAX_ZOOM,
+					factor * crop->height / MAX_ZOOM
+				};
+				struct v4l2_rect max_r = {
+					0, 0,
+					crop->width * MAX_ZOOM,
+					factor * crop->height * MAX_ZOOM
+				};
+
+				rect_set_min_size(compose, &min_r);
+				rect_set_max_size(compose, &max_r);
+				rect_map_inside(compose, &dev->compose_bounds_out);
+			}
+		} else if (dev->has_compose_out && !dev->has_crop_out) {
+			rect_set_size_to(crop, &r);
+			r.height *= factor;
+			rect_set_size_to(compose, &r);
+			rect_map_inside(compose, &dev->compose_bounds_out);
+		} else if (!dev->has_compose_out) {
+			rect_map_inside(crop, &r);
+			r.height /= factor;
+			rect_set_size_to(compose, &r);
+		} else {
+			r.height *= factor;
+			rect_set_max_size(compose, &r);
+			rect_map_inside(compose, &dev->compose_bounds_out);
+			crop->top *= factor;
+			crop->height *= factor;
+			rect_set_size_to(crop, compose);
+			rect_map_inside(crop, &r);
+			crop->top /= factor;
+			crop->height /= factor;
+		}
+	} else {
+		struct v4l2_rect r = { 0, 0, mp->width, mp->height };
+
+		rect_set_size_to(crop, &r);
+		r.height /= factor;
+		rect_set_size_to(compose, &r);
+	}
+
+	dev->fmt_out_rect.width = mp->width;
+	dev->fmt_out_rect.height = mp->height;
+	for (p = 0; p < mp->num_planes; p++)
+		dev->bytesperline_out[p] = mp->plane_fmt[p].bytesperline;
+	for (p = dev->fmt_out->buffers; p < dev->fmt_out->planes; p++)
+		dev->bytesperline_out[p] =
+			(dev->bytesperline_out[0] * dev->fmt_out->bit_depth[p]) /
+			dev->fmt_out->bit_depth[0];
+	dev->field_out = mp->field;
+	if (vivid_is_svid_out(dev))
+		dev->tv_field_out = mp->field;
+
+set_colorspace:
+	dev->colorspace_out = mp->colorspace;
+	dev->xfer_func_out = mp->xfer_func;
+	dev->ycbcr_enc_out = mp->ycbcr_enc;
+	dev->quantization_out = mp->quantization;
+	if (dev->loop_video) {
+		vivid_send_source_change(dev, SVID);
+		vivid_send_source_change(dev, HDMI);
+	}
+	return 0;
+}
+
+int vidioc_g_fmt_vid_out_mplane(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!dev->multiplanar)
+		return -ENOTTY;
+	return vivid_g_fmt_vid_out(file, priv, f);
+}
+
+int vidioc_try_fmt_vid_out_mplane(struct file *file, void *priv,
+			struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!dev->multiplanar)
+		return -ENOTTY;
+	return vivid_try_fmt_vid_out(file, priv, f);
+}
+
+int vidioc_s_fmt_vid_out_mplane(struct file *file, void *priv,
+			struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!dev->multiplanar)
+		return -ENOTTY;
+	return vivid_s_fmt_vid_out(file, priv, f);
+}
+
+int vidioc_g_fmt_vid_out(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (dev->multiplanar)
+		return -ENOTTY;
+	return fmt_sp2mp_func(file, priv, f, vivid_g_fmt_vid_out);
+}
+
+int vidioc_try_fmt_vid_out(struct file *file, void *priv,
+			struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (dev->multiplanar)
+		return -ENOTTY;
+	return fmt_sp2mp_func(file, priv, f, vivid_try_fmt_vid_out);
+}
+
+int vidioc_s_fmt_vid_out(struct file *file, void *priv,
+			struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (dev->multiplanar)
+		return -ENOTTY;
+	return fmt_sp2mp_func(file, priv, f, vivid_s_fmt_vid_out);
+}
+
+int vivid_vid_out_g_selection(struct file *file, void *priv,
+			      struct v4l2_selection *sel)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!dev->has_crop_out && !dev->has_compose_out)
+		return -ENOTTY;
+	if (sel->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		return -EINVAL;
+
+	sel->r.left = sel->r.top = 0;
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		if (!dev->has_crop_out)
+			return -EINVAL;
+		sel->r = dev->crop_out;
+		break;
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		if (!dev->has_crop_out)
+			return -EINVAL;
+		sel->r = dev->fmt_out_rect;
+		break;
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		if (!dev->has_crop_out)
+			return -EINVAL;
+		sel->r = vivid_max_rect;
+		break;
+	case V4L2_SEL_TGT_COMPOSE:
+		if (!dev->has_compose_out)
+			return -EINVAL;
+		sel->r = dev->compose_out;
+		break;
+	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+		if (!dev->has_compose_out)
+			return -EINVAL;
+		sel->r = dev->sink_rect;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+int vivid_vid_out_s_selection(struct file *file, void *fh, struct v4l2_selection *s)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct v4l2_rect *crop = &dev->crop_out;
+	struct v4l2_rect *compose = &dev->compose_out;
+	unsigned factor = V4L2_FIELD_HAS_T_OR_B(dev->field_out) ? 2 : 1;
+	int ret;
+
+	if (!dev->has_crop_out && !dev->has_compose_out)
+		return -ENOTTY;
+	if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		return -EINVAL;
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_CROP:
+		if (!dev->has_crop_out)
+			return -EINVAL;
+		ret = vivid_vid_adjust_sel(s->flags, &s->r);
+		if (ret)
+			return ret;
+		rect_set_min_size(&s->r, &vivid_min_rect);
+		rect_set_max_size(&s->r, &dev->fmt_out_rect);
+		if (dev->has_scaler_out) {
+			struct v4l2_rect max_rect = {
+				0, 0,
+				dev->sink_rect.width * MAX_ZOOM,
+				(dev->sink_rect.height / factor) * MAX_ZOOM
+			};
+
+			rect_set_max_size(&s->r, &max_rect);
+			if (dev->has_compose_out) {
+				struct v4l2_rect min_rect = {
+					0, 0,
+					s->r.width / MAX_ZOOM,
+					(s->r.height * factor) / MAX_ZOOM
+				};
+				struct v4l2_rect max_rect = {
+					0, 0,
+					s->r.width * MAX_ZOOM,
+					(s->r.height * factor) * MAX_ZOOM
+				};
+
+				rect_set_min_size(compose, &min_rect);
+				rect_set_max_size(compose, &max_rect);
+				rect_map_inside(compose, &dev->compose_bounds_out);
+			}
+		} else if (dev->has_compose_out) {
+			s->r.top *= factor;
+			s->r.height *= factor;
+			rect_set_max_size(&s->r, &dev->sink_rect);
+			rect_set_size_to(compose, &s->r);
+			rect_map_inside(compose, &dev->compose_bounds_out);
+			s->r.top /= factor;
+			s->r.height /= factor;
+		} else {
+			rect_set_size_to(&s->r, &dev->sink_rect);
+			s->r.height /= factor;
+		}
+		rect_map_inside(&s->r, &dev->fmt_out_rect);
+		*crop = s->r;
+		break;
+	case V4L2_SEL_TGT_COMPOSE:
+		if (!dev->has_compose_out)
+			return -EINVAL;
+		ret = vivid_vid_adjust_sel(s->flags, &s->r);
+		if (ret)
+			return ret;
+		rect_set_min_size(&s->r, &vivid_min_rect);
+		rect_set_max_size(&s->r, &dev->sink_rect);
+		rect_map_inside(&s->r, &dev->compose_bounds_out);
+		s->r.top /= factor;
+		s->r.height /= factor;
+		if (dev->has_scaler_out) {
+			struct v4l2_rect fmt = dev->fmt_out_rect;
+			struct v4l2_rect max_rect = {
+				0, 0,
+				s->r.width * MAX_ZOOM,
+				s->r.height * MAX_ZOOM
+			};
+			struct v4l2_rect min_rect = {
+				0, 0,
+				s->r.width / MAX_ZOOM,
+				s->r.height / MAX_ZOOM
+			};
+
+			rect_set_min_size(&fmt, &min_rect);
+			if (!dev->has_crop_out)
+				rect_set_max_size(&fmt, &max_rect);
+			if (!rect_same_size(&dev->fmt_out_rect, &fmt) &&
+			    vb2_is_busy(&dev->vb_vid_out_q))
+				return -EBUSY;
+			if (dev->has_crop_out) {
+				rect_set_min_size(crop, &min_rect);
+				rect_set_max_size(crop, &max_rect);
+			}
+			dev->fmt_out_rect = fmt;
+		} else if (dev->has_crop_out) {
+			struct v4l2_rect fmt = dev->fmt_out_rect;
+
+			rect_set_min_size(&fmt, &s->r);
+			if (!rect_same_size(&dev->fmt_out_rect, &fmt) &&
+			    vb2_is_busy(&dev->vb_vid_out_q))
+				return -EBUSY;
+			dev->fmt_out_rect = fmt;
+			rect_set_size_to(crop, &s->r);
+			rect_map_inside(crop, &dev->fmt_out_rect);
+		} else {
+			if (!rect_same_size(&s->r, &dev->fmt_out_rect) &&
+			    vb2_is_busy(&dev->vb_vid_out_q))
+				return -EBUSY;
+			rect_set_size_to(&dev->fmt_out_rect, &s->r);
+			rect_set_size_to(crop, &s->r);
+			crop->height /= factor;
+			rect_map_inside(crop, &dev->fmt_out_rect);
+		}
+		s->r.top *= factor;
+		s->r.height *= factor;
+		if (dev->bitmap_out && (compose->width != s->r.width ||
+					compose->height != s->r.height)) {
+			kfree(dev->bitmap_out);
+			dev->bitmap_out = NULL;
+		}
+		*compose = s->r;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+int vivid_vid_out_cropcap(struct file *file, void *priv,
+			      struct v4l2_cropcap *cap)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (cap->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		return -EINVAL;
+
+	switch (vivid_get_pixel_aspect(dev)) {
+	case TPG_PIXEL_ASPECT_NTSC:
+		cap->pixelaspect.numerator = 11;
+		cap->pixelaspect.denominator = 10;
+		break;
+	case TPG_PIXEL_ASPECT_PAL:
+		cap->pixelaspect.numerator = 54;
+		cap->pixelaspect.denominator = 59;
+		break;
+	case TPG_PIXEL_ASPECT_SQUARE:
+		cap->pixelaspect.numerator = 1;
+		cap->pixelaspect.denominator = 1;
+		break;
+	}
+	return 0;
+}
+
+int vidioc_g_fmt_vid_out_overlay(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	const struct v4l2_rect *compose = &dev->compose_out;
+	struct v4l2_window *win = &f->fmt.win;
+	unsigned clipcount = win->clipcount;
+
+	if (!dev->has_fb)
+		return -EINVAL;
+	win->w.top = dev->overlay_out_top;
+	win->w.left = dev->overlay_out_left;
+	win->w.width = compose->width;
+	win->w.height = compose->height;
+	win->clipcount = dev->clipcount_out;
+	win->field = V4L2_FIELD_ANY;
+	win->chromakey = dev->chromakey_out;
+	win->global_alpha = dev->global_alpha_out;
+	if (clipcount > dev->clipcount_out)
+		clipcount = dev->clipcount_out;
+	if (dev->bitmap_out == NULL)
+		win->bitmap = NULL;
+	else if (win->bitmap) {
+		if (copy_to_user(win->bitmap, dev->bitmap_out,
+		    ((dev->compose_out.width + 7) / 8) * dev->compose_out.height))
+			return -EFAULT;
+	}
+	if (clipcount && win->clips) {
+		if (copy_to_user(win->clips, dev->clips_out,
+				 clipcount * sizeof(dev->clips_out[0])))
+			return -EFAULT;
+	}
+	return 0;
+}
+
+int vidioc_try_fmt_vid_out_overlay(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	const struct v4l2_rect *compose = &dev->compose_out;
+	struct v4l2_window *win = &f->fmt.win;
+	int i, j;
+
+	if (!dev->has_fb)
+		return -EINVAL;
+	win->w.left = clamp_t(int, win->w.left,
+			      -dev->display_width, dev->display_width);
+	win->w.top = clamp_t(int, win->w.top,
+			     -dev->display_height, dev->display_height);
+	win->w.width = compose->width;
+	win->w.height = compose->height;
+	/*
+	 * It makes no sense for an OSD to overlay only top or bottom fields,
+	 * so always set this to ANY.
+	 */
+	win->field = V4L2_FIELD_ANY;
+	if (win->clipcount && !win->clips)
+		win->clipcount = 0;
+	if (win->clipcount > MAX_CLIPS)
+		win->clipcount = MAX_CLIPS;
+	if (win->clipcount) {
+		if (copy_from_user(dev->try_clips_out, win->clips,
+				   win->clipcount * sizeof(dev->clips_out[0])))
+			return -EFAULT;
+		for (i = 0; i < win->clipcount; i++) {
+			struct v4l2_rect *r = &dev->try_clips_out[i].c;
+
+			r->top = clamp_t(s32, r->top, 0, dev->display_height - 1);
+			r->height = clamp_t(s32, r->height, 1, dev->display_height - r->top);
+			r->left = clamp_t(u32, r->left, 0, dev->display_width - 1);
+			r->width = clamp_t(u32, r->width, 1, dev->display_width - r->left);
+		}
+		/*
+		 * Yeah, so sue me, it's an O(n^2) algorithm. But n is a small
+		 * number and it's typically a one-time deal.
+		 */
+		for (i = 0; i < win->clipcount - 1; i++) {
+			struct v4l2_rect *r1 = &dev->try_clips_out[i].c;
+
+			for (j = i + 1; j < win->clipcount; j++) {
+				struct v4l2_rect *r2 = &dev->try_clips_out[j].c;
+
+				if (rect_overlap(r1, r2))
+					return -EINVAL;
+			}
+		}
+		if (copy_to_user(win->clips, dev->try_clips_out,
+				 win->clipcount * sizeof(dev->clips_out[0])))
+			return -EFAULT;
+	}
+	return 0;
+}
+
+int vidioc_s_fmt_vid_out_overlay(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	const struct v4l2_rect *compose = &dev->compose_out;
+	struct v4l2_window *win = &f->fmt.win;
+	int ret = vidioc_try_fmt_vid_out_overlay(file, priv, f);
+	unsigned bitmap_size = ((compose->width + 7) / 8) * compose->height;
+	unsigned clips_size = win->clipcount * sizeof(dev->clips_out[0]);
+	void *new_bitmap = NULL;
+
+	if (ret)
+		return ret;
+
+	if (win->bitmap) {
+		new_bitmap = memdup_user(win->bitmap, bitmap_size);
+
+		if (IS_ERR(new_bitmap))
+			return PTR_ERR(new_bitmap);
+	}
+
+	dev->overlay_out_top = win->w.top;
+	dev->overlay_out_left = win->w.left;
+	kfree(dev->bitmap_out);
+	dev->bitmap_out = new_bitmap;
+	dev->clipcount_out = win->clipcount;
+	if (dev->clipcount_out)
+		memcpy(dev->clips_out, dev->try_clips_out, clips_size);
+	dev->chromakey_out = win->chromakey;
+	dev->global_alpha_out = win->global_alpha;
+	return ret;
+}
+
+int vivid_vid_out_overlay(struct file *file, void *fh, unsigned i)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (i && !dev->fmt_out->can_do_overlay) {
+		dprintk(dev, 1, "unsupported output format for output overlay\n");
+		return -EINVAL;
+	}
+
+	dev->overlay_out_enabled = i;
+	return 0;
+}
+
+int vivid_vid_out_g_fbuf(struct file *file, void *fh,
+				struct v4l2_framebuffer *a)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	a->capability = V4L2_FBUF_CAP_EXTERNOVERLAY |
+			V4L2_FBUF_CAP_BITMAP_CLIPPING |
+			V4L2_FBUF_CAP_LIST_CLIPPING |
+			V4L2_FBUF_CAP_CHROMAKEY |
+			V4L2_FBUF_CAP_SRC_CHROMAKEY |
+			V4L2_FBUF_CAP_GLOBAL_ALPHA |
+			V4L2_FBUF_CAP_LOCAL_ALPHA |
+			V4L2_FBUF_CAP_LOCAL_INV_ALPHA;
+	a->flags = V4L2_FBUF_FLAG_OVERLAY | dev->fbuf_out_flags;
+	a->base = (void *)dev->video_pbase;
+	a->fmt.width = dev->display_width;
+	a->fmt.height = dev->display_height;
+	if (dev->fb_defined.green.length == 5)
+		a->fmt.pixelformat = V4L2_PIX_FMT_ARGB555;
+	else
+		a->fmt.pixelformat = V4L2_PIX_FMT_RGB565;
+	a->fmt.bytesperline = dev->display_byte_stride;
+	a->fmt.sizeimage = a->fmt.height * a->fmt.bytesperline;
+	a->fmt.field = V4L2_FIELD_NONE;
+	a->fmt.colorspace = V4L2_COLORSPACE_SRGB;
+	a->fmt.priv = 0;
+	return 0;
+}
+
+int vivid_vid_out_s_fbuf(struct file *file, void *fh,
+				const struct v4l2_framebuffer *a)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	const unsigned chroma_flags = V4L2_FBUF_FLAG_CHROMAKEY |
+				      V4L2_FBUF_FLAG_SRC_CHROMAKEY;
+	const unsigned alpha_flags = V4L2_FBUF_FLAG_GLOBAL_ALPHA |
+				     V4L2_FBUF_FLAG_LOCAL_ALPHA |
+				     V4L2_FBUF_FLAG_LOCAL_INV_ALPHA;
+
+
+	if ((a->flags & chroma_flags) == chroma_flags)
+		return -EINVAL;
+	switch (a->flags & alpha_flags) {
+	case 0:
+	case V4L2_FBUF_FLAG_GLOBAL_ALPHA:
+	case V4L2_FBUF_FLAG_LOCAL_ALPHA:
+	case V4L2_FBUF_FLAG_LOCAL_INV_ALPHA:
+		break;
+	default:
+		return -EINVAL;
+	}
+	dev->fbuf_out_flags &= ~(chroma_flags | alpha_flags);
+	dev->fbuf_out_flags = a->flags & (chroma_flags | alpha_flags);
+	return 0;
+}
+
+static const struct v4l2_audioout vivid_audio_outputs[] = {
+	{ 0, "Line-Out 1" },
+	{ 1, "Line-Out 2" },
+};
+
+int vidioc_enum_output(struct file *file, void *priv,
+				struct v4l2_output *out)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (out->index >= dev->num_outputs)
+		return -EINVAL;
+
+	out->type = V4L2_OUTPUT_TYPE_ANALOG;
+	switch (dev->output_type[out->index]) {
+	case SVID:
+		snprintf(out->name, sizeof(out->name), "S-Video %u",
+				dev->output_name_counter[out->index]);
+		out->std = V4L2_STD_ALL;
+		if (dev->has_audio_outputs)
+			out->audioset = (1 << ARRAY_SIZE(vivid_audio_outputs)) - 1;
+		out->capabilities = V4L2_OUT_CAP_STD;
+		break;
+	case HDMI:
+		snprintf(out->name, sizeof(out->name), "HDMI %u",
+				dev->output_name_counter[out->index]);
+		out->capabilities = V4L2_OUT_CAP_DV_TIMINGS;
+		break;
+	}
+	return 0;
+}
+
+int vidioc_g_output(struct file *file, void *priv, unsigned *o)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	*o = dev->output;
+	return 0;
+}
+
+int vidioc_s_output(struct file *file, void *priv, unsigned o)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (o >= dev->num_outputs)
+		return -EINVAL;
+
+	if (o == dev->output)
+		return 0;
+
+	if (vb2_is_busy(&dev->vb_vid_out_q) || vb2_is_busy(&dev->vb_vbi_out_q))
+		return -EBUSY;
+
+	dev->output = o;
+	dev->tv_audio_output = 0;
+	if (dev->output_type[o] == SVID)
+		dev->vid_out_dev.tvnorms = V4L2_STD_ALL;
+	else
+		dev->vid_out_dev.tvnorms = 0;
+
+	dev->vbi_out_dev.tvnorms = dev->vid_out_dev.tvnorms;
+	vivid_update_format_out(dev);
+	return 0;
+}
+
+int vidioc_enumaudout(struct file *file, void *fh, struct v4l2_audioout *vout)
+{
+	if (vout->index >= ARRAY_SIZE(vivid_audio_outputs))
+		return -EINVAL;
+	*vout = vivid_audio_outputs[vout->index];
+	return 0;
+}
+
+int vidioc_g_audout(struct file *file, void *fh, struct v4l2_audioout *vout)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!vivid_is_svid_out(dev))
+		return -EINVAL;
+	*vout = vivid_audio_outputs[dev->tv_audio_output];
+	return 0;
+}
+
+int vidioc_s_audout(struct file *file, void *fh, const struct v4l2_audioout *vout)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!vivid_is_svid_out(dev))
+		return -EINVAL;
+	if (vout->index >= ARRAY_SIZE(vivid_audio_outputs))
+		return -EINVAL;
+	dev->tv_audio_output = vout->index;
+	return 0;
+}
+
+int vivid_vid_out_s_std(struct file *file, void *priv, v4l2_std_id id)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!vivid_is_svid_out(dev))
+		return -ENODATA;
+	if (dev->std_out == id)
+		return 0;
+	if (vb2_is_busy(&dev->vb_vid_out_q) || vb2_is_busy(&dev->vb_vbi_out_q))
+		return -EBUSY;
+	dev->std_out = id;
+	vivid_update_format_out(dev);
+	return 0;
+}
+
+static bool valid_cvt_gtf_timings(struct v4l2_dv_timings *timings)
+{
+	struct v4l2_bt_timings *bt = &timings->bt;
+
+	if ((bt->standards & (V4L2_DV_BT_STD_CVT | V4L2_DV_BT_STD_GTF)) &&
+	    v4l2_valid_dv_timings(timings, &vivid_dv_timings_cap, NULL, NULL))
+		return true;
+
+	return false;
+}
+
+int vivid_vid_out_s_dv_timings(struct file *file, void *_fh,
+				    struct v4l2_dv_timings *timings)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	if (!vivid_is_hdmi_out(dev))
+		return -ENODATA;
+	if (!v4l2_find_dv_timings_cap(timings, &vivid_dv_timings_cap,
+				0, NULL, NULL) &&
+	    !valid_cvt_gtf_timings(timings))
+		return -EINVAL;
+	if (v4l2_match_dv_timings(timings, &dev->dv_timings_out, 0))
+		return 0;
+	if (vb2_is_busy(&dev->vb_vid_out_q))
+		return -EBUSY;
+	dev->dv_timings_out = *timings;
+	vivid_update_format_out(dev);
+	return 0;
+}
+
+int vivid_vid_out_g_parm(struct file *file, void *priv,
+			  struct v4l2_streamparm *parm)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (parm->type != (dev->multiplanar ?
+			   V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE :
+			   V4L2_BUF_TYPE_VIDEO_OUTPUT))
+		return -EINVAL;
+
+	parm->parm.output.capability   = V4L2_CAP_TIMEPERFRAME;
+	parm->parm.output.timeperframe = dev->timeperframe_vid_out;
+	parm->parm.output.writebuffers  = 1;
+
+	return 0;
+}
+
+int vidioc_subscribe_event(struct v4l2_fh *fh,
+			const struct v4l2_event_subscription *sub)
+{
+	switch (sub->type) {
+	case V4L2_EVENT_CTRL:
+		return v4l2_ctrl_subscribe_event(fh, sub);
+	case V4L2_EVENT_SOURCE_CHANGE:
+		if (fh->vdev->vfl_dir == VFL_DIR_RX)
+			return v4l2_src_change_event_subscribe(fh, sub);
+		break;
+	default:
+		break;
+	}
+	return -EINVAL;
+}
diff --git a/drivers/media/platform/vivid/vivid-vid-out.h b/drivers/media/platform/vivid/vivid-vid-out.h
new file mode 100644
index 0000000..dfa84db
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-vid-out.h
@@ -0,0 +1,56 @@
+/*
+ * vivid-vid-out.h - video output support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VIVID_VID_OUT_H_
+#define _VIVID_VID_OUT_H_
+
+extern const struct vb2_ops vivid_vid_out_qops;
+
+void vivid_update_format_out(struct vivid_dev *dev);
+
+int vivid_g_fmt_vid_out(struct file *file, void *priv, struct v4l2_format *f);
+int vivid_try_fmt_vid_out(struct file *file, void *priv, struct v4l2_format *f);
+int vivid_s_fmt_vid_out(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_g_fmt_vid_out_mplane(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_try_fmt_vid_out_mplane(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_s_fmt_vid_out_mplane(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_g_fmt_vid_out(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_try_fmt_vid_out(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_s_fmt_vid_out(struct file *file, void *priv, struct v4l2_format *f);
+int vivid_vid_out_g_selection(struct file *file, void *priv, struct v4l2_selection *sel);
+int vivid_vid_out_s_selection(struct file *file, void *fh, struct v4l2_selection *s);
+int vivid_vid_out_cropcap(struct file *file, void *fh, struct v4l2_cropcap *cap);
+int vidioc_enum_fmt_vid_out_overlay(struct file *file, void  *priv, struct v4l2_fmtdesc *f);
+int vidioc_g_fmt_vid_out_overlay(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_try_fmt_vid_out_overlay(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_s_fmt_vid_out_overlay(struct file *file, void *priv, struct v4l2_format *f);
+int vivid_vid_out_overlay(struct file *file, void *fh, unsigned i);
+int vivid_vid_out_g_fbuf(struct file *file, void *fh, struct v4l2_framebuffer *a);
+int vivid_vid_out_s_fbuf(struct file *file, void *fh, const struct v4l2_framebuffer *a);
+int vidioc_enum_output(struct file *file, void *priv, struct v4l2_output *out);
+int vidioc_g_output(struct file *file, void *priv, unsigned *i);
+int vidioc_s_output(struct file *file, void *priv, unsigned i);
+int vidioc_enumaudout(struct file *file, void *fh, struct v4l2_audioout *vout);
+int vidioc_g_audout(struct file *file, void *fh, struct v4l2_audioout *vout);
+int vidioc_s_audout(struct file *file, void *fh, const struct v4l2_audioout *vout);
+int vivid_vid_out_s_std(struct file *file, void *priv, v4l2_std_id id);
+int vivid_vid_out_s_dv_timings(struct file *file, void *_fh, struct v4l2_dv_timings *timings);
+int vivid_vid_out_g_parm(struct file *file, void *priv, struct v4l2_streamparm *parm);
+
+#endif
diff --git a/drivers/media/platform/vsp1/Makefile b/drivers/media/platform/vsp1/Makefile
new file mode 100644
index 0000000..6a93f92
--- /dev/null
+++ b/drivers/media/platform/vsp1/Makefile
@@ -0,0 +1,6 @@
+vsp1-y					:= vsp1_drv.o vsp1_entity.o vsp1_video.o
+vsp1-y					+= vsp1_rpf.o vsp1_rwpf.o vsp1_wpf.o
+vsp1-y					+= vsp1_hsit.o vsp1_lif.o vsp1_lut.o
+vsp1-y					+= vsp1_bru.o vsp1_sru.o vsp1_uds.o
+
+obj-$(CONFIG_VIDEO_RENESAS_VSP1)	+= vsp1.o
diff --git a/drivers/media/platform/vsp1/vsp1.h b/drivers/media/platform/vsp1/vsp1.h
new file mode 100644
index 0000000..989e96f
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1.h
@@ -0,0 +1,92 @@
+/*
+ * vsp1.h  --  R-Car VSP1 Driver
+ *
+ * Copyright (C) 2013-2014 Renesas Electronics Corporation
+ *
+ * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+#ifndef __VSP1_H__
+#define __VSP1_H__
+
+#include <linux/io.h>
+#include <linux/list.h>
+#include <linux/mutex.h>
+
+#include <media/media-device.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+
+#include "vsp1_regs.h"
+
+struct clk;
+struct device;
+
+struct vsp1_platform_data;
+struct vsp1_bru;
+struct vsp1_hsit;
+struct vsp1_lif;
+struct vsp1_lut;
+struct vsp1_rwpf;
+struct vsp1_sru;
+struct vsp1_uds;
+
+#define VSP1_MAX_RPF		5
+#define VSP1_MAX_UDS		3
+#define VSP1_MAX_WPF		4
+
+#define VSP1_HAS_LIF		(1 << 0)
+#define VSP1_HAS_LUT		(1 << 1)
+#define VSP1_HAS_SRU		(1 << 2)
+
+struct vsp1_platform_data {
+	unsigned int features;
+	unsigned int rpf_count;
+	unsigned int uds_count;
+	unsigned int wpf_count;
+};
+
+struct vsp1_device {
+	struct device *dev;
+	struct vsp1_platform_data pdata;
+
+	void __iomem *mmio;
+	struct clk *clock;
+
+	struct mutex lock;
+	int ref_count;
+
+	struct vsp1_bru *bru;
+	struct vsp1_hsit *hsi;
+	struct vsp1_hsit *hst;
+	struct vsp1_lif *lif;
+	struct vsp1_lut *lut;
+	struct vsp1_rwpf *rpf[VSP1_MAX_RPF];
+	struct vsp1_sru *sru;
+	struct vsp1_uds *uds[VSP1_MAX_UDS];
+	struct vsp1_rwpf *wpf[VSP1_MAX_WPF];
+
+	struct list_head entities;
+
+	struct v4l2_device v4l2_dev;
+	struct media_device media_dev;
+};
+
+int vsp1_device_get(struct vsp1_device *vsp1);
+void vsp1_device_put(struct vsp1_device *vsp1);
+
+static inline u32 vsp1_read(struct vsp1_device *vsp1, u32 reg)
+{
+	return ioread32(vsp1->mmio + reg);
+}
+
+static inline void vsp1_write(struct vsp1_device *vsp1, u32 reg, u32 data)
+{
+	iowrite32(data, vsp1->mmio + reg);
+}
+
+#endif /* __VSP1_H__ */
diff --git a/drivers/media/platform/vsp1/vsp1_bru.c b/drivers/media/platform/vsp1/vsp1_bru.c
new file mode 100644
index 0000000..7dd7633
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_bru.c
@@ -0,0 +1,454 @@
+/*
+ * vsp1_bru.c  --  R-Car VSP1 Blend ROP Unit
+ *
+ * Copyright (C) 2013 Renesas Corporation
+ *
+ * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/device.h>
+#include <linux/gfp.h>
+
+#include <media/v4l2-subdev.h>
+
+#include "vsp1.h"
+#include "vsp1_bru.h"
+#include "vsp1_rwpf.h"
+
+#define BRU_MIN_SIZE				1U
+#define BRU_MAX_SIZE				8190U
+
+/* -----------------------------------------------------------------------------
+ * Device Access
+ */
+
+static inline u32 vsp1_bru_read(struct vsp1_bru *bru, u32 reg)
+{
+	return vsp1_read(bru->entity.vsp1, reg);
+}
+
+static inline void vsp1_bru_write(struct vsp1_bru *bru, u32 reg, u32 data)
+{
+	vsp1_write(bru->entity.vsp1, reg, data);
+}
+
+/* -----------------------------------------------------------------------------
+ * Controls
+ */
+
+static int bru_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vsp1_bru *bru =
+		container_of(ctrl->handler, struct vsp1_bru, ctrls);
+
+	if (!vsp1_entity_is_streaming(&bru->entity))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BG_COLOR:
+		vsp1_bru_write(bru, VI6_BRU_VIRRPF_COL, ctrl->val |
+			       (0xff << VI6_BRU_VIRRPF_COL_A_SHIFT));
+		break;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops bru_ctrl_ops = {
+	.s_ctrl = bru_s_ctrl,
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Core Operations
+ */
+
+static int bru_s_stream(struct v4l2_subdev *subdev, int enable)
+{
+	struct vsp1_pipeline *pipe = to_vsp1_pipeline(&subdev->entity);
+	struct vsp1_bru *bru = to_bru(subdev);
+	struct v4l2_mbus_framefmt *format;
+	unsigned int flags;
+	unsigned int i;
+	int ret;
+
+	ret = vsp1_entity_set_streaming(&bru->entity, enable);
+	if (ret < 0)
+		return ret;
+
+	if (!enable)
+		return 0;
+
+	format = &bru->entity.formats[BRU_PAD_SOURCE];
+
+	/* The hardware is extremely flexible but we have no userspace API to
+	 * expose all the parameters, nor is it clear whether we would have use
+	 * cases for all the supported modes. Let's just harcode the parameters
+	 * to sane default values for now.
+	 */
+
+	/* Disable dithering and enable color data normalization unless the
+	 * format at the pipeline output is premultiplied.
+	 */
+	flags = pipe->output ? pipe->output->video.format.flags : 0;
+	vsp1_bru_write(bru, VI6_BRU_INCTRL,
+		       flags & V4L2_PIX_FMT_FLAG_PREMUL_ALPHA ?
+		       0 : VI6_BRU_INCTRL_NRM);
+
+	/* Set the background position to cover the whole output image. */
+	vsp1_bru_write(bru, VI6_BRU_VIRRPF_SIZE,
+		       (format->width << VI6_BRU_VIRRPF_SIZE_HSIZE_SHIFT) |
+		       (format->height << VI6_BRU_VIRRPF_SIZE_VSIZE_SHIFT));
+	vsp1_bru_write(bru, VI6_BRU_VIRRPF_LOC, 0);
+
+	/* Route BRU input 1 as SRC input to the ROP unit and configure the ROP
+	 * unit with a NOP operation to make BRU input 1 available as the
+	 * Blend/ROP unit B SRC input.
+	 */
+	vsp1_bru_write(bru, VI6_BRU_ROP, VI6_BRU_ROP_DSTSEL_BRUIN(1) |
+		       VI6_BRU_ROP_CROP(VI6_ROP_NOP) |
+		       VI6_BRU_ROP_AROP(VI6_ROP_NOP));
+
+	for (i = 0; i < 4; ++i) {
+		bool premultiplied = false;
+		u32 ctrl = 0;
+
+		/* Configure all Blend/ROP units corresponding to an enabled BRU
+		 * input for alpha blending. Blend/ROP units corresponding to
+		 * disabled BRU inputs are used in ROP NOP mode to ignore the
+		 * SRC input.
+		 */
+		if (bru->inputs[i].rpf) {
+			ctrl |= VI6_BRU_CTRL_RBC;
+
+			premultiplied = bru->inputs[i].rpf->video.format.flags
+				      & V4L2_PIX_FMT_FLAG_PREMUL_ALPHA;
+		} else {
+			ctrl |= VI6_BRU_CTRL_CROP(VI6_ROP_NOP)
+			     |  VI6_BRU_CTRL_AROP(VI6_ROP_NOP);
+		}
+
+		/* Select the virtual RPF as the Blend/ROP unit A DST input to
+		 * serve as a background color.
+		 */
+		if (i == 0)
+			ctrl |= VI6_BRU_CTRL_DSTSEL_VRPF;
+
+		/* Route BRU inputs 0 to 3 as SRC inputs to Blend/ROP units A to
+		 * D in that order. The Blend/ROP unit B SRC is hardwired to the
+		 * ROP unit output, the corresponding register bits must be set
+		 * to 0.
+		 */
+		if (i != 1)
+			ctrl |= VI6_BRU_CTRL_SRCSEL_BRUIN(i);
+
+		vsp1_bru_write(bru, VI6_BRU_CTRL(i), ctrl);
+
+		/* Harcode the blending formula to
+		 *
+		 *	DSTc = DSTc * (1 - SRCa) + SRCc * SRCa
+		 *	DSTa = DSTa * (1 - SRCa) + SRCa
+		 *
+		 * when the SRC input isn't premultiplied, and to
+		 *
+		 *	DSTc = DSTc * (1 - SRCa) + SRCc
+		 *	DSTa = DSTa * (1 - SRCa) + SRCa
+		 *
+		 * otherwise.
+		 */
+		vsp1_bru_write(bru, VI6_BRU_BLD(i),
+			       VI6_BRU_BLD_CCMDX_255_SRC_A |
+			       (premultiplied ? VI6_BRU_BLD_CCMDY_COEFY :
+						VI6_BRU_BLD_CCMDY_SRC_A) |
+			       VI6_BRU_BLD_ACMDX_255_SRC_A |
+			       VI6_BRU_BLD_ACMDY_COEFY |
+			       (0xff << VI6_BRU_BLD_COEFY_SHIFT));
+	}
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Pad Operations
+ */
+
+/*
+ * The BRU can't perform format conversion, all sink and source formats must be
+ * identical. We pick the format on the first sink pad (pad 0) and propagate it
+ * to all other pads.
+ */
+
+static int bru_enum_mbus_code(struct v4l2_subdev *subdev,
+			      struct v4l2_subdev_pad_config *cfg,
+			      struct v4l2_subdev_mbus_code_enum *code)
+{
+	static const unsigned int codes[] = {
+		MEDIA_BUS_FMT_ARGB8888_1X32,
+		MEDIA_BUS_FMT_AYUV8_1X32,
+	};
+	struct vsp1_bru *bru = to_bru(subdev);
+	struct v4l2_mbus_framefmt *format;
+
+	if (code->pad == BRU_PAD_SINK(0)) {
+		if (code->index >= ARRAY_SIZE(codes))
+			return -EINVAL;
+
+		code->code = codes[code->index];
+	} else {
+		if (code->index)
+			return -EINVAL;
+
+		format = vsp1_entity_get_pad_format(&bru->entity, cfg,
+						    BRU_PAD_SINK(0), code->which);
+		code->code = format->code;
+	}
+
+	return 0;
+}
+
+static int bru_enum_frame_size(struct v4l2_subdev *subdev,
+			       struct v4l2_subdev_pad_config *cfg,
+			       struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->index)
+		return -EINVAL;
+
+	if (fse->code != MEDIA_BUS_FMT_ARGB8888_1X32 &&
+	    fse->code != MEDIA_BUS_FMT_AYUV8_1X32)
+		return -EINVAL;
+
+	fse->min_width = BRU_MIN_SIZE;
+	fse->max_width = BRU_MAX_SIZE;
+	fse->min_height = BRU_MIN_SIZE;
+	fse->max_height = BRU_MAX_SIZE;
+
+	return 0;
+}
+
+static struct v4l2_rect *bru_get_compose(struct vsp1_bru *bru,
+					 struct v4l2_subdev_pad_config *cfg,
+					 unsigned int pad, u32 which)
+{
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_crop(&bru->entity.subdev, cfg, pad);
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return &bru->inputs[pad].compose;
+	default:
+		return NULL;
+	}
+}
+
+static int bru_get_format(struct v4l2_subdev *subdev, struct v4l2_subdev_pad_config *cfg,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct vsp1_bru *bru = to_bru(subdev);
+
+	fmt->format = *vsp1_entity_get_pad_format(&bru->entity, cfg, fmt->pad,
+						  fmt->which);
+
+	return 0;
+}
+
+static void bru_try_format(struct vsp1_bru *bru, struct v4l2_subdev_pad_config *cfg,
+			   unsigned int pad, struct v4l2_mbus_framefmt *fmt,
+			   enum v4l2_subdev_format_whence which)
+{
+	struct v4l2_mbus_framefmt *format;
+
+	switch (pad) {
+	case BRU_PAD_SINK(0):
+		/* Default to YUV if the requested format is not supported. */
+		if (fmt->code != MEDIA_BUS_FMT_ARGB8888_1X32 &&
+		    fmt->code != MEDIA_BUS_FMT_AYUV8_1X32)
+			fmt->code = MEDIA_BUS_FMT_AYUV8_1X32;
+		break;
+
+	default:
+		/* The BRU can't perform format conversion. */
+		format = vsp1_entity_get_pad_format(&bru->entity, cfg,
+						    BRU_PAD_SINK(0), which);
+		fmt->code = format->code;
+		break;
+	}
+
+	fmt->width = clamp(fmt->width, BRU_MIN_SIZE, BRU_MAX_SIZE);
+	fmt->height = clamp(fmt->height, BRU_MIN_SIZE, BRU_MAX_SIZE);
+	fmt->field = V4L2_FIELD_NONE;
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+}
+
+static int bru_set_format(struct v4l2_subdev *subdev, struct v4l2_subdev_pad_config *cfg,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct vsp1_bru *bru = to_bru(subdev);
+	struct v4l2_mbus_framefmt *format;
+
+	bru_try_format(bru, cfg, fmt->pad, &fmt->format, fmt->which);
+
+	format = vsp1_entity_get_pad_format(&bru->entity, cfg, fmt->pad,
+					    fmt->which);
+	*format = fmt->format;
+
+	/* Reset the compose rectangle */
+	if (fmt->pad != BRU_PAD_SOURCE) {
+		struct v4l2_rect *compose;
+
+		compose = bru_get_compose(bru, cfg, fmt->pad, fmt->which);
+		compose->left = 0;
+		compose->top = 0;
+		compose->width = format->width;
+		compose->height = format->height;
+	}
+
+	/* Propagate the format code to all pads */
+	if (fmt->pad == BRU_PAD_SINK(0)) {
+		unsigned int i;
+
+		for (i = 0; i <= BRU_PAD_SOURCE; ++i) {
+			format = vsp1_entity_get_pad_format(&bru->entity, cfg,
+							    i, fmt->which);
+			format->code = fmt->format.code;
+		}
+	}
+
+	return 0;
+}
+
+static int bru_get_selection(struct v4l2_subdev *subdev,
+			     struct v4l2_subdev_pad_config *cfg,
+			     struct v4l2_subdev_selection *sel)
+{
+	struct vsp1_bru *bru = to_bru(subdev);
+
+	if (sel->pad == BRU_PAD_SOURCE)
+		return -EINVAL;
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = BRU_MAX_SIZE;
+		sel->r.height = BRU_MAX_SIZE;
+		return 0;
+
+	case V4L2_SEL_TGT_COMPOSE:
+		sel->r = *bru_get_compose(bru, cfg, sel->pad, sel->which);
+		return 0;
+
+	default:
+		return -EINVAL;
+	}
+}
+
+static int bru_set_selection(struct v4l2_subdev *subdev,
+			     struct v4l2_subdev_pad_config *cfg,
+			     struct v4l2_subdev_selection *sel)
+{
+	struct vsp1_bru *bru = to_bru(subdev);
+	struct v4l2_mbus_framefmt *format;
+	struct v4l2_rect *compose;
+
+	if (sel->pad == BRU_PAD_SOURCE)
+		return -EINVAL;
+
+	if (sel->target != V4L2_SEL_TGT_COMPOSE)
+		return -EINVAL;
+
+	/* The compose rectangle top left corner must be inside the output
+	 * frame.
+	 */
+	format = vsp1_entity_get_pad_format(&bru->entity, cfg, BRU_PAD_SOURCE,
+					    sel->which);
+	sel->r.left = clamp_t(unsigned int, sel->r.left, 0, format->width - 1);
+	sel->r.top = clamp_t(unsigned int, sel->r.top, 0, format->height - 1);
+
+	/* Scaling isn't supported, the compose rectangle size must be identical
+	 * to the sink format size.
+	 */
+	format = vsp1_entity_get_pad_format(&bru->entity, cfg, sel->pad,
+					    sel->which);
+	sel->r.width = format->width;
+	sel->r.height = format->height;
+
+	compose = bru_get_compose(bru, cfg, sel->pad, sel->which);
+	*compose = sel->r;
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Operations
+ */
+
+static struct v4l2_subdev_video_ops bru_video_ops = {
+	.s_stream = bru_s_stream,
+};
+
+static struct v4l2_subdev_pad_ops bru_pad_ops = {
+	.enum_mbus_code = bru_enum_mbus_code,
+	.enum_frame_size = bru_enum_frame_size,
+	.get_fmt = bru_get_format,
+	.set_fmt = bru_set_format,
+	.get_selection = bru_get_selection,
+	.set_selection = bru_set_selection,
+};
+
+static struct v4l2_subdev_ops bru_ops = {
+	.video	= &bru_video_ops,
+	.pad    = &bru_pad_ops,
+};
+
+/* -----------------------------------------------------------------------------
+ * Initialization and Cleanup
+ */
+
+struct vsp1_bru *vsp1_bru_create(struct vsp1_device *vsp1)
+{
+	struct v4l2_subdev *subdev;
+	struct vsp1_bru *bru;
+	int ret;
+
+	bru = devm_kzalloc(vsp1->dev, sizeof(*bru), GFP_KERNEL);
+	if (bru == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	bru->entity.type = VSP1_ENTITY_BRU;
+
+	ret = vsp1_entity_init(vsp1, &bru->entity, 5);
+	if (ret < 0)
+		return ERR_PTR(ret);
+
+	/* Initialize the V4L2 subdev. */
+	subdev = &bru->entity.subdev;
+	v4l2_subdev_init(subdev, &bru_ops);
+
+	subdev->entity.ops = &vsp1_media_ops;
+	subdev->internal_ops = &vsp1_subdev_internal_ops;
+	snprintf(subdev->name, sizeof(subdev->name), "%s bru",
+		 dev_name(vsp1->dev));
+	v4l2_set_subdevdata(subdev, bru);
+	subdev->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	vsp1_entity_init_formats(subdev, NULL);
+
+	/* Initialize the control handler. */
+	v4l2_ctrl_handler_init(&bru->ctrls, 1);
+	v4l2_ctrl_new_std(&bru->ctrls, &bru_ctrl_ops, V4L2_CID_BG_COLOR,
+			  0, 0xffffff, 1, 0);
+
+	bru->entity.subdev.ctrl_handler = &bru->ctrls;
+
+	if (bru->ctrls.error) {
+		dev_err(vsp1->dev, "bru: failed to initialize controls\n");
+		ret = bru->ctrls.error;
+		vsp1_entity_destroy(&bru->entity);
+		return ERR_PTR(ret);
+	}
+
+	return bru;
+}
diff --git a/drivers/media/platform/vsp1/vsp1_bru.h b/drivers/media/platform/vsp1/vsp1_bru.h
new file mode 100644
index 0000000..16b1c65
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_bru.h
@@ -0,0 +1,46 @@
+/*
+ * vsp1_bru.h  --  R-Car VSP1 Blend ROP Unit
+ *
+ * Copyright (C) 2013 Renesas Corporation
+ *
+ * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+#ifndef __VSP1_BRU_H__
+#define __VSP1_BRU_H__
+
+#include <media/media-entity.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-subdev.h>
+
+#include "vsp1_entity.h"
+
+struct vsp1_device;
+struct vsp1_rwpf;
+
+#define BRU_PAD_SINK(n)				(n)
+#define BRU_PAD_SOURCE				4
+
+struct vsp1_bru {
+	struct vsp1_entity entity;
+
+	struct v4l2_ctrl_handler ctrls;
+
+	struct {
+		struct vsp1_rwpf *rpf;
+		struct v4l2_rect compose;
+	} inputs[4];
+};
+
+static inline struct vsp1_bru *to_bru(struct v4l2_subdev *subdev)
+{
+	return container_of(subdev, struct vsp1_bru, entity.subdev);
+}
+
+struct vsp1_bru *vsp1_bru_create(struct vsp1_device *vsp1);
+
+#endif /* __VSP1_BRU_H__ */
diff --git a/drivers/media/platform/vsp1/vsp1_drv.c b/drivers/media/platform/vsp1/vsp1_drv.c
new file mode 100644
index 0000000..4e61886
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_drv.c
@@ -0,0 +1,560 @@
+/*
+ * vsp1_drv.c  --  R-Car VSP1 Driver
+ *
+ * Copyright (C) 2013-2015 Renesas Electronics Corporation
+ *
+ * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/videodev2.h>
+
+#include "vsp1.h"
+#include "vsp1_bru.h"
+#include "vsp1_hsit.h"
+#include "vsp1_lif.h"
+#include "vsp1_lut.h"
+#include "vsp1_rwpf.h"
+#include "vsp1_sru.h"
+#include "vsp1_uds.h"
+
+/* -----------------------------------------------------------------------------
+ * Interrupt Handling
+ */
+
+static irqreturn_t vsp1_irq_handler(int irq, void *data)
+{
+	u32 mask = VI6_WFP_IRQ_STA_DFE | VI6_WFP_IRQ_STA_FRE;
+	struct vsp1_device *vsp1 = data;
+	irqreturn_t ret = IRQ_NONE;
+	unsigned int i;
+
+	for (i = 0; i < vsp1->pdata.wpf_count; ++i) {
+		struct vsp1_rwpf *wpf = vsp1->wpf[i];
+		struct vsp1_pipeline *pipe;
+		u32 status;
+
+		if (wpf == NULL)
+			continue;
+
+		pipe = to_vsp1_pipeline(&wpf->entity.subdev.entity);
+		status = vsp1_read(vsp1, VI6_WPF_IRQ_STA(i));
+		vsp1_write(vsp1, VI6_WPF_IRQ_STA(i), ~status & mask);
+
+		if (status & VI6_WFP_IRQ_STA_FRE) {
+			vsp1_pipeline_frame_end(pipe);
+			ret = IRQ_HANDLED;
+		}
+	}
+
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * Entities
+ */
+
+/*
+ * vsp1_create_links - Create links from all sources to the given sink
+ *
+ * This function creates media links from all valid sources to the given sink
+ * pad. Links that would be invalid according to the VSP1 hardware capabilities
+ * are skipped. Those include all links
+ *
+ * - from a UDS to a UDS (UDS entities can't be chained)
+ * - from an entity to itself (no loops are allowed)
+ */
+static int vsp1_create_links(struct vsp1_device *vsp1, struct vsp1_entity *sink)
+{
+	struct media_entity *entity = &sink->subdev.entity;
+	struct vsp1_entity *source;
+	unsigned int pad;
+	int ret;
+
+	list_for_each_entry(source, &vsp1->entities, list_dev) {
+		u32 flags;
+
+		if (source->type == sink->type)
+			continue;
+
+		if (source->type == VSP1_ENTITY_LIF ||
+		    source->type == VSP1_ENTITY_WPF)
+			continue;
+
+		flags = source->type == VSP1_ENTITY_RPF &&
+			sink->type == VSP1_ENTITY_WPF &&
+			source->index == sink->index
+		      ? MEDIA_LNK_FL_ENABLED : 0;
+
+		for (pad = 0; pad < entity->num_pads; ++pad) {
+			if (!(entity->pads[pad].flags & MEDIA_PAD_FL_SINK))
+				continue;
+
+			ret = media_entity_create_link(&source->subdev.entity,
+						       source->source_pad,
+						       entity, pad, flags);
+			if (ret < 0)
+				return ret;
+
+			if (flags & MEDIA_LNK_FL_ENABLED)
+				source->sink = entity;
+		}
+	}
+
+	return 0;
+}
+
+static void vsp1_destroy_entities(struct vsp1_device *vsp1)
+{
+	struct vsp1_entity *entity;
+	struct vsp1_entity *next;
+
+	list_for_each_entry_safe(entity, next, &vsp1->entities, list_dev) {
+		list_del(&entity->list_dev);
+		vsp1_entity_destroy(entity);
+	}
+
+	v4l2_device_unregister(&vsp1->v4l2_dev);
+	media_device_unregister(&vsp1->media_dev);
+}
+
+static int vsp1_create_entities(struct vsp1_device *vsp1)
+{
+	struct media_device *mdev = &vsp1->media_dev;
+	struct v4l2_device *vdev = &vsp1->v4l2_dev;
+	struct vsp1_entity *entity;
+	unsigned int i;
+	int ret;
+
+	mdev->dev = vsp1->dev;
+	strlcpy(mdev->model, "VSP1", sizeof(mdev->model));
+	snprintf(mdev->bus_info, sizeof(mdev->bus_info), "platform:%s",
+		 dev_name(mdev->dev));
+	ret = media_device_register(mdev);
+	if (ret < 0) {
+		dev_err(vsp1->dev, "media device registration failed (%d)\n",
+			ret);
+		return ret;
+	}
+
+	vdev->mdev = mdev;
+	ret = v4l2_device_register(vsp1->dev, vdev);
+	if (ret < 0) {
+		dev_err(vsp1->dev, "V4L2 device registration failed (%d)\n",
+			ret);
+		goto done;
+	}
+
+	/* Instantiate all the entities. */
+	vsp1->bru = vsp1_bru_create(vsp1);
+	if (IS_ERR(vsp1->bru)) {
+		ret = PTR_ERR(vsp1->bru);
+		goto done;
+	}
+
+	list_add_tail(&vsp1->bru->entity.list_dev, &vsp1->entities);
+
+	vsp1->hsi = vsp1_hsit_create(vsp1, true);
+	if (IS_ERR(vsp1->hsi)) {
+		ret = PTR_ERR(vsp1->hsi);
+		goto done;
+	}
+
+	list_add_tail(&vsp1->hsi->entity.list_dev, &vsp1->entities);
+
+	vsp1->hst = vsp1_hsit_create(vsp1, false);
+	if (IS_ERR(vsp1->hst)) {
+		ret = PTR_ERR(vsp1->hst);
+		goto done;
+	}
+
+	list_add_tail(&vsp1->hst->entity.list_dev, &vsp1->entities);
+
+	if (vsp1->pdata.features & VSP1_HAS_LIF) {
+		vsp1->lif = vsp1_lif_create(vsp1);
+		if (IS_ERR(vsp1->lif)) {
+			ret = PTR_ERR(vsp1->lif);
+			goto done;
+		}
+
+		list_add_tail(&vsp1->lif->entity.list_dev, &vsp1->entities);
+	}
+
+	if (vsp1->pdata.features & VSP1_HAS_LUT) {
+		vsp1->lut = vsp1_lut_create(vsp1);
+		if (IS_ERR(vsp1->lut)) {
+			ret = PTR_ERR(vsp1->lut);
+			goto done;
+		}
+
+		list_add_tail(&vsp1->lut->entity.list_dev, &vsp1->entities);
+	}
+
+	for (i = 0; i < vsp1->pdata.rpf_count; ++i) {
+		struct vsp1_rwpf *rpf;
+
+		rpf = vsp1_rpf_create(vsp1, i);
+		if (IS_ERR(rpf)) {
+			ret = PTR_ERR(rpf);
+			goto done;
+		}
+
+		vsp1->rpf[i] = rpf;
+		list_add_tail(&rpf->entity.list_dev, &vsp1->entities);
+	}
+
+	if (vsp1->pdata.features & VSP1_HAS_SRU) {
+		vsp1->sru = vsp1_sru_create(vsp1);
+		if (IS_ERR(vsp1->sru)) {
+			ret = PTR_ERR(vsp1->sru);
+			goto done;
+		}
+
+		list_add_tail(&vsp1->sru->entity.list_dev, &vsp1->entities);
+	}
+
+	for (i = 0; i < vsp1->pdata.uds_count; ++i) {
+		struct vsp1_uds *uds;
+
+		uds = vsp1_uds_create(vsp1, i);
+		if (IS_ERR(uds)) {
+			ret = PTR_ERR(uds);
+			goto done;
+		}
+
+		vsp1->uds[i] = uds;
+		list_add_tail(&uds->entity.list_dev, &vsp1->entities);
+	}
+
+	for (i = 0; i < vsp1->pdata.wpf_count; ++i) {
+		struct vsp1_rwpf *wpf;
+
+		wpf = vsp1_wpf_create(vsp1, i);
+		if (IS_ERR(wpf)) {
+			ret = PTR_ERR(wpf);
+			goto done;
+		}
+
+		vsp1->wpf[i] = wpf;
+		list_add_tail(&wpf->entity.list_dev, &vsp1->entities);
+	}
+
+	/* Create links. */
+	list_for_each_entry(entity, &vsp1->entities, list_dev) {
+		if (entity->type == VSP1_ENTITY_LIF ||
+		    entity->type == VSP1_ENTITY_RPF)
+			continue;
+
+		ret = vsp1_create_links(vsp1, entity);
+		if (ret < 0)
+			goto done;
+	}
+
+	if (vsp1->pdata.features & VSP1_HAS_LIF) {
+		ret = media_entity_create_link(
+			&vsp1->wpf[0]->entity.subdev.entity, RWPF_PAD_SOURCE,
+			&vsp1->lif->entity.subdev.entity, LIF_PAD_SINK, 0);
+		if (ret < 0)
+			return ret;
+	}
+
+	/* Register all subdevs. */
+	list_for_each_entry(entity, &vsp1->entities, list_dev) {
+		ret = v4l2_device_register_subdev(&vsp1->v4l2_dev,
+						  &entity->subdev);
+		if (ret < 0)
+			goto done;
+	}
+
+	ret = v4l2_device_register_subdev_nodes(&vsp1->v4l2_dev);
+
+done:
+	if (ret < 0)
+		vsp1_destroy_entities(vsp1);
+
+	return ret;
+}
+
+static int vsp1_device_init(struct vsp1_device *vsp1)
+{
+	unsigned int i;
+	u32 status;
+
+	/* Reset any channel that might be running. */
+	status = vsp1_read(vsp1, VI6_STATUS);
+
+	for (i = 0; i < vsp1->pdata.wpf_count; ++i) {
+		unsigned int timeout;
+
+		if (!(status & VI6_STATUS_SYS_ACT(i)))
+			continue;
+
+		vsp1_write(vsp1, VI6_SRESET, VI6_SRESET_SRTS(i));
+		for (timeout = 10; timeout > 0; --timeout) {
+			status = vsp1_read(vsp1, VI6_STATUS);
+			if (!(status & VI6_STATUS_SYS_ACT(i)))
+				break;
+
+			usleep_range(1000, 2000);
+		}
+
+		if (!timeout) {
+			dev_err(vsp1->dev, "failed to reset wpf.%u\n", i);
+			return -ETIMEDOUT;
+		}
+	}
+
+	vsp1_write(vsp1, VI6_CLK_DCSWT, (8 << VI6_CLK_DCSWT_CSTPW_SHIFT) |
+		   (8 << VI6_CLK_DCSWT_CSTRW_SHIFT));
+
+	for (i = 0; i < vsp1->pdata.rpf_count; ++i)
+		vsp1_write(vsp1, VI6_DPR_RPF_ROUTE(i), VI6_DPR_NODE_UNUSED);
+
+	for (i = 0; i < vsp1->pdata.uds_count; ++i)
+		vsp1_write(vsp1, VI6_DPR_UDS_ROUTE(i), VI6_DPR_NODE_UNUSED);
+
+	vsp1_write(vsp1, VI6_DPR_SRU_ROUTE, VI6_DPR_NODE_UNUSED);
+	vsp1_write(vsp1, VI6_DPR_LUT_ROUTE, VI6_DPR_NODE_UNUSED);
+	vsp1_write(vsp1, VI6_DPR_CLU_ROUTE, VI6_DPR_NODE_UNUSED);
+	vsp1_write(vsp1, VI6_DPR_HST_ROUTE, VI6_DPR_NODE_UNUSED);
+	vsp1_write(vsp1, VI6_DPR_HSI_ROUTE, VI6_DPR_NODE_UNUSED);
+	vsp1_write(vsp1, VI6_DPR_BRU_ROUTE, VI6_DPR_NODE_UNUSED);
+
+	vsp1_write(vsp1, VI6_DPR_HGO_SMPPT, (7 << VI6_DPR_SMPPT_TGW_SHIFT) |
+		   (VI6_DPR_NODE_UNUSED << VI6_DPR_SMPPT_PT_SHIFT));
+	vsp1_write(vsp1, VI6_DPR_HGT_SMPPT, (7 << VI6_DPR_SMPPT_TGW_SHIFT) |
+		   (VI6_DPR_NODE_UNUSED << VI6_DPR_SMPPT_PT_SHIFT));
+
+	return 0;
+}
+
+/*
+ * vsp1_device_get - Acquire the VSP1 device
+ *
+ * Increment the VSP1 reference count and initialize the device if the first
+ * reference is taken.
+ *
+ * Return 0 on success or a negative error code otherwise.
+ */
+int vsp1_device_get(struct vsp1_device *vsp1)
+{
+	int ret = 0;
+
+	mutex_lock(&vsp1->lock);
+	if (vsp1->ref_count > 0)
+		goto done;
+
+	ret = clk_prepare_enable(vsp1->clock);
+	if (ret < 0)
+		goto done;
+
+	ret = vsp1_device_init(vsp1);
+	if (ret < 0) {
+		clk_disable_unprepare(vsp1->clock);
+		goto done;
+	}
+
+done:
+	if (!ret)
+		vsp1->ref_count++;
+
+	mutex_unlock(&vsp1->lock);
+	return ret;
+}
+
+/*
+ * vsp1_device_put - Release the VSP1 device
+ *
+ * Decrement the VSP1 reference count and cleanup the device if the last
+ * reference is released.
+ */
+void vsp1_device_put(struct vsp1_device *vsp1)
+{
+	mutex_lock(&vsp1->lock);
+
+	if (--vsp1->ref_count == 0)
+		clk_disable_unprepare(vsp1->clock);
+
+	mutex_unlock(&vsp1->lock);
+}
+
+/* -----------------------------------------------------------------------------
+ * Power Management
+ */
+
+#ifdef CONFIG_PM_SLEEP
+static int vsp1_pm_suspend(struct device *dev)
+{
+	struct vsp1_device *vsp1 = dev_get_drvdata(dev);
+
+	WARN_ON(mutex_is_locked(&vsp1->lock));
+
+	if (vsp1->ref_count == 0)
+		return 0;
+
+	vsp1_pipelines_suspend(vsp1);
+
+	clk_disable_unprepare(vsp1->clock);
+
+	return 0;
+}
+
+static int vsp1_pm_resume(struct device *dev)
+{
+	struct vsp1_device *vsp1 = dev_get_drvdata(dev);
+
+	WARN_ON(mutex_is_locked(&vsp1->lock));
+
+	if (vsp1->ref_count == 0)
+		return 0;
+
+	clk_prepare_enable(vsp1->clock);
+
+	vsp1_pipelines_resume(vsp1);
+
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops vsp1_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(vsp1_pm_suspend, vsp1_pm_resume)
+};
+
+/* -----------------------------------------------------------------------------
+ * Platform Driver
+ */
+
+static int vsp1_parse_dt(struct vsp1_device *vsp1)
+{
+	struct device_node *np = vsp1->dev->of_node;
+	struct vsp1_platform_data *pdata = &vsp1->pdata;
+
+	if (of_property_read_bool(np, "renesas,has-lif"))
+		pdata->features |= VSP1_HAS_LIF;
+	if (of_property_read_bool(np, "renesas,has-lut"))
+		pdata->features |= VSP1_HAS_LUT;
+	if (of_property_read_bool(np, "renesas,has-sru"))
+		pdata->features |= VSP1_HAS_SRU;
+
+	of_property_read_u32(np, "renesas,#rpf", &pdata->rpf_count);
+	of_property_read_u32(np, "renesas,#uds", &pdata->uds_count);
+	of_property_read_u32(np, "renesas,#wpf", &pdata->wpf_count);
+
+	if (pdata->rpf_count <= 0 || pdata->rpf_count > VSP1_MAX_RPF) {
+		dev_err(vsp1->dev, "invalid number of RPF (%u)\n",
+			pdata->rpf_count);
+		return -EINVAL;
+	}
+
+	if (pdata->uds_count <= 0 || pdata->uds_count > VSP1_MAX_UDS) {
+		dev_err(vsp1->dev, "invalid number of UDS (%u)\n",
+			pdata->uds_count);
+		return -EINVAL;
+	}
+
+	if (pdata->wpf_count <= 0 || pdata->wpf_count > VSP1_MAX_WPF) {
+		dev_err(vsp1->dev, "invalid number of WPF (%u)\n",
+			pdata->wpf_count);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int vsp1_probe(struct platform_device *pdev)
+{
+	struct vsp1_device *vsp1;
+	struct resource *irq;
+	struct resource *io;
+	int ret;
+
+	vsp1 = devm_kzalloc(&pdev->dev, sizeof(*vsp1), GFP_KERNEL);
+	if (vsp1 == NULL)
+		return -ENOMEM;
+
+	vsp1->dev = &pdev->dev;
+	mutex_init(&vsp1->lock);
+	INIT_LIST_HEAD(&vsp1->entities);
+
+	ret = vsp1_parse_dt(vsp1);
+	if (ret < 0)
+		return ret;
+
+	/* I/O, IRQ and clock resources */
+	io = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	vsp1->mmio = devm_ioremap_resource(&pdev->dev, io);
+	if (IS_ERR(vsp1->mmio))
+		return PTR_ERR(vsp1->mmio);
+
+	vsp1->clock = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(vsp1->clock)) {
+		dev_err(&pdev->dev, "failed to get clock\n");
+		return PTR_ERR(vsp1->clock);
+	}
+
+	irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!irq) {
+		dev_err(&pdev->dev, "missing IRQ\n");
+		return -EINVAL;
+	}
+
+	ret = devm_request_irq(&pdev->dev, irq->start, vsp1_irq_handler,
+			      IRQF_SHARED, dev_name(&pdev->dev), vsp1);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to request IRQ\n");
+		return ret;
+	}
+
+	/* Instanciate entities */
+	ret = vsp1_create_entities(vsp1);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to create entities\n");
+		return ret;
+	}
+
+	platform_set_drvdata(pdev, vsp1);
+
+	return 0;
+}
+
+static int vsp1_remove(struct platform_device *pdev)
+{
+	struct vsp1_device *vsp1 = platform_get_drvdata(pdev);
+
+	vsp1_destroy_entities(vsp1);
+
+	return 0;
+}
+
+static const struct of_device_id vsp1_of_match[] = {
+	{ .compatible = "renesas,vsp1" },
+	{ },
+};
+
+static struct platform_driver vsp1_platform_driver = {
+	.probe		= vsp1_probe,
+	.remove		= vsp1_remove,
+	.driver		= {
+		.name	= "vsp1",
+		.pm	= &vsp1_pm_ops,
+		.of_match_table = vsp1_of_match,
+	},
+};
+
+module_platform_driver(vsp1_platform_driver);
+
+MODULE_ALIAS("vsp1");
+MODULE_AUTHOR("Laurent Pinchart <laurent.pinchart@ideasonboard.com>");
+MODULE_DESCRIPTION("Renesas VSP1 Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/vsp1/vsp1_entity.c b/drivers/media/platform/vsp1/vsp1_entity.c
new file mode 100644
index 0000000..fd95a75
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_entity.c
@@ -0,0 +1,233 @@
+/*
+ * vsp1_entity.c  --  R-Car VSP1 Base Entity
+ *
+ * Copyright (C) 2013-2014 Renesas Electronics Corporation
+ *
+ * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/device.h>
+#include <linux/gfp.h>
+
+#include <media/media-entity.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-subdev.h>
+
+#include "vsp1.h"
+#include "vsp1_entity.h"
+#include "vsp1_video.h"
+
+bool vsp1_entity_is_streaming(struct vsp1_entity *entity)
+{
+	unsigned long flags;
+	bool streaming;
+
+	spin_lock_irqsave(&entity->lock, flags);
+	streaming = entity->streaming;
+	spin_unlock_irqrestore(&entity->lock, flags);
+
+	return streaming;
+}
+
+int vsp1_entity_set_streaming(struct vsp1_entity *entity, bool streaming)
+{
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&entity->lock, flags);
+	entity->streaming = streaming;
+	spin_unlock_irqrestore(&entity->lock, flags);
+
+	if (!streaming)
+		return 0;
+
+	if (!entity->subdev.ctrl_handler)
+		return 0;
+
+	ret = v4l2_ctrl_handler_setup(entity->subdev.ctrl_handler);
+	if (ret < 0) {
+		spin_lock_irqsave(&entity->lock, flags);
+		entity->streaming = false;
+		spin_unlock_irqrestore(&entity->lock, flags);
+	}
+
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Operations
+ */
+
+struct v4l2_mbus_framefmt *
+vsp1_entity_get_pad_format(struct vsp1_entity *entity,
+			   struct v4l2_subdev_pad_config *cfg,
+			   unsigned int pad, u32 which)
+{
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_format(&entity->subdev, cfg, pad);
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return &entity->formats[pad];
+	default:
+		return NULL;
+	}
+}
+
+/*
+ * vsp1_entity_init_formats - Initialize formats on all pads
+ * @subdev: V4L2 subdevice
+ * @cfg: V4L2 subdev pad configuration
+ *
+ * Initialize all pad formats with default values. If cfg is not NULL, try
+ * formats are initialized on the file handle. Otherwise active formats are
+ * initialized on the device.
+ */
+void vsp1_entity_init_formats(struct v4l2_subdev *subdev,
+			    struct v4l2_subdev_pad_config *cfg)
+{
+	struct v4l2_subdev_format format;
+	unsigned int pad;
+
+	for (pad = 0; pad < subdev->entity.num_pads - 1; ++pad) {
+		memset(&format, 0, sizeof(format));
+
+		format.pad = pad;
+		format.which = cfg ? V4L2_SUBDEV_FORMAT_TRY
+			     : V4L2_SUBDEV_FORMAT_ACTIVE;
+
+		v4l2_subdev_call(subdev, pad, set_fmt, cfg, &format);
+	}
+}
+
+static int vsp1_entity_open(struct v4l2_subdev *subdev,
+			    struct v4l2_subdev_fh *fh)
+{
+	vsp1_entity_init_formats(subdev, fh->pad);
+
+	return 0;
+}
+
+const struct v4l2_subdev_internal_ops vsp1_subdev_internal_ops = {
+	.open = vsp1_entity_open,
+};
+
+/* -----------------------------------------------------------------------------
+ * Media Operations
+ */
+
+static int vsp1_entity_link_setup(struct media_entity *entity,
+				  const struct media_pad *local,
+				  const struct media_pad *remote, u32 flags)
+{
+	struct vsp1_entity *source;
+
+	if (!(local->flags & MEDIA_PAD_FL_SOURCE))
+		return 0;
+
+	source = container_of(local->entity, struct vsp1_entity, subdev.entity);
+
+	if (!source->route)
+		return 0;
+
+	if (flags & MEDIA_LNK_FL_ENABLED) {
+		if (source->sink)
+			return -EBUSY;
+		source->sink = remote->entity;
+		source->sink_pad = remote->index;
+	} else {
+		source->sink = NULL;
+		source->sink_pad = 0;
+	}
+
+	return 0;
+}
+
+const struct media_entity_operations vsp1_media_ops = {
+	.link_setup = vsp1_entity_link_setup,
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+/* -----------------------------------------------------------------------------
+ * Initialization
+ */
+
+static const struct vsp1_route vsp1_routes[] = {
+	{ VSP1_ENTITY_BRU, 0, VI6_DPR_BRU_ROUTE,
+	  { VI6_DPR_NODE_BRU_IN(0), VI6_DPR_NODE_BRU_IN(1),
+	    VI6_DPR_NODE_BRU_IN(2), VI6_DPR_NODE_BRU_IN(3), } },
+	{ VSP1_ENTITY_HSI, 0, VI6_DPR_HSI_ROUTE, { VI6_DPR_NODE_HSI, } },
+	{ VSP1_ENTITY_HST, 0, VI6_DPR_HST_ROUTE, { VI6_DPR_NODE_HST, } },
+	{ VSP1_ENTITY_LIF, 0, 0, { VI6_DPR_NODE_LIF, } },
+	{ VSP1_ENTITY_LUT, 0, VI6_DPR_LUT_ROUTE, { VI6_DPR_NODE_LUT, } },
+	{ VSP1_ENTITY_RPF, 0, VI6_DPR_RPF_ROUTE(0), { VI6_DPR_NODE_RPF(0), } },
+	{ VSP1_ENTITY_RPF, 1, VI6_DPR_RPF_ROUTE(1), { VI6_DPR_NODE_RPF(1), } },
+	{ VSP1_ENTITY_RPF, 2, VI6_DPR_RPF_ROUTE(2), { VI6_DPR_NODE_RPF(2), } },
+	{ VSP1_ENTITY_RPF, 3, VI6_DPR_RPF_ROUTE(3), { VI6_DPR_NODE_RPF(3), } },
+	{ VSP1_ENTITY_RPF, 4, VI6_DPR_RPF_ROUTE(4), { VI6_DPR_NODE_RPF(4), } },
+	{ VSP1_ENTITY_SRU, 0, VI6_DPR_SRU_ROUTE, { VI6_DPR_NODE_SRU, } },
+	{ VSP1_ENTITY_UDS, 0, VI6_DPR_UDS_ROUTE(0), { VI6_DPR_NODE_UDS(0), } },
+	{ VSP1_ENTITY_UDS, 1, VI6_DPR_UDS_ROUTE(1), { VI6_DPR_NODE_UDS(1), } },
+	{ VSP1_ENTITY_UDS, 2, VI6_DPR_UDS_ROUTE(2), { VI6_DPR_NODE_UDS(2), } },
+	{ VSP1_ENTITY_WPF, 0, 0, { VI6_DPR_NODE_WPF(0), } },
+	{ VSP1_ENTITY_WPF, 1, 0, { VI6_DPR_NODE_WPF(1), } },
+	{ VSP1_ENTITY_WPF, 2, 0, { VI6_DPR_NODE_WPF(2), } },
+	{ VSP1_ENTITY_WPF, 3, 0, { VI6_DPR_NODE_WPF(3), } },
+};
+
+int vsp1_entity_init(struct vsp1_device *vsp1, struct vsp1_entity *entity,
+		     unsigned int num_pads)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(vsp1_routes); ++i) {
+		if (vsp1_routes[i].type == entity->type &&
+		    vsp1_routes[i].index == entity->index) {
+			entity->route = &vsp1_routes[i];
+			break;
+		}
+	}
+
+	if (i == ARRAY_SIZE(vsp1_routes))
+		return -EINVAL;
+
+	spin_lock_init(&entity->lock);
+
+	entity->vsp1 = vsp1;
+	entity->source_pad = num_pads - 1;
+
+	/* Allocate formats and pads. */
+	entity->formats = devm_kzalloc(vsp1->dev,
+				       num_pads * sizeof(*entity->formats),
+				       GFP_KERNEL);
+	if (entity->formats == NULL)
+		return -ENOMEM;
+
+	entity->pads = devm_kzalloc(vsp1->dev, num_pads * sizeof(*entity->pads),
+				    GFP_KERNEL);
+	if (entity->pads == NULL)
+		return -ENOMEM;
+
+	/* Initialize pads. */
+	for (i = 0; i < num_pads - 1; ++i)
+		entity->pads[i].flags = MEDIA_PAD_FL_SINK;
+
+	entity->pads[num_pads - 1].flags = MEDIA_PAD_FL_SOURCE;
+
+	/* Initialize the media entity. */
+	return media_entity_init(&entity->subdev.entity, num_pads,
+				 entity->pads, 0);
+}
+
+void vsp1_entity_destroy(struct vsp1_entity *entity)
+{
+	if (entity->video)
+		vsp1_video_cleanup(entity->video);
+	if (entity->subdev.ctrl_handler)
+		v4l2_ctrl_handler_free(entity->subdev.ctrl_handler);
+	media_entity_cleanup(&entity->subdev.entity);
+}
diff --git a/drivers/media/platform/vsp1/vsp1_entity.h b/drivers/media/platform/vsp1/vsp1_entity.h
new file mode 100644
index 0000000..8867a57
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_entity.h
@@ -0,0 +1,102 @@
+/*
+ * vsp1_entity.h  --  R-Car VSP1 Base Entity
+ *
+ * Copyright (C) 2013-2014 Renesas Electronics Corporation
+ *
+ * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+#ifndef __VSP1_ENTITY_H__
+#define __VSP1_ENTITY_H__
+
+#include <linux/list.h>
+#include <linux/spinlock.h>
+
+#include <media/v4l2-subdev.h>
+
+struct vsp1_device;
+struct vsp1_video;
+
+enum vsp1_entity_type {
+	VSP1_ENTITY_BRU,
+	VSP1_ENTITY_HSI,
+	VSP1_ENTITY_HST,
+	VSP1_ENTITY_LIF,
+	VSP1_ENTITY_LUT,
+	VSP1_ENTITY_RPF,
+	VSP1_ENTITY_SRU,
+	VSP1_ENTITY_UDS,
+	VSP1_ENTITY_WPF,
+};
+
+/*
+ * struct vsp1_route - Entity routing configuration
+ * @type: Entity type this routing entry is associated with
+ * @index: Entity index this routing entry is associated with
+ * @reg: Output routing configuration register
+ * @inputs: Target node value for each input
+ *
+ * Each $vsp1_route entry describes routing configuration for the entity
+ * specified by the entry's @type and @index. @reg indicates the register that
+ * holds output routing configuration for the entity, and the @inputs array
+ * store the target node value for each input of the entity.
+ */
+struct vsp1_route {
+	enum vsp1_entity_type type;
+	unsigned int index;
+	unsigned int reg;
+	unsigned int inputs[4];
+};
+
+struct vsp1_entity {
+	struct vsp1_device *vsp1;
+
+	enum vsp1_entity_type type;
+	unsigned int index;
+	const struct vsp1_route *route;
+
+	struct list_head list_dev;
+	struct list_head list_pipe;
+
+	struct media_pad *pads;
+	unsigned int source_pad;
+
+	struct media_entity *sink;
+	unsigned int sink_pad;
+
+	struct v4l2_subdev subdev;
+	struct v4l2_mbus_framefmt *formats;
+
+	struct vsp1_video *video;
+
+	spinlock_t lock;		/* Protects the streaming field */
+	bool streaming;
+};
+
+static inline struct vsp1_entity *to_vsp1_entity(struct v4l2_subdev *subdev)
+{
+	return container_of(subdev, struct vsp1_entity, subdev);
+}
+
+int vsp1_entity_init(struct vsp1_device *vsp1, struct vsp1_entity *entity,
+		     unsigned int num_pads);
+void vsp1_entity_destroy(struct vsp1_entity *entity);
+
+extern const struct v4l2_subdev_internal_ops vsp1_subdev_internal_ops;
+extern const struct media_entity_operations vsp1_media_ops;
+
+struct v4l2_mbus_framefmt *
+vsp1_entity_get_pad_format(struct vsp1_entity *entity,
+			   struct v4l2_subdev_pad_config *cfg,
+			   unsigned int pad, u32 which);
+void vsp1_entity_init_formats(struct v4l2_subdev *subdev,
+			      struct v4l2_subdev_pad_config *cfg);
+
+bool vsp1_entity_is_streaming(struct vsp1_entity *entity);
+int vsp1_entity_set_streaming(struct vsp1_entity *entity, bool streaming);
+
+#endif /* __VSP1_ENTITY_H__ */
diff --git a/drivers/media/platform/vsp1/vsp1_hsit.c b/drivers/media/platform/vsp1/vsp1_hsit.c
new file mode 100644
index 0000000..8ffb817
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_hsit.c
@@ -0,0 +1,216 @@
+/*
+ * vsp1_hsit.c  --  R-Car VSP1 Hue Saturation value (Inverse) Transform
+ *
+ * Copyright (C) 2013 Renesas Corporation
+ *
+ * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/device.h>
+#include <linux/gfp.h>
+
+#include <media/v4l2-subdev.h>
+
+#include "vsp1.h"
+#include "vsp1_hsit.h"
+
+#define HSIT_MIN_SIZE				4U
+#define HSIT_MAX_SIZE				8190U
+
+/* -----------------------------------------------------------------------------
+ * Device Access
+ */
+
+static inline void vsp1_hsit_write(struct vsp1_hsit *hsit, u32 reg, u32 data)
+{
+	vsp1_write(hsit->entity.vsp1, reg, data);
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Core Operations
+ */
+
+static int hsit_s_stream(struct v4l2_subdev *subdev, int enable)
+{
+	struct vsp1_hsit *hsit = to_hsit(subdev);
+
+	if (!enable)
+		return 0;
+
+	if (hsit->inverse)
+		vsp1_hsit_write(hsit, VI6_HSI_CTRL, VI6_HSI_CTRL_EN);
+	else
+		vsp1_hsit_write(hsit, VI6_HST_CTRL, VI6_HST_CTRL_EN);
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Pad Operations
+ */
+
+static int hsit_enum_mbus_code(struct v4l2_subdev *subdev,
+			       struct v4l2_subdev_pad_config *cfg,
+			       struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct vsp1_hsit *hsit = to_hsit(subdev);
+
+	if (code->index > 0)
+		return -EINVAL;
+
+	if ((code->pad == HSIT_PAD_SINK && !hsit->inverse) |
+	    (code->pad == HSIT_PAD_SOURCE && hsit->inverse))
+		code->code = MEDIA_BUS_FMT_ARGB8888_1X32;
+	else
+		code->code = MEDIA_BUS_FMT_AHSV8888_1X32;
+
+	return 0;
+}
+
+static int hsit_enum_frame_size(struct v4l2_subdev *subdev,
+				struct v4l2_subdev_pad_config *cfg,
+				struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct vsp1_hsit *hsit = to_hsit(subdev);
+	struct v4l2_mbus_framefmt *format;
+
+	format = vsp1_entity_get_pad_format(&hsit->entity, cfg, fse->pad,
+					    fse->which);
+
+	if (fse->index || fse->code != format->code)
+		return -EINVAL;
+
+	if (fse->pad == HSIT_PAD_SINK) {
+		fse->min_width = HSIT_MIN_SIZE;
+		fse->max_width = HSIT_MAX_SIZE;
+		fse->min_height = HSIT_MIN_SIZE;
+		fse->max_height = HSIT_MAX_SIZE;
+	} else {
+		/* The size on the source pad are fixed and always identical to
+		 * the size on the sink pad.
+		 */
+		fse->min_width = format->width;
+		fse->max_width = format->width;
+		fse->min_height = format->height;
+		fse->max_height = format->height;
+	}
+
+	return 0;
+}
+
+static int hsit_get_format(struct v4l2_subdev *subdev,
+			   struct v4l2_subdev_pad_config *cfg,
+			   struct v4l2_subdev_format *fmt)
+{
+	struct vsp1_hsit *hsit = to_hsit(subdev);
+
+	fmt->format = *vsp1_entity_get_pad_format(&hsit->entity, cfg, fmt->pad,
+						  fmt->which);
+
+	return 0;
+}
+
+static int hsit_set_format(struct v4l2_subdev *subdev,
+			   struct v4l2_subdev_pad_config *cfg,
+			   struct v4l2_subdev_format *fmt)
+{
+	struct vsp1_hsit *hsit = to_hsit(subdev);
+	struct v4l2_mbus_framefmt *format;
+
+	format = vsp1_entity_get_pad_format(&hsit->entity, cfg, fmt->pad,
+					    fmt->which);
+
+	if (fmt->pad == HSIT_PAD_SOURCE) {
+		/* The HST and HSI output format code and resolution can't be
+		 * modified.
+		 */
+		fmt->format = *format;
+		return 0;
+	}
+
+	format->code = hsit->inverse ? MEDIA_BUS_FMT_AHSV8888_1X32
+		     : MEDIA_BUS_FMT_ARGB8888_1X32;
+	format->width = clamp_t(unsigned int, fmt->format.width,
+				HSIT_MIN_SIZE, HSIT_MAX_SIZE);
+	format->height = clamp_t(unsigned int, fmt->format.height,
+				 HSIT_MIN_SIZE, HSIT_MAX_SIZE);
+	format->field = V4L2_FIELD_NONE;
+	format->colorspace = V4L2_COLORSPACE_SRGB;
+
+	fmt->format = *format;
+
+	/* Propagate the format to the source pad. */
+	format = vsp1_entity_get_pad_format(&hsit->entity, cfg, HSIT_PAD_SOURCE,
+					    fmt->which);
+	*format = fmt->format;
+	format->code = hsit->inverse ? MEDIA_BUS_FMT_ARGB8888_1X32
+		     : MEDIA_BUS_FMT_AHSV8888_1X32;
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Operations
+ */
+
+static struct v4l2_subdev_video_ops hsit_video_ops = {
+	.s_stream = hsit_s_stream,
+};
+
+static struct v4l2_subdev_pad_ops hsit_pad_ops = {
+	.enum_mbus_code = hsit_enum_mbus_code,
+	.enum_frame_size = hsit_enum_frame_size,
+	.get_fmt = hsit_get_format,
+	.set_fmt = hsit_set_format,
+};
+
+static struct v4l2_subdev_ops hsit_ops = {
+	.video	= &hsit_video_ops,
+	.pad    = &hsit_pad_ops,
+};
+
+/* -----------------------------------------------------------------------------
+ * Initialization and Cleanup
+ */
+
+struct vsp1_hsit *vsp1_hsit_create(struct vsp1_device *vsp1, bool inverse)
+{
+	struct v4l2_subdev *subdev;
+	struct vsp1_hsit *hsit;
+	int ret;
+
+	hsit = devm_kzalloc(vsp1->dev, sizeof(*hsit), GFP_KERNEL);
+	if (hsit == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	hsit->inverse = inverse;
+
+	if (inverse)
+		hsit->entity.type = VSP1_ENTITY_HSI;
+	else
+		hsit->entity.type = VSP1_ENTITY_HST;
+
+	ret = vsp1_entity_init(vsp1, &hsit->entity, 2);
+	if (ret < 0)
+		return ERR_PTR(ret);
+
+	/* Initialize the V4L2 subdev. */
+	subdev = &hsit->entity.subdev;
+	v4l2_subdev_init(subdev, &hsit_ops);
+
+	subdev->entity.ops = &vsp1_media_ops;
+	subdev->internal_ops = &vsp1_subdev_internal_ops;
+	snprintf(subdev->name, sizeof(subdev->name), "%s %s",
+		 dev_name(vsp1->dev), inverse ? "hsi" : "hst");
+	v4l2_set_subdevdata(subdev, hsit);
+	subdev->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	vsp1_entity_init_formats(subdev, NULL);
+
+	return hsit;
+}
diff --git a/drivers/media/platform/vsp1/vsp1_hsit.h b/drivers/media/platform/vsp1/vsp1_hsit.h
new file mode 100644
index 0000000..82f1c84
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_hsit.h
@@ -0,0 +1,38 @@
+/*
+ * vsp1_hsit.h  --  R-Car VSP1 Hue Saturation value (Inverse) Transform
+ *
+ * Copyright (C) 2013 Renesas Corporation
+ *
+ * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+#ifndef __VSP1_HSIT_H__
+#define __VSP1_HSIT_H__
+
+#include <media/media-entity.h>
+#include <media/v4l2-subdev.h>
+
+#include "vsp1_entity.h"
+
+struct vsp1_device;
+
+#define HSIT_PAD_SINK				0
+#define HSIT_PAD_SOURCE				1
+
+struct vsp1_hsit {
+	struct vsp1_entity entity;
+	bool inverse;
+};
+
+static inline struct vsp1_hsit *to_hsit(struct v4l2_subdev *subdev)
+{
+	return container_of(subdev, struct vsp1_hsit, entity.subdev);
+}
+
+struct vsp1_hsit *vsp1_hsit_create(struct vsp1_device *vsp1, bool inverse);
+
+#endif /* __VSP1_HSIT_H__ */
diff --git a/drivers/media/platform/vsp1/vsp1_lif.c b/drivers/media/platform/vsp1/vsp1_lif.c
new file mode 100644
index 0000000..39fa5ef
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_lif.c
@@ -0,0 +1,241 @@
+/*
+ * vsp1_lif.c  --  R-Car VSP1 LCD Controller Interface
+ *
+ * Copyright (C) 2013-2014 Renesas Electronics Corporation
+ *
+ * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/device.h>
+#include <linux/gfp.h>
+
+#include <media/v4l2-subdev.h>
+
+#include "vsp1.h"
+#include "vsp1_lif.h"
+
+#define LIF_MIN_SIZE				2U
+#define LIF_MAX_SIZE				2048U
+
+/* -----------------------------------------------------------------------------
+ * Device Access
+ */
+
+static inline u32 vsp1_lif_read(struct vsp1_lif *lif, u32 reg)
+{
+	return vsp1_read(lif->entity.vsp1, reg);
+}
+
+static inline void vsp1_lif_write(struct vsp1_lif *lif, u32 reg, u32 data)
+{
+	vsp1_write(lif->entity.vsp1, reg, data);
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Core Operations
+ */
+
+static int lif_s_stream(struct v4l2_subdev *subdev, int enable)
+{
+	const struct v4l2_mbus_framefmt *format;
+	struct vsp1_lif *lif = to_lif(subdev);
+	unsigned int hbth = 1300;
+	unsigned int obth = 400;
+	unsigned int lbth = 200;
+
+	if (!enable) {
+		vsp1_lif_write(lif, VI6_LIF_CTRL, 0);
+		return 0;
+	}
+
+	format = &lif->entity.formats[LIF_PAD_SOURCE];
+
+	obth = min(obth, (format->width + 1) / 2 * format->height - 4);
+
+	vsp1_lif_write(lif, VI6_LIF_CSBTH,
+			(hbth << VI6_LIF_CSBTH_HBTH_SHIFT) |
+			(lbth << VI6_LIF_CSBTH_LBTH_SHIFT));
+
+	vsp1_lif_write(lif, VI6_LIF_CTRL,
+			(obth << VI6_LIF_CTRL_OBTH_SHIFT) |
+			(format->code == 0 ? VI6_LIF_CTRL_CFMT : 0) |
+			VI6_LIF_CTRL_REQSEL | VI6_LIF_CTRL_LIF_EN);
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Pad Operations
+ */
+
+static int lif_enum_mbus_code(struct v4l2_subdev *subdev,
+			      struct v4l2_subdev_pad_config *cfg,
+			      struct v4l2_subdev_mbus_code_enum *code)
+{
+	static const unsigned int codes[] = {
+		MEDIA_BUS_FMT_ARGB8888_1X32,
+		MEDIA_BUS_FMT_AYUV8_1X32,
+	};
+	struct vsp1_lif *lif = to_lif(subdev);
+
+	if (code->pad == LIF_PAD_SINK) {
+		if (code->index >= ARRAY_SIZE(codes))
+			return -EINVAL;
+
+		code->code = codes[code->index];
+	} else {
+		struct v4l2_mbus_framefmt *format;
+
+		/* The LIF can't perform format conversion, the sink format is
+		 * always identical to the source format.
+		 */
+		if (code->index)
+			return -EINVAL;
+
+		format = vsp1_entity_get_pad_format(&lif->entity, cfg,
+						    LIF_PAD_SINK, code->which);
+		code->code = format->code;
+	}
+
+	return 0;
+}
+
+static int lif_enum_frame_size(struct v4l2_subdev *subdev,
+			       struct v4l2_subdev_pad_config *cfg,
+			       struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct vsp1_lif *lif = to_lif(subdev);
+	struct v4l2_mbus_framefmt *format;
+
+	format = vsp1_entity_get_pad_format(&lif->entity, cfg, LIF_PAD_SINK,
+					    fse->which);
+
+	if (fse->index || fse->code != format->code)
+		return -EINVAL;
+
+	if (fse->pad == LIF_PAD_SINK) {
+		fse->min_width = LIF_MIN_SIZE;
+		fse->max_width = LIF_MAX_SIZE;
+		fse->min_height = LIF_MIN_SIZE;
+		fse->max_height = LIF_MAX_SIZE;
+	} else {
+		fse->min_width = format->width;
+		fse->max_width = format->width;
+		fse->min_height = format->height;
+		fse->max_height = format->height;
+	}
+
+	return 0;
+}
+
+static int lif_get_format(struct v4l2_subdev *subdev, struct v4l2_subdev_pad_config *cfg,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct vsp1_lif *lif = to_lif(subdev);
+
+	fmt->format = *vsp1_entity_get_pad_format(&lif->entity, cfg, fmt->pad,
+						  fmt->which);
+
+	return 0;
+}
+
+static int lif_set_format(struct v4l2_subdev *subdev, struct v4l2_subdev_pad_config *cfg,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct vsp1_lif *lif = to_lif(subdev);
+	struct v4l2_mbus_framefmt *format;
+
+	/* Default to YUV if the requested format is not supported. */
+	if (fmt->format.code != MEDIA_BUS_FMT_ARGB8888_1X32 &&
+	    fmt->format.code != MEDIA_BUS_FMT_AYUV8_1X32)
+		fmt->format.code = MEDIA_BUS_FMT_AYUV8_1X32;
+
+	format = vsp1_entity_get_pad_format(&lif->entity, cfg, fmt->pad,
+					    fmt->which);
+
+	if (fmt->pad == LIF_PAD_SOURCE) {
+		/* The LIF source format is always identical to its sink
+		 * format.
+		 */
+		fmt->format = *format;
+		return 0;
+	}
+
+	format->code = fmt->format.code;
+	format->width = clamp_t(unsigned int, fmt->format.width,
+				LIF_MIN_SIZE, LIF_MAX_SIZE);
+	format->height = clamp_t(unsigned int, fmt->format.height,
+				 LIF_MIN_SIZE, LIF_MAX_SIZE);
+	format->field = V4L2_FIELD_NONE;
+	format->colorspace = V4L2_COLORSPACE_SRGB;
+
+	fmt->format = *format;
+
+	/* Propagate the format to the source pad. */
+	format = vsp1_entity_get_pad_format(&lif->entity, cfg, LIF_PAD_SOURCE,
+					    fmt->which);
+	*format = fmt->format;
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Operations
+ */
+
+static struct v4l2_subdev_video_ops lif_video_ops = {
+	.s_stream = lif_s_stream,
+};
+
+static struct v4l2_subdev_pad_ops lif_pad_ops = {
+	.enum_mbus_code = lif_enum_mbus_code,
+	.enum_frame_size = lif_enum_frame_size,
+	.get_fmt = lif_get_format,
+	.set_fmt = lif_set_format,
+};
+
+static struct v4l2_subdev_ops lif_ops = {
+	.video	= &lif_video_ops,
+	.pad    = &lif_pad_ops,
+};
+
+/* -----------------------------------------------------------------------------
+ * Initialization and Cleanup
+ */
+
+struct vsp1_lif *vsp1_lif_create(struct vsp1_device *vsp1)
+{
+	struct v4l2_subdev *subdev;
+	struct vsp1_lif *lif;
+	int ret;
+
+	lif = devm_kzalloc(vsp1->dev, sizeof(*lif), GFP_KERNEL);
+	if (lif == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	lif->entity.type = VSP1_ENTITY_LIF;
+
+	ret = vsp1_entity_init(vsp1, &lif->entity, 2);
+	if (ret < 0)
+		return ERR_PTR(ret);
+
+	/* Initialize the V4L2 subdev. */
+	subdev = &lif->entity.subdev;
+	v4l2_subdev_init(subdev, &lif_ops);
+
+	subdev->entity.ops = &vsp1_media_ops;
+	subdev->internal_ops = &vsp1_subdev_internal_ops;
+	snprintf(subdev->name, sizeof(subdev->name), "%s lif",
+		 dev_name(vsp1->dev));
+	v4l2_set_subdevdata(subdev, lif);
+	subdev->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	vsp1_entity_init_formats(subdev, NULL);
+
+	return lif;
+}
diff --git a/drivers/media/platform/vsp1/vsp1_lif.h b/drivers/media/platform/vsp1/vsp1_lif.h
new file mode 100644
index 0000000..7b35879
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_lif.h
@@ -0,0 +1,37 @@
+/*
+ * vsp1_lif.h  --  R-Car VSP1 LCD Controller Interface
+ *
+ * Copyright (C) 2013-2014 Renesas Electronics Corporation
+ *
+ * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+#ifndef __VSP1_LIF_H__
+#define __VSP1_LIF_H__
+
+#include <media/media-entity.h>
+#include <media/v4l2-subdev.h>
+
+#include "vsp1_entity.h"
+
+struct vsp1_device;
+
+#define LIF_PAD_SINK				0
+#define LIF_PAD_SOURCE				1
+
+struct vsp1_lif {
+	struct vsp1_entity entity;
+};
+
+static inline struct vsp1_lif *to_lif(struct v4l2_subdev *subdev)
+{
+	return container_of(subdev, struct vsp1_lif, entity.subdev);
+}
+
+struct vsp1_lif *vsp1_lif_create(struct vsp1_device *vsp1);
+
+#endif /* __VSP1_LIF_H__ */
diff --git a/drivers/media/platform/vsp1/vsp1_lut.c b/drivers/media/platform/vsp1/vsp1_lut.c
new file mode 100644
index 0000000..656ec27
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_lut.c
@@ -0,0 +1,255 @@
+/*
+ * vsp1_lut.c  --  R-Car VSP1 Look-Up Table
+ *
+ * Copyright (C) 2013 Renesas Corporation
+ *
+ * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/device.h>
+#include <linux/gfp.h>
+#include <linux/vsp1.h>
+
+#include <media/v4l2-subdev.h>
+
+#include "vsp1.h"
+#include "vsp1_lut.h"
+
+#define LUT_MIN_SIZE				4U
+#define LUT_MAX_SIZE				8190U
+
+/* -----------------------------------------------------------------------------
+ * Device Access
+ */
+
+static inline u32 vsp1_lut_read(struct vsp1_lut *lut, u32 reg)
+{
+	return vsp1_read(lut->entity.vsp1, reg);
+}
+
+static inline void vsp1_lut_write(struct vsp1_lut *lut, u32 reg, u32 data)
+{
+	vsp1_write(lut->entity.vsp1, reg, data);
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Core Operations
+ */
+
+static void lut_configure(struct vsp1_lut *lut, struct vsp1_lut_config *config)
+{
+	memcpy_toio(lut->entity.vsp1->mmio + VI6_LUT_TABLE, config->lut,
+		    sizeof(config->lut));
+}
+
+static long lut_ioctl(struct v4l2_subdev *subdev, unsigned int cmd, void *arg)
+{
+	struct vsp1_lut *lut = to_lut(subdev);
+
+	switch (cmd) {
+	case VIDIOC_VSP1_LUT_CONFIG:
+		lut_configure(lut, arg);
+		return 0;
+
+	default:
+		return -ENOIOCTLCMD;
+	}
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Video Operations
+ */
+
+static int lut_s_stream(struct v4l2_subdev *subdev, int enable)
+{
+	struct vsp1_lut *lut = to_lut(subdev);
+
+	if (!enable)
+		return 0;
+
+	vsp1_lut_write(lut, VI6_LUT_CTRL, VI6_LUT_CTRL_EN);
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Pad Operations
+ */
+
+static int lut_enum_mbus_code(struct v4l2_subdev *subdev,
+			      struct v4l2_subdev_pad_config *cfg,
+			      struct v4l2_subdev_mbus_code_enum *code)
+{
+	static const unsigned int codes[] = {
+		MEDIA_BUS_FMT_ARGB8888_1X32,
+		MEDIA_BUS_FMT_AHSV8888_1X32,
+		MEDIA_BUS_FMT_AYUV8_1X32,
+	};
+	struct vsp1_lut *lut = to_lut(subdev);
+	struct v4l2_mbus_framefmt *format;
+
+	if (code->pad == LUT_PAD_SINK) {
+		if (code->index >= ARRAY_SIZE(codes))
+			return -EINVAL;
+
+		code->code = codes[code->index];
+	} else {
+		/* The LUT can't perform format conversion, the sink format is
+		 * always identical to the source format.
+		 */
+		if (code->index)
+			return -EINVAL;
+
+		format = vsp1_entity_get_pad_format(&lut->entity, cfg,
+						    LUT_PAD_SINK, code->which);
+		code->code = format->code;
+	}
+
+	return 0;
+}
+
+static int lut_enum_frame_size(struct v4l2_subdev *subdev,
+			       struct v4l2_subdev_pad_config *cfg,
+			       struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct vsp1_lut *lut = to_lut(subdev);
+	struct v4l2_mbus_framefmt *format;
+
+	format = vsp1_entity_get_pad_format(&lut->entity, cfg,
+					    fse->pad, fse->which);
+
+	if (fse->index || fse->code != format->code)
+		return -EINVAL;
+
+	if (fse->pad == LUT_PAD_SINK) {
+		fse->min_width = LUT_MIN_SIZE;
+		fse->max_width = LUT_MAX_SIZE;
+		fse->min_height = LUT_MIN_SIZE;
+		fse->max_height = LUT_MAX_SIZE;
+	} else {
+		/* The size on the source pad are fixed and always identical to
+		 * the size on the sink pad.
+		 */
+		fse->min_width = format->width;
+		fse->max_width = format->width;
+		fse->min_height = format->height;
+		fse->max_height = format->height;
+	}
+
+	return 0;
+}
+
+static int lut_get_format(struct v4l2_subdev *subdev, struct v4l2_subdev_pad_config *cfg,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct vsp1_lut *lut = to_lut(subdev);
+
+	fmt->format = *vsp1_entity_get_pad_format(&lut->entity, cfg, fmt->pad,
+						  fmt->which);
+
+	return 0;
+}
+
+static int lut_set_format(struct v4l2_subdev *subdev, struct v4l2_subdev_pad_config *cfg,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct vsp1_lut *lut = to_lut(subdev);
+	struct v4l2_mbus_framefmt *format;
+
+	/* Default to YUV if the requested format is not supported. */
+	if (fmt->format.code != MEDIA_BUS_FMT_ARGB8888_1X32 &&
+	    fmt->format.code != MEDIA_BUS_FMT_AHSV8888_1X32 &&
+	    fmt->format.code != MEDIA_BUS_FMT_AYUV8_1X32)
+		fmt->format.code = MEDIA_BUS_FMT_AYUV8_1X32;
+
+	format = vsp1_entity_get_pad_format(&lut->entity, cfg, fmt->pad,
+					    fmt->which);
+
+	if (fmt->pad == LUT_PAD_SOURCE) {
+		/* The LUT output format can't be modified. */
+		fmt->format = *format;
+		return 0;
+	}
+
+	format->width = clamp_t(unsigned int, fmt->format.width,
+				LUT_MIN_SIZE, LUT_MAX_SIZE);
+	format->height = clamp_t(unsigned int, fmt->format.height,
+				 LUT_MIN_SIZE, LUT_MAX_SIZE);
+	format->field = V4L2_FIELD_NONE;
+	format->colorspace = V4L2_COLORSPACE_SRGB;
+
+	fmt->format = *format;
+
+	/* Propagate the format to the source pad. */
+	format = vsp1_entity_get_pad_format(&lut->entity, cfg, LUT_PAD_SOURCE,
+					    fmt->which);
+	*format = fmt->format;
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Operations
+ */
+
+static struct v4l2_subdev_core_ops lut_core_ops = {
+	.ioctl = lut_ioctl,
+};
+
+static struct v4l2_subdev_video_ops lut_video_ops = {
+	.s_stream = lut_s_stream,
+};
+
+static struct v4l2_subdev_pad_ops lut_pad_ops = {
+	.enum_mbus_code = lut_enum_mbus_code,
+	.enum_frame_size = lut_enum_frame_size,
+	.get_fmt = lut_get_format,
+	.set_fmt = lut_set_format,
+};
+
+static struct v4l2_subdev_ops lut_ops = {
+	.core	= &lut_core_ops,
+	.video	= &lut_video_ops,
+	.pad    = &lut_pad_ops,
+};
+
+/* -----------------------------------------------------------------------------
+ * Initialization and Cleanup
+ */
+
+struct vsp1_lut *vsp1_lut_create(struct vsp1_device *vsp1)
+{
+	struct v4l2_subdev *subdev;
+	struct vsp1_lut *lut;
+	int ret;
+
+	lut = devm_kzalloc(vsp1->dev, sizeof(*lut), GFP_KERNEL);
+	if (lut == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	lut->entity.type = VSP1_ENTITY_LUT;
+
+	ret = vsp1_entity_init(vsp1, &lut->entity, 2);
+	if (ret < 0)
+		return ERR_PTR(ret);
+
+	/* Initialize the V4L2 subdev. */
+	subdev = &lut->entity.subdev;
+	v4l2_subdev_init(subdev, &lut_ops);
+
+	subdev->entity.ops = &vsp1_media_ops;
+	subdev->internal_ops = &vsp1_subdev_internal_ops;
+	snprintf(subdev->name, sizeof(subdev->name), "%s lut",
+		 dev_name(vsp1->dev));
+	v4l2_set_subdevdata(subdev, lut);
+	subdev->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	vsp1_entity_init_formats(subdev, NULL);
+
+	return lut;
+}
diff --git a/drivers/media/platform/vsp1/vsp1_lut.h b/drivers/media/platform/vsp1/vsp1_lut.h
new file mode 100644
index 0000000..f92ffb8
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_lut.h
@@ -0,0 +1,38 @@
+/*
+ * vsp1_lut.h  --  R-Car VSP1 Look-Up Table
+ *
+ * Copyright (C) 2013 Renesas Corporation
+ *
+ * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+#ifndef __VSP1_LUT_H__
+#define __VSP1_LUT_H__
+
+#include <media/media-entity.h>
+#include <media/v4l2-subdev.h>
+
+#include "vsp1_entity.h"
+
+struct vsp1_device;
+
+#define LUT_PAD_SINK				0
+#define LUT_PAD_SOURCE				1
+
+struct vsp1_lut {
+	struct vsp1_entity entity;
+	u32 lut[256];
+};
+
+static inline struct vsp1_lut *to_lut(struct v4l2_subdev *subdev)
+{
+	return container_of(subdev, struct vsp1_lut, entity.subdev);
+}
+
+struct vsp1_lut *vsp1_lut_create(struct vsp1_device *vsp1);
+
+#endif /* __VSP1_LUT_H__ */
diff --git a/drivers/media/platform/vsp1/vsp1_regs.h b/drivers/media/platform/vsp1/vsp1_regs.h
new file mode 100644
index 0000000..25b4873
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_regs.h
@@ -0,0 +1,697 @@
+/*
+ * vsp1_regs.h  --  R-Car VSP1 Registers Definitions
+ *
+ * Copyright (C) 2013 Renesas Electronics Corporation
+ *
+ * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.
+ */
+
+#ifndef __VSP1_REGS_H__
+#define __VSP1_REGS_H__
+
+/* -----------------------------------------------------------------------------
+ * General Control Registers
+ */
+
+#define VI6_CMD(n)			(0x0000 + (n) * 4)
+#define VI6_CMD_STRCMD			(1 << 0)
+
+#define VI6_CLK_DCSWT			0x0018
+#define VI6_CLK_DCSWT_CSTPW_MASK	(0xff << 8)
+#define VI6_CLK_DCSWT_CSTPW_SHIFT	8
+#define VI6_CLK_DCSWT_CSTRW_MASK	(0xff << 0)
+#define VI6_CLK_DCSWT_CSTRW_SHIFT	0
+
+#define VI6_SRESET			0x0028
+#define VI6_SRESET_SRTS(n)		(1 << (n))
+
+#define VI6_STATUS			0x0038
+#define VI6_STATUS_SYS_ACT(n)		(1 << ((n) + 8))
+
+#define VI6_WPF_IRQ_ENB(n)		(0x0048 + (n) * 12)
+#define VI6_WFP_IRQ_ENB_DFEE		(1 << 1)
+#define VI6_WFP_IRQ_ENB_FREE		(1 << 0)
+
+#define VI6_WPF_IRQ_STA(n)		(0x004c + (n) * 12)
+#define VI6_WFP_IRQ_STA_DFE		(1 << 1)
+#define VI6_WFP_IRQ_STA_FRE		(1 << 0)
+
+#define VI6_DISP_IRQ_ENB		0x0078
+#define VI6_DISP_IRQ_ENB_DSTE		(1 << 8)
+#define VI6_DISP_IRQ_ENB_MAEE		(1 << 5)
+#define VI6_DISP_IRQ_ENB_LNEE(n)	(1 << (n))
+
+#define VI6_DISP_IRQ_STA		0x007c
+#define VI6_DISP_IRQ_STA_DSE		(1 << 8)
+#define VI6_DISP_IRQ_STA_MAE		(1 << 5)
+#define VI6_DISP_IRQ_STA_LNE(n)		(1 << (n))
+
+#define VI6_WPF_LINE_COUNT(n)		(0x0084 + (n) * 4)
+#define VI6_WPF_LINE_COUNT_MASK		(0x1fffff << 0)
+
+/* -----------------------------------------------------------------------------
+ * Display List Control Registers
+ */
+
+#define VI6_DL_CTRL			0x0100
+#define VI6_DL_CTRL_AR_WAIT_MASK	(0xffff << 16)
+#define VI6_DL_CTRL_AR_WAIT_SHIFT	16
+#define VI6_DL_CTRL_DC2			(1 << 12)
+#define VI6_DL_CTRL_DC1			(1 << 8)
+#define VI6_DL_CTRL_DC0			(1 << 4)
+#define VI6_DL_CTRL_CFM0		(1 << 2)
+#define VI6_DL_CTRL_NH0			(1 << 1)
+#define VI6_DL_CTRL_DLE			(1 << 0)
+
+#define VI6_DL_HDR_ADDR(n)		(0x0104 + (n) * 4)
+
+#define VI6_DL_SWAP			0x0114
+#define VI6_DL_SWAP_LWS			(1 << 2)
+#define VI6_DL_SWAP_WDS			(1 << 1)
+#define VI6_DL_SWAP_BTS			(1 << 0)
+
+#define VI6_DL_EXT_CTRL			0x011c
+#define VI6_DL_EXT_CTRL_NWE		(1 << 16)
+#define VI6_DL_EXT_CTRL_POLINT_MASK	(0x3f << 8)
+#define VI6_DL_EXT_CTRL_POLINT_SHIFT	8
+#define VI6_DL_EXT_CTRL_DLPRI		(1 << 5)
+#define VI6_DL_EXT_CTRL_EXPRI		(1 << 4)
+#define VI6_DL_EXT_CTRL_EXT		(1 << 0)
+
+#define VI6_DL_BODY_SIZE		0x0120
+#define VI6_DL_BODY_SIZE_UPD		(1 << 24)
+#define VI6_DL_BODY_SIZE_BS_MASK	(0x1ffff << 0)
+#define VI6_DL_BODY_SIZE_BS_SHIFT	0
+
+/* -----------------------------------------------------------------------------
+ * RPF Control Registers
+ */
+
+#define VI6_RPF_OFFSET			0x100
+
+#define VI6_RPF_SRC_BSIZE		0x0300
+#define VI6_RPF_SRC_BSIZE_BHSIZE_MASK	(0x1fff << 16)
+#define VI6_RPF_SRC_BSIZE_BHSIZE_SHIFT	16
+#define VI6_RPF_SRC_BSIZE_BVSIZE_MASK	(0x1fff << 0)
+#define VI6_RPF_SRC_BSIZE_BVSIZE_SHIFT	0
+
+#define VI6_RPF_SRC_ESIZE		0x0304
+#define VI6_RPF_SRC_ESIZE_EHSIZE_MASK	(0x1fff << 16)
+#define VI6_RPF_SRC_ESIZE_EHSIZE_SHIFT	16
+#define VI6_RPF_SRC_ESIZE_EVSIZE_MASK	(0x1fff << 0)
+#define VI6_RPF_SRC_ESIZE_EVSIZE_SHIFT	0
+
+#define VI6_RPF_INFMT			0x0308
+#define VI6_RPF_INFMT_VIR		(1 << 28)
+#define VI6_RPF_INFMT_CIPM		(1 << 16)
+#define VI6_RPF_INFMT_SPYCS		(1 << 15)
+#define VI6_RPF_INFMT_SPUVS		(1 << 14)
+#define VI6_RPF_INFMT_CEXT_ZERO		(0 << 12)
+#define VI6_RPF_INFMT_CEXT_EXT		(1 << 12)
+#define VI6_RPF_INFMT_CEXT_ONE		(2 << 12)
+#define VI6_RPF_INFMT_CEXT_MASK		(3 << 12)
+#define VI6_RPF_INFMT_RDTM_BT601	(0 << 9)
+#define VI6_RPF_INFMT_RDTM_BT601_EXT	(1 << 9)
+#define VI6_RPF_INFMT_RDTM_BT709	(2 << 9)
+#define VI6_RPF_INFMT_RDTM_BT709_EXT	(3 << 9)
+#define VI6_RPF_INFMT_RDTM_MASK		(7 << 9)
+#define VI6_RPF_INFMT_CSC		(1 << 8)
+#define VI6_RPF_INFMT_RDFMT_MASK	(0x7f << 0)
+#define VI6_RPF_INFMT_RDFMT_SHIFT	0
+
+#define VI6_RPF_DSWAP			0x030c
+#define VI6_RPF_DSWAP_A_LLS		(1 << 11)
+#define VI6_RPF_DSWAP_A_LWS		(1 << 10)
+#define VI6_RPF_DSWAP_A_WDS		(1 << 9)
+#define VI6_RPF_DSWAP_A_BTS		(1 << 8)
+#define VI6_RPF_DSWAP_P_LLS		(1 << 3)
+#define VI6_RPF_DSWAP_P_LWS		(1 << 2)
+#define VI6_RPF_DSWAP_P_WDS		(1 << 1)
+#define VI6_RPF_DSWAP_P_BTS		(1 << 0)
+
+#define VI6_RPF_LOC			0x0310
+#define VI6_RPF_LOC_HCOORD_MASK		(0x1fff << 16)
+#define VI6_RPF_LOC_HCOORD_SHIFT	16
+#define VI6_RPF_LOC_VCOORD_MASK		(0x1fff << 0)
+#define VI6_RPF_LOC_VCOORD_SHIFT	0
+
+#define VI6_RPF_ALPH_SEL		0x0314
+#define VI6_RPF_ALPH_SEL_ASEL_PACKED	(0 << 28)
+#define VI6_RPF_ALPH_SEL_ASEL_8B_PLANE	(1 << 28)
+#define VI6_RPF_ALPH_SEL_ASEL_SELECT	(2 << 28)
+#define VI6_RPF_ALPH_SEL_ASEL_1B_PLANE	(3 << 28)
+#define VI6_RPF_ALPH_SEL_ASEL_FIXED	(4 << 28)
+#define VI6_RPF_ALPH_SEL_ASEL_MASK	(7 << 28)
+#define VI6_RPF_ALPH_SEL_ASEL_SHIFT	28
+#define VI6_RPF_ALPH_SEL_IROP_MASK	(0xf << 24)
+#define VI6_RPF_ALPH_SEL_IROP_SHIFT	24
+#define VI6_RPF_ALPH_SEL_BSEL		(1 << 23)
+#define VI6_RPF_ALPH_SEL_AEXT_ZERO	(0 << 18)
+#define VI6_RPF_ALPH_SEL_AEXT_EXT	(1 << 18)
+#define VI6_RPF_ALPH_SEL_AEXT_ONE	(2 << 18)
+#define VI6_RPF_ALPH_SEL_AEXT_MASK	(3 << 18)
+#define VI6_RPF_ALPH_SEL_ALPHA0_MASK	(0xff << 8)
+#define VI6_RPF_ALPH_SEL_ALPHA0_SHIFT	8
+#define VI6_RPF_ALPH_SEL_ALPHA1_MASK	(0xff << 0)
+#define VI6_RPF_ALPH_SEL_ALPHA1_SHIFT	0
+
+#define VI6_RPF_VRTCOL_SET		0x0318
+#define VI6_RPF_VRTCOL_SET_LAYA_MASK	(0xff << 24)
+#define VI6_RPF_VRTCOL_SET_LAYA_SHIFT	24
+#define VI6_RPF_VRTCOL_SET_LAYR_MASK	(0xff << 16)
+#define VI6_RPF_VRTCOL_SET_LAYR_SHIFT	16
+#define VI6_RPF_VRTCOL_SET_LAYG_MASK	(0xff << 8)
+#define VI6_RPF_VRTCOL_SET_LAYG_SHIFT	8
+#define VI6_RPF_VRTCOL_SET_LAYB_MASK	(0xff << 0)
+#define VI6_RPF_VRTCOL_SET_LAYB_SHIFT	0
+
+#define VI6_RPF_MSK_CTRL		0x031c
+#define VI6_RPF_MSK_CTRL_MSK_EN		(1 << 24)
+#define VI6_RPF_MSK_CTRL_MGR_MASK	(0xff << 16)
+#define VI6_RPF_MSK_CTRL_MGR_SHIFT	16
+#define VI6_RPF_MSK_CTRL_MGG_MASK	(0xff << 8)
+#define VI6_RPF_MSK_CTRL_MGG_SHIFT	8
+#define VI6_RPF_MSK_CTRL_MGB_MASK	(0xff << 0)
+#define VI6_RPF_MSK_CTRL_MGB_SHIFT	0
+
+#define VI6_RPF_MSK_SET0		0x0320
+#define VI6_RPF_MSK_SET1		0x0324
+#define VI6_RPF_MSK_SET_MSA_MASK	(0xff << 24)
+#define VI6_RPF_MSK_SET_MSA_SHIFT	24
+#define VI6_RPF_MSK_SET_MSR_MASK	(0xff << 16)
+#define VI6_RPF_MSK_SET_MSR_SHIFT	16
+#define VI6_RPF_MSK_SET_MSG_MASK	(0xff << 8)
+#define VI6_RPF_MSK_SET_MSG_SHIFT	8
+#define VI6_RPF_MSK_SET_MSB_MASK	(0xff << 0)
+#define VI6_RPF_MSK_SET_MSB_SHIFT	0
+
+#define VI6_RPF_CKEY_CTRL		0x0328
+#define VI6_RPF_CKEY_CTRL_CV		(1 << 4)
+#define VI6_RPF_CKEY_CTRL_SAPE1		(1 << 1)
+#define VI6_RPF_CKEY_CTRL_SAPE0		(1 << 0)
+
+#define VI6_RPF_CKEY_SET0		0x032c
+#define VI6_RPF_CKEY_SET1		0x0330
+#define VI6_RPF_CKEY_SET_AP_MASK	(0xff << 24)
+#define VI6_RPF_CKEY_SET_AP_SHIFT	24
+#define VI6_RPF_CKEY_SET_R_MASK		(0xff << 16)
+#define VI6_RPF_CKEY_SET_R_SHIFT	16
+#define VI6_RPF_CKEY_SET_GY_MASK	(0xff << 8)
+#define VI6_RPF_CKEY_SET_GY_SHIFT	8
+#define VI6_RPF_CKEY_SET_B_MASK		(0xff << 0)
+#define VI6_RPF_CKEY_SET_B_SHIFT	0
+
+#define VI6_RPF_SRCM_PSTRIDE		0x0334
+#define VI6_RPF_SRCM_PSTRIDE_Y_SHIFT	16
+#define VI6_RPF_SRCM_PSTRIDE_C_SHIFT	0
+
+#define VI6_RPF_SRCM_ASTRIDE		0x0338
+#define VI6_RPF_SRCM_PSTRIDE_A_SHIFT	0
+
+#define VI6_RPF_SRCM_ADDR_Y		0x033c
+#define VI6_RPF_SRCM_ADDR_C0		0x0340
+#define VI6_RPF_SRCM_ADDR_C1		0x0344
+#define VI6_RPF_SRCM_ADDR_AI		0x0348
+
+/* -----------------------------------------------------------------------------
+ * WPF Control Registers
+ */
+
+#define VI6_WPF_OFFSET			0x100
+
+#define VI6_WPF_SRCRPF			0x1000
+#define VI6_WPF_SRCRPF_VIRACT_DIS	(0 << 28)
+#define VI6_WPF_SRCRPF_VIRACT_SUB	(1 << 28)
+#define VI6_WPF_SRCRPF_VIRACT_MST	(2 << 28)
+#define VI6_WPF_SRCRPF_VIRACT_MASK	(3 << 28)
+#define VI6_WPF_SRCRPF_RPF_ACT_DIS(n)	(0 << ((n) * 2))
+#define VI6_WPF_SRCRPF_RPF_ACT_SUB(n)	(1 << ((n) * 2))
+#define VI6_WPF_SRCRPF_RPF_ACT_MST(n)	(2 << ((n) * 2))
+#define VI6_WPF_SRCRPF_RPF_ACT_MASK(n)	(3 << ((n) * 2))
+
+#define VI6_WPF_HSZCLIP			0x1004
+#define VI6_WPF_VSZCLIP			0x1008
+#define VI6_WPF_SZCLIP_EN		(1 << 28)
+#define VI6_WPF_SZCLIP_OFST_MASK	(0xff << 16)
+#define VI6_WPF_SZCLIP_OFST_SHIFT	16
+#define VI6_WPF_SZCLIP_SIZE_MASK	(0xfff << 0)
+#define VI6_WPF_SZCLIP_SIZE_SHIFT	0
+
+#define VI6_WPF_OUTFMT			0x100c
+#define VI6_WPF_OUTFMT_PDV_MASK		(0xff << 24)
+#define VI6_WPF_OUTFMT_PDV_SHIFT	24
+#define VI6_WPF_OUTFMT_PXA		(1 << 23)
+#define VI6_WPF_OUTFMT_FLP		(1 << 16)
+#define VI6_WPF_OUTFMT_SPYCS		(1 << 15)
+#define VI6_WPF_OUTFMT_SPUVS		(1 << 14)
+#define VI6_WPF_OUTFMT_DITH_DIS		(0 << 12)
+#define VI6_WPF_OUTFMT_DITH_EN		(3 << 12)
+#define VI6_WPF_OUTFMT_DITH_MASK	(3 << 12)
+#define VI6_WPF_OUTFMT_WRTM_BT601	(0 << 9)
+#define VI6_WPF_OUTFMT_WRTM_BT601_EXT	(1 << 9)
+#define VI6_WPF_OUTFMT_WRTM_BT709	(2 << 9)
+#define VI6_WPF_OUTFMT_WRTM_BT709_EXT	(3 << 9)
+#define VI6_WPF_OUTFMT_WRTM_MASK	(7 << 9)
+#define VI6_WPF_OUTFMT_CSC		(1 << 8)
+#define VI6_WPF_OUTFMT_WRFMT_MASK	(0x7f << 0)
+#define VI6_WPF_OUTFMT_WRFMT_SHIFT	0
+
+#define VI6_WPF_DSWAP			0x1010
+#define VI6_WPF_DSWAP_P_LLS		(1 << 3)
+#define VI6_WPF_DSWAP_P_LWS		(1 << 2)
+#define VI6_WPF_DSWAP_P_WDS		(1 << 1)
+#define VI6_WPF_DSWAP_P_BTS		(1 << 0)
+
+#define VI6_WPF_RNDCTRL			0x1014
+#define VI6_WPF_RNDCTRL_CBRM		(1 << 28)
+#define VI6_WPF_RNDCTRL_ABRM_TRUNC	(0 << 24)
+#define VI6_WPF_RNDCTRL_ABRM_ROUND	(1 << 24)
+#define VI6_WPF_RNDCTRL_ABRM_THRESH	(2 << 24)
+#define VI6_WPF_RNDCTRL_ABRM_MASK	(3 << 24)
+#define VI6_WPF_RNDCTRL_ATHRESH_MASK	(0xff << 16)
+#define VI6_WPF_RNDCTRL_ATHRESH_SHIFT	16
+#define VI6_WPF_RNDCTRL_CLMD_FULL	(0 << 12)
+#define VI6_WPF_RNDCTRL_CLMD_CLIP	(1 << 12)
+#define VI6_WPF_RNDCTRL_CLMD_EXT	(2 << 12)
+#define VI6_WPF_RNDCTRL_CLMD_MASK	(3 << 12)
+
+#define VI6_WPF_DSTM_STRIDE_Y		0x101c
+#define VI6_WPF_DSTM_STRIDE_C		0x1020
+#define VI6_WPF_DSTM_ADDR_Y		0x1024
+#define VI6_WPF_DSTM_ADDR_C0		0x1028
+#define VI6_WPF_DSTM_ADDR_C1		0x102c
+
+#define VI6_WPF_WRBCK_CTRL		0x1034
+#define VI6_WPF_WRBCK_CTRL_WBMD		(1 << 0)
+
+/* -----------------------------------------------------------------------------
+ * DPR Control Registers
+ */
+
+#define VI6_DPR_RPF_ROUTE(n)		(0x2000 + (n) * 4)
+
+#define VI6_DPR_WPF_FPORCH(n)		(0x2014 + (n) * 4)
+#define VI6_DPR_WPF_FPORCH_FP_WPFN	(5 << 8)
+
+#define VI6_DPR_SRU_ROUTE		0x2024
+#define VI6_DPR_UDS_ROUTE(n)		(0x2028 + (n) * 4)
+#define VI6_DPR_LUT_ROUTE		0x203c
+#define VI6_DPR_CLU_ROUTE		0x2040
+#define VI6_DPR_HST_ROUTE		0x2044
+#define VI6_DPR_HSI_ROUTE		0x2048
+#define VI6_DPR_BRU_ROUTE		0x204c
+#define VI6_DPR_ROUTE_FXA_MASK		(0xff << 16)
+#define VI6_DPR_ROUTE_FXA_SHIFT		16
+#define VI6_DPR_ROUTE_FP_MASK		(0x3f << 8)
+#define VI6_DPR_ROUTE_FP_SHIFT		8
+#define VI6_DPR_ROUTE_RT_MASK		(0x3f << 0)
+#define VI6_DPR_ROUTE_RT_SHIFT		0
+
+#define VI6_DPR_HGO_SMPPT		0x2050
+#define VI6_DPR_HGT_SMPPT		0x2054
+#define VI6_DPR_SMPPT_TGW_MASK		(7 << 8)
+#define VI6_DPR_SMPPT_TGW_SHIFT		8
+#define VI6_DPR_SMPPT_PT_MASK		(0x3f << 0)
+#define VI6_DPR_SMPPT_PT_SHIFT		0
+
+#define VI6_DPR_NODE_RPF(n)		(n)
+#define VI6_DPR_NODE_SRU		16
+#define VI6_DPR_NODE_UDS(n)		(17 + (n))
+#define VI6_DPR_NODE_LUT		22
+#define VI6_DPR_NODE_BRU_IN(n)		(23 + (n))
+#define VI6_DPR_NODE_BRU_OUT		27
+#define VI6_DPR_NODE_CLU		29
+#define VI6_DPR_NODE_HST		30
+#define VI6_DPR_NODE_HSI		31
+#define VI6_DPR_NODE_LIF		55
+#define VI6_DPR_NODE_WPF(n)		(56 + (n))
+#define VI6_DPR_NODE_UNUSED		63
+
+/* -----------------------------------------------------------------------------
+ * SRU Control Registers
+ */
+
+#define VI6_SRU_CTRL0			0x2200
+#define VI6_SRU_CTRL0_PARAM0_MASK	(0x1ff << 16)
+#define VI6_SRU_CTRL0_PARAM0_SHIFT	16
+#define VI6_SRU_CTRL0_PARAM1_MASK	(0x1f << 8)
+#define VI6_SRU_CTRL0_PARAM1_SHIFT	8
+#define VI6_SRU_CTRL0_MODE_UPSCALE	(4 << 4)
+#define VI6_SRU_CTRL0_PARAM2		(1 << 3)
+#define VI6_SRU_CTRL0_PARAM3		(1 << 2)
+#define VI6_SRU_CTRL0_PARAM4		(1 << 1)
+#define VI6_SRU_CTRL0_EN		(1 << 0)
+
+#define VI6_SRU_CTRL1			0x2204
+#define VI6_SRU_CTRL1_PARAM5		0x7ff
+
+#define VI6_SRU_CTRL2			0x2208
+#define VI6_SRU_CTRL2_PARAM6_SHIFT	16
+#define VI6_SRU_CTRL2_PARAM7_SHIFT	8
+#define VI6_SRU_CTRL2_PARAM8_SHIFT	0
+
+/* -----------------------------------------------------------------------------
+ * UDS Control Registers
+ */
+
+#define VI6_UDS_OFFSET			0x100
+
+#define VI6_UDS_CTRL			0x2300
+#define VI6_UDS_CTRL_AMD		(1 << 30)
+#define VI6_UDS_CTRL_FMD		(1 << 29)
+#define VI6_UDS_CTRL_BLADV		(1 << 28)
+#define VI6_UDS_CTRL_AON		(1 << 25)
+#define VI6_UDS_CTRL_ATHON		(1 << 24)
+#define VI6_UDS_CTRL_BC			(1 << 20)
+#define VI6_UDS_CTRL_NE_A		(1 << 19)
+#define VI6_UDS_CTRL_NE_RCR		(1 << 18)
+#define VI6_UDS_CTRL_NE_GY		(1 << 17)
+#define VI6_UDS_CTRL_NE_BCB		(1 << 16)
+#define VI6_UDS_CTRL_TDIPC		(1 << 1)
+
+#define VI6_UDS_SCALE			0x2304
+#define VI6_UDS_SCALE_HMANT_MASK	(0xf << 28)
+#define VI6_UDS_SCALE_HMANT_SHIFT	28
+#define VI6_UDS_SCALE_HFRAC_MASK	(0xfff << 16)
+#define VI6_UDS_SCALE_HFRAC_SHIFT	16
+#define VI6_UDS_SCALE_VMANT_MASK	(0xf << 12)
+#define VI6_UDS_SCALE_VMANT_SHIFT	12
+#define VI6_UDS_SCALE_VFRAC_MASK	(0xfff << 0)
+#define VI6_UDS_SCALE_VFRAC_SHIFT	0
+
+#define VI6_UDS_ALPTH			0x2308
+#define VI6_UDS_ALPTH_TH1_MASK		(0xff << 8)
+#define VI6_UDS_ALPTH_TH1_SHIFT		8
+#define VI6_UDS_ALPTH_TH0_MASK		(0xff << 0)
+#define VI6_UDS_ALPTH_TH0_SHIFT		0
+
+#define VI6_UDS_ALPVAL			0x230c
+#define VI6_UDS_ALPVAL_VAL2_MASK	(0xff << 16)
+#define VI6_UDS_ALPVAL_VAL2_SHIFT	16
+#define VI6_UDS_ALPVAL_VAL1_MASK	(0xff << 8)
+#define VI6_UDS_ALPVAL_VAL1_SHIFT	8
+#define VI6_UDS_ALPVAL_VAL0_MASK	(0xff << 0)
+#define VI6_UDS_ALPVAL_VAL0_SHIFT	0
+
+#define VI6_UDS_PASS_BWIDTH		0x2310
+#define VI6_UDS_PASS_BWIDTH_H_MASK	(0x7f << 16)
+#define VI6_UDS_PASS_BWIDTH_H_SHIFT	16
+#define VI6_UDS_PASS_BWIDTH_V_MASK	(0x7f << 0)
+#define VI6_UDS_PASS_BWIDTH_V_SHIFT	0
+
+#define VI6_UDS_IPC			0x2318
+#define VI6_UDS_IPC_FIELD		(1 << 27)
+#define VI6_UDS_IPC_VEDP_MASK		(0xfff << 0)
+#define VI6_UDS_IPC_VEDP_SHIFT		0
+
+#define VI6_UDS_CLIP_SIZE		0x2324
+#define VI6_UDS_CLIP_SIZE_HSIZE_MASK	(0x1fff << 16)
+#define VI6_UDS_CLIP_SIZE_HSIZE_SHIFT	16
+#define VI6_UDS_CLIP_SIZE_VSIZE_MASK	(0x1fff << 0)
+#define VI6_UDS_CLIP_SIZE_VSIZE_SHIFT	0
+
+#define VI6_UDS_FILL_COLOR		0x2328
+#define VI6_UDS_FILL_COLOR_RFILC_MASK	(0xff << 16)
+#define VI6_UDS_FILL_COLOR_RFILC_SHIFT	16
+#define VI6_UDS_FILL_COLOR_GFILC_MASK	(0xff << 8)
+#define VI6_UDS_FILL_COLOR_GFILC_SHIFT	8
+#define VI6_UDS_FILL_COLOR_BFILC_MASK	(0xff << 0)
+#define VI6_UDS_FILL_COLOR_BFILC_SHIFT	0
+
+/* -----------------------------------------------------------------------------
+ * LUT Control Registers
+ */
+
+#define VI6_LUT_CTRL			0x2800
+#define VI6_LUT_CTRL_EN			(1 << 0)
+
+/* -----------------------------------------------------------------------------
+ * CLU Control Registers
+ */
+
+#define VI6_CLU_CTRL			0x2900
+
+/* -----------------------------------------------------------------------------
+ * HST Control Registers
+ */
+
+#define VI6_HST_CTRL			0x2a00
+#define VI6_HST_CTRL_EN			(1 << 0)
+
+/* -----------------------------------------------------------------------------
+ * HSI Control Registers
+ */
+
+#define VI6_HSI_CTRL			0x2b00
+#define VI6_HSI_CTRL_EN			(1 << 0)
+
+/* -----------------------------------------------------------------------------
+ * BRU Control Registers
+ */
+
+#define VI6_ROP_NOP			0
+#define VI6_ROP_AND			1
+#define VI6_ROP_AND_REV			2
+#define VI6_ROP_COPY			3
+#define VI6_ROP_AND_INV			4
+#define VI6_ROP_CLEAR			5
+#define VI6_ROP_XOR			6
+#define VI6_ROP_OR			7
+#define VI6_ROP_NOR			8
+#define VI6_ROP_EQUIV			9
+#define VI6_ROP_INVERT			10
+#define VI6_ROP_OR_REV			11
+#define VI6_ROP_COPY_INV		12
+#define VI6_ROP_OR_INV			13
+#define VI6_ROP_NAND			14
+#define VI6_ROP_SET			15
+
+#define VI6_BRU_INCTRL			0x2c00
+#define VI6_BRU_INCTRL_NRM		(1 << 28)
+#define VI6_BRU_INCTRL_DnON		(1 << (16 + (n)))
+#define VI6_BRU_INCTRL_DITHn_OFF	(0 << ((n) * 4))
+#define VI6_BRU_INCTRL_DITHn_18BPP	(1 << ((n) * 4))
+#define VI6_BRU_INCTRL_DITHn_16BPP	(2 << ((n) * 4))
+#define VI6_BRU_INCTRL_DITHn_15BPP	(3 << ((n) * 4))
+#define VI6_BRU_INCTRL_DITHn_12BPP	(4 << ((n) * 4))
+#define VI6_BRU_INCTRL_DITHn_8BPP	(5 << ((n) * 4))
+#define VI6_BRU_INCTRL_DITHn_MASK	(7 << ((n) * 4))
+#define VI6_BRU_INCTRL_DITHn_SHIFT	((n) * 4)
+
+#define VI6_BRU_VIRRPF_SIZE		0x2c04
+#define VI6_BRU_VIRRPF_SIZE_HSIZE_MASK	(0x1fff << 16)
+#define VI6_BRU_VIRRPF_SIZE_HSIZE_SHIFT	16
+#define VI6_BRU_VIRRPF_SIZE_VSIZE_MASK	(0x1fff << 0)
+#define VI6_BRU_VIRRPF_SIZE_VSIZE_SHIFT	0
+
+#define VI6_BRU_VIRRPF_LOC		0x2c08
+#define VI6_BRU_VIRRPF_LOC_HCOORD_MASK	(0x1fff << 16)
+#define VI6_BRU_VIRRPF_LOC_HCOORD_SHIFT	16
+#define VI6_BRU_VIRRPF_LOC_VCOORD_MASK	(0x1fff << 0)
+#define VI6_BRU_VIRRPF_LOC_VCOORD_SHIFT	0
+
+#define VI6_BRU_VIRRPF_COL		0x2c0c
+#define VI6_BRU_VIRRPF_COL_A_MASK	(0xff << 24)
+#define VI6_BRU_VIRRPF_COL_A_SHIFT	24
+#define VI6_BRU_VIRRPF_COL_RCR_MASK	(0xff << 16)
+#define VI6_BRU_VIRRPF_COL_RCR_SHIFT	16
+#define VI6_BRU_VIRRPF_COL_GY_MASK	(0xff << 8)
+#define VI6_BRU_VIRRPF_COL_GY_SHIFT	8
+#define VI6_BRU_VIRRPF_COL_BCB_MASK	(0xff << 0)
+#define VI6_BRU_VIRRPF_COL_BCB_SHIFT	0
+
+#define VI6_BRU_CTRL(n)			(0x2c10 + (n) * 8)
+#define VI6_BRU_CTRL_RBC		(1 << 31)
+#define VI6_BRU_CTRL_DSTSEL_BRUIN(n)	((n) << 20)
+#define VI6_BRU_CTRL_DSTSEL_VRPF	(4 << 20)
+#define VI6_BRU_CTRL_DSTSEL_MASK	(7 << 20)
+#define VI6_BRU_CTRL_SRCSEL_BRUIN(n)	((n) << 16)
+#define VI6_BRU_CTRL_SRCSEL_VRPF	(4 << 16)
+#define VI6_BRU_CTRL_SRCSEL_MASK	(7 << 16)
+#define VI6_BRU_CTRL_CROP(rop)		((rop) << 4)
+#define VI6_BRU_CTRL_CROP_MASK		(0xf << 4)
+#define VI6_BRU_CTRL_AROP(rop)		((rop) << 0)
+#define VI6_BRU_CTRL_AROP_MASK		(0xf << 0)
+
+#define VI6_BRU_BLD(n)			(0x2c14 + (n) * 8)
+#define VI6_BRU_BLD_CBES		(1 << 31)
+#define VI6_BRU_BLD_CCMDX_DST_A		(0 << 28)
+#define VI6_BRU_BLD_CCMDX_255_DST_A	(1 << 28)
+#define VI6_BRU_BLD_CCMDX_SRC_A		(2 << 28)
+#define VI6_BRU_BLD_CCMDX_255_SRC_A	(3 << 28)
+#define VI6_BRU_BLD_CCMDX_COEFX		(4 << 28)
+#define VI6_BRU_BLD_CCMDX_MASK		(7 << 28)
+#define VI6_BRU_BLD_CCMDY_DST_A		(0 << 24)
+#define VI6_BRU_BLD_CCMDY_255_DST_A	(1 << 24)
+#define VI6_BRU_BLD_CCMDY_SRC_A		(2 << 24)
+#define VI6_BRU_BLD_CCMDY_255_SRC_A	(3 << 24)
+#define VI6_BRU_BLD_CCMDY_COEFY		(4 << 24)
+#define VI6_BRU_BLD_CCMDY_MASK		(7 << 24)
+#define VI6_BRU_BLD_CCMDY_SHIFT		24
+#define VI6_BRU_BLD_ABES		(1 << 23)
+#define VI6_BRU_BLD_ACMDX_DST_A		(0 << 20)
+#define VI6_BRU_BLD_ACMDX_255_DST_A	(1 << 20)
+#define VI6_BRU_BLD_ACMDX_SRC_A		(2 << 20)
+#define VI6_BRU_BLD_ACMDX_255_SRC_A	(3 << 20)
+#define VI6_BRU_BLD_ACMDX_COEFX		(4 << 20)
+#define VI6_BRU_BLD_ACMDX_MASK		(7 << 20)
+#define VI6_BRU_BLD_ACMDY_DST_A		(0 << 16)
+#define VI6_BRU_BLD_ACMDY_255_DST_A	(1 << 16)
+#define VI6_BRU_BLD_ACMDY_SRC_A		(2 << 16)
+#define VI6_BRU_BLD_ACMDY_255_SRC_A	(3 << 16)
+#define VI6_BRU_BLD_ACMDY_COEFY		(4 << 16)
+#define VI6_BRU_BLD_ACMDY_MASK		(7 << 16)
+#define VI6_BRU_BLD_COEFX_MASK		(0xff << 8)
+#define VI6_BRU_BLD_COEFX_SHIFT		8
+#define VI6_BRU_BLD_COEFY_MASK		(0xff << 0)
+#define VI6_BRU_BLD_COEFY_SHIFT		0
+
+#define VI6_BRU_ROP			0x2c30
+#define VI6_BRU_ROP_DSTSEL_BRUIN(n)	((n) << 20)
+#define VI6_BRU_ROP_DSTSEL_VRPF		(4 << 20)
+#define VI6_BRU_ROP_DSTSEL_MASK		(7 << 20)
+#define VI6_BRU_ROP_CROP(rop)		((rop) << 4)
+#define VI6_BRU_ROP_CROP_MASK		(0xf << 4)
+#define VI6_BRU_ROP_AROP(rop)		((rop) << 0)
+#define VI6_BRU_ROP_AROP_MASK		(0xf << 0)
+
+/* -----------------------------------------------------------------------------
+ * HGO Control Registers
+ */
+
+#define VI6_HGO_OFFSET			0x3000
+#define VI6_HGO_SIZE			0x3004
+#define VI6_HGO_MODE			0x3008
+#define VI6_HGO_LB_TH			0x300c
+#define VI6_HGO_LBn_H(n)		(0x3010 + (n) * 8)
+#define VI6_HGO_LBn_V(n)		(0x3014 + (n) * 8)
+#define VI6_HGO_R_HISTO			0x3030
+#define VI6_HGO_R_MAXMIN		0x3130
+#define VI6_HGO_R_SUM			0x3134
+#define VI6_HGO_R_LB_DET		0x3138
+#define VI6_HGO_G_HISTO			0x3140
+#define VI6_HGO_G_MAXMIN		0x3240
+#define VI6_HGO_G_SUM			0x3244
+#define VI6_HGO_G_LB_DET		0x3248
+#define VI6_HGO_B_HISTO			0x3250
+#define VI6_HGO_B_MAXMIN		0x3350
+#define VI6_HGO_B_SUM			0x3354
+#define VI6_HGO_B_LB_DET		0x3358
+#define VI6_HGO_REGRST			0x33fc
+
+/* -----------------------------------------------------------------------------
+ * HGT Control Registers
+ */
+
+#define VI6_HGT_OFFSET			0x3400
+#define VI6_HGT_SIZE			0x3404
+#define VI6_HGT_MODE			0x3408
+#define VI6_HGT_HUE_AREA(n)		(0x340c + (n) * 4)
+#define VI6_HGT_LB_TH			0x3424
+#define VI6_HGT_LBn_H(n)		(0x3438 + (n) * 8)
+#define VI6_HGT_LBn_V(n)		(0x342c + (n) * 8)
+#define VI6_HGT_HISTO(m, n)		(0x3450 + (m) * 128 + (n) * 4)
+#define VI6_HGT_MAXMIN			0x3750
+#define VI6_HGT_SUM			0x3754
+#define VI6_HGT_LB_DET			0x3758
+#define VI6_HGT_REGRST			0x37fc
+
+/* -----------------------------------------------------------------------------
+ * LIF Control Registers
+ */
+
+#define VI6_LIF_CTRL			0x3b00
+#define VI6_LIF_CTRL_OBTH_MASK		(0x7ff << 16)
+#define VI6_LIF_CTRL_OBTH_SHIFT		16
+#define VI6_LIF_CTRL_CFMT		(1 << 4)
+#define VI6_LIF_CTRL_REQSEL		(1 << 1)
+#define VI6_LIF_CTRL_LIF_EN		(1 << 0)
+
+#define VI6_LIF_CSBTH			0x3b04
+#define VI6_LIF_CSBTH_HBTH_MASK		(0x7ff << 16)
+#define VI6_LIF_CSBTH_HBTH_SHIFT	16
+#define VI6_LIF_CSBTH_LBTH_MASK		(0x7ff << 0)
+#define VI6_LIF_CSBTH_LBTH_SHIFT	0
+
+/* -----------------------------------------------------------------------------
+ * Security Control Registers
+ */
+
+#define VI6_SECURITY_CTRL0		0x3d00
+#define VI6_SECURITY_CTRL1		0x3d04
+
+/* -----------------------------------------------------------------------------
+ * RPF CLUT Registers
+ */
+
+#define VI6_CLUT_TABLE			0x4000
+
+/* -----------------------------------------------------------------------------
+ * 1D LUT Registers
+ */
+
+#define VI6_LUT_TABLE			0x7000
+
+/* -----------------------------------------------------------------------------
+ * 3D LUT Registers
+ */
+
+#define VI6_CLU_ADDR			0x7400
+#define VI6_CLU_DATA			0x7404
+
+/* -----------------------------------------------------------------------------
+ * Formats
+ */
+
+#define VI6_FMT_RGB_332			0x00
+#define VI6_FMT_XRGB_4444		0x01
+#define VI6_FMT_RGBX_4444		0x02
+#define VI6_FMT_XRGB_1555		0x04
+#define VI6_FMT_RGBX_5551		0x05
+#define VI6_FMT_RGB_565			0x06
+#define VI6_FMT_AXRGB_86666		0x07
+#define VI6_FMT_RGBXA_66668		0x08
+#define VI6_FMT_XRGBA_66668		0x09
+#define VI6_FMT_ARGBX_86666		0x0a
+#define VI6_FMT_AXRXGXB_8262626		0x0b
+#define VI6_FMT_XRXGXBA_2626268		0x0c
+#define VI6_FMT_ARXGXBX_8626262		0x0d
+#define VI6_FMT_RXGXBXA_6262628		0x0e
+#define VI6_FMT_XRGB_6666		0x0f
+#define VI6_FMT_RGBX_6666		0x10
+#define VI6_FMT_XRXGXB_262626		0x11
+#define VI6_FMT_RXGXBX_626262		0x12
+#define VI6_FMT_ARGB_8888		0x13
+#define VI6_FMT_RGBA_8888		0x14
+#define VI6_FMT_RGB_888			0x15
+#define VI6_FMT_XRGXGB_763763		0x16
+#define VI6_FMT_XXRGB_86666		0x17
+#define VI6_FMT_BGR_888			0x18
+#define VI6_FMT_ARGB_4444		0x19
+#define VI6_FMT_RGBA_4444		0x1a
+#define VI6_FMT_ARGB_1555		0x1b
+#define VI6_FMT_RGBA_5551		0x1c
+#define VI6_FMT_ABGR_4444		0x1d
+#define VI6_FMT_BGRA_4444		0x1e
+#define VI6_FMT_ABGR_1555		0x1f
+#define VI6_FMT_BGRA_5551		0x20
+#define VI6_FMT_XBXGXR_262626		0x21
+#define VI6_FMT_ABGR_8888		0x22
+#define VI6_FMT_XXRGB_88565		0x23
+
+#define VI6_FMT_Y_UV_444		0x40
+#define VI6_FMT_Y_UV_422		0x41
+#define VI6_FMT_Y_UV_420		0x42
+#define VI6_FMT_YUV_444			0x46
+#define VI6_FMT_YUYV_422		0x47
+#define VI6_FMT_YYUV_422		0x48
+#define VI6_FMT_YUV_420			0x49
+#define VI6_FMT_Y_U_V_444		0x4a
+#define VI6_FMT_Y_U_V_422		0x4b
+#define VI6_FMT_Y_U_V_420		0x4c
+
+#endif /* __VSP1_REGS_H__ */
diff --git a/drivers/media/platform/vsp1/vsp1_rpf.c b/drivers/media/platform/vsp1/vsp1_rpf.c
new file mode 100644
index 0000000..cd5248a
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_rpf.c
@@ -0,0 +1,294 @@
+/*
+ * vsp1_rpf.c  --  R-Car VSP1 Read Pixel Formatter
+ *
+ * Copyright (C) 2013-2014 Renesas Electronics Corporation
+ *
+ * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/device.h>
+
+#include <media/v4l2-subdev.h>
+
+#include "vsp1.h"
+#include "vsp1_rwpf.h"
+#include "vsp1_video.h"
+
+#define RPF_MAX_WIDTH				8190
+#define RPF_MAX_HEIGHT				8190
+
+/* -----------------------------------------------------------------------------
+ * Device Access
+ */
+
+static inline u32 vsp1_rpf_read(struct vsp1_rwpf *rpf, u32 reg)
+{
+	return vsp1_read(rpf->entity.vsp1,
+			 reg + rpf->entity.index * VI6_RPF_OFFSET);
+}
+
+static inline void vsp1_rpf_write(struct vsp1_rwpf *rpf, u32 reg, u32 data)
+{
+	vsp1_write(rpf->entity.vsp1,
+		   reg + rpf->entity.index * VI6_RPF_OFFSET, data);
+}
+
+/* -----------------------------------------------------------------------------
+ * Controls
+ */
+
+static int rpf_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vsp1_rwpf *rpf =
+		container_of(ctrl->handler, struct vsp1_rwpf, ctrls);
+	struct vsp1_pipeline *pipe;
+
+	if (!vsp1_entity_is_streaming(&rpf->entity))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_ALPHA_COMPONENT:
+		vsp1_rpf_write(rpf, VI6_RPF_VRTCOL_SET,
+			       ctrl->val << VI6_RPF_VRTCOL_SET_LAYA_SHIFT);
+
+		pipe = to_vsp1_pipeline(&rpf->entity.subdev.entity);
+		vsp1_pipeline_propagate_alpha(pipe, &rpf->entity, ctrl->val);
+		break;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops rpf_ctrl_ops = {
+	.s_ctrl = rpf_s_ctrl,
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Core Operations
+ */
+
+static int rpf_s_stream(struct v4l2_subdev *subdev, int enable)
+{
+	struct vsp1_rwpf *rpf = to_rwpf(subdev);
+	const struct vsp1_format_info *fmtinfo = rpf->video.fmtinfo;
+	const struct v4l2_pix_format_mplane *format = &rpf->video.format;
+	const struct v4l2_rect *crop = &rpf->crop;
+	u32 pstride;
+	u32 infmt;
+	int ret;
+
+	ret = vsp1_entity_set_streaming(&rpf->entity, enable);
+	if (ret < 0)
+		return ret;
+
+	if (!enable)
+		return 0;
+
+	/* Source size, stride and crop offsets.
+	 *
+	 * The crop offsets correspond to the location of the crop rectangle top
+	 * left corner in the plane buffer. Only two offsets are needed, as
+	 * planes 2 and 3 always have identical strides.
+	 */
+	vsp1_rpf_write(rpf, VI6_RPF_SRC_BSIZE,
+		       (crop->width << VI6_RPF_SRC_BSIZE_BHSIZE_SHIFT) |
+		       (crop->height << VI6_RPF_SRC_BSIZE_BVSIZE_SHIFT));
+	vsp1_rpf_write(rpf, VI6_RPF_SRC_ESIZE,
+		       (crop->width << VI6_RPF_SRC_ESIZE_EHSIZE_SHIFT) |
+		       (crop->height << VI6_RPF_SRC_ESIZE_EVSIZE_SHIFT));
+
+	rpf->offsets[0] = crop->top * format->plane_fmt[0].bytesperline
+			+ crop->left * fmtinfo->bpp[0] / 8;
+	pstride = format->plane_fmt[0].bytesperline
+		<< VI6_RPF_SRCM_PSTRIDE_Y_SHIFT;
+
+	vsp1_rpf_write(rpf, VI6_RPF_SRCM_ADDR_Y,
+		       rpf->buf_addr[0] + rpf->offsets[0]);
+
+	if (format->num_planes > 1) {
+		rpf->offsets[1] = crop->top * format->plane_fmt[1].bytesperline
+				+ crop->left * fmtinfo->bpp[1] / 8;
+		pstride |= format->plane_fmt[1].bytesperline
+			<< VI6_RPF_SRCM_PSTRIDE_C_SHIFT;
+
+		vsp1_rpf_write(rpf, VI6_RPF_SRCM_ADDR_C0,
+			       rpf->buf_addr[1] + rpf->offsets[1]);
+
+		if (format->num_planes > 2)
+			vsp1_rpf_write(rpf, VI6_RPF_SRCM_ADDR_C1,
+				       rpf->buf_addr[2] + rpf->offsets[1]);
+	}
+
+	vsp1_rpf_write(rpf, VI6_RPF_SRCM_PSTRIDE, pstride);
+
+	/* Format */
+	infmt = VI6_RPF_INFMT_CIPM
+	      | (fmtinfo->hwfmt << VI6_RPF_INFMT_RDFMT_SHIFT);
+
+	if (fmtinfo->swap_yc)
+		infmt |= VI6_RPF_INFMT_SPYCS;
+	if (fmtinfo->swap_uv)
+		infmt |= VI6_RPF_INFMT_SPUVS;
+
+	if (rpf->entity.formats[RWPF_PAD_SINK].code !=
+	    rpf->entity.formats[RWPF_PAD_SOURCE].code)
+		infmt |= VI6_RPF_INFMT_CSC;
+
+	vsp1_rpf_write(rpf, VI6_RPF_INFMT, infmt);
+	vsp1_rpf_write(rpf, VI6_RPF_DSWAP, fmtinfo->swap);
+
+	/* Output location */
+	vsp1_rpf_write(rpf, VI6_RPF_LOC,
+		       (rpf->location.left << VI6_RPF_LOC_HCOORD_SHIFT) |
+		       (rpf->location.top << VI6_RPF_LOC_VCOORD_SHIFT));
+
+	/* Use the alpha channel (extended to 8 bits) when available or an
+	 * alpha value set through the V4L2_CID_ALPHA_COMPONENT control
+	 * otherwise. Disable color keying.
+	 */
+	vsp1_rpf_write(rpf, VI6_RPF_ALPH_SEL, VI6_RPF_ALPH_SEL_AEXT_EXT |
+		       (fmtinfo->alpha ? VI6_RPF_ALPH_SEL_ASEL_PACKED
+				       : VI6_RPF_ALPH_SEL_ASEL_FIXED));
+	vsp1_rpf_write(rpf, VI6_RPF_MSK_CTRL, 0);
+	vsp1_rpf_write(rpf, VI6_RPF_CKEY_CTRL, 0);
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Operations
+ */
+
+static struct v4l2_subdev_video_ops rpf_video_ops = {
+	.s_stream = rpf_s_stream,
+};
+
+static struct v4l2_subdev_pad_ops rpf_pad_ops = {
+	.enum_mbus_code = vsp1_rwpf_enum_mbus_code,
+	.enum_frame_size = vsp1_rwpf_enum_frame_size,
+	.get_fmt = vsp1_rwpf_get_format,
+	.set_fmt = vsp1_rwpf_set_format,
+	.get_selection = vsp1_rwpf_get_selection,
+	.set_selection = vsp1_rwpf_set_selection,
+};
+
+static struct v4l2_subdev_ops rpf_ops = {
+	.video	= &rpf_video_ops,
+	.pad    = &rpf_pad_ops,
+};
+
+/* -----------------------------------------------------------------------------
+ * Video Device Operations
+ */
+
+static void rpf_vdev_queue(struct vsp1_video *video,
+			   struct vsp1_video_buffer *buf)
+{
+	struct vsp1_rwpf *rpf = container_of(video, struct vsp1_rwpf, video);
+	unsigned int i;
+
+	for (i = 0; i < 3; ++i)
+		rpf->buf_addr[i] = buf->addr[i];
+
+	if (!vsp1_entity_is_streaming(&rpf->entity))
+		return;
+
+	vsp1_rpf_write(rpf, VI6_RPF_SRCM_ADDR_Y,
+		       buf->addr[0] + rpf->offsets[0]);
+	if (buf->buf.vb2_buf.num_planes > 1)
+		vsp1_rpf_write(rpf, VI6_RPF_SRCM_ADDR_C0,
+			       buf->addr[1] + rpf->offsets[1]);
+	if (buf->buf.vb2_buf.num_planes > 2)
+		vsp1_rpf_write(rpf, VI6_RPF_SRCM_ADDR_C1,
+			       buf->addr[2] + rpf->offsets[1]);
+}
+
+static const struct vsp1_video_operations rpf_vdev_ops = {
+	.queue = rpf_vdev_queue,
+};
+
+/* -----------------------------------------------------------------------------
+ * Initialization and Cleanup
+ */
+
+struct vsp1_rwpf *vsp1_rpf_create(struct vsp1_device *vsp1, unsigned int index)
+{
+	struct v4l2_subdev *subdev;
+	struct vsp1_video *video;
+	struct vsp1_rwpf *rpf;
+	int ret;
+
+	rpf = devm_kzalloc(vsp1->dev, sizeof(*rpf), GFP_KERNEL);
+	if (rpf == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	rpf->max_width = RPF_MAX_WIDTH;
+	rpf->max_height = RPF_MAX_HEIGHT;
+
+	rpf->entity.type = VSP1_ENTITY_RPF;
+	rpf->entity.index = index;
+
+	ret = vsp1_entity_init(vsp1, &rpf->entity, 2);
+	if (ret < 0)
+		return ERR_PTR(ret);
+
+	/* Initialize the V4L2 subdev. */
+	subdev = &rpf->entity.subdev;
+	v4l2_subdev_init(subdev, &rpf_ops);
+
+	subdev->entity.ops = &vsp1_media_ops;
+	subdev->internal_ops = &vsp1_subdev_internal_ops;
+	snprintf(subdev->name, sizeof(subdev->name), "%s rpf.%u",
+		 dev_name(vsp1->dev), index);
+	v4l2_set_subdevdata(subdev, rpf);
+	subdev->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	vsp1_entity_init_formats(subdev, NULL);
+
+	/* Initialize the control handler. */
+	v4l2_ctrl_handler_init(&rpf->ctrls, 1);
+	v4l2_ctrl_new_std(&rpf->ctrls, &rpf_ctrl_ops, V4L2_CID_ALPHA_COMPONENT,
+			  0, 255, 1, 255);
+
+	rpf->entity.subdev.ctrl_handler = &rpf->ctrls;
+
+	if (rpf->ctrls.error) {
+		dev_err(vsp1->dev, "rpf%u: failed to initialize controls\n",
+			index);
+		ret = rpf->ctrls.error;
+		goto error;
+	}
+
+	/* Initialize the video device. */
+	video = &rpf->video;
+
+	video->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+	video->vsp1 = vsp1;
+	video->ops = &rpf_vdev_ops;
+
+	ret = vsp1_video_init(video, &rpf->entity);
+	if (ret < 0)
+		goto error;
+
+	rpf->entity.video = video;
+
+	/* Connect the video device to the RPF. */
+	ret = media_entity_create_link(&rpf->video.video.entity, 0,
+				       &rpf->entity.subdev.entity,
+				       RWPF_PAD_SINK,
+				       MEDIA_LNK_FL_ENABLED |
+				       MEDIA_LNK_FL_IMMUTABLE);
+	if (ret < 0)
+		goto error;
+
+	return rpf;
+
+error:
+	vsp1_entity_destroy(&rpf->entity);
+	return ERR_PTR(ret);
+}
diff --git a/drivers/media/platform/vsp1/vsp1_rwpf.c b/drivers/media/platform/vsp1/vsp1_rwpf.c
new file mode 100644
index 0000000..9688c21
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_rwpf.c
@@ -0,0 +1,232 @@
+/*
+ * vsp1_rwpf.c  --  R-Car VSP1 Read and Write Pixel Formatters
+ *
+ * Copyright (C) 2013-2014 Renesas Electronics Corporation
+ *
+ * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <media/v4l2-subdev.h>
+
+#include "vsp1.h"
+#include "vsp1_rwpf.h"
+#include "vsp1_video.h"
+
+#define RWPF_MIN_WIDTH				1
+#define RWPF_MIN_HEIGHT				1
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Pad Operations
+ */
+
+int vsp1_rwpf_enum_mbus_code(struct v4l2_subdev *subdev,
+			     struct v4l2_subdev_pad_config *cfg,
+			     struct v4l2_subdev_mbus_code_enum *code)
+{
+	static const unsigned int codes[] = {
+		MEDIA_BUS_FMT_ARGB8888_1X32,
+		MEDIA_BUS_FMT_AYUV8_1X32,
+	};
+
+	if (code->index >= ARRAY_SIZE(codes))
+		return -EINVAL;
+
+	code->code = codes[code->index];
+
+	return 0;
+}
+
+int vsp1_rwpf_enum_frame_size(struct v4l2_subdev *subdev,
+			      struct v4l2_subdev_pad_config *cfg,
+			      struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct vsp1_rwpf *rwpf = to_rwpf(subdev);
+	struct v4l2_mbus_framefmt *format;
+
+	format = vsp1_entity_get_pad_format(&rwpf->entity, cfg, fse->pad,
+					    fse->which);
+
+	if (fse->index || fse->code != format->code)
+		return -EINVAL;
+
+	if (fse->pad == RWPF_PAD_SINK) {
+		fse->min_width = RWPF_MIN_WIDTH;
+		fse->max_width = rwpf->max_width;
+		fse->min_height = RWPF_MIN_HEIGHT;
+		fse->max_height = rwpf->max_height;
+	} else {
+		/* The size on the source pad are fixed and always identical to
+		 * the size on the sink pad.
+		 */
+		fse->min_width = format->width;
+		fse->max_width = format->width;
+		fse->min_height = format->height;
+		fse->max_height = format->height;
+	}
+
+	return 0;
+}
+
+static struct v4l2_rect *
+vsp1_rwpf_get_crop(struct vsp1_rwpf *rwpf, struct v4l2_subdev_pad_config *cfg, u32 which)
+{
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_crop(&rwpf->entity.subdev, cfg, RWPF_PAD_SINK);
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return &rwpf->crop;
+	default:
+		return NULL;
+	}
+}
+
+int vsp1_rwpf_get_format(struct v4l2_subdev *subdev, struct v4l2_subdev_pad_config *cfg,
+			 struct v4l2_subdev_format *fmt)
+{
+	struct vsp1_rwpf *rwpf = to_rwpf(subdev);
+
+	fmt->format = *vsp1_entity_get_pad_format(&rwpf->entity, cfg, fmt->pad,
+						  fmt->which);
+
+	return 0;
+}
+
+int vsp1_rwpf_set_format(struct v4l2_subdev *subdev, struct v4l2_subdev_pad_config *cfg,
+			 struct v4l2_subdev_format *fmt)
+{
+	struct vsp1_rwpf *rwpf = to_rwpf(subdev);
+	struct v4l2_mbus_framefmt *format;
+	struct v4l2_rect *crop;
+
+	/* Default to YUV if the requested format is not supported. */
+	if (fmt->format.code != MEDIA_BUS_FMT_ARGB8888_1X32 &&
+	    fmt->format.code != MEDIA_BUS_FMT_AYUV8_1X32)
+		fmt->format.code = MEDIA_BUS_FMT_AYUV8_1X32;
+
+	format = vsp1_entity_get_pad_format(&rwpf->entity, cfg, fmt->pad,
+					    fmt->which);
+
+	if (fmt->pad == RWPF_PAD_SOURCE) {
+		/* The RWPF performs format conversion but can't scale, only the
+		 * format code can be changed on the source pad.
+		 */
+		format->code = fmt->format.code;
+		fmt->format = *format;
+		return 0;
+	}
+
+	format->code = fmt->format.code;
+	format->width = clamp_t(unsigned int, fmt->format.width,
+				RWPF_MIN_WIDTH, rwpf->max_width);
+	format->height = clamp_t(unsigned int, fmt->format.height,
+				 RWPF_MIN_HEIGHT, rwpf->max_height);
+	format->field = V4L2_FIELD_NONE;
+	format->colorspace = V4L2_COLORSPACE_SRGB;
+
+	fmt->format = *format;
+
+	/* Update the sink crop rectangle. */
+	crop = vsp1_rwpf_get_crop(rwpf, cfg, fmt->which);
+	crop->left = 0;
+	crop->top = 0;
+	crop->width = fmt->format.width;
+	crop->height = fmt->format.height;
+
+	/* Propagate the format to the source pad. */
+	format = vsp1_entity_get_pad_format(&rwpf->entity, cfg, RWPF_PAD_SOURCE,
+					    fmt->which);
+	*format = fmt->format;
+
+	return 0;
+}
+
+int vsp1_rwpf_get_selection(struct v4l2_subdev *subdev,
+			    struct v4l2_subdev_pad_config *cfg,
+			    struct v4l2_subdev_selection *sel)
+{
+	struct vsp1_rwpf *rwpf = to_rwpf(subdev);
+	struct v4l2_mbus_framefmt *format;
+
+	/* Cropping is implemented on the sink pad. */
+	if (sel->pad != RWPF_PAD_SINK)
+		return -EINVAL;
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		sel->r = *vsp1_rwpf_get_crop(rwpf, cfg, sel->which);
+		break;
+
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		format = vsp1_entity_get_pad_format(&rwpf->entity, cfg,
+						    RWPF_PAD_SINK, sel->which);
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = format->width;
+		sel->r.height = format->height;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+int vsp1_rwpf_set_selection(struct v4l2_subdev *subdev,
+			    struct v4l2_subdev_pad_config *cfg,
+			    struct v4l2_subdev_selection *sel)
+{
+	struct vsp1_rwpf *rwpf = to_rwpf(subdev);
+	struct v4l2_mbus_framefmt *format;
+	struct v4l2_rect *crop;
+
+	/* Cropping is implemented on the sink pad. */
+	if (sel->pad != RWPF_PAD_SINK)
+		return -EINVAL;
+
+	if (sel->target != V4L2_SEL_TGT_CROP)
+		return -EINVAL;
+
+	/* Make sure the crop rectangle is entirely contained in the image. The
+	 * WPF top and left offsets are limited to 255.
+	 */
+	format = vsp1_entity_get_pad_format(&rwpf->entity, cfg, RWPF_PAD_SINK,
+					    sel->which);
+
+	/* Restrict the crop rectangle coordinates to multiples of 2 to avoid
+	 * shifting the color plane.
+	 */
+	if (format->code == MEDIA_BUS_FMT_AYUV8_1X32) {
+		sel->r.left = ALIGN(sel->r.left, 2);
+		sel->r.top = ALIGN(sel->r.top, 2);
+		sel->r.width = round_down(sel->r.width, 2);
+		sel->r.height = round_down(sel->r.height, 2);
+	}
+
+	sel->r.left = min_t(unsigned int, sel->r.left, format->width - 2);
+	sel->r.top = min_t(unsigned int, sel->r.top, format->height - 2);
+	if (rwpf->entity.type == VSP1_ENTITY_WPF) {
+		sel->r.left = min_t(unsigned int, sel->r.left, 255);
+		sel->r.top = min_t(unsigned int, sel->r.top, 255);
+	}
+	sel->r.width = min_t(unsigned int, sel->r.width,
+			     format->width - sel->r.left);
+	sel->r.height = min_t(unsigned int, sel->r.height,
+			      format->height - sel->r.top);
+
+	crop = vsp1_rwpf_get_crop(rwpf, cfg, sel->which);
+	*crop = sel->r;
+
+	/* Propagate the format to the source pad. */
+	format = vsp1_entity_get_pad_format(&rwpf->entity, cfg, RWPF_PAD_SOURCE,
+					    sel->which);
+	format->width = crop->width;
+	format->height = crop->height;
+
+	return 0;
+}
diff --git a/drivers/media/platform/vsp1/vsp1_rwpf.h b/drivers/media/platform/vsp1/vsp1_rwpf.h
new file mode 100644
index 0000000..f452dce
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_rwpf.h
@@ -0,0 +1,70 @@
+/*
+ * vsp1_rwpf.h  --  R-Car VSP1 Read and Write Pixel Formatters
+ *
+ * Copyright (C) 2013-2014 Renesas Electronics Corporation
+ *
+ * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+#ifndef __VSP1_RWPF_H__
+#define __VSP1_RWPF_H__
+
+#include <media/media-entity.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-subdev.h>
+
+#include "vsp1.h"
+#include "vsp1_entity.h"
+#include "vsp1_video.h"
+
+#define RWPF_PAD_SINK				0
+#define RWPF_PAD_SOURCE				1
+
+struct vsp1_rwpf {
+	struct vsp1_entity entity;
+	struct vsp1_video video;
+	struct v4l2_ctrl_handler ctrls;
+
+	unsigned int max_width;
+	unsigned int max_height;
+
+	struct {
+		unsigned int left;
+		unsigned int top;
+	} location;
+	struct v4l2_rect crop;
+
+	unsigned int offsets[2];
+	dma_addr_t buf_addr[3];
+};
+
+static inline struct vsp1_rwpf *to_rwpf(struct v4l2_subdev *subdev)
+{
+	return container_of(subdev, struct vsp1_rwpf, entity.subdev);
+}
+
+struct vsp1_rwpf *vsp1_rpf_create(struct vsp1_device *vsp1, unsigned int index);
+struct vsp1_rwpf *vsp1_wpf_create(struct vsp1_device *vsp1, unsigned int index);
+
+int vsp1_rwpf_enum_mbus_code(struct v4l2_subdev *subdev,
+			     struct v4l2_subdev_pad_config *cfg,
+			     struct v4l2_subdev_mbus_code_enum *code);
+int vsp1_rwpf_enum_frame_size(struct v4l2_subdev *subdev,
+			      struct v4l2_subdev_pad_config *cfg,
+			      struct v4l2_subdev_frame_size_enum *fse);
+int vsp1_rwpf_get_format(struct v4l2_subdev *subdev, struct v4l2_subdev_pad_config *cfg,
+			 struct v4l2_subdev_format *fmt);
+int vsp1_rwpf_set_format(struct v4l2_subdev *subdev, struct v4l2_subdev_pad_config *cfg,
+			 struct v4l2_subdev_format *fmt);
+int vsp1_rwpf_get_selection(struct v4l2_subdev *subdev,
+			    struct v4l2_subdev_pad_config *cfg,
+			    struct v4l2_subdev_selection *sel);
+int vsp1_rwpf_set_selection(struct v4l2_subdev *subdev,
+			    struct v4l2_subdev_pad_config *cfg,
+			    struct v4l2_subdev_selection *sel);
+
+#endif /* __VSP1_RWPF_H__ */
diff --git a/drivers/media/platform/vsp1/vsp1_sru.c b/drivers/media/platform/vsp1/vsp1_sru.c
new file mode 100644
index 0000000..d41ae95
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_sru.c
@@ -0,0 +1,387 @@
+/*
+ * vsp1_sru.c  --  R-Car VSP1 Super Resolution Unit
+ *
+ * Copyright (C) 2013 Renesas Corporation
+ *
+ * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/device.h>
+#include <linux/gfp.h>
+
+#include <media/v4l2-subdev.h>
+
+#include "vsp1.h"
+#include "vsp1_sru.h"
+
+#define SRU_MIN_SIZE				4U
+#define SRU_MAX_SIZE				8190U
+
+/* -----------------------------------------------------------------------------
+ * Device Access
+ */
+
+static inline u32 vsp1_sru_read(struct vsp1_sru *sru, u32 reg)
+{
+	return vsp1_read(sru->entity.vsp1, reg);
+}
+
+static inline void vsp1_sru_write(struct vsp1_sru *sru, u32 reg, u32 data)
+{
+	vsp1_write(sru->entity.vsp1, reg, data);
+}
+
+/* -----------------------------------------------------------------------------
+ * Controls
+ */
+
+#define V4L2_CID_VSP1_SRU_INTENSITY		(V4L2_CID_USER_BASE + 1)
+
+struct vsp1_sru_param {
+	u32 ctrl0;
+	u32 ctrl2;
+};
+
+#define VI6_SRU_CTRL0_PARAMS(p0, p1)			\
+	(((p0) << VI6_SRU_CTRL0_PARAM0_SHIFT) |		\
+	 ((p1) << VI6_SRU_CTRL0_PARAM1_SHIFT))
+
+#define VI6_SRU_CTRL2_PARAMS(p6, p7, p8)		\
+	(((p6) << VI6_SRU_CTRL2_PARAM6_SHIFT) |		\
+	 ((p7) << VI6_SRU_CTRL2_PARAM7_SHIFT) |		\
+	 ((p8) << VI6_SRU_CTRL2_PARAM8_SHIFT))
+
+static const struct vsp1_sru_param vsp1_sru_params[] = {
+	{
+		.ctrl0 = VI6_SRU_CTRL0_PARAMS(256, 4) | VI6_SRU_CTRL0_EN,
+		.ctrl2 = VI6_SRU_CTRL2_PARAMS(24, 40, 255),
+	}, {
+		.ctrl0 = VI6_SRU_CTRL0_PARAMS(256, 4) | VI6_SRU_CTRL0_EN,
+		.ctrl2 = VI6_SRU_CTRL2_PARAMS(8, 16, 255),
+	}, {
+		.ctrl0 = VI6_SRU_CTRL0_PARAMS(384, 5) | VI6_SRU_CTRL0_EN,
+		.ctrl2 = VI6_SRU_CTRL2_PARAMS(36, 60, 255),
+	}, {
+		.ctrl0 = VI6_SRU_CTRL0_PARAMS(384, 5) | VI6_SRU_CTRL0_EN,
+		.ctrl2 = VI6_SRU_CTRL2_PARAMS(12, 27, 255),
+	}, {
+		.ctrl0 = VI6_SRU_CTRL0_PARAMS(511, 6) | VI6_SRU_CTRL0_EN,
+		.ctrl2 = VI6_SRU_CTRL2_PARAMS(48, 80, 255),
+	}, {
+		.ctrl0 = VI6_SRU_CTRL0_PARAMS(511, 6) | VI6_SRU_CTRL0_EN,
+		.ctrl2 = VI6_SRU_CTRL2_PARAMS(16, 36, 255),
+	},
+};
+
+static int sru_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vsp1_sru *sru =
+		container_of(ctrl->handler, struct vsp1_sru, ctrls);
+	const struct vsp1_sru_param *param;
+	u32 value;
+
+	switch (ctrl->id) {
+	case V4L2_CID_VSP1_SRU_INTENSITY:
+		param = &vsp1_sru_params[ctrl->val - 1];
+
+		value = vsp1_sru_read(sru, VI6_SRU_CTRL0);
+		value &= ~(VI6_SRU_CTRL0_PARAM0_MASK |
+			   VI6_SRU_CTRL0_PARAM1_MASK);
+		value |= param->ctrl0;
+		vsp1_sru_write(sru, VI6_SRU_CTRL0, value);
+
+		vsp1_sru_write(sru, VI6_SRU_CTRL2, param->ctrl2);
+		break;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops sru_ctrl_ops = {
+	.s_ctrl = sru_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config sru_intensity_control = {
+	.ops = &sru_ctrl_ops,
+	.id = V4L2_CID_VSP1_SRU_INTENSITY,
+	.name = "Intensity",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.min = 1,
+	.max = 6,
+	.def = 1,
+	.step = 1,
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Core Operations
+ */
+
+static int sru_s_stream(struct v4l2_subdev *subdev, int enable)
+{
+	struct vsp1_sru *sru = to_sru(subdev);
+	struct v4l2_mbus_framefmt *input;
+	struct v4l2_mbus_framefmt *output;
+	u32 ctrl0;
+	int ret;
+
+	ret = vsp1_entity_set_streaming(&sru->entity, enable);
+	if (ret < 0)
+		return ret;
+
+	if (!enable)
+		return 0;
+
+	input = &sru->entity.formats[SRU_PAD_SINK];
+	output = &sru->entity.formats[SRU_PAD_SOURCE];
+
+	if (input->code == MEDIA_BUS_FMT_ARGB8888_1X32)
+		ctrl0 = VI6_SRU_CTRL0_PARAM2 | VI6_SRU_CTRL0_PARAM3
+		      | VI6_SRU_CTRL0_PARAM4;
+	else
+		ctrl0 = VI6_SRU_CTRL0_PARAM3;
+
+	if (input->width != output->width)
+		ctrl0 |= VI6_SRU_CTRL0_MODE_UPSCALE;
+
+	/* Take the control handler lock to ensure that the CTRL0 value won't be
+	 * changed behind our back by a set control operation.
+	 */
+	mutex_lock(sru->ctrls.lock);
+	ctrl0 |= vsp1_sru_read(sru, VI6_SRU_CTRL0)
+	       & (VI6_SRU_CTRL0_PARAM0_MASK | VI6_SRU_CTRL0_PARAM1_MASK);
+	vsp1_sru_write(sru, VI6_SRU_CTRL0, ctrl0);
+	mutex_unlock(sru->ctrls.lock);
+
+	vsp1_sru_write(sru, VI6_SRU_CTRL1, VI6_SRU_CTRL1_PARAM5);
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Pad Operations
+ */
+
+static int sru_enum_mbus_code(struct v4l2_subdev *subdev,
+			      struct v4l2_subdev_pad_config *cfg,
+			      struct v4l2_subdev_mbus_code_enum *code)
+{
+	static const unsigned int codes[] = {
+		MEDIA_BUS_FMT_ARGB8888_1X32,
+		MEDIA_BUS_FMT_AYUV8_1X32,
+	};
+	struct vsp1_sru *sru = to_sru(subdev);
+	struct v4l2_mbus_framefmt *format;
+
+	if (code->pad == SRU_PAD_SINK) {
+		if (code->index >= ARRAY_SIZE(codes))
+			return -EINVAL;
+
+		code->code = codes[code->index];
+	} else {
+		/* The SRU can't perform format conversion, the sink format is
+		 * always identical to the source format.
+		 */
+		if (code->index)
+			return -EINVAL;
+
+		format = vsp1_entity_get_pad_format(&sru->entity, cfg,
+						    SRU_PAD_SINK, code->which);
+		code->code = format->code;
+	}
+
+	return 0;
+}
+
+static int sru_enum_frame_size(struct v4l2_subdev *subdev,
+			       struct v4l2_subdev_pad_config *cfg,
+			       struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct vsp1_sru *sru = to_sru(subdev);
+	struct v4l2_mbus_framefmt *format;
+
+	format = vsp1_entity_get_pad_format(&sru->entity, cfg,
+					    SRU_PAD_SINK, fse->which);
+
+	if (fse->index || fse->code != format->code)
+		return -EINVAL;
+
+	if (fse->pad == SRU_PAD_SINK) {
+		fse->min_width = SRU_MIN_SIZE;
+		fse->max_width = SRU_MAX_SIZE;
+		fse->min_height = SRU_MIN_SIZE;
+		fse->max_height = SRU_MAX_SIZE;
+	} else {
+		fse->min_width = format->width;
+		fse->min_height = format->height;
+		if (format->width <= SRU_MAX_SIZE / 2 &&
+		    format->height <= SRU_MAX_SIZE / 2) {
+			fse->max_width = format->width * 2;
+			fse->max_height = format->height * 2;
+		} else {
+			fse->max_width = format->width;
+			fse->max_height = format->height;
+		}
+	}
+
+	return 0;
+}
+
+static int sru_get_format(struct v4l2_subdev *subdev, struct v4l2_subdev_pad_config *cfg,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct vsp1_sru *sru = to_sru(subdev);
+
+	fmt->format = *vsp1_entity_get_pad_format(&sru->entity, cfg, fmt->pad,
+						  fmt->which);
+
+	return 0;
+}
+
+static void sru_try_format(struct vsp1_sru *sru, struct v4l2_subdev_pad_config *cfg,
+			   unsigned int pad, struct v4l2_mbus_framefmt *fmt,
+			   enum v4l2_subdev_format_whence which)
+{
+	struct v4l2_mbus_framefmt *format;
+	unsigned int input_area;
+	unsigned int output_area;
+
+	switch (pad) {
+	case SRU_PAD_SINK:
+		/* Default to YUV if the requested format is not supported. */
+		if (fmt->code != MEDIA_BUS_FMT_ARGB8888_1X32 &&
+		    fmt->code != MEDIA_BUS_FMT_AYUV8_1X32)
+			fmt->code = MEDIA_BUS_FMT_AYUV8_1X32;
+
+		fmt->width = clamp(fmt->width, SRU_MIN_SIZE, SRU_MAX_SIZE);
+		fmt->height = clamp(fmt->height, SRU_MIN_SIZE, SRU_MAX_SIZE);
+		break;
+
+	case SRU_PAD_SOURCE:
+		/* The SRU can't perform format conversion. */
+		format = vsp1_entity_get_pad_format(&sru->entity, cfg,
+						    SRU_PAD_SINK, which);
+		fmt->code = format->code;
+
+		/* We can upscale by 2 in both direction, but not independently.
+		 * Compare the input and output rectangles areas (avoiding
+		 * integer overflows on the output): if the requested output
+		 * area is larger than 1.5^2 the input area upscale by two,
+		 * otherwise don't scale.
+		 */
+		input_area = format->width * format->height;
+		output_area = min(fmt->width, SRU_MAX_SIZE)
+			    * min(fmt->height, SRU_MAX_SIZE);
+
+		if (fmt->width <= SRU_MAX_SIZE / 2 &&
+		    fmt->height <= SRU_MAX_SIZE / 2 &&
+		    output_area > input_area * 9 / 4) {
+			fmt->width = format->width * 2;
+			fmt->height = format->height * 2;
+		} else {
+			fmt->width = format->width;
+			fmt->height = format->height;
+		}
+		break;
+	}
+
+	fmt->field = V4L2_FIELD_NONE;
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+}
+
+static int sru_set_format(struct v4l2_subdev *subdev, struct v4l2_subdev_pad_config *cfg,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct vsp1_sru *sru = to_sru(subdev);
+	struct v4l2_mbus_framefmt *format;
+
+	sru_try_format(sru, cfg, fmt->pad, &fmt->format, fmt->which);
+
+	format = vsp1_entity_get_pad_format(&sru->entity, cfg, fmt->pad,
+					    fmt->which);
+	*format = fmt->format;
+
+	if (fmt->pad == SRU_PAD_SINK) {
+		/* Propagate the format to the source pad. */
+		format = vsp1_entity_get_pad_format(&sru->entity, cfg,
+						    SRU_PAD_SOURCE, fmt->which);
+		*format = fmt->format;
+
+		sru_try_format(sru, cfg, SRU_PAD_SOURCE, format, fmt->which);
+	}
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Operations
+ */
+
+static struct v4l2_subdev_video_ops sru_video_ops = {
+	.s_stream = sru_s_stream,
+};
+
+static struct v4l2_subdev_pad_ops sru_pad_ops = {
+	.enum_mbus_code = sru_enum_mbus_code,
+	.enum_frame_size = sru_enum_frame_size,
+	.get_fmt = sru_get_format,
+	.set_fmt = sru_set_format,
+};
+
+static struct v4l2_subdev_ops sru_ops = {
+	.video	= &sru_video_ops,
+	.pad    = &sru_pad_ops,
+};
+
+/* -----------------------------------------------------------------------------
+ * Initialization and Cleanup
+ */
+
+struct vsp1_sru *vsp1_sru_create(struct vsp1_device *vsp1)
+{
+	struct v4l2_subdev *subdev;
+	struct vsp1_sru *sru;
+	int ret;
+
+	sru = devm_kzalloc(vsp1->dev, sizeof(*sru), GFP_KERNEL);
+	if (sru == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	sru->entity.type = VSP1_ENTITY_SRU;
+
+	ret = vsp1_entity_init(vsp1, &sru->entity, 2);
+	if (ret < 0)
+		return ERR_PTR(ret);
+
+	/* Initialize the V4L2 subdev. */
+	subdev = &sru->entity.subdev;
+	v4l2_subdev_init(subdev, &sru_ops);
+
+	subdev->entity.ops = &vsp1_media_ops;
+	subdev->internal_ops = &vsp1_subdev_internal_ops;
+	snprintf(subdev->name, sizeof(subdev->name), "%s sru",
+		 dev_name(vsp1->dev));
+	v4l2_set_subdevdata(subdev, sru);
+	subdev->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	vsp1_entity_init_formats(subdev, NULL);
+
+	/* Initialize the control handler. */
+	v4l2_ctrl_handler_init(&sru->ctrls, 1);
+	v4l2_ctrl_new_custom(&sru->ctrls, &sru_intensity_control, NULL);
+
+	sru->entity.subdev.ctrl_handler = &sru->ctrls;
+
+	if (sru->ctrls.error) {
+		dev_err(vsp1->dev, "sru: failed to initialize controls\n");
+		ret = sru->ctrls.error;
+		vsp1_entity_destroy(&sru->entity);
+		return ERR_PTR(ret);
+	}
+
+	return sru;
+}
diff --git a/drivers/media/platform/vsp1/vsp1_sru.h b/drivers/media/platform/vsp1/vsp1_sru.h
new file mode 100644
index 0000000..b6768bf
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_sru.h
@@ -0,0 +1,40 @@
+/*
+ * vsp1_sru.h  --  R-Car VSP1 Super Resolution Unit
+ *
+ * Copyright (C) 2013 Renesas Corporation
+ *
+ * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+#ifndef __VSP1_SRU_H__
+#define __VSP1_SRU_H__
+
+#include <media/media-entity.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-subdev.h>
+
+#include "vsp1_entity.h"
+
+struct vsp1_device;
+
+#define SRU_PAD_SINK				0
+#define SRU_PAD_SOURCE				1
+
+struct vsp1_sru {
+	struct vsp1_entity entity;
+
+	struct v4l2_ctrl_handler ctrls;
+};
+
+static inline struct vsp1_sru *to_sru(struct v4l2_subdev *subdev)
+{
+	return container_of(subdev, struct vsp1_sru, entity.subdev);
+}
+
+struct vsp1_sru *vsp1_sru_create(struct vsp1_device *vsp1);
+
+#endif /* __VSP1_SRU_H__ */
diff --git a/drivers/media/platform/vsp1/vsp1_uds.c b/drivers/media/platform/vsp1/vsp1_uds.c
new file mode 100644
index 0000000..ccc8243
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_uds.c
@@ -0,0 +1,357 @@
+/*
+ * vsp1_uds.c  --  R-Car VSP1 Up and Down Scaler
+ *
+ * Copyright (C) 2013-2014 Renesas Electronics Corporation
+ *
+ * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/device.h>
+#include <linux/gfp.h>
+
+#include <media/v4l2-subdev.h>
+
+#include "vsp1.h"
+#include "vsp1_uds.h"
+
+#define UDS_MIN_SIZE				4U
+#define UDS_MAX_SIZE				8190U
+
+#define UDS_MIN_FACTOR				0x0100
+#define UDS_MAX_FACTOR				0xffff
+
+/* -----------------------------------------------------------------------------
+ * Device Access
+ */
+
+static inline u32 vsp1_uds_read(struct vsp1_uds *uds, u32 reg)
+{
+	return vsp1_read(uds->entity.vsp1,
+			 reg + uds->entity.index * VI6_UDS_OFFSET);
+}
+
+static inline void vsp1_uds_write(struct vsp1_uds *uds, u32 reg, u32 data)
+{
+	vsp1_write(uds->entity.vsp1,
+		   reg + uds->entity.index * VI6_UDS_OFFSET, data);
+}
+
+/* -----------------------------------------------------------------------------
+ * Scaling Computation
+ */
+
+void vsp1_uds_set_alpha(struct vsp1_uds *uds, unsigned int alpha)
+{
+	vsp1_uds_write(uds, VI6_UDS_ALPVAL, alpha << VI6_UDS_ALPVAL_VAL0_SHIFT);
+}
+
+/*
+ * uds_output_size - Return the output size for an input size and scaling ratio
+ * @input: input size in pixels
+ * @ratio: scaling ratio in U4.12 fixed-point format
+ */
+static unsigned int uds_output_size(unsigned int input, unsigned int ratio)
+{
+	if (ratio > 4096) {
+		/* Down-scaling */
+		unsigned int mp;
+
+		mp = ratio / 4096;
+		mp = mp < 4 ? 1 : (mp < 8 ? 2 : 4);
+
+		return (input - 1) / mp * mp * 4096 / ratio + 1;
+	} else {
+		/* Up-scaling */
+		return (input - 1) * 4096 / ratio + 1;
+	}
+}
+
+/*
+ * uds_output_limits - Return the min and max output sizes for an input size
+ * @input: input size in pixels
+ * @minimum: minimum output size (returned)
+ * @maximum: maximum output size (returned)
+ */
+static void uds_output_limits(unsigned int input,
+			      unsigned int *minimum, unsigned int *maximum)
+{
+	*minimum = max(uds_output_size(input, UDS_MAX_FACTOR), UDS_MIN_SIZE);
+	*maximum = min(uds_output_size(input, UDS_MIN_FACTOR), UDS_MAX_SIZE);
+}
+
+/*
+ * uds_passband_width - Return the passband filter width for a scaling ratio
+ * @ratio: scaling ratio in U4.12 fixed-point format
+ */
+static unsigned int uds_passband_width(unsigned int ratio)
+{
+	if (ratio >= 4096) {
+		/* Down-scaling */
+		unsigned int mp;
+
+		mp = ratio / 4096;
+		mp = mp < 4 ? 1 : (mp < 8 ? 2 : 4);
+
+		return 64 * 4096 * mp / ratio;
+	} else {
+		/* Up-scaling */
+		return 64;
+	}
+}
+
+static unsigned int uds_compute_ratio(unsigned int input, unsigned int output)
+{
+	/* TODO: This is an approximation that will need to be refined. */
+	return (input - 1) * 4096 / (output - 1);
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Core Operations
+ */
+
+static int uds_s_stream(struct v4l2_subdev *subdev, int enable)
+{
+	struct vsp1_uds *uds = to_uds(subdev);
+	const struct v4l2_mbus_framefmt *output;
+	const struct v4l2_mbus_framefmt *input;
+	unsigned int hscale;
+	unsigned int vscale;
+	bool multitap;
+
+	if (!enable)
+		return 0;
+
+	input = &uds->entity.formats[UDS_PAD_SINK];
+	output = &uds->entity.formats[UDS_PAD_SOURCE];
+
+	hscale = uds_compute_ratio(input->width, output->width);
+	vscale = uds_compute_ratio(input->height, output->height);
+
+	dev_dbg(uds->entity.vsp1->dev, "hscale %u vscale %u\n", hscale, vscale);
+
+	/* Multi-tap scaling can't be enabled along with alpha scaling when
+	 * scaling down with a factor lower than or equal to 1/2 in either
+	 * direction.
+	 */
+	if (uds->scale_alpha && (hscale >= 8192 || vscale >= 8192))
+		multitap = false;
+	else
+		multitap = true;
+
+	vsp1_uds_write(uds, VI6_UDS_CTRL,
+		       (uds->scale_alpha ? VI6_UDS_CTRL_AON : 0) |
+		       (multitap ? VI6_UDS_CTRL_BC : 0));
+
+	vsp1_uds_write(uds, VI6_UDS_PASS_BWIDTH,
+		       (uds_passband_width(hscale)
+				<< VI6_UDS_PASS_BWIDTH_H_SHIFT) |
+		       (uds_passband_width(vscale)
+				<< VI6_UDS_PASS_BWIDTH_V_SHIFT));
+
+	/* Set the scaling ratios and the output size. */
+	vsp1_uds_write(uds, VI6_UDS_SCALE,
+		       (hscale << VI6_UDS_SCALE_HFRAC_SHIFT) |
+		       (vscale << VI6_UDS_SCALE_VFRAC_SHIFT));
+	vsp1_uds_write(uds, VI6_UDS_CLIP_SIZE,
+		       (output->width << VI6_UDS_CLIP_SIZE_HSIZE_SHIFT) |
+		       (output->height << VI6_UDS_CLIP_SIZE_VSIZE_SHIFT));
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Pad Operations
+ */
+
+static int uds_enum_mbus_code(struct v4l2_subdev *subdev,
+			      struct v4l2_subdev_pad_config *cfg,
+			      struct v4l2_subdev_mbus_code_enum *code)
+{
+	static const unsigned int codes[] = {
+		MEDIA_BUS_FMT_ARGB8888_1X32,
+		MEDIA_BUS_FMT_AYUV8_1X32,
+	};
+	struct vsp1_uds *uds = to_uds(subdev);
+
+	if (code->pad == UDS_PAD_SINK) {
+		if (code->index >= ARRAY_SIZE(codes))
+			return -EINVAL;
+
+		code->code = codes[code->index];
+	} else {
+		struct v4l2_mbus_framefmt *format;
+
+		/* The UDS can't perform format conversion, the sink format is
+		 * always identical to the source format.
+		 */
+		if (code->index)
+			return -EINVAL;
+
+		format = vsp1_entity_get_pad_format(&uds->entity, cfg,
+						    UDS_PAD_SINK, code->which);
+		code->code = format->code;
+	}
+
+	return 0;
+}
+
+static int uds_enum_frame_size(struct v4l2_subdev *subdev,
+			       struct v4l2_subdev_pad_config *cfg,
+			       struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct vsp1_uds *uds = to_uds(subdev);
+	struct v4l2_mbus_framefmt *format;
+
+	format = vsp1_entity_get_pad_format(&uds->entity, cfg,
+					    UDS_PAD_SINK, fse->which);
+
+	if (fse->index || fse->code != format->code)
+		return -EINVAL;
+
+	if (fse->pad == UDS_PAD_SINK) {
+		fse->min_width = UDS_MIN_SIZE;
+		fse->max_width = UDS_MAX_SIZE;
+		fse->min_height = UDS_MIN_SIZE;
+		fse->max_height = UDS_MAX_SIZE;
+	} else {
+		uds_output_limits(format->width, &fse->min_width,
+				  &fse->max_width);
+		uds_output_limits(format->height, &fse->min_height,
+				  &fse->max_height);
+	}
+
+	return 0;
+}
+
+static int uds_get_format(struct v4l2_subdev *subdev, struct v4l2_subdev_pad_config *cfg,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct vsp1_uds *uds = to_uds(subdev);
+
+	fmt->format = *vsp1_entity_get_pad_format(&uds->entity, cfg, fmt->pad,
+						  fmt->which);
+
+	return 0;
+}
+
+static void uds_try_format(struct vsp1_uds *uds, struct v4l2_subdev_pad_config *cfg,
+			   unsigned int pad, struct v4l2_mbus_framefmt *fmt,
+			   enum v4l2_subdev_format_whence which)
+{
+	struct v4l2_mbus_framefmt *format;
+	unsigned int minimum;
+	unsigned int maximum;
+
+	switch (pad) {
+	case UDS_PAD_SINK:
+		/* Default to YUV if the requested format is not supported. */
+		if (fmt->code != MEDIA_BUS_FMT_ARGB8888_1X32 &&
+		    fmt->code != MEDIA_BUS_FMT_AYUV8_1X32)
+			fmt->code = MEDIA_BUS_FMT_AYUV8_1X32;
+
+		fmt->width = clamp(fmt->width, UDS_MIN_SIZE, UDS_MAX_SIZE);
+		fmt->height = clamp(fmt->height, UDS_MIN_SIZE, UDS_MAX_SIZE);
+		break;
+
+	case UDS_PAD_SOURCE:
+		/* The UDS scales but can't perform format conversion. */
+		format = vsp1_entity_get_pad_format(&uds->entity, cfg,
+						    UDS_PAD_SINK, which);
+		fmt->code = format->code;
+
+		uds_output_limits(format->width, &minimum, &maximum);
+		fmt->width = clamp(fmt->width, minimum, maximum);
+		uds_output_limits(format->height, &minimum, &maximum);
+		fmt->height = clamp(fmt->height, minimum, maximum);
+		break;
+	}
+
+	fmt->field = V4L2_FIELD_NONE;
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+}
+
+static int uds_set_format(struct v4l2_subdev *subdev, struct v4l2_subdev_pad_config *cfg,
+			  struct v4l2_subdev_format *fmt)
+{
+	struct vsp1_uds *uds = to_uds(subdev);
+	struct v4l2_mbus_framefmt *format;
+
+	uds_try_format(uds, cfg, fmt->pad, &fmt->format, fmt->which);
+
+	format = vsp1_entity_get_pad_format(&uds->entity, cfg, fmt->pad,
+					    fmt->which);
+	*format = fmt->format;
+
+	if (fmt->pad == UDS_PAD_SINK) {
+		/* Propagate the format to the source pad. */
+		format = vsp1_entity_get_pad_format(&uds->entity, cfg,
+						    UDS_PAD_SOURCE, fmt->which);
+		*format = fmt->format;
+
+		uds_try_format(uds, cfg, UDS_PAD_SOURCE, format, fmt->which);
+	}
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Operations
+ */
+
+static struct v4l2_subdev_video_ops uds_video_ops = {
+	.s_stream = uds_s_stream,
+};
+
+static struct v4l2_subdev_pad_ops uds_pad_ops = {
+	.enum_mbus_code = uds_enum_mbus_code,
+	.enum_frame_size = uds_enum_frame_size,
+	.get_fmt = uds_get_format,
+	.set_fmt = uds_set_format,
+};
+
+static struct v4l2_subdev_ops uds_ops = {
+	.video	= &uds_video_ops,
+	.pad    = &uds_pad_ops,
+};
+
+/* -----------------------------------------------------------------------------
+ * Initialization and Cleanup
+ */
+
+struct vsp1_uds *vsp1_uds_create(struct vsp1_device *vsp1, unsigned int index)
+{
+	struct v4l2_subdev *subdev;
+	struct vsp1_uds *uds;
+	int ret;
+
+	uds = devm_kzalloc(vsp1->dev, sizeof(*uds), GFP_KERNEL);
+	if (uds == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	uds->entity.type = VSP1_ENTITY_UDS;
+	uds->entity.index = index;
+
+	ret = vsp1_entity_init(vsp1, &uds->entity, 2);
+	if (ret < 0)
+		return ERR_PTR(ret);
+
+	/* Initialize the V4L2 subdev. */
+	subdev = &uds->entity.subdev;
+	v4l2_subdev_init(subdev, &uds_ops);
+
+	subdev->entity.ops = &vsp1_media_ops;
+	subdev->internal_ops = &vsp1_subdev_internal_ops;
+	snprintf(subdev->name, sizeof(subdev->name), "%s uds.%u",
+		 dev_name(vsp1->dev), index);
+	v4l2_set_subdevdata(subdev, uds);
+	subdev->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	vsp1_entity_init_formats(subdev, NULL);
+
+	return uds;
+}
diff --git a/drivers/media/platform/vsp1/vsp1_uds.h b/drivers/media/platform/vsp1/vsp1_uds.h
new file mode 100644
index 0000000..031ac0d
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_uds.h
@@ -0,0 +1,40 @@
+/*
+ * vsp1_uds.h  --  R-Car VSP1 Up and Down Scaler
+ *
+ * Copyright (C) 2013-2014 Renesas Electronics Corporation
+ *
+ * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+#ifndef __VSP1_UDS_H__
+#define __VSP1_UDS_H__
+
+#include <media/media-entity.h>
+#include <media/v4l2-subdev.h>
+
+#include "vsp1_entity.h"
+
+struct vsp1_device;
+
+#define UDS_PAD_SINK				0
+#define UDS_PAD_SOURCE				1
+
+struct vsp1_uds {
+	struct vsp1_entity entity;
+	bool scale_alpha;
+};
+
+static inline struct vsp1_uds *to_uds(struct v4l2_subdev *subdev)
+{
+	return container_of(subdev, struct vsp1_uds, entity.subdev);
+}
+
+struct vsp1_uds *vsp1_uds_create(struct vsp1_device *vsp1, unsigned int index);
+
+void vsp1_uds_set_alpha(struct vsp1_uds *uds, unsigned int alpha);
+
+#endif /* __VSP1_UDS_H__ */
diff --git a/drivers/media/platform/vsp1/vsp1_video.c b/drivers/media/platform/vsp1/vsp1_video.c
new file mode 100644
index 0000000..5ce88e1
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_video.c
@@ -0,0 +1,1299 @@
+/*
+ * vsp1_video.c  --  R-Car VSP1 Video Node
+ *
+ * Copyright (C) 2013-2015 Renesas Electronics Corporation
+ *
+ * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/v4l2-mediabus.h>
+#include <linux/videodev2.h>
+
+#include <media/media-entity.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-subdev.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "vsp1.h"
+#include "vsp1_bru.h"
+#include "vsp1_entity.h"
+#include "vsp1_rwpf.h"
+#include "vsp1_uds.h"
+#include "vsp1_video.h"
+
+#define VSP1_VIDEO_DEF_FORMAT		V4L2_PIX_FMT_YUYV
+#define VSP1_VIDEO_DEF_WIDTH		1024
+#define VSP1_VIDEO_DEF_HEIGHT		768
+
+#define VSP1_VIDEO_MIN_WIDTH		2U
+#define VSP1_VIDEO_MAX_WIDTH		8190U
+#define VSP1_VIDEO_MIN_HEIGHT		2U
+#define VSP1_VIDEO_MAX_HEIGHT		8190U
+
+/* -----------------------------------------------------------------------------
+ * Helper functions
+ */
+
+static const struct vsp1_format_info vsp1_video_formats[] = {
+	{ V4L2_PIX_FMT_RGB332, MEDIA_BUS_FMT_ARGB8888_1X32,
+	  VI6_FMT_RGB_332, VI6_RPF_DSWAP_P_LLS | VI6_RPF_DSWAP_P_LWS |
+	  VI6_RPF_DSWAP_P_WDS | VI6_RPF_DSWAP_P_BTS,
+	  1, { 8, 0, 0 }, false, false, 1, 1, false },
+	{ V4L2_PIX_FMT_ARGB444, MEDIA_BUS_FMT_ARGB8888_1X32,
+	  VI6_FMT_ARGB_4444, VI6_RPF_DSWAP_P_LLS | VI6_RPF_DSWAP_P_LWS |
+	  VI6_RPF_DSWAP_P_WDS,
+	  1, { 16, 0, 0 }, false, false, 1, 1, true },
+	{ V4L2_PIX_FMT_XRGB444, MEDIA_BUS_FMT_ARGB8888_1X32,
+	  VI6_FMT_XRGB_4444, VI6_RPF_DSWAP_P_LLS | VI6_RPF_DSWAP_P_LWS |
+	  VI6_RPF_DSWAP_P_WDS,
+	  1, { 16, 0, 0 }, false, false, 1, 1, true },
+	{ V4L2_PIX_FMT_ARGB555, MEDIA_BUS_FMT_ARGB8888_1X32,
+	  VI6_FMT_ARGB_1555, VI6_RPF_DSWAP_P_LLS | VI6_RPF_DSWAP_P_LWS |
+	  VI6_RPF_DSWAP_P_WDS,
+	  1, { 16, 0, 0 }, false, false, 1, 1, true },
+	{ V4L2_PIX_FMT_XRGB555, MEDIA_BUS_FMT_ARGB8888_1X32,
+	  VI6_FMT_XRGB_1555, VI6_RPF_DSWAP_P_LLS | VI6_RPF_DSWAP_P_LWS |
+	  VI6_RPF_DSWAP_P_WDS,
+	  1, { 16, 0, 0 }, false, false, 1, 1, false },
+	{ V4L2_PIX_FMT_RGB565, MEDIA_BUS_FMT_ARGB8888_1X32,
+	  VI6_FMT_RGB_565, VI6_RPF_DSWAP_P_LLS | VI6_RPF_DSWAP_P_LWS |
+	  VI6_RPF_DSWAP_P_WDS,
+	  1, { 16, 0, 0 }, false, false, 1, 1, false },
+	{ V4L2_PIX_FMT_BGR24, MEDIA_BUS_FMT_ARGB8888_1X32,
+	  VI6_FMT_BGR_888, VI6_RPF_DSWAP_P_LLS | VI6_RPF_DSWAP_P_LWS |
+	  VI6_RPF_DSWAP_P_WDS | VI6_RPF_DSWAP_P_BTS,
+	  1, { 24, 0, 0 }, false, false, 1, 1, false },
+	{ V4L2_PIX_FMT_RGB24, MEDIA_BUS_FMT_ARGB8888_1X32,
+	  VI6_FMT_RGB_888, VI6_RPF_DSWAP_P_LLS | VI6_RPF_DSWAP_P_LWS |
+	  VI6_RPF_DSWAP_P_WDS | VI6_RPF_DSWAP_P_BTS,
+	  1, { 24, 0, 0 }, false, false, 1, 1, false },
+	{ V4L2_PIX_FMT_ABGR32, MEDIA_BUS_FMT_ARGB8888_1X32,
+	  VI6_FMT_ARGB_8888, VI6_RPF_DSWAP_P_LLS | VI6_RPF_DSWAP_P_LWS,
+	  1, { 32, 0, 0 }, false, false, 1, 1, true },
+	{ V4L2_PIX_FMT_XBGR32, MEDIA_BUS_FMT_ARGB8888_1X32,
+	  VI6_FMT_ARGB_8888, VI6_RPF_DSWAP_P_LLS | VI6_RPF_DSWAP_P_LWS,
+	  1, { 32, 0, 0 }, false, false, 1, 1, false },
+	{ V4L2_PIX_FMT_ARGB32, MEDIA_BUS_FMT_ARGB8888_1X32,
+	  VI6_FMT_ARGB_8888, VI6_RPF_DSWAP_P_LLS | VI6_RPF_DSWAP_P_LWS |
+	  VI6_RPF_DSWAP_P_WDS | VI6_RPF_DSWAP_P_BTS,
+	  1, { 32, 0, 0 }, false, false, 1, 1, true },
+	{ V4L2_PIX_FMT_XRGB32, MEDIA_BUS_FMT_ARGB8888_1X32,
+	  VI6_FMT_ARGB_8888, VI6_RPF_DSWAP_P_LLS | VI6_RPF_DSWAP_P_LWS |
+	  VI6_RPF_DSWAP_P_WDS | VI6_RPF_DSWAP_P_BTS,
+	  1, { 32, 0, 0 }, false, false, 1, 1, false },
+	{ V4L2_PIX_FMT_UYVY, MEDIA_BUS_FMT_AYUV8_1X32,
+	  VI6_FMT_YUYV_422, VI6_RPF_DSWAP_P_LLS | VI6_RPF_DSWAP_P_LWS |
+	  VI6_RPF_DSWAP_P_WDS | VI6_RPF_DSWAP_P_BTS,
+	  1, { 16, 0, 0 }, false, false, 2, 1, false },
+	{ V4L2_PIX_FMT_VYUY, MEDIA_BUS_FMT_AYUV8_1X32,
+	  VI6_FMT_YUYV_422, VI6_RPF_DSWAP_P_LLS | VI6_RPF_DSWAP_P_LWS |
+	  VI6_RPF_DSWAP_P_WDS | VI6_RPF_DSWAP_P_BTS,
+	  1, { 16, 0, 0 }, false, true, 2, 1, false },
+	{ V4L2_PIX_FMT_YUYV, MEDIA_BUS_FMT_AYUV8_1X32,
+	  VI6_FMT_YUYV_422, VI6_RPF_DSWAP_P_LLS | VI6_RPF_DSWAP_P_LWS |
+	  VI6_RPF_DSWAP_P_WDS | VI6_RPF_DSWAP_P_BTS,
+	  1, { 16, 0, 0 }, true, false, 2, 1, false },
+	{ V4L2_PIX_FMT_YVYU, MEDIA_BUS_FMT_AYUV8_1X32,
+	  VI6_FMT_YUYV_422, VI6_RPF_DSWAP_P_LLS | VI6_RPF_DSWAP_P_LWS |
+	  VI6_RPF_DSWAP_P_WDS | VI6_RPF_DSWAP_P_BTS,
+	  1, { 16, 0, 0 }, true, true, 2, 1, false },
+	{ V4L2_PIX_FMT_NV12M, MEDIA_BUS_FMT_AYUV8_1X32,
+	  VI6_FMT_Y_UV_420, VI6_RPF_DSWAP_P_LLS | VI6_RPF_DSWAP_P_LWS |
+	  VI6_RPF_DSWAP_P_WDS | VI6_RPF_DSWAP_P_BTS,
+	  2, { 8, 16, 0 }, false, false, 2, 2, false },
+	{ V4L2_PIX_FMT_NV21M, MEDIA_BUS_FMT_AYUV8_1X32,
+	  VI6_FMT_Y_UV_420, VI6_RPF_DSWAP_P_LLS | VI6_RPF_DSWAP_P_LWS |
+	  VI6_RPF_DSWAP_P_WDS | VI6_RPF_DSWAP_P_BTS,
+	  2, { 8, 16, 0 }, false, true, 2, 2, false },
+	{ V4L2_PIX_FMT_NV16M, MEDIA_BUS_FMT_AYUV8_1X32,
+	  VI6_FMT_Y_UV_422, VI6_RPF_DSWAP_P_LLS | VI6_RPF_DSWAP_P_LWS |
+	  VI6_RPF_DSWAP_P_WDS | VI6_RPF_DSWAP_P_BTS,
+	  2, { 8, 16, 0 }, false, false, 2, 1, false },
+	{ V4L2_PIX_FMT_NV61M, MEDIA_BUS_FMT_AYUV8_1X32,
+	  VI6_FMT_Y_UV_422, VI6_RPF_DSWAP_P_LLS | VI6_RPF_DSWAP_P_LWS |
+	  VI6_RPF_DSWAP_P_WDS | VI6_RPF_DSWAP_P_BTS,
+	  2, { 8, 16, 0 }, false, true, 2, 1, false },
+	{ V4L2_PIX_FMT_YUV420M, MEDIA_BUS_FMT_AYUV8_1X32,
+	  VI6_FMT_Y_U_V_420, VI6_RPF_DSWAP_P_LLS | VI6_RPF_DSWAP_P_LWS |
+	  VI6_RPF_DSWAP_P_WDS | VI6_RPF_DSWAP_P_BTS,
+	  3, { 8, 8, 8 }, false, false, 2, 2, false },
+};
+
+/*
+ * vsp1_get_format_info - Retrieve format information for a 4CC
+ * @fourcc: the format 4CC
+ *
+ * Return a pointer to the format information structure corresponding to the
+ * given V4L2 format 4CC, or NULL if no corresponding format can be found.
+ */
+static const struct vsp1_format_info *vsp1_get_format_info(u32 fourcc)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(vsp1_video_formats); ++i) {
+		const struct vsp1_format_info *info = &vsp1_video_formats[i];
+
+		if (info->fourcc == fourcc)
+			return info;
+	}
+
+	return NULL;
+}
+
+
+static struct v4l2_subdev *
+vsp1_video_remote_subdev(struct media_pad *local, u32 *pad)
+{
+	struct media_pad *remote;
+
+	remote = media_entity_remote_pad(local);
+	if (remote == NULL ||
+	    media_entity_type(remote->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+		return NULL;
+
+	if (pad)
+		*pad = remote->index;
+
+	return media_entity_to_v4l2_subdev(remote->entity);
+}
+
+static int vsp1_video_verify_format(struct vsp1_video *video)
+{
+	struct v4l2_subdev_format fmt;
+	struct v4l2_subdev *subdev;
+	int ret;
+
+	subdev = vsp1_video_remote_subdev(&video->pad, &fmt.pad);
+	if (subdev == NULL)
+		return -EINVAL;
+
+	fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt);
+	if (ret < 0)
+		return ret == -ENOIOCTLCMD ? -EINVAL : ret;
+
+	if (video->fmtinfo->mbus != fmt.format.code ||
+	    video->format.height != fmt.format.height ||
+	    video->format.width != fmt.format.width)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int __vsp1_video_try_format(struct vsp1_video *video,
+				   struct v4l2_pix_format_mplane *pix,
+				   const struct vsp1_format_info **fmtinfo)
+{
+	static const u32 xrgb_formats[][2] = {
+		{ V4L2_PIX_FMT_RGB444, V4L2_PIX_FMT_XRGB444 },
+		{ V4L2_PIX_FMT_RGB555, V4L2_PIX_FMT_XRGB555 },
+		{ V4L2_PIX_FMT_BGR32, V4L2_PIX_FMT_XBGR32 },
+		{ V4L2_PIX_FMT_RGB32, V4L2_PIX_FMT_XRGB32 },
+	};
+
+	const struct vsp1_format_info *info;
+	unsigned int width = pix->width;
+	unsigned int height = pix->height;
+	unsigned int i;
+
+	/* Backward compatibility: replace deprecated RGB formats by their XRGB
+	 * equivalent. This selects the format older userspace applications want
+	 * while still exposing the new format.
+	 */
+	for (i = 0; i < ARRAY_SIZE(xrgb_formats); ++i) {
+		if (xrgb_formats[i][0] == pix->pixelformat) {
+			pix->pixelformat = xrgb_formats[i][1];
+			break;
+		}
+	}
+
+	/* Retrieve format information and select the default format if the
+	 * requested format isn't supported.
+	 */
+	info = vsp1_get_format_info(pix->pixelformat);
+	if (info == NULL)
+		info = vsp1_get_format_info(VSP1_VIDEO_DEF_FORMAT);
+
+	pix->pixelformat = info->fourcc;
+	pix->colorspace = V4L2_COLORSPACE_SRGB;
+	pix->field = V4L2_FIELD_NONE;
+	memset(pix->reserved, 0, sizeof(pix->reserved));
+
+	/* Align the width and height for YUV 4:2:2 and 4:2:0 formats. */
+	width = round_down(width, info->hsub);
+	height = round_down(height, info->vsub);
+
+	/* Clamp the width and height. */
+	pix->width = clamp(width, VSP1_VIDEO_MIN_WIDTH, VSP1_VIDEO_MAX_WIDTH);
+	pix->height = clamp(height, VSP1_VIDEO_MIN_HEIGHT,
+			    VSP1_VIDEO_MAX_HEIGHT);
+
+	/* Compute and clamp the stride and image size. While not documented in
+	 * the datasheet, strides not aligned to a multiple of 128 bytes result
+	 * in image corruption.
+	 */
+	for (i = 0; i < min(info->planes, 2U); ++i) {
+		unsigned int hsub = i > 0 ? info->hsub : 1;
+		unsigned int vsub = i > 0 ? info->vsub : 1;
+		unsigned int align = 128;
+		unsigned int bpl;
+
+		bpl = clamp_t(unsigned int, pix->plane_fmt[i].bytesperline,
+			      pix->width / hsub * info->bpp[i] / 8,
+			      round_down(65535U, align));
+
+		pix->plane_fmt[i].bytesperline = round_up(bpl, align);
+		pix->plane_fmt[i].sizeimage = pix->plane_fmt[i].bytesperline
+					    * pix->height / vsub;
+	}
+
+	if (info->planes == 3) {
+		/* The second and third planes must have the same stride. */
+		pix->plane_fmt[2].bytesperline = pix->plane_fmt[1].bytesperline;
+		pix->plane_fmt[2].sizeimage = pix->plane_fmt[1].sizeimage;
+	}
+
+	pix->num_planes = info->planes;
+
+	if (fmtinfo)
+		*fmtinfo = info;
+
+	return 0;
+}
+
+static bool
+vsp1_video_format_adjust(struct vsp1_video *video,
+			 const struct v4l2_pix_format_mplane *format,
+			 struct v4l2_pix_format_mplane *adjust)
+{
+	unsigned int i;
+
+	*adjust = *format;
+	__vsp1_video_try_format(video, adjust, NULL);
+
+	if (format->width != adjust->width ||
+	    format->height != adjust->height ||
+	    format->pixelformat != adjust->pixelformat ||
+	    format->num_planes != adjust->num_planes)
+		return false;
+
+	for (i = 0; i < format->num_planes; ++i) {
+		if (format->plane_fmt[i].bytesperline !=
+		    adjust->plane_fmt[i].bytesperline)
+			return false;
+
+		adjust->plane_fmt[i].sizeimage =
+			max(adjust->plane_fmt[i].sizeimage,
+			    format->plane_fmt[i].sizeimage);
+	}
+
+	return true;
+}
+
+/* -----------------------------------------------------------------------------
+ * Pipeline Management
+ */
+
+static int vsp1_pipeline_validate_branch(struct vsp1_pipeline *pipe,
+					 struct vsp1_rwpf *input,
+					 struct vsp1_rwpf *output)
+{
+	struct vsp1_entity *entity;
+	unsigned int entities = 0;
+	struct media_pad *pad;
+	bool bru_found = false;
+
+	input->location.left = 0;
+	input->location.top = 0;
+
+	pad = media_entity_remote_pad(&input->entity.pads[RWPF_PAD_SOURCE]);
+
+	while (1) {
+		if (pad == NULL)
+			return -EPIPE;
+
+		/* We've reached a video node, that shouldn't have happened. */
+		if (media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+			return -EPIPE;
+
+		entity = to_vsp1_entity(media_entity_to_v4l2_subdev(pad->entity));
+
+		/* A BRU is present in the pipeline, store the compose rectangle
+		 * location in the input RPF for use when configuring the RPF.
+		 */
+		if (entity->type == VSP1_ENTITY_BRU) {
+			struct vsp1_bru *bru = to_bru(&entity->subdev);
+			struct v4l2_rect *rect =
+				&bru->inputs[pad->index].compose;
+
+			bru->inputs[pad->index].rpf = input;
+
+			input->location.left = rect->left;
+			input->location.top = rect->top;
+
+			bru_found = true;
+		}
+
+		/* We've reached the WPF, we're done. */
+		if (entity->type == VSP1_ENTITY_WPF)
+			break;
+
+		/* Ensure the branch has no loop. */
+		if (entities & (1 << entity->subdev.entity.id))
+			return -EPIPE;
+
+		entities |= 1 << entity->subdev.entity.id;
+
+		/* UDS can't be chained. */
+		if (entity->type == VSP1_ENTITY_UDS) {
+			if (pipe->uds)
+				return -EPIPE;
+
+			pipe->uds = entity;
+			pipe->uds_input = bru_found ? pipe->bru
+					: &input->entity;
+		}
+
+		/* Follow the source link. The link setup operations ensure
+		 * that the output fan-out can't be more than one, there is thus
+		 * no need to verify here that only a single source link is
+		 * activated.
+		 */
+		pad = &entity->pads[entity->source_pad];
+		pad = media_entity_remote_pad(pad);
+	}
+
+	/* The last entity must be the output WPF. */
+	if (entity != &output->entity)
+		return -EPIPE;
+
+	return 0;
+}
+
+static void __vsp1_pipeline_cleanup(struct vsp1_pipeline *pipe)
+{
+	if (pipe->bru) {
+		struct vsp1_bru *bru = to_bru(&pipe->bru->subdev);
+		unsigned int i;
+
+		for (i = 0; i < ARRAY_SIZE(bru->inputs); ++i)
+			bru->inputs[i].rpf = NULL;
+	}
+
+	INIT_LIST_HEAD(&pipe->entities);
+	pipe->state = VSP1_PIPELINE_STOPPED;
+	pipe->buffers_ready = 0;
+	pipe->num_video = 0;
+	pipe->num_inputs = 0;
+	pipe->output = NULL;
+	pipe->bru = NULL;
+	pipe->lif = NULL;
+	pipe->uds = NULL;
+}
+
+static int vsp1_pipeline_validate(struct vsp1_pipeline *pipe,
+				  struct vsp1_video *video)
+{
+	struct media_entity_graph graph;
+	struct media_entity *entity = &video->video.entity;
+	struct media_device *mdev = entity->parent;
+	unsigned int i;
+	int ret;
+
+	mutex_lock(&mdev->graph_mutex);
+
+	/* Walk the graph to locate the entities and video nodes. */
+	media_entity_graph_walk_start(&graph, entity);
+
+	while ((entity = media_entity_graph_walk_next(&graph))) {
+		struct v4l2_subdev *subdev;
+		struct vsp1_rwpf *rwpf;
+		struct vsp1_entity *e;
+
+		if (media_entity_type(entity) != MEDIA_ENT_T_V4L2_SUBDEV) {
+			pipe->num_video++;
+			continue;
+		}
+
+		subdev = media_entity_to_v4l2_subdev(entity);
+		e = to_vsp1_entity(subdev);
+		list_add_tail(&e->list_pipe, &pipe->entities);
+
+		if (e->type == VSP1_ENTITY_RPF) {
+			rwpf = to_rwpf(subdev);
+			pipe->inputs[pipe->num_inputs++] = rwpf;
+			rwpf->video.pipe_index = pipe->num_inputs;
+		} else if (e->type == VSP1_ENTITY_WPF) {
+			rwpf = to_rwpf(subdev);
+			pipe->output = to_rwpf(subdev);
+			rwpf->video.pipe_index = 0;
+		} else if (e->type == VSP1_ENTITY_LIF) {
+			pipe->lif = e;
+		} else if (e->type == VSP1_ENTITY_BRU) {
+			pipe->bru = e;
+		}
+	}
+
+	mutex_unlock(&mdev->graph_mutex);
+
+	/* We need one output and at least one input. */
+	if (pipe->num_inputs == 0 || !pipe->output) {
+		ret = -EPIPE;
+		goto error;
+	}
+
+	/* Follow links downstream for each input and make sure the graph
+	 * contains no loop and that all branches end at the output WPF.
+	 */
+	for (i = 0; i < pipe->num_inputs; ++i) {
+		ret = vsp1_pipeline_validate_branch(pipe, pipe->inputs[i],
+						    pipe->output);
+		if (ret < 0)
+			goto error;
+	}
+
+	return 0;
+
+error:
+	__vsp1_pipeline_cleanup(pipe);
+	return ret;
+}
+
+static int vsp1_pipeline_init(struct vsp1_pipeline *pipe,
+			      struct vsp1_video *video)
+{
+	int ret;
+
+	mutex_lock(&pipe->lock);
+
+	/* If we're the first user validate and initialize the pipeline. */
+	if (pipe->use_count == 0) {
+		ret = vsp1_pipeline_validate(pipe, video);
+		if (ret < 0)
+			goto done;
+	}
+
+	pipe->use_count++;
+	ret = 0;
+
+done:
+	mutex_unlock(&pipe->lock);
+	return ret;
+}
+
+static void vsp1_pipeline_cleanup(struct vsp1_pipeline *pipe)
+{
+	mutex_lock(&pipe->lock);
+
+	/* If we're the last user clean up the pipeline. */
+	if (--pipe->use_count == 0)
+		__vsp1_pipeline_cleanup(pipe);
+
+	mutex_unlock(&pipe->lock);
+}
+
+static void vsp1_pipeline_run(struct vsp1_pipeline *pipe)
+{
+	struct vsp1_device *vsp1 = pipe->output->entity.vsp1;
+
+	vsp1_write(vsp1, VI6_CMD(pipe->output->entity.index), VI6_CMD_STRCMD);
+	pipe->state = VSP1_PIPELINE_RUNNING;
+	pipe->buffers_ready = 0;
+}
+
+static bool vsp1_pipeline_stopped(struct vsp1_pipeline *pipe)
+{
+	unsigned long flags;
+	bool stopped;
+
+	spin_lock_irqsave(&pipe->irqlock, flags);
+	stopped = pipe->state == VSP1_PIPELINE_STOPPED,
+	spin_unlock_irqrestore(&pipe->irqlock, flags);
+
+	return stopped;
+}
+
+static int vsp1_pipeline_stop(struct vsp1_pipeline *pipe)
+{
+	struct vsp1_entity *entity;
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&pipe->irqlock, flags);
+	if (pipe->state == VSP1_PIPELINE_RUNNING)
+		pipe->state = VSP1_PIPELINE_STOPPING;
+	spin_unlock_irqrestore(&pipe->irqlock, flags);
+
+	ret = wait_event_timeout(pipe->wq, vsp1_pipeline_stopped(pipe),
+				 msecs_to_jiffies(500));
+	ret = ret == 0 ? -ETIMEDOUT : 0;
+
+	list_for_each_entry(entity, &pipe->entities, list_pipe) {
+		if (entity->route && entity->route->reg)
+			vsp1_write(entity->vsp1, entity->route->reg,
+				   VI6_DPR_NODE_UNUSED);
+
+		v4l2_subdev_call(&entity->subdev, video, s_stream, 0);
+	}
+
+	return ret;
+}
+
+static bool vsp1_pipeline_ready(struct vsp1_pipeline *pipe)
+{
+	unsigned int mask;
+
+	mask = ((1 << pipe->num_inputs) - 1) << 1;
+	if (!pipe->lif)
+		mask |= 1 << 0;
+
+	return pipe->buffers_ready == mask;
+}
+
+/*
+ * vsp1_video_complete_buffer - Complete the current buffer
+ * @video: the video node
+ *
+ * This function completes the current buffer by filling its sequence number,
+ * time stamp and payload size, and hands it back to the videobuf core.
+ *
+ * When operating in DU output mode (deep pipeline to the DU through the LIF),
+ * the VSP1 needs to constantly supply frames to the display. In that case, if
+ * no other buffer is queued, reuse the one that has just been processed instead
+ * of handing it back to the videobuf core.
+ *
+ * Return the next queued buffer or NULL if the queue is empty.
+ */
+static struct vsp1_video_buffer *
+vsp1_video_complete_buffer(struct vsp1_video *video)
+{
+	struct vsp1_pipeline *pipe = to_vsp1_pipeline(&video->video.entity);
+	struct vsp1_video_buffer *next = NULL;
+	struct vsp1_video_buffer *done;
+	unsigned long flags;
+	unsigned int i;
+
+	spin_lock_irqsave(&video->irqlock, flags);
+
+	if (list_empty(&video->irqqueue)) {
+		spin_unlock_irqrestore(&video->irqlock, flags);
+		return NULL;
+	}
+
+	done = list_first_entry(&video->irqqueue,
+				struct vsp1_video_buffer, queue);
+
+	/* In DU output mode reuse the buffer if the list is singular. */
+	if (pipe->lif && list_is_singular(&video->irqqueue)) {
+		spin_unlock_irqrestore(&video->irqlock, flags);
+		return done;
+	}
+
+	list_del(&done->queue);
+
+	if (!list_empty(&video->irqqueue))
+		next = list_first_entry(&video->irqqueue,
+					struct vsp1_video_buffer, queue);
+
+	spin_unlock_irqrestore(&video->irqlock, flags);
+
+	done->buf.sequence = video->sequence++;
+	v4l2_get_timestamp(&done->buf.timestamp);
+	for (i = 0; i < done->buf.vb2_buf.num_planes; ++i)
+		vb2_set_plane_payload(&done->buf.vb2_buf, i, done->length[i]);
+	vb2_buffer_done(&done->buf.vb2_buf, VB2_BUF_STATE_DONE);
+
+	return next;
+}
+
+static void vsp1_video_frame_end(struct vsp1_pipeline *pipe,
+				 struct vsp1_video *video)
+{
+	struct vsp1_video_buffer *buf;
+	unsigned long flags;
+
+	buf = vsp1_video_complete_buffer(video);
+	if (buf == NULL)
+		return;
+
+	spin_lock_irqsave(&pipe->irqlock, flags);
+
+	video->ops->queue(video, buf);
+	pipe->buffers_ready |= 1 << video->pipe_index;
+
+	spin_unlock_irqrestore(&pipe->irqlock, flags);
+}
+
+void vsp1_pipeline_frame_end(struct vsp1_pipeline *pipe)
+{
+	enum vsp1_pipeline_state state;
+	unsigned long flags;
+	unsigned int i;
+
+	if (pipe == NULL)
+		return;
+
+	/* Complete buffers on all video nodes. */
+	for (i = 0; i < pipe->num_inputs; ++i)
+		vsp1_video_frame_end(pipe, &pipe->inputs[i]->video);
+
+	if (!pipe->lif)
+		vsp1_video_frame_end(pipe, &pipe->output->video);
+
+	spin_lock_irqsave(&pipe->irqlock, flags);
+
+	state = pipe->state;
+	pipe->state = VSP1_PIPELINE_STOPPED;
+
+	/* If a stop has been requested, mark the pipeline as stopped and
+	 * return.
+	 */
+	if (state == VSP1_PIPELINE_STOPPING) {
+		wake_up(&pipe->wq);
+		goto done;
+	}
+
+	/* Restart the pipeline if ready. */
+	if (vsp1_pipeline_ready(pipe))
+		vsp1_pipeline_run(pipe);
+
+done:
+	spin_unlock_irqrestore(&pipe->irqlock, flags);
+}
+
+/*
+ * Propagate the alpha value through the pipeline.
+ *
+ * As the UDS has restricted scaling capabilities when the alpha component needs
+ * to be scaled, we disable alpha scaling when the UDS input has a fixed alpha
+ * value. The UDS then outputs a fixed alpha value which needs to be programmed
+ * from the input RPF alpha.
+ */
+void vsp1_pipeline_propagate_alpha(struct vsp1_pipeline *pipe,
+				   struct vsp1_entity *input,
+				   unsigned int alpha)
+{
+	struct vsp1_entity *entity;
+	struct media_pad *pad;
+
+	pad = media_entity_remote_pad(&input->pads[RWPF_PAD_SOURCE]);
+
+	while (pad) {
+		if (media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+			break;
+
+		entity = to_vsp1_entity(media_entity_to_v4l2_subdev(pad->entity));
+
+		/* The BRU background color has a fixed alpha value set to 255,
+		 * the output alpha value is thus always equal to 255.
+		 */
+		if (entity->type == VSP1_ENTITY_BRU)
+			alpha = 255;
+
+		if (entity->type == VSP1_ENTITY_UDS) {
+			struct vsp1_uds *uds = to_uds(&entity->subdev);
+
+			vsp1_uds_set_alpha(uds, alpha);
+			break;
+		}
+
+		pad = &entity->pads[entity->source_pad];
+		pad = media_entity_remote_pad(pad);
+	}
+}
+
+void vsp1_pipelines_suspend(struct vsp1_device *vsp1)
+{
+	unsigned long flags;
+	unsigned int i;
+	int ret;
+
+	/* To avoid increasing the system suspend time needlessly, loop over the
+	 * pipelines twice, first to set them all to the stopping state, and then
+	 * to wait for the stop to complete.
+	 */
+	for (i = 0; i < vsp1->pdata.wpf_count; ++i) {
+		struct vsp1_rwpf *wpf = vsp1->wpf[i];
+		struct vsp1_pipeline *pipe;
+
+		if (wpf == NULL)
+			continue;
+
+		pipe = to_vsp1_pipeline(&wpf->entity.subdev.entity);
+		if (pipe == NULL)
+			continue;
+
+		spin_lock_irqsave(&pipe->irqlock, flags);
+		if (pipe->state == VSP1_PIPELINE_RUNNING)
+			pipe->state = VSP1_PIPELINE_STOPPING;
+		spin_unlock_irqrestore(&pipe->irqlock, flags);
+	}
+
+	for (i = 0; i < vsp1->pdata.wpf_count; ++i) {
+		struct vsp1_rwpf *wpf = vsp1->wpf[i];
+		struct vsp1_pipeline *pipe;
+
+		if (wpf == NULL)
+			continue;
+
+		pipe = to_vsp1_pipeline(&wpf->entity.subdev.entity);
+		if (pipe == NULL)
+			continue;
+
+		ret = wait_event_timeout(pipe->wq, vsp1_pipeline_stopped(pipe),
+					 msecs_to_jiffies(500));
+		if (ret == 0)
+			dev_warn(vsp1->dev, "pipeline %u stop timeout\n",
+				 wpf->entity.index);
+	}
+}
+
+void vsp1_pipelines_resume(struct vsp1_device *vsp1)
+{
+	unsigned int i;
+
+	/* Resume pipeline all running pipelines. */
+	for (i = 0; i < vsp1->pdata.wpf_count; ++i) {
+		struct vsp1_rwpf *wpf = vsp1->wpf[i];
+		struct vsp1_pipeline *pipe;
+
+		if (wpf == NULL)
+			continue;
+
+		pipe = to_vsp1_pipeline(&wpf->entity.subdev.entity);
+		if (pipe == NULL)
+			continue;
+
+		if (vsp1_pipeline_ready(pipe))
+			vsp1_pipeline_run(pipe);
+	}
+}
+
+/* -----------------------------------------------------------------------------
+ * videobuf2 Queue Operations
+ */
+
+static int
+vsp1_video_queue_setup(struct vb2_queue *vq, const void *parg,
+		     unsigned int *nbuffers, unsigned int *nplanes,
+		     unsigned int sizes[], void *alloc_ctxs[])
+{
+	const struct v4l2_format *fmt = parg;
+	struct vsp1_video *video = vb2_get_drv_priv(vq);
+	const struct v4l2_pix_format_mplane *format;
+	struct v4l2_pix_format_mplane pix_mp;
+	unsigned int i;
+
+	if (fmt) {
+		/* Make sure the format is valid and adjust the sizeimage field
+		 * if needed.
+		 */
+		if (!vsp1_video_format_adjust(video, &fmt->fmt.pix_mp, &pix_mp))
+			return -EINVAL;
+
+		format = &pix_mp;
+	} else {
+		format = &video->format;
+	}
+
+	*nplanes = format->num_planes;
+
+	for (i = 0; i < format->num_planes; ++i) {
+		sizes[i] = format->plane_fmt[i].sizeimage;
+		alloc_ctxs[i] = video->alloc_ctx;
+	}
+
+	return 0;
+}
+
+static int vsp1_video_buffer_prepare(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct vsp1_video *video = vb2_get_drv_priv(vb->vb2_queue);
+	struct vsp1_video_buffer *buf = to_vsp1_video_buffer(vbuf);
+	const struct v4l2_pix_format_mplane *format = &video->format;
+	unsigned int i;
+
+	if (vb->num_planes < format->num_planes)
+		return -EINVAL;
+
+	for (i = 0; i < vb->num_planes; ++i) {
+		buf->addr[i] = vb2_dma_contig_plane_dma_addr(vb, i);
+		buf->length[i] = vb2_plane_size(vb, i);
+
+		if (buf->length[i] < format->plane_fmt[i].sizeimage)
+			return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void vsp1_video_buffer_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct vsp1_video *video = vb2_get_drv_priv(vb->vb2_queue);
+	struct vsp1_pipeline *pipe = to_vsp1_pipeline(&video->video.entity);
+	struct vsp1_video_buffer *buf = to_vsp1_video_buffer(vbuf);
+	unsigned long flags;
+	bool empty;
+
+	spin_lock_irqsave(&video->irqlock, flags);
+	empty = list_empty(&video->irqqueue);
+	list_add_tail(&buf->queue, &video->irqqueue);
+	spin_unlock_irqrestore(&video->irqlock, flags);
+
+	if (!empty)
+		return;
+
+	spin_lock_irqsave(&pipe->irqlock, flags);
+
+	video->ops->queue(video, buf);
+	pipe->buffers_ready |= 1 << video->pipe_index;
+
+	if (vb2_is_streaming(&video->queue) &&
+	    vsp1_pipeline_ready(pipe))
+		vsp1_pipeline_run(pipe);
+
+	spin_unlock_irqrestore(&pipe->irqlock, flags);
+}
+
+static void vsp1_entity_route_setup(struct vsp1_entity *source)
+{
+	struct vsp1_entity *sink;
+
+	if (source->route->reg == 0)
+		return;
+
+	sink = container_of(source->sink, struct vsp1_entity, subdev.entity);
+	vsp1_write(source->vsp1, source->route->reg,
+		   sink->route->inputs[source->sink_pad]);
+}
+
+static int vsp1_video_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct vsp1_video *video = vb2_get_drv_priv(vq);
+	struct vsp1_pipeline *pipe = to_vsp1_pipeline(&video->video.entity);
+	struct vsp1_entity *entity;
+	unsigned long flags;
+	int ret;
+
+	mutex_lock(&pipe->lock);
+	if (pipe->stream_count == pipe->num_video - 1) {
+		if (pipe->uds) {
+			struct vsp1_uds *uds = to_uds(&pipe->uds->subdev);
+
+			/* If a BRU is present in the pipeline before the UDS,
+			 * the alpha component doesn't need to be scaled as the
+			 * BRU output alpha value is fixed to 255. Otherwise we
+			 * need to scale the alpha component only when available
+			 * at the input RPF.
+			 */
+			if (pipe->uds_input->type == VSP1_ENTITY_BRU) {
+				uds->scale_alpha = false;
+			} else {
+				struct vsp1_rwpf *rpf =
+					to_rwpf(&pipe->uds_input->subdev);
+
+				uds->scale_alpha = rpf->video.fmtinfo->alpha;
+			}
+		}
+
+		list_for_each_entry(entity, &pipe->entities, list_pipe) {
+			vsp1_entity_route_setup(entity);
+
+			ret = v4l2_subdev_call(&entity->subdev, video,
+					       s_stream, 1);
+			if (ret < 0) {
+				mutex_unlock(&pipe->lock);
+				return ret;
+			}
+		}
+	}
+
+	pipe->stream_count++;
+	mutex_unlock(&pipe->lock);
+
+	spin_lock_irqsave(&pipe->irqlock, flags);
+	if (vsp1_pipeline_ready(pipe))
+		vsp1_pipeline_run(pipe);
+	spin_unlock_irqrestore(&pipe->irqlock, flags);
+
+	return 0;
+}
+
+static void vsp1_video_stop_streaming(struct vb2_queue *vq)
+{
+	struct vsp1_video *video = vb2_get_drv_priv(vq);
+	struct vsp1_pipeline *pipe = to_vsp1_pipeline(&video->video.entity);
+	struct vsp1_video_buffer *buffer;
+	unsigned long flags;
+	int ret;
+
+	mutex_lock(&pipe->lock);
+	if (--pipe->stream_count == 0) {
+		/* Stop the pipeline. */
+		ret = vsp1_pipeline_stop(pipe);
+		if (ret == -ETIMEDOUT)
+			dev_err(video->vsp1->dev, "pipeline stop timeout\n");
+	}
+	mutex_unlock(&pipe->lock);
+
+	vsp1_pipeline_cleanup(pipe);
+	media_entity_pipeline_stop(&video->video.entity);
+
+	/* Remove all buffers from the IRQ queue. */
+	spin_lock_irqsave(&video->irqlock, flags);
+	list_for_each_entry(buffer, &video->irqqueue, queue)
+		vb2_buffer_done(&buffer->buf.vb2_buf, VB2_BUF_STATE_ERROR);
+	INIT_LIST_HEAD(&video->irqqueue);
+	spin_unlock_irqrestore(&video->irqlock, flags);
+}
+
+static struct vb2_ops vsp1_video_queue_qops = {
+	.queue_setup = vsp1_video_queue_setup,
+	.buf_prepare = vsp1_video_buffer_prepare,
+	.buf_queue = vsp1_video_buffer_queue,
+	.wait_prepare = vb2_ops_wait_prepare,
+	.wait_finish = vb2_ops_wait_finish,
+	.start_streaming = vsp1_video_start_streaming,
+	.stop_streaming = vsp1_video_stop_streaming,
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 ioctls
+ */
+
+static int
+vsp1_video_querycap(struct file *file, void *fh, struct v4l2_capability *cap)
+{
+	struct v4l2_fh *vfh = file->private_data;
+	struct vsp1_video *video = to_vsp1_video(vfh->vdev);
+
+	cap->capabilities = V4L2_CAP_DEVICE_CAPS | V4L2_CAP_STREAMING
+			  | V4L2_CAP_VIDEO_CAPTURE_MPLANE
+			  | V4L2_CAP_VIDEO_OUTPUT_MPLANE;
+
+	if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		cap->device_caps = V4L2_CAP_VIDEO_CAPTURE_MPLANE
+				 | V4L2_CAP_STREAMING;
+	else
+		cap->device_caps = V4L2_CAP_VIDEO_OUTPUT_MPLANE
+				 | V4L2_CAP_STREAMING;
+
+	strlcpy(cap->driver, "vsp1", sizeof(cap->driver));
+	strlcpy(cap->card, video->video.name, sizeof(cap->card));
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
+		 dev_name(video->vsp1->dev));
+
+	return 0;
+}
+
+static int
+vsp1_video_get_format(struct file *file, void *fh, struct v4l2_format *format)
+{
+	struct v4l2_fh *vfh = file->private_data;
+	struct vsp1_video *video = to_vsp1_video(vfh->vdev);
+
+	if (format->type != video->queue.type)
+		return -EINVAL;
+
+	mutex_lock(&video->lock);
+	format->fmt.pix_mp = video->format;
+	mutex_unlock(&video->lock);
+
+	return 0;
+}
+
+static int
+vsp1_video_try_format(struct file *file, void *fh, struct v4l2_format *format)
+{
+	struct v4l2_fh *vfh = file->private_data;
+	struct vsp1_video *video = to_vsp1_video(vfh->vdev);
+
+	if (format->type != video->queue.type)
+		return -EINVAL;
+
+	return __vsp1_video_try_format(video, &format->fmt.pix_mp, NULL);
+}
+
+static int
+vsp1_video_set_format(struct file *file, void *fh, struct v4l2_format *format)
+{
+	struct v4l2_fh *vfh = file->private_data;
+	struct vsp1_video *video = to_vsp1_video(vfh->vdev);
+	const struct vsp1_format_info *info;
+	int ret;
+
+	if (format->type != video->queue.type)
+		return -EINVAL;
+
+	ret = __vsp1_video_try_format(video, &format->fmt.pix_mp, &info);
+	if (ret < 0)
+		return ret;
+
+	mutex_lock(&video->lock);
+
+	if (vb2_is_busy(&video->queue)) {
+		ret = -EBUSY;
+		goto done;
+	}
+
+	video->format = format->fmt.pix_mp;
+	video->fmtinfo = info;
+
+done:
+	mutex_unlock(&video->lock);
+	return ret;
+}
+
+static int
+vsp1_video_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
+{
+	struct v4l2_fh *vfh = file->private_data;
+	struct vsp1_video *video = to_vsp1_video(vfh->vdev);
+	struct vsp1_pipeline *pipe;
+	int ret;
+
+	if (video->queue.owner && video->queue.owner != file->private_data)
+		return -EBUSY;
+
+	video->sequence = 0;
+
+	/* Start streaming on the pipeline. No link touching an entity in the
+	 * pipeline can be activated or deactivated once streaming is started.
+	 *
+	 * Use the VSP1 pipeline object embedded in the first video object that
+	 * starts streaming.
+	 */
+	pipe = video->video.entity.pipe
+	     ? to_vsp1_pipeline(&video->video.entity) : &video->pipe;
+
+	ret = media_entity_pipeline_start(&video->video.entity, &pipe->pipe);
+	if (ret < 0)
+		return ret;
+
+	/* Verify that the configured format matches the output of the connected
+	 * subdev.
+	 */
+	ret = vsp1_video_verify_format(video);
+	if (ret < 0)
+		goto err_stop;
+
+	ret = vsp1_pipeline_init(pipe, video);
+	if (ret < 0)
+		goto err_stop;
+
+	/* Start the queue. */
+	ret = vb2_streamon(&video->queue, type);
+	if (ret < 0)
+		goto err_cleanup;
+
+	return 0;
+
+err_cleanup:
+	vsp1_pipeline_cleanup(pipe);
+err_stop:
+	media_entity_pipeline_stop(&video->video.entity);
+	return ret;
+}
+
+static const struct v4l2_ioctl_ops vsp1_video_ioctl_ops = {
+	.vidioc_querycap		= vsp1_video_querycap,
+	.vidioc_g_fmt_vid_cap_mplane	= vsp1_video_get_format,
+	.vidioc_s_fmt_vid_cap_mplane	= vsp1_video_set_format,
+	.vidioc_try_fmt_vid_cap_mplane	= vsp1_video_try_format,
+	.vidioc_g_fmt_vid_out_mplane	= vsp1_video_get_format,
+	.vidioc_s_fmt_vid_out_mplane	= vsp1_video_set_format,
+	.vidioc_try_fmt_vid_out_mplane	= vsp1_video_try_format,
+	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
+	.vidioc_querybuf		= vb2_ioctl_querybuf,
+	.vidioc_qbuf			= vb2_ioctl_qbuf,
+	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
+	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
+	.vidioc_prepare_buf		= vb2_ioctl_prepare_buf,
+	.vidioc_streamon		= vsp1_video_streamon,
+	.vidioc_streamoff		= vb2_ioctl_streamoff,
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 File Operations
+ */
+
+static int vsp1_video_open(struct file *file)
+{
+	struct vsp1_video *video = video_drvdata(file);
+	struct v4l2_fh *vfh;
+	int ret = 0;
+
+	vfh = kzalloc(sizeof(*vfh), GFP_KERNEL);
+	if (vfh == NULL)
+		return -ENOMEM;
+
+	v4l2_fh_init(vfh, &video->video);
+	v4l2_fh_add(vfh);
+
+	file->private_data = vfh;
+
+	ret = vsp1_device_get(video->vsp1);
+	if (ret < 0) {
+		v4l2_fh_del(vfh);
+		kfree(vfh);
+	}
+
+	return ret;
+}
+
+static int vsp1_video_release(struct file *file)
+{
+	struct vsp1_video *video = video_drvdata(file);
+	struct v4l2_fh *vfh = file->private_data;
+
+	mutex_lock(&video->lock);
+	if (video->queue.owner == vfh) {
+		vb2_queue_release(&video->queue);
+		video->queue.owner = NULL;
+	}
+	mutex_unlock(&video->lock);
+
+	vsp1_device_put(video->vsp1);
+
+	v4l2_fh_release(file);
+
+	file->private_data = NULL;
+
+	return 0;
+}
+
+static struct v4l2_file_operations vsp1_video_fops = {
+	.owner = THIS_MODULE,
+	.unlocked_ioctl = video_ioctl2,
+	.open = vsp1_video_open,
+	.release = vsp1_video_release,
+	.poll = vb2_fop_poll,
+	.mmap = vb2_fop_mmap,
+};
+
+/* -----------------------------------------------------------------------------
+ * Initialization and Cleanup
+ */
+
+int vsp1_video_init(struct vsp1_video *video, struct vsp1_entity *rwpf)
+{
+	const char *direction;
+	int ret;
+
+	switch (video->type) {
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
+		direction = "output";
+		video->pad.flags = MEDIA_PAD_FL_SINK;
+		break;
+
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
+		direction = "input";
+		video->pad.flags = MEDIA_PAD_FL_SOURCE;
+		video->video.vfl_dir = VFL_DIR_TX;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	video->rwpf = rwpf;
+
+	mutex_init(&video->lock);
+	spin_lock_init(&video->irqlock);
+	INIT_LIST_HEAD(&video->irqqueue);
+
+	mutex_init(&video->pipe.lock);
+	spin_lock_init(&video->pipe.irqlock);
+	INIT_LIST_HEAD(&video->pipe.entities);
+	init_waitqueue_head(&video->pipe.wq);
+	video->pipe.state = VSP1_PIPELINE_STOPPED;
+
+	/* Initialize the media entity... */
+	ret = media_entity_init(&video->video.entity, 1, &video->pad, 0);
+	if (ret < 0)
+		return ret;
+
+	/* ... and the format ... */
+	video->fmtinfo = vsp1_get_format_info(VSP1_VIDEO_DEF_FORMAT);
+	video->format.pixelformat = video->fmtinfo->fourcc;
+	video->format.colorspace = V4L2_COLORSPACE_SRGB;
+	video->format.field = V4L2_FIELD_NONE;
+	video->format.width = VSP1_VIDEO_DEF_WIDTH;
+	video->format.height = VSP1_VIDEO_DEF_HEIGHT;
+	video->format.num_planes = 1;
+	video->format.plane_fmt[0].bytesperline =
+		video->format.width * video->fmtinfo->bpp[0] / 8;
+	video->format.plane_fmt[0].sizeimage =
+		video->format.plane_fmt[0].bytesperline * video->format.height;
+
+	/* ... and the video node... */
+	video->video.v4l2_dev = &video->vsp1->v4l2_dev;
+	video->video.fops = &vsp1_video_fops;
+	snprintf(video->video.name, sizeof(video->video.name), "%s %s",
+		 rwpf->subdev.name, direction);
+	video->video.vfl_type = VFL_TYPE_GRABBER;
+	video->video.release = video_device_release_empty;
+	video->video.ioctl_ops = &vsp1_video_ioctl_ops;
+
+	video_set_drvdata(&video->video, video);
+
+	/* ... and the buffers queue... */
+	video->alloc_ctx = vb2_dma_contig_init_ctx(video->vsp1->dev);
+	if (IS_ERR(video->alloc_ctx)) {
+		ret = PTR_ERR(video->alloc_ctx);
+		goto error;
+	}
+
+	video->queue.type = video->type;
+	video->queue.io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
+	video->queue.lock = &video->lock;
+	video->queue.drv_priv = video;
+	video->queue.buf_struct_size = sizeof(struct vsp1_video_buffer);
+	video->queue.ops = &vsp1_video_queue_qops;
+	video->queue.mem_ops = &vb2_dma_contig_memops;
+	video->queue.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	ret = vb2_queue_init(&video->queue);
+	if (ret < 0) {
+		dev_err(video->vsp1->dev, "failed to initialize vb2 queue\n");
+		goto error;
+	}
+
+	/* ... and register the video device. */
+	video->video.queue = &video->queue;
+	ret = video_register_device(&video->video, VFL_TYPE_GRABBER, -1);
+	if (ret < 0) {
+		dev_err(video->vsp1->dev, "failed to register video device\n");
+		goto error;
+	}
+
+	return 0;
+
+error:
+	vb2_dma_contig_cleanup_ctx(video->alloc_ctx);
+	vsp1_video_cleanup(video);
+	return ret;
+}
+
+void vsp1_video_cleanup(struct vsp1_video *video)
+{
+	if (video_is_registered(&video->video))
+		video_unregister_device(&video->video);
+
+	vb2_dma_contig_cleanup_ctx(video->alloc_ctx);
+	media_entity_cleanup(&video->video.entity);
+}
diff --git a/drivers/media/platform/vsp1/vsp1_video.h b/drivers/media/platform/vsp1/vsp1_video.h
new file mode 100644
index 0000000..a929aa8
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_video.h
@@ -0,0 +1,155 @@
+/*
+ * vsp1_video.h  --  R-Car VSP1 Video Node
+ *
+ * Copyright (C) 2013-2015 Renesas Electronics Corporation
+ *
+ * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+#ifndef __VSP1_VIDEO_H__
+#define __VSP1_VIDEO_H__
+
+#include <linux/list.h>
+#include <linux/spinlock.h>
+#include <linux/wait.h>
+
+#include <media/media-entity.h>
+#include <media/videobuf2-v4l2.h>
+
+struct vsp1_video;
+
+/*
+ * struct vsp1_format_info - VSP1 video format description
+ * @mbus: media bus format code
+ * @fourcc: V4L2 pixel format FCC identifier
+ * @planes: number of planes
+ * @bpp: bits per pixel
+ * @hwfmt: VSP1 hardware format
+ * @swap_yc: the Y and C components are swapped (Y comes before C)
+ * @swap_uv: the U and V components are swapped (V comes before U)
+ * @hsub: horizontal subsampling factor
+ * @vsub: vertical subsampling factor
+ * @alpha: has an alpha channel
+ */
+struct vsp1_format_info {
+	u32 fourcc;
+	unsigned int mbus;
+	unsigned int hwfmt;
+	unsigned int swap;
+	unsigned int planes;
+	unsigned int bpp[3];
+	bool swap_yc;
+	bool swap_uv;
+	unsigned int hsub;
+	unsigned int vsub;
+	bool alpha;
+};
+
+enum vsp1_pipeline_state {
+	VSP1_PIPELINE_STOPPED,
+	VSP1_PIPELINE_RUNNING,
+	VSP1_PIPELINE_STOPPING,
+};
+
+/*
+ * struct vsp1_pipeline - A VSP1 hardware pipeline
+ * @media: the media pipeline
+ * @irqlock: protects the pipeline state
+ * @lock: protects the pipeline use count and stream count
+ */
+struct vsp1_pipeline {
+	struct media_pipeline pipe;
+
+	spinlock_t irqlock;
+	enum vsp1_pipeline_state state;
+	wait_queue_head_t wq;
+
+	struct mutex lock;
+	unsigned int use_count;
+	unsigned int stream_count;
+	unsigned int buffers_ready;
+
+	unsigned int num_video;
+	unsigned int num_inputs;
+	struct vsp1_rwpf *inputs[VSP1_MAX_RPF];
+	struct vsp1_rwpf *output;
+	struct vsp1_entity *bru;
+	struct vsp1_entity *lif;
+	struct vsp1_entity *uds;
+	struct vsp1_entity *uds_input;
+
+	struct list_head entities;
+};
+
+static inline struct vsp1_pipeline *to_vsp1_pipeline(struct media_entity *e)
+{
+	if (likely(e->pipe))
+		return container_of(e->pipe, struct vsp1_pipeline, pipe);
+	else
+		return NULL;
+}
+
+struct vsp1_video_buffer {
+	struct vb2_v4l2_buffer buf;
+	struct list_head queue;
+
+	dma_addr_t addr[3];
+	unsigned int length[3];
+};
+
+static inline struct vsp1_video_buffer *
+to_vsp1_video_buffer(struct vb2_v4l2_buffer *vbuf)
+{
+	return container_of(vbuf, struct vsp1_video_buffer, buf);
+}
+
+struct vsp1_video_operations {
+	void (*queue)(struct vsp1_video *video, struct vsp1_video_buffer *buf);
+};
+
+struct vsp1_video {
+	struct vsp1_device *vsp1;
+	struct vsp1_entity *rwpf;
+
+	const struct vsp1_video_operations *ops;
+
+	struct video_device video;
+	enum v4l2_buf_type type;
+	struct media_pad pad;
+
+	struct mutex lock;
+	struct v4l2_pix_format_mplane format;
+	const struct vsp1_format_info *fmtinfo;
+
+	struct vsp1_pipeline pipe;
+	unsigned int pipe_index;
+
+	struct vb2_queue queue;
+	void *alloc_ctx;
+	spinlock_t irqlock;
+	struct list_head irqqueue;
+	unsigned int sequence;
+};
+
+static inline struct vsp1_video *to_vsp1_video(struct video_device *vdev)
+{
+	return container_of(vdev, struct vsp1_video, video);
+}
+
+int vsp1_video_init(struct vsp1_video *video, struct vsp1_entity *rwpf);
+void vsp1_video_cleanup(struct vsp1_video *video);
+
+void vsp1_pipeline_frame_end(struct vsp1_pipeline *pipe);
+
+void vsp1_pipeline_propagate_alpha(struct vsp1_pipeline *pipe,
+				   struct vsp1_entity *input,
+				   unsigned int alpha);
+
+void vsp1_pipelines_suspend(struct vsp1_device *vsp1);
+void vsp1_pipelines_resume(struct vsp1_device *vsp1);
+
+#endif /* __VSP1_VIDEO_H__ */
diff --git a/drivers/media/platform/vsp1/vsp1_wpf.c b/drivers/media/platform/vsp1/vsp1_wpf.c
new file mode 100644
index 0000000..95b62f4
--- /dev/null
+++ b/drivers/media/platform/vsp1/vsp1_wpf.c
@@ -0,0 +1,300 @@
+/*
+ * vsp1_wpf.c  --  R-Car VSP1 Write Pixel Formatter
+ *
+ * Copyright (C) 2013-2014 Renesas Electronics Corporation
+ *
+ * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/device.h>
+
+#include <media/v4l2-subdev.h>
+
+#include "vsp1.h"
+#include "vsp1_rwpf.h"
+#include "vsp1_video.h"
+
+#define WPF_MAX_WIDTH				2048
+#define WPF_MAX_HEIGHT				2048
+
+/* -----------------------------------------------------------------------------
+ * Device Access
+ */
+
+static inline u32 vsp1_wpf_read(struct vsp1_rwpf *wpf, u32 reg)
+{
+	return vsp1_read(wpf->entity.vsp1,
+			 reg + wpf->entity.index * VI6_WPF_OFFSET);
+}
+
+static inline void vsp1_wpf_write(struct vsp1_rwpf *wpf, u32 reg, u32 data)
+{
+	vsp1_write(wpf->entity.vsp1,
+		   reg + wpf->entity.index * VI6_WPF_OFFSET, data);
+}
+
+/* -----------------------------------------------------------------------------
+ * Controls
+ */
+
+static int wpf_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vsp1_rwpf *wpf =
+		container_of(ctrl->handler, struct vsp1_rwpf, ctrls);
+	u32 value;
+
+	if (!vsp1_entity_is_streaming(&wpf->entity))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_ALPHA_COMPONENT:
+		value = vsp1_wpf_read(wpf, VI6_WPF_OUTFMT);
+		value &= ~VI6_WPF_OUTFMT_PDV_MASK;
+		value |= ctrl->val << VI6_WPF_OUTFMT_PDV_SHIFT;
+		vsp1_wpf_write(wpf, VI6_WPF_OUTFMT, value);
+		break;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops wpf_ctrl_ops = {
+	.s_ctrl = wpf_s_ctrl,
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Core Operations
+ */
+
+static int wpf_s_stream(struct v4l2_subdev *subdev, int enable)
+{
+	struct vsp1_pipeline *pipe = to_vsp1_pipeline(&subdev->entity);
+	struct vsp1_rwpf *wpf = to_rwpf(subdev);
+	struct vsp1_device *vsp1 = wpf->entity.vsp1;
+	const struct v4l2_rect *crop = &wpf->crop;
+	unsigned int i;
+	u32 srcrpf = 0;
+	u32 outfmt = 0;
+	int ret;
+
+	ret = vsp1_entity_set_streaming(&wpf->entity, enable);
+	if (ret < 0)
+		return ret;
+
+	if (!enable) {
+		vsp1_write(vsp1, VI6_WPF_IRQ_ENB(wpf->entity.index), 0);
+		vsp1_wpf_write(wpf, VI6_WPF_SRCRPF, 0);
+		return 0;
+	}
+
+	/* Sources. If the pipeline has a single input and BRU is not used,
+	 * configure it as the master layer. Otherwise configure all
+	 * inputs as sub-layers and select the virtual RPF as the master
+	 * layer.
+	 */
+	for (i = 0; i < pipe->num_inputs; ++i) {
+		struct vsp1_rwpf *input = pipe->inputs[i];
+
+		srcrpf |= (!pipe->bru && pipe->num_inputs == 1)
+			? VI6_WPF_SRCRPF_RPF_ACT_MST(input->entity.index)
+			: VI6_WPF_SRCRPF_RPF_ACT_SUB(input->entity.index);
+	}
+
+	if (pipe->bru || pipe->num_inputs > 1)
+		srcrpf |= VI6_WPF_SRCRPF_VIRACT_MST;
+
+	vsp1_wpf_write(wpf, VI6_WPF_SRCRPF, srcrpf);
+
+	/* Destination stride. */
+	if (!pipe->lif) {
+		struct v4l2_pix_format_mplane *format = &wpf->video.format;
+
+		vsp1_wpf_write(wpf, VI6_WPF_DSTM_STRIDE_Y,
+			       format->plane_fmt[0].bytesperline);
+		if (format->num_planes > 1)
+			vsp1_wpf_write(wpf, VI6_WPF_DSTM_STRIDE_C,
+				       format->plane_fmt[1].bytesperline);
+	}
+
+	vsp1_wpf_write(wpf, VI6_WPF_HSZCLIP, VI6_WPF_SZCLIP_EN |
+		       (crop->left << VI6_WPF_SZCLIP_OFST_SHIFT) |
+		       (crop->width << VI6_WPF_SZCLIP_SIZE_SHIFT));
+	vsp1_wpf_write(wpf, VI6_WPF_VSZCLIP, VI6_WPF_SZCLIP_EN |
+		       (crop->top << VI6_WPF_SZCLIP_OFST_SHIFT) |
+		       (crop->height << VI6_WPF_SZCLIP_SIZE_SHIFT));
+
+	/* Format */
+	if (!pipe->lif) {
+		const struct vsp1_format_info *fmtinfo = wpf->video.fmtinfo;
+
+		outfmt = fmtinfo->hwfmt << VI6_WPF_OUTFMT_WRFMT_SHIFT;
+
+		if (fmtinfo->alpha)
+			outfmt |= VI6_WPF_OUTFMT_PXA;
+		if (fmtinfo->swap_yc)
+			outfmt |= VI6_WPF_OUTFMT_SPYCS;
+		if (fmtinfo->swap_uv)
+			outfmt |= VI6_WPF_OUTFMT_SPUVS;
+
+		vsp1_wpf_write(wpf, VI6_WPF_DSWAP, fmtinfo->swap);
+	}
+
+	if (wpf->entity.formats[RWPF_PAD_SINK].code !=
+	    wpf->entity.formats[RWPF_PAD_SOURCE].code)
+		outfmt |= VI6_WPF_OUTFMT_CSC;
+
+	/* Take the control handler lock to ensure that the PDV value won't be
+	 * changed behind our back by a set control operation.
+	 */
+	mutex_lock(wpf->ctrls.lock);
+	outfmt |= vsp1_wpf_read(wpf, VI6_WPF_OUTFMT) & VI6_WPF_OUTFMT_PDV_MASK;
+	vsp1_wpf_write(wpf, VI6_WPF_OUTFMT, outfmt);
+	mutex_unlock(wpf->ctrls.lock);
+
+	vsp1_write(vsp1, VI6_DPR_WPF_FPORCH(wpf->entity.index),
+		   VI6_DPR_WPF_FPORCH_FP_WPFN);
+
+	vsp1_write(vsp1, VI6_WPF_WRBCK_CTRL, 0);
+
+	/* Enable interrupts */
+	vsp1_write(vsp1, VI6_WPF_IRQ_STA(wpf->entity.index), 0);
+	vsp1_write(vsp1, VI6_WPF_IRQ_ENB(wpf->entity.index),
+		   VI6_WFP_IRQ_ENB_FREE);
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Operations
+ */
+
+static struct v4l2_subdev_video_ops wpf_video_ops = {
+	.s_stream = wpf_s_stream,
+};
+
+static struct v4l2_subdev_pad_ops wpf_pad_ops = {
+	.enum_mbus_code = vsp1_rwpf_enum_mbus_code,
+	.enum_frame_size = vsp1_rwpf_enum_frame_size,
+	.get_fmt = vsp1_rwpf_get_format,
+	.set_fmt = vsp1_rwpf_set_format,
+	.get_selection = vsp1_rwpf_get_selection,
+	.set_selection = vsp1_rwpf_set_selection,
+};
+
+static struct v4l2_subdev_ops wpf_ops = {
+	.video	= &wpf_video_ops,
+	.pad    = &wpf_pad_ops,
+};
+
+/* -----------------------------------------------------------------------------
+ * Video Device Operations
+ */
+
+static void wpf_vdev_queue(struct vsp1_video *video,
+			   struct vsp1_video_buffer *buf)
+{
+	struct vsp1_rwpf *wpf = container_of(video, struct vsp1_rwpf, video);
+
+	vsp1_wpf_write(wpf, VI6_WPF_DSTM_ADDR_Y, buf->addr[0]);
+	if (buf->buf.vb2_buf.num_planes > 1)
+		vsp1_wpf_write(wpf, VI6_WPF_DSTM_ADDR_C0, buf->addr[1]);
+	if (buf->buf.vb2_buf.num_planes > 2)
+		vsp1_wpf_write(wpf, VI6_WPF_DSTM_ADDR_C1, buf->addr[2]);
+}
+
+static const struct vsp1_video_operations wpf_vdev_ops = {
+	.queue = wpf_vdev_queue,
+};
+
+/* -----------------------------------------------------------------------------
+ * Initialization and Cleanup
+ */
+
+struct vsp1_rwpf *vsp1_wpf_create(struct vsp1_device *vsp1, unsigned int index)
+{
+	struct v4l2_subdev *subdev;
+	struct vsp1_video *video;
+	struct vsp1_rwpf *wpf;
+	unsigned int flags;
+	int ret;
+
+	wpf = devm_kzalloc(vsp1->dev, sizeof(*wpf), GFP_KERNEL);
+	if (wpf == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	wpf->max_width = WPF_MAX_WIDTH;
+	wpf->max_height = WPF_MAX_HEIGHT;
+
+	wpf->entity.type = VSP1_ENTITY_WPF;
+	wpf->entity.index = index;
+
+	ret = vsp1_entity_init(vsp1, &wpf->entity, 2);
+	if (ret < 0)
+		return ERR_PTR(ret);
+
+	/* Initialize the V4L2 subdev. */
+	subdev = &wpf->entity.subdev;
+	v4l2_subdev_init(subdev, &wpf_ops);
+
+	subdev->entity.ops = &vsp1_media_ops;
+	subdev->internal_ops = &vsp1_subdev_internal_ops;
+	snprintf(subdev->name, sizeof(subdev->name), "%s wpf.%u",
+		 dev_name(vsp1->dev), index);
+	v4l2_set_subdevdata(subdev, wpf);
+	subdev->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	vsp1_entity_init_formats(subdev, NULL);
+
+	/* Initialize the control handler. */
+	v4l2_ctrl_handler_init(&wpf->ctrls, 1);
+	v4l2_ctrl_new_std(&wpf->ctrls, &wpf_ctrl_ops, V4L2_CID_ALPHA_COMPONENT,
+			  0, 255, 1, 255);
+
+	wpf->entity.subdev.ctrl_handler = &wpf->ctrls;
+
+	if (wpf->ctrls.error) {
+		dev_err(vsp1->dev, "wpf%u: failed to initialize controls\n",
+			index);
+		ret = wpf->ctrls.error;
+		goto error;
+	}
+
+	/* Initialize the video device. */
+	video = &wpf->video;
+
+	video->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	video->vsp1 = vsp1;
+	video->ops = &wpf_vdev_ops;
+
+	ret = vsp1_video_init(video, &wpf->entity);
+	if (ret < 0)
+		goto error;
+
+	wpf->entity.video = video;
+
+	/* Connect the video device to the WPF. All connections are immutable
+	 * except for the WPF0 source link if a LIF is present.
+	 */
+	flags = MEDIA_LNK_FL_ENABLED;
+	if (!(vsp1->pdata.features & VSP1_HAS_LIF) || index != 0)
+		flags |= MEDIA_LNK_FL_IMMUTABLE;
+
+	ret = media_entity_create_link(&wpf->entity.subdev.entity,
+				       RWPF_PAD_SOURCE,
+				       &wpf->video.video.entity, 0, flags);
+	if (ret < 0)
+		goto error;
+
+	wpf->entity.sink = &wpf->video.video.entity;
+
+	return wpf;
+
+error:
+	vsp1_entity_destroy(&wpf->entity);
+	return ERR_PTR(ret);
+}
diff --git a/drivers/media/platform/xilinx/Kconfig b/drivers/media/platform/xilinx/Kconfig
new file mode 100644
index 0000000..84bae79
--- /dev/null
+++ b/drivers/media/platform/xilinx/Kconfig
@@ -0,0 +1,23 @@
+config VIDEO_XILINX
+	tristate "Xilinx Video IP (EXPERIMENTAL)"
+	depends on VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API && OF && HAS_DMA
+	select VIDEOBUF2_DMA_CONTIG
+	---help---
+	  Driver for Xilinx Video IP Pipelines
+
+if VIDEO_XILINX
+
+config VIDEO_XILINX_TPG
+	tristate "Xilinx Video Test Pattern Generator"
+	depends on VIDEO_XILINX
+	select VIDEO_XILINX_VTC
+	---help---
+	   Driver for the Xilinx Video Test Pattern Generator
+
+config VIDEO_XILINX_VTC
+	tristate "Xilinx Video Timing Controller"
+	depends on VIDEO_XILINX
+	---help---
+	   Driver for the Xilinx Video Timing Controller
+
+endif #VIDEO_XILINX
diff --git a/drivers/media/platform/xilinx/Makefile b/drivers/media/platform/xilinx/Makefile
new file mode 100644
index 0000000..e8a0f2a
--- /dev/null
+++ b/drivers/media/platform/xilinx/Makefile
@@ -0,0 +1,5 @@
+xilinx-video-objs += xilinx-dma.o xilinx-vip.o xilinx-vipp.o
+
+obj-$(CONFIG_VIDEO_XILINX) += xilinx-video.o
+obj-$(CONFIG_VIDEO_XILINX_TPG) += xilinx-tpg.o
+obj-$(CONFIG_VIDEO_XILINX_VTC) += xilinx-vtc.o
diff --git a/drivers/media/platform/xilinx/xilinx-dma.c b/drivers/media/platform/xilinx/xilinx-dma.c
new file mode 100644
index 0000000..d11cc70
--- /dev/null
+++ b/drivers/media/platform/xilinx/xilinx-dma.c
@@ -0,0 +1,771 @@
+/*
+ * Xilinx Video DMA
+ *
+ * Copyright (C) 2013-2015 Ideas on Board
+ * Copyright (C) 2013-2015 Xilinx, Inc.
+ *
+ * Contacts: Hyun Kwon <hyun.kwon@xilinx.com>
+ *           Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/dma/xilinx_dma.h>
+#include <linux/lcm.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/slab.h>
+
+#include <media/v4l2-dev.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "xilinx-dma.h"
+#include "xilinx-vip.h"
+#include "xilinx-vipp.h"
+
+#define XVIP_DMA_DEF_FORMAT		V4L2_PIX_FMT_YUYV
+#define XVIP_DMA_DEF_WIDTH		1920
+#define XVIP_DMA_DEF_HEIGHT		1080
+
+/* Minimum and maximum widths are expressed in bytes */
+#define XVIP_DMA_MIN_WIDTH		1U
+#define XVIP_DMA_MAX_WIDTH		65535U
+#define XVIP_DMA_MIN_HEIGHT		1U
+#define XVIP_DMA_MAX_HEIGHT		8191U
+
+/* -----------------------------------------------------------------------------
+ * Helper functions
+ */
+
+static struct v4l2_subdev *
+xvip_dma_remote_subdev(struct media_pad *local, u32 *pad)
+{
+	struct media_pad *remote;
+
+	remote = media_entity_remote_pad(local);
+	if (remote == NULL ||
+	    media_entity_type(remote->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+		return NULL;
+
+	if (pad)
+		*pad = remote->index;
+
+	return media_entity_to_v4l2_subdev(remote->entity);
+}
+
+static int xvip_dma_verify_format(struct xvip_dma *dma)
+{
+	struct v4l2_subdev_format fmt;
+	struct v4l2_subdev *subdev;
+	int ret;
+
+	subdev = xvip_dma_remote_subdev(&dma->pad, &fmt.pad);
+	if (subdev == NULL)
+		return -EPIPE;
+
+	fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt);
+	if (ret < 0)
+		return ret == -ENOIOCTLCMD ? -EINVAL : ret;
+
+	if (dma->fmtinfo->code != fmt.format.code ||
+	    dma->format.height != fmt.format.height ||
+	    dma->format.width != fmt.format.width ||
+	    dma->format.colorspace != fmt.format.colorspace)
+		return -EINVAL;
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * Pipeline Stream Management
+ */
+
+/**
+ * xvip_pipeline_start_stop - Start ot stop streaming on a pipeline
+ * @pipe: The pipeline
+ * @start: Start (when true) or stop (when false) the pipeline
+ *
+ * Walk the entities chain starting at the pipeline output video node and start
+ * or stop all of them.
+ *
+ * Return: 0 if successful, or the return value of the failed video::s_stream
+ * operation otherwise.
+ */
+static int xvip_pipeline_start_stop(struct xvip_pipeline *pipe, bool start)
+{
+	struct xvip_dma *dma = pipe->output;
+	struct media_entity *entity;
+	struct media_pad *pad;
+	struct v4l2_subdev *subdev;
+	int ret;
+
+	entity = &dma->video.entity;
+	while (1) {
+		pad = &entity->pads[0];
+		if (!(pad->flags & MEDIA_PAD_FL_SINK))
+			break;
+
+		pad = media_entity_remote_pad(pad);
+		if (pad == NULL ||
+		    media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+			break;
+
+		entity = pad->entity;
+		subdev = media_entity_to_v4l2_subdev(entity);
+
+		ret = v4l2_subdev_call(subdev, video, s_stream, start);
+		if (start && ret < 0 && ret != -ENOIOCTLCMD)
+			return ret;
+	}
+
+	return 0;
+}
+
+/**
+ * xvip_pipeline_set_stream - Enable/disable streaming on a pipeline
+ * @pipe: The pipeline
+ * @on: Turn the stream on when true or off when false
+ *
+ * The pipeline is shared between all DMA engines connect at its input and
+ * output. While the stream state of DMA engines can be controlled
+ * independently, pipelines have a shared stream state that enable or disable
+ * all entities in the pipeline. For this reason the pipeline uses a streaming
+ * counter that tracks the number of DMA engines that have requested the stream
+ * to be enabled.
+ *
+ * When called with the @on argument set to true, this function will increment
+ * the pipeline streaming count. If the streaming count reaches the number of
+ * DMA engines in the pipeline it will enable all entities that belong to the
+ * pipeline.
+ *
+ * Similarly, when called with the @on argument set to false, this function will
+ * decrement the pipeline streaming count and disable all entities in the
+ * pipeline when the streaming count reaches zero.
+ *
+ * Return: 0 if successful, or the return value of the failed video::s_stream
+ * operation otherwise. Stopping the pipeline never fails. The pipeline state is
+ * not updated when the operation fails.
+ */
+static int xvip_pipeline_set_stream(struct xvip_pipeline *pipe, bool on)
+{
+	int ret = 0;
+
+	mutex_lock(&pipe->lock);
+
+	if (on) {
+		if (pipe->stream_count == pipe->num_dmas - 1) {
+			ret = xvip_pipeline_start_stop(pipe, true);
+			if (ret < 0)
+				goto done;
+		}
+		pipe->stream_count++;
+	} else {
+		if (--pipe->stream_count == 0)
+			xvip_pipeline_start_stop(pipe, false);
+	}
+
+done:
+	mutex_unlock(&pipe->lock);
+	return ret;
+}
+
+static int xvip_pipeline_validate(struct xvip_pipeline *pipe,
+				  struct xvip_dma *start)
+{
+	struct media_entity_graph graph;
+	struct media_entity *entity = &start->video.entity;
+	struct media_device *mdev = entity->parent;
+	unsigned int num_inputs = 0;
+	unsigned int num_outputs = 0;
+
+	mutex_lock(&mdev->graph_mutex);
+
+	/* Walk the graph to locate the video nodes. */
+	media_entity_graph_walk_start(&graph, entity);
+
+	while ((entity = media_entity_graph_walk_next(&graph))) {
+		struct xvip_dma *dma;
+
+		if (entity->type != MEDIA_ENT_T_DEVNODE_V4L)
+			continue;
+
+		dma = to_xvip_dma(media_entity_to_video_device(entity));
+
+		if (dma->pad.flags & MEDIA_PAD_FL_SINK) {
+			pipe->output = dma;
+			num_outputs++;
+		} else {
+			num_inputs++;
+		}
+	}
+
+	mutex_unlock(&mdev->graph_mutex);
+
+	/* We need exactly one output and zero or one input. */
+	if (num_outputs != 1 || num_inputs > 1)
+		return -EPIPE;
+
+	pipe->num_dmas = num_inputs + num_outputs;
+
+	return 0;
+}
+
+static void __xvip_pipeline_cleanup(struct xvip_pipeline *pipe)
+{
+	pipe->num_dmas = 0;
+	pipe->output = NULL;
+}
+
+/**
+ * xvip_pipeline_cleanup - Cleanup the pipeline after streaming
+ * @pipe: the pipeline
+ *
+ * Decrease the pipeline use count and clean it up if we were the last user.
+ */
+static void xvip_pipeline_cleanup(struct xvip_pipeline *pipe)
+{
+	mutex_lock(&pipe->lock);
+
+	/* If we're the last user clean up the pipeline. */
+	if (--pipe->use_count == 0)
+		__xvip_pipeline_cleanup(pipe);
+
+	mutex_unlock(&pipe->lock);
+}
+
+/**
+ * xvip_pipeline_prepare - Prepare the pipeline for streaming
+ * @pipe: the pipeline
+ * @dma: DMA engine at one end of the pipeline
+ *
+ * Validate the pipeline if no user exists yet, otherwise just increase the use
+ * count.
+ *
+ * Return: 0 if successful or -EPIPE if the pipeline is not valid.
+ */
+static int xvip_pipeline_prepare(struct xvip_pipeline *pipe,
+				 struct xvip_dma *dma)
+{
+	int ret;
+
+	mutex_lock(&pipe->lock);
+
+	/* If we're the first user validate and initialize the pipeline. */
+	if (pipe->use_count == 0) {
+		ret = xvip_pipeline_validate(pipe, dma);
+		if (ret < 0) {
+			__xvip_pipeline_cleanup(pipe);
+			goto done;
+		}
+	}
+
+	pipe->use_count++;
+	ret = 0;
+
+done:
+	mutex_unlock(&pipe->lock);
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * videobuf2 queue operations
+ */
+
+/**
+ * struct xvip_dma_buffer - Video DMA buffer
+ * @buf: vb2 buffer base object
+ * @queue: buffer list entry in the DMA engine queued buffers list
+ * @dma: DMA channel that uses the buffer
+ */
+struct xvip_dma_buffer {
+	struct vb2_v4l2_buffer buf;
+	struct list_head queue;
+	struct xvip_dma *dma;
+};
+
+#define to_xvip_dma_buffer(vb)	container_of(vb, struct xvip_dma_buffer, buf)
+
+static void xvip_dma_complete(void *param)
+{
+	struct xvip_dma_buffer *buf = param;
+	struct xvip_dma *dma = buf->dma;
+
+	spin_lock(&dma->queued_lock);
+	list_del(&buf->queue);
+	spin_unlock(&dma->queued_lock);
+
+	buf->buf.field = V4L2_FIELD_NONE;
+	buf->buf.sequence = dma->sequence++;
+	v4l2_get_timestamp(&buf->buf.timestamp);
+	vb2_set_plane_payload(&buf->buf.vb2_buf, 0, dma->format.sizeimage);
+	vb2_buffer_done(&buf->buf.vb2_buf, VB2_BUF_STATE_DONE);
+}
+
+static int
+xvip_dma_queue_setup(struct vb2_queue *vq, const void *parg,
+		     unsigned int *nbuffers, unsigned int *nplanes,
+		     unsigned int sizes[], void *alloc_ctxs[])
+{
+	const struct v4l2_format *fmt = parg;
+	struct xvip_dma *dma = vb2_get_drv_priv(vq);
+
+	/* Make sure the image size is large enough. */
+	if (fmt && fmt->fmt.pix.sizeimage < dma->format.sizeimage)
+		return -EINVAL;
+
+	*nplanes = 1;
+
+	sizes[0] = fmt ? fmt->fmt.pix.sizeimage : dma->format.sizeimage;
+	alloc_ctxs[0] = dma->alloc_ctx;
+
+	return 0;
+}
+
+static int xvip_dma_buffer_prepare(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct xvip_dma *dma = vb2_get_drv_priv(vb->vb2_queue);
+	struct xvip_dma_buffer *buf = to_xvip_dma_buffer(vbuf);
+
+	buf->dma = dma;
+
+	return 0;
+}
+
+static void xvip_dma_buffer_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct xvip_dma *dma = vb2_get_drv_priv(vb->vb2_queue);
+	struct xvip_dma_buffer *buf = to_xvip_dma_buffer(vbuf);
+	struct dma_async_tx_descriptor *desc;
+	dma_addr_t addr = vb2_dma_contig_plane_dma_addr(vb, 0);
+	u32 flags;
+
+	if (dma->queue.type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+		flags = DMA_PREP_INTERRUPT | DMA_CTRL_ACK;
+		dma->xt.dir = DMA_DEV_TO_MEM;
+		dma->xt.src_sgl = false;
+		dma->xt.dst_sgl = true;
+		dma->xt.dst_start = addr;
+	} else {
+		flags = DMA_PREP_INTERRUPT | DMA_CTRL_ACK;
+		dma->xt.dir = DMA_MEM_TO_DEV;
+		dma->xt.src_sgl = true;
+		dma->xt.dst_sgl = false;
+		dma->xt.src_start = addr;
+	}
+
+	dma->xt.frame_size = 1;
+	dma->sgl[0].size = dma->format.width * dma->fmtinfo->bpp;
+	dma->sgl[0].icg = dma->format.bytesperline - dma->sgl[0].size;
+	dma->xt.numf = dma->format.height;
+
+	desc = dmaengine_prep_interleaved_dma(dma->dma, &dma->xt, flags);
+	if (!desc) {
+		dev_err(dma->xdev->dev, "Failed to prepare DMA transfer\n");
+		vb2_buffer_done(&buf->buf.vb2_buf, VB2_BUF_STATE_ERROR);
+		return;
+	}
+	desc->callback = xvip_dma_complete;
+	desc->callback_param = buf;
+
+	spin_lock_irq(&dma->queued_lock);
+	list_add_tail(&buf->queue, &dma->queued_bufs);
+	spin_unlock_irq(&dma->queued_lock);
+
+	dmaengine_submit(desc);
+
+	if (vb2_is_streaming(&dma->queue))
+		dma_async_issue_pending(dma->dma);
+}
+
+static int xvip_dma_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct xvip_dma *dma = vb2_get_drv_priv(vq);
+	struct xvip_dma_buffer *buf, *nbuf;
+	struct xvip_pipeline *pipe;
+	int ret;
+
+	dma->sequence = 0;
+
+	/*
+	 * Start streaming on the pipeline. No link touching an entity in the
+	 * pipeline can be activated or deactivated once streaming is started.
+	 *
+	 * Use the pipeline object embedded in the first DMA object that starts
+	 * streaming.
+	 */
+	pipe = dma->video.entity.pipe
+	     ? to_xvip_pipeline(&dma->video.entity) : &dma->pipe;
+
+	ret = media_entity_pipeline_start(&dma->video.entity, &pipe->pipe);
+	if (ret < 0)
+		goto error;
+
+	/* Verify that the configured format matches the output of the
+	 * connected subdev.
+	 */
+	ret = xvip_dma_verify_format(dma);
+	if (ret < 0)
+		goto error_stop;
+
+	ret = xvip_pipeline_prepare(pipe, dma);
+	if (ret < 0)
+		goto error_stop;
+
+	/* Start the DMA engine. This must be done before starting the blocks
+	 * in the pipeline to avoid DMA synchronization issues.
+	 */
+	dma_async_issue_pending(dma->dma);
+
+	/* Start the pipeline. */
+	xvip_pipeline_set_stream(pipe, true);
+
+	return 0;
+
+error_stop:
+	media_entity_pipeline_stop(&dma->video.entity);
+
+error:
+	/* Give back all queued buffers to videobuf2. */
+	spin_lock_irq(&dma->queued_lock);
+	list_for_each_entry_safe(buf, nbuf, &dma->queued_bufs, queue) {
+		vb2_buffer_done(&buf->buf.vb2_buf, VB2_BUF_STATE_QUEUED);
+		list_del(&buf->queue);
+	}
+	spin_unlock_irq(&dma->queued_lock);
+
+	return ret;
+}
+
+static void xvip_dma_stop_streaming(struct vb2_queue *vq)
+{
+	struct xvip_dma *dma = vb2_get_drv_priv(vq);
+	struct xvip_pipeline *pipe = to_xvip_pipeline(&dma->video.entity);
+	struct xvip_dma_buffer *buf, *nbuf;
+
+	/* Stop the pipeline. */
+	xvip_pipeline_set_stream(pipe, false);
+
+	/* Stop and reset the DMA engine. */
+	dmaengine_terminate_all(dma->dma);
+
+	/* Cleanup the pipeline and mark it as being stopped. */
+	xvip_pipeline_cleanup(pipe);
+	media_entity_pipeline_stop(&dma->video.entity);
+
+	/* Give back all queued buffers to videobuf2. */
+	spin_lock_irq(&dma->queued_lock);
+	list_for_each_entry_safe(buf, nbuf, &dma->queued_bufs, queue) {
+		vb2_buffer_done(&buf->buf.vb2_buf, VB2_BUF_STATE_ERROR);
+		list_del(&buf->queue);
+	}
+	spin_unlock_irq(&dma->queued_lock);
+}
+
+static struct vb2_ops xvip_dma_queue_qops = {
+	.queue_setup = xvip_dma_queue_setup,
+	.buf_prepare = xvip_dma_buffer_prepare,
+	.buf_queue = xvip_dma_buffer_queue,
+	.wait_prepare = vb2_ops_wait_prepare,
+	.wait_finish = vb2_ops_wait_finish,
+	.start_streaming = xvip_dma_start_streaming,
+	.stop_streaming = xvip_dma_stop_streaming,
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 ioctls
+ */
+
+static int
+xvip_dma_querycap(struct file *file, void *fh, struct v4l2_capability *cap)
+{
+	struct v4l2_fh *vfh = file->private_data;
+	struct xvip_dma *dma = to_xvip_dma(vfh->vdev);
+
+	cap->capabilities = V4L2_CAP_DEVICE_CAPS | V4L2_CAP_STREAMING
+			  | dma->xdev->v4l2_caps;
+
+	if (dma->queue.type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+	else
+		cap->device_caps = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING;
+
+	strlcpy(cap->driver, "xilinx-vipp", sizeof(cap->driver));
+	strlcpy(cap->card, dma->video.name, sizeof(cap->card));
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s:%u",
+		 dma->xdev->dev->of_node->name, dma->port);
+
+	return 0;
+}
+
+/* FIXME: without this callback function, some applications are not configured
+ * with correct formats, and it results in frames in wrong format. Whether this
+ * callback needs to be required is not clearly defined, so it should be
+ * clarified through the mailing list.
+ */
+static int
+xvip_dma_enum_format(struct file *file, void *fh, struct v4l2_fmtdesc *f)
+{
+	struct v4l2_fh *vfh = file->private_data;
+	struct xvip_dma *dma = to_xvip_dma(vfh->vdev);
+
+	if (f->index > 0)
+		return -EINVAL;
+
+	f->pixelformat = dma->format.pixelformat;
+	strlcpy(f->description, dma->fmtinfo->description,
+		sizeof(f->description));
+
+	return 0;
+}
+
+static int
+xvip_dma_get_format(struct file *file, void *fh, struct v4l2_format *format)
+{
+	struct v4l2_fh *vfh = file->private_data;
+	struct xvip_dma *dma = to_xvip_dma(vfh->vdev);
+
+	format->fmt.pix = dma->format;
+
+	return 0;
+}
+
+static void
+__xvip_dma_try_format(struct xvip_dma *dma, struct v4l2_pix_format *pix,
+		      const struct xvip_video_format **fmtinfo)
+{
+	const struct xvip_video_format *info;
+	unsigned int min_width;
+	unsigned int max_width;
+	unsigned int min_bpl;
+	unsigned int max_bpl;
+	unsigned int width;
+	unsigned int align;
+	unsigned int bpl;
+
+	/* Retrieve format information and select the default format if the
+	 * requested format isn't supported.
+	 */
+	info = xvip_get_format_by_fourcc(pix->pixelformat);
+	if (IS_ERR(info))
+		info = xvip_get_format_by_fourcc(XVIP_DMA_DEF_FORMAT);
+
+	pix->pixelformat = info->fourcc;
+	pix->field = V4L2_FIELD_NONE;
+
+	/* The transfer alignment requirements are expressed in bytes. Compute
+	 * the minimum and maximum values, clamp the requested width and convert
+	 * it back to pixels.
+	 */
+	align = lcm(dma->align, info->bpp);
+	min_width = roundup(XVIP_DMA_MIN_WIDTH, align);
+	max_width = rounddown(XVIP_DMA_MAX_WIDTH, align);
+	width = rounddown(pix->width * info->bpp, align);
+
+	pix->width = clamp(width, min_width, max_width) / info->bpp;
+	pix->height = clamp(pix->height, XVIP_DMA_MIN_HEIGHT,
+			    XVIP_DMA_MAX_HEIGHT);
+
+	/* Clamp the requested bytes per line value. If the maximum bytes per
+	 * line value is zero, the module doesn't support user configurable line
+	 * sizes. Override the requested value with the minimum in that case.
+	 */
+	min_bpl = pix->width * info->bpp;
+	max_bpl = rounddown(XVIP_DMA_MAX_WIDTH, dma->align);
+	bpl = rounddown(pix->bytesperline, dma->align);
+
+	pix->bytesperline = clamp(bpl, min_bpl, max_bpl);
+	pix->sizeimage = pix->bytesperline * pix->height;
+
+	if (fmtinfo)
+		*fmtinfo = info;
+}
+
+static int
+xvip_dma_try_format(struct file *file, void *fh, struct v4l2_format *format)
+{
+	struct v4l2_fh *vfh = file->private_data;
+	struct xvip_dma *dma = to_xvip_dma(vfh->vdev);
+
+	__xvip_dma_try_format(dma, &format->fmt.pix, NULL);
+	return 0;
+}
+
+static int
+xvip_dma_set_format(struct file *file, void *fh, struct v4l2_format *format)
+{
+	struct v4l2_fh *vfh = file->private_data;
+	struct xvip_dma *dma = to_xvip_dma(vfh->vdev);
+	const struct xvip_video_format *info;
+
+	__xvip_dma_try_format(dma, &format->fmt.pix, &info);
+
+	if (vb2_is_busy(&dma->queue))
+		return -EBUSY;
+
+	dma->format = format->fmt.pix;
+	dma->fmtinfo = info;
+
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops xvip_dma_ioctl_ops = {
+	.vidioc_querycap		= xvip_dma_querycap,
+	.vidioc_enum_fmt_vid_cap	= xvip_dma_enum_format,
+	.vidioc_g_fmt_vid_cap		= xvip_dma_get_format,
+	.vidioc_g_fmt_vid_out		= xvip_dma_get_format,
+	.vidioc_s_fmt_vid_cap		= xvip_dma_set_format,
+	.vidioc_s_fmt_vid_out		= xvip_dma_set_format,
+	.vidioc_try_fmt_vid_cap		= xvip_dma_try_format,
+	.vidioc_try_fmt_vid_out		= xvip_dma_try_format,
+	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
+	.vidioc_querybuf		= vb2_ioctl_querybuf,
+	.vidioc_qbuf			= vb2_ioctl_qbuf,
+	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
+	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
+	.vidioc_expbuf			= vb2_ioctl_expbuf,
+	.vidioc_streamon		= vb2_ioctl_streamon,
+	.vidioc_streamoff		= vb2_ioctl_streamoff,
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 file operations
+ */
+
+static const struct v4l2_file_operations xvip_dma_fops = {
+	.owner		= THIS_MODULE,
+	.unlocked_ioctl	= video_ioctl2,
+	.open		= v4l2_fh_open,
+	.release	= vb2_fop_release,
+	.poll		= vb2_fop_poll,
+	.mmap		= vb2_fop_mmap,
+};
+
+/* -----------------------------------------------------------------------------
+ * Xilinx Video DMA Core
+ */
+
+int xvip_dma_init(struct xvip_composite_device *xdev, struct xvip_dma *dma,
+		  enum v4l2_buf_type type, unsigned int port)
+{
+	char name[16];
+	int ret;
+
+	dma->xdev = xdev;
+	dma->port = port;
+	mutex_init(&dma->lock);
+	mutex_init(&dma->pipe.lock);
+	INIT_LIST_HEAD(&dma->queued_bufs);
+	spin_lock_init(&dma->queued_lock);
+
+	dma->fmtinfo = xvip_get_format_by_fourcc(XVIP_DMA_DEF_FORMAT);
+	dma->format.pixelformat = dma->fmtinfo->fourcc;
+	dma->format.colorspace = V4L2_COLORSPACE_SRGB;
+	dma->format.field = V4L2_FIELD_NONE;
+	dma->format.width = XVIP_DMA_DEF_WIDTH;
+	dma->format.height = XVIP_DMA_DEF_HEIGHT;
+	dma->format.bytesperline = dma->format.width * dma->fmtinfo->bpp;
+	dma->format.sizeimage = dma->format.bytesperline * dma->format.height;
+
+	/* Initialize the media entity... */
+	dma->pad.flags = type == V4L2_BUF_TYPE_VIDEO_CAPTURE
+		       ? MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
+
+	ret = media_entity_init(&dma->video.entity, 1, &dma->pad, 0);
+	if (ret < 0)
+		goto error;
+
+	/* ... and the video node... */
+	dma->video.fops = &xvip_dma_fops;
+	dma->video.v4l2_dev = &xdev->v4l2_dev;
+	dma->video.queue = &dma->queue;
+	snprintf(dma->video.name, sizeof(dma->video.name), "%s %s %u",
+		 xdev->dev->of_node->name,
+		 type == V4L2_BUF_TYPE_VIDEO_CAPTURE ? "output" : "input",
+		 port);
+	dma->video.vfl_type = VFL_TYPE_GRABBER;
+	dma->video.vfl_dir = type == V4L2_BUF_TYPE_VIDEO_CAPTURE
+			   ? VFL_DIR_RX : VFL_DIR_TX;
+	dma->video.release = video_device_release_empty;
+	dma->video.ioctl_ops = &xvip_dma_ioctl_ops;
+	dma->video.lock = &dma->lock;
+
+	video_set_drvdata(&dma->video, dma);
+
+	/* ... and the buffers queue... */
+	dma->alloc_ctx = vb2_dma_contig_init_ctx(dma->xdev->dev);
+	if (IS_ERR(dma->alloc_ctx)) {
+		ret = PTR_ERR(dma->alloc_ctx);
+		goto error;
+	}
+
+	/* Don't enable VB2_READ and VB2_WRITE, as using the read() and write()
+	 * V4L2 APIs would be inefficient. Testing on the command line with a
+	 * 'cat /dev/video?' thus won't be possible, but given that the driver
+	 * anyway requires a test tool to setup the pipeline before any video
+	 * stream can be started, requiring a specific V4L2 test tool as well
+	 * instead of 'cat' isn't really a drawback.
+	 */
+	dma->queue.type = type;
+	dma->queue.io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
+	dma->queue.lock = &dma->lock;
+	dma->queue.drv_priv = dma;
+	dma->queue.buf_struct_size = sizeof(struct xvip_dma_buffer);
+	dma->queue.ops = &xvip_dma_queue_qops;
+	dma->queue.mem_ops = &vb2_dma_contig_memops;
+	dma->queue.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC
+				   | V4L2_BUF_FLAG_TSTAMP_SRC_EOF;
+	ret = vb2_queue_init(&dma->queue);
+	if (ret < 0) {
+		dev_err(dma->xdev->dev, "failed to initialize VB2 queue\n");
+		goto error;
+	}
+
+	/* ... and the DMA channel. */
+	snprintf(name, sizeof(name), "port%u", port);
+	dma->dma = dma_request_slave_channel(dma->xdev->dev, name);
+	if (dma->dma == NULL) {
+		dev_err(dma->xdev->dev, "no VDMA channel found\n");
+		ret = -ENODEV;
+		goto error;
+	}
+
+	dma->align = 1 << dma->dma->device->copy_align;
+
+	ret = video_register_device(&dma->video, VFL_TYPE_GRABBER, -1);
+	if (ret < 0) {
+		dev_err(dma->xdev->dev, "failed to register video device\n");
+		goto error;
+	}
+
+	return 0;
+
+error:
+	xvip_dma_cleanup(dma);
+	return ret;
+}
+
+void xvip_dma_cleanup(struct xvip_dma *dma)
+{
+	if (video_is_registered(&dma->video))
+		video_unregister_device(&dma->video);
+
+	if (dma->dma)
+		dma_release_channel(dma->dma);
+
+	if (!IS_ERR_OR_NULL(dma->alloc_ctx))
+		vb2_dma_contig_cleanup_ctx(dma->alloc_ctx);
+
+	media_entity_cleanup(&dma->video.entity);
+
+	mutex_destroy(&dma->lock);
+	mutex_destroy(&dma->pipe.lock);
+}
diff --git a/drivers/media/platform/xilinx/xilinx-dma.h b/drivers/media/platform/xilinx/xilinx-dma.h
new file mode 100644
index 0000000..7a1621a
--- /dev/null
+++ b/drivers/media/platform/xilinx/xilinx-dma.h
@@ -0,0 +1,109 @@
+/*
+ * Xilinx Video DMA
+ *
+ * Copyright (C) 2013-2015 Ideas on Board
+ * Copyright (C) 2013-2015 Xilinx, Inc.
+ *
+ * Contacts: Hyun Kwon <hyun.kwon@xilinx.com>
+ *           Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef __XILINX_VIP_DMA_H__
+#define __XILINX_VIP_DMA_H__
+
+#include <linux/dmaengine.h>
+#include <linux/mutex.h>
+#include <linux/spinlock.h>
+#include <linux/videodev2.h>
+
+#include <media/media-entity.h>
+#include <media/v4l2-dev.h>
+#include <media/videobuf2-v4l2.h>
+
+struct dma_chan;
+struct xvip_composite_device;
+struct xvip_video_format;
+
+/**
+ * struct xvip_pipeline - Xilinx Video IP pipeline structure
+ * @pipe: media pipeline
+ * @lock: protects the pipeline @stream_count
+ * @use_count: number of DMA engines using the pipeline
+ * @stream_count: number of DMA engines currently streaming
+ * @num_dmas: number of DMA engines in the pipeline
+ * @output: DMA engine at the output of the pipeline
+ */
+struct xvip_pipeline {
+	struct media_pipeline pipe;
+
+	struct mutex lock;
+	unsigned int use_count;
+	unsigned int stream_count;
+
+	unsigned int num_dmas;
+	struct xvip_dma *output;
+};
+
+static inline struct xvip_pipeline *to_xvip_pipeline(struct media_entity *e)
+{
+	return container_of(e->pipe, struct xvip_pipeline, pipe);
+}
+
+/**
+ * struct xvip_dma - Video DMA channel
+ * @list: list entry in a composite device dmas list
+ * @video: V4L2 video device associated with the DMA channel
+ * @pad: media pad for the video device entity
+ * @xdev: composite device the DMA channel belongs to
+ * @pipe: pipeline belonging to the DMA channel
+ * @port: composite device DT node port number for the DMA channel
+ * @lock: protects the @format, @fmtinfo and @queue fields
+ * @format: active V4L2 pixel format
+ * @fmtinfo: format information corresponding to the active @format
+ * @queue: vb2 buffers queue
+ * @alloc_ctx: allocation context for the vb2 @queue
+ * @sequence: V4L2 buffers sequence number
+ * @queued_bufs: list of queued buffers
+ * @queued_lock: protects the buf_queued list
+ * @dma: DMA engine channel
+ * @align: transfer alignment required by the DMA channel (in bytes)
+ * @xt: dma interleaved template for dma configuration
+ * @sgl: data chunk structure for dma_interleaved_template
+ */
+struct xvip_dma {
+	struct list_head list;
+	struct video_device video;
+	struct media_pad pad;
+
+	struct xvip_composite_device *xdev;
+	struct xvip_pipeline pipe;
+	unsigned int port;
+
+	struct mutex lock;
+	struct v4l2_pix_format format;
+	const struct xvip_video_format *fmtinfo;
+
+	struct vb2_queue queue;
+	void *alloc_ctx;
+	unsigned int sequence;
+
+	struct list_head queued_bufs;
+	spinlock_t queued_lock;
+
+	struct dma_chan *dma;
+	unsigned int align;
+	struct dma_interleaved_template xt;
+	struct data_chunk sgl[1];
+};
+
+#define to_xvip_dma(vdev)	container_of(vdev, struct xvip_dma, video)
+
+int xvip_dma_init(struct xvip_composite_device *xdev, struct xvip_dma *dma,
+		  enum v4l2_buf_type type, unsigned int port);
+void xvip_dma_cleanup(struct xvip_dma *dma);
+
+#endif /* __XILINX_VIP_DMA_H__ */
diff --git a/drivers/media/platform/xilinx/xilinx-tpg.c b/drivers/media/platform/xilinx/xilinx-tpg.c
new file mode 100644
index 0000000..b5f7d5e
--- /dev/null
+++ b/drivers/media/platform/xilinx/xilinx-tpg.c
@@ -0,0 +1,931 @@
+/*
+ * Xilinx Test Pattern Generator
+ *
+ * Copyright (C) 2013-2015 Ideas on Board
+ * Copyright (C) 2013-2015 Xilinx, Inc.
+ *
+ * Contacts: Hyun Kwon <hyun.kwon@xilinx.com>
+ *           Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/device.h>
+#include <linux/gpio/consumer.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/xilinx-v4l2-controls.h>
+
+#include <media/v4l2-async.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-subdev.h>
+
+#include "xilinx-vip.h"
+#include "xilinx-vtc.h"
+
+#define XTPG_CTRL_STATUS_SLAVE_ERROR		(1 << 16)
+#define XTPG_CTRL_IRQ_SLAVE_ERROR		(1 << 16)
+
+#define XTPG_PATTERN_CONTROL			0x0100
+#define XTPG_PATTERN_MASK			(0xf << 0)
+#define XTPG_PATTERN_CONTROL_CROSS_HAIRS	(1 << 4)
+#define XTPG_PATTERN_CONTROL_MOVING_BOX		(1 << 5)
+#define XTPG_PATTERN_CONTROL_COLOR_MASK_SHIFT	6
+#define XTPG_PATTERN_CONTROL_COLOR_MASK_MASK	(0xf << 6)
+#define XTPG_PATTERN_CONTROL_STUCK_PIXEL	(1 << 9)
+#define XTPG_PATTERN_CONTROL_NOISE		(1 << 10)
+#define XTPG_PATTERN_CONTROL_MOTION		(1 << 12)
+#define XTPG_MOTION_SPEED			0x0104
+#define XTPG_CROSS_HAIRS			0x0108
+#define XTPG_CROSS_HAIRS_ROW_SHIFT		0
+#define XTPG_CROSS_HAIRS_ROW_MASK		(0xfff << 0)
+#define XTPG_CROSS_HAIRS_COLUMN_SHIFT		16
+#define XTPG_CROSS_HAIRS_COLUMN_MASK		(0xfff << 16)
+#define XTPG_ZPLATE_HOR_CONTROL			0x010c
+#define XTPG_ZPLATE_VER_CONTROL			0x0110
+#define XTPG_ZPLATE_START_SHIFT			0
+#define XTPG_ZPLATE_START_MASK			(0xffff << 0)
+#define XTPG_ZPLATE_SPEED_SHIFT			16
+#define XTPG_ZPLATE_SPEED_MASK			(0xffff << 16)
+#define XTPG_BOX_SIZE				0x0114
+#define XTPG_BOX_COLOR				0x0118
+#define XTPG_STUCK_PIXEL_THRESH			0x011c
+#define XTPG_NOISE_GAIN				0x0120
+#define XTPG_BAYER_PHASE			0x0124
+#define XTPG_BAYER_PHASE_RGGB			0
+#define XTPG_BAYER_PHASE_GRBG			1
+#define XTPG_BAYER_PHASE_GBRG			2
+#define XTPG_BAYER_PHASE_BGGR			3
+#define XTPG_BAYER_PHASE_OFF			4
+
+/*
+ * The minimum blanking value is one clock cycle for the front porch, one clock
+ * cycle for the sync pulse and one clock cycle for the back porch.
+ */
+#define XTPG_MIN_HBLANK			3
+#define XTPG_MAX_HBLANK			(XVTC_MAX_HSIZE - XVIP_MIN_WIDTH)
+#define XTPG_MIN_VBLANK			3
+#define XTPG_MAX_VBLANK			(XVTC_MAX_VSIZE - XVIP_MIN_HEIGHT)
+
+/**
+ * struct xtpg_device - Xilinx Test Pattern Generator device structure
+ * @xvip: Xilinx Video IP device
+ * @pads: media pads
+ * @npads: number of pads (1 or 2)
+ * @has_input: whether an input is connected to the sink pad
+ * @formats: active V4L2 media bus format for each pad
+ * @default_format: default V4L2 media bus format
+ * @vip_format: format information corresponding to the active format
+ * @bayer: boolean flag if TPG is set to any bayer format
+ * @ctrl_handler: control handler
+ * @hblank: horizontal blanking control
+ * @vblank: vertical blanking control
+ * @pattern: test pattern control
+ * @streaming: is the video stream active
+ * @vtc: video timing controller
+ * @vtmux_gpio: video timing mux GPIO
+ */
+struct xtpg_device {
+	struct xvip_device xvip;
+
+	struct media_pad pads[2];
+	unsigned int npads;
+	bool has_input;
+
+	struct v4l2_mbus_framefmt formats[2];
+	struct v4l2_mbus_framefmt default_format;
+	const struct xvip_video_format *vip_format;
+	bool bayer;
+
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *pattern;
+	bool streaming;
+
+	struct xvtc_device *vtc;
+	struct gpio_desc *vtmux_gpio;
+};
+
+static inline struct xtpg_device *to_tpg(struct v4l2_subdev *subdev)
+{
+	return container_of(subdev, struct xtpg_device, xvip.subdev);
+}
+
+static u32 xtpg_get_bayer_phase(unsigned int code)
+{
+	switch (code) {
+	case MEDIA_BUS_FMT_SRGGB8_1X8:
+		return XTPG_BAYER_PHASE_RGGB;
+	case MEDIA_BUS_FMT_SGRBG8_1X8:
+		return XTPG_BAYER_PHASE_GRBG;
+	case MEDIA_BUS_FMT_SGBRG8_1X8:
+		return XTPG_BAYER_PHASE_GBRG;
+	case MEDIA_BUS_FMT_SBGGR8_1X8:
+		return XTPG_BAYER_PHASE_BGGR;
+	default:
+		return XTPG_BAYER_PHASE_OFF;
+	}
+}
+
+static void __xtpg_update_pattern_control(struct xtpg_device *xtpg,
+					  bool passthrough, bool pattern)
+{
+	u32 pattern_mask = (1 << (xtpg->pattern->maximum + 1)) - 1;
+
+	/*
+	 * If the TPG has no sink pad or no input connected to its sink pad
+	 * passthrough mode can't be enabled.
+	 */
+	if (xtpg->npads == 1 || !xtpg->has_input)
+		passthrough = false;
+
+	/* If passthrough mode is allowed unmask bit 0. */
+	if (passthrough)
+		pattern_mask &= ~1;
+
+	/* If test pattern mode is allowed unmask all other bits. */
+	if (pattern)
+		pattern_mask &= 1;
+
+	__v4l2_ctrl_modify_range(xtpg->pattern, 0, xtpg->pattern->maximum,
+				 pattern_mask, pattern ? 9 : 0);
+}
+
+static void xtpg_update_pattern_control(struct xtpg_device *xtpg,
+					bool passthrough, bool pattern)
+{
+	mutex_lock(xtpg->ctrl_handler.lock);
+	__xtpg_update_pattern_control(xtpg, passthrough, pattern);
+	mutex_unlock(xtpg->ctrl_handler.lock);
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Video Operations
+ */
+
+static int xtpg_s_stream(struct v4l2_subdev *subdev, int enable)
+{
+	struct xtpg_device *xtpg = to_tpg(subdev);
+	unsigned int width = xtpg->formats[0].width;
+	unsigned int height = xtpg->formats[0].height;
+	bool passthrough;
+	u32 bayer_phase;
+
+	if (!enable) {
+		xvip_stop(&xtpg->xvip);
+		if (xtpg->vtc)
+			xvtc_generator_stop(xtpg->vtc);
+
+		xtpg_update_pattern_control(xtpg, true, true);
+		xtpg->streaming = false;
+		return 0;
+	}
+
+	xvip_set_frame_size(&xtpg->xvip, &xtpg->formats[0]);
+
+	if (xtpg->vtc) {
+		struct xvtc_config config = {
+			.hblank_start = width,
+			.hsync_start = width + 1,
+			.vblank_start = height,
+			.vsync_start = height + 1,
+		};
+		unsigned int htotal;
+		unsigned int vtotal;
+
+		htotal = min_t(unsigned int, XVTC_MAX_HSIZE,
+			       v4l2_ctrl_g_ctrl(xtpg->hblank) + width);
+		vtotal = min_t(unsigned int, XVTC_MAX_VSIZE,
+			       v4l2_ctrl_g_ctrl(xtpg->vblank) + height);
+
+		config.hsync_end = htotal - 1;
+		config.hsize = htotal;
+		config.vsync_end = vtotal - 1;
+		config.vsize = vtotal;
+
+		xvtc_generator_start(xtpg->vtc, &config);
+	}
+
+	/*
+	 * Configure the bayer phase and video timing mux based on the
+	 * operation mode (passthrough or test pattern generation). The test
+	 * pattern can be modified by the control set handler, we thus need to
+	 * take the control lock here to avoid races.
+	 */
+	mutex_lock(xtpg->ctrl_handler.lock);
+
+	xvip_clr_and_set(&xtpg->xvip, XTPG_PATTERN_CONTROL,
+			 XTPG_PATTERN_MASK, xtpg->pattern->cur.val);
+
+	/*
+	 * Switching between passthrough and test pattern generation modes isn't
+	 * allowed during streaming, update the control range accordingly.
+	 */
+	passthrough = xtpg->pattern->cur.val == 0;
+	__xtpg_update_pattern_control(xtpg, passthrough, !passthrough);
+
+	xtpg->streaming = true;
+
+	mutex_unlock(xtpg->ctrl_handler.lock);
+
+	/*
+	 * For TPG v5.0, the bayer phase needs to be off for the pass through
+	 * mode, otherwise the external input would be subsampled.
+	 */
+	bayer_phase = passthrough ? XTPG_BAYER_PHASE_OFF
+		    : xtpg_get_bayer_phase(xtpg->formats[0].code);
+	xvip_write(&xtpg->xvip, XTPG_BAYER_PHASE, bayer_phase);
+
+	if (xtpg->vtmux_gpio)
+		gpiod_set_value_cansleep(xtpg->vtmux_gpio, !passthrough);
+
+	xvip_start(&xtpg->xvip);
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Pad Operations
+ */
+
+static struct v4l2_mbus_framefmt *
+__xtpg_get_pad_format(struct xtpg_device *xtpg,
+		      struct v4l2_subdev_pad_config *cfg,
+		      unsigned int pad, u32 which)
+{
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_get_try_format(&xtpg->xvip.subdev, cfg, pad);
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return &xtpg->formats[pad];
+	default:
+		return NULL;
+	}
+}
+
+static int xtpg_get_format(struct v4l2_subdev *subdev,
+			   struct v4l2_subdev_pad_config *cfg,
+			   struct v4l2_subdev_format *fmt)
+{
+	struct xtpg_device *xtpg = to_tpg(subdev);
+
+	fmt->format = *__xtpg_get_pad_format(xtpg, cfg, fmt->pad, fmt->which);
+
+	return 0;
+}
+
+static int xtpg_set_format(struct v4l2_subdev *subdev,
+			   struct v4l2_subdev_pad_config *cfg,
+			   struct v4l2_subdev_format *fmt)
+{
+	struct xtpg_device *xtpg = to_tpg(subdev);
+	struct v4l2_mbus_framefmt *__format;
+	u32 bayer_phase;
+
+	__format = __xtpg_get_pad_format(xtpg, cfg, fmt->pad, fmt->which);
+
+	/* In two pads mode the source pad format is always identical to the
+	 * sink pad format.
+	 */
+	if (xtpg->npads == 2 && fmt->pad == 1) {
+		fmt->format = *__format;
+		return 0;
+	}
+
+	/* Bayer phase is configurable at runtime */
+	if (xtpg->bayer) {
+		bayer_phase = xtpg_get_bayer_phase(fmt->format.code);
+		if (bayer_phase != XTPG_BAYER_PHASE_OFF)
+			__format->code = fmt->format.code;
+	}
+
+	xvip_set_format_size(__format, fmt);
+
+	fmt->format = *__format;
+
+	/* Propagate the format to the source pad. */
+	if (xtpg->npads == 2) {
+		__format = __xtpg_get_pad_format(xtpg, cfg, 1, fmt->which);
+		*__format = fmt->format;
+	}
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Operations
+ */
+
+static int xtpg_enum_frame_size(struct v4l2_subdev *subdev,
+				struct v4l2_subdev_pad_config *cfg,
+				struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct v4l2_mbus_framefmt *format;
+
+	format = v4l2_subdev_get_try_format(subdev, cfg, fse->pad);
+
+	if (fse->index || fse->code != format->code)
+		return -EINVAL;
+
+	/* Min / max values for pad 0 is always fixed in both one and two pads
+	 * modes. In two pads mode, the source pad(= 1) size is identical to
+	 * the sink pad size */
+	if (fse->pad == 0) {
+		fse->min_width = XVIP_MIN_WIDTH;
+		fse->max_width = XVIP_MAX_WIDTH;
+		fse->min_height = XVIP_MIN_HEIGHT;
+		fse->max_height = XVIP_MAX_HEIGHT;
+	} else {
+		fse->min_width = format->width;
+		fse->max_width = format->width;
+		fse->min_height = format->height;
+		fse->max_height = format->height;
+	}
+
+	return 0;
+}
+
+static int xtpg_open(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
+{
+	struct xtpg_device *xtpg = to_tpg(subdev);
+	struct v4l2_mbus_framefmt *format;
+
+	format = v4l2_subdev_get_try_format(subdev, fh->pad, 0);
+	*format = xtpg->default_format;
+
+	if (xtpg->npads == 2) {
+		format = v4l2_subdev_get_try_format(subdev, fh->pad, 1);
+		*format = xtpg->default_format;
+	}
+
+	return 0;
+}
+
+static int xtpg_close(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
+{
+	return 0;
+}
+
+static int xtpg_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct xtpg_device *xtpg = container_of(ctrl->handler,
+						struct xtpg_device,
+						ctrl_handler);
+	switch (ctrl->id) {
+	case V4L2_CID_TEST_PATTERN:
+		xvip_clr_and_set(&xtpg->xvip, XTPG_PATTERN_CONTROL,
+				 XTPG_PATTERN_MASK, ctrl->val);
+		return 0;
+	case V4L2_CID_XILINX_TPG_CROSS_HAIRS:
+		xvip_clr_or_set(&xtpg->xvip, XTPG_PATTERN_CONTROL,
+				XTPG_PATTERN_CONTROL_CROSS_HAIRS, ctrl->val);
+		return 0;
+	case V4L2_CID_XILINX_TPG_MOVING_BOX:
+		xvip_clr_or_set(&xtpg->xvip, XTPG_PATTERN_CONTROL,
+				XTPG_PATTERN_CONTROL_MOVING_BOX, ctrl->val);
+		return 0;
+	case V4L2_CID_XILINX_TPG_COLOR_MASK:
+		xvip_clr_and_set(&xtpg->xvip, XTPG_PATTERN_CONTROL,
+				 XTPG_PATTERN_CONTROL_COLOR_MASK_MASK,
+				 ctrl->val <<
+				 XTPG_PATTERN_CONTROL_COLOR_MASK_SHIFT);
+		return 0;
+	case V4L2_CID_XILINX_TPG_STUCK_PIXEL:
+		xvip_clr_or_set(&xtpg->xvip, XTPG_PATTERN_CONTROL,
+				XTPG_PATTERN_CONTROL_STUCK_PIXEL, ctrl->val);
+		return 0;
+	case V4L2_CID_XILINX_TPG_NOISE:
+		xvip_clr_or_set(&xtpg->xvip, XTPG_PATTERN_CONTROL,
+				XTPG_PATTERN_CONTROL_NOISE, ctrl->val);
+		return 0;
+	case V4L2_CID_XILINX_TPG_MOTION:
+		xvip_clr_or_set(&xtpg->xvip, XTPG_PATTERN_CONTROL,
+				XTPG_PATTERN_CONTROL_MOTION, ctrl->val);
+		return 0;
+	case V4L2_CID_XILINX_TPG_MOTION_SPEED:
+		xvip_write(&xtpg->xvip, XTPG_MOTION_SPEED, ctrl->val);
+		return 0;
+	case V4L2_CID_XILINX_TPG_CROSS_HAIR_ROW:
+		xvip_clr_and_set(&xtpg->xvip, XTPG_CROSS_HAIRS,
+				 XTPG_CROSS_HAIRS_ROW_MASK,
+				 ctrl->val << XTPG_CROSS_HAIRS_ROW_SHIFT);
+		return 0;
+	case V4L2_CID_XILINX_TPG_CROSS_HAIR_COLUMN:
+		xvip_clr_and_set(&xtpg->xvip, XTPG_CROSS_HAIRS,
+				 XTPG_CROSS_HAIRS_COLUMN_MASK,
+				 ctrl->val << XTPG_CROSS_HAIRS_COLUMN_SHIFT);
+		return 0;
+	case V4L2_CID_XILINX_TPG_ZPLATE_HOR_START:
+		xvip_clr_and_set(&xtpg->xvip, XTPG_ZPLATE_HOR_CONTROL,
+				 XTPG_ZPLATE_START_MASK,
+				 ctrl->val << XTPG_ZPLATE_START_SHIFT);
+		return 0;
+	case V4L2_CID_XILINX_TPG_ZPLATE_HOR_SPEED:
+		xvip_clr_and_set(&xtpg->xvip, XTPG_ZPLATE_HOR_CONTROL,
+				 XTPG_ZPLATE_SPEED_MASK,
+				 ctrl->val << XTPG_ZPLATE_SPEED_SHIFT);
+		return 0;
+	case V4L2_CID_XILINX_TPG_ZPLATE_VER_START:
+		xvip_clr_and_set(&xtpg->xvip, XTPG_ZPLATE_VER_CONTROL,
+				 XTPG_ZPLATE_START_MASK,
+				 ctrl->val << XTPG_ZPLATE_START_SHIFT);
+		return 0;
+	case V4L2_CID_XILINX_TPG_ZPLATE_VER_SPEED:
+		xvip_clr_and_set(&xtpg->xvip, XTPG_ZPLATE_VER_CONTROL,
+				 XTPG_ZPLATE_SPEED_MASK,
+				 ctrl->val << XTPG_ZPLATE_SPEED_SHIFT);
+		return 0;
+	case V4L2_CID_XILINX_TPG_BOX_SIZE:
+		xvip_write(&xtpg->xvip, XTPG_BOX_SIZE, ctrl->val);
+		return 0;
+	case V4L2_CID_XILINX_TPG_BOX_COLOR:
+		xvip_write(&xtpg->xvip, XTPG_BOX_COLOR, ctrl->val);
+		return 0;
+	case V4L2_CID_XILINX_TPG_STUCK_PIXEL_THRESH:
+		xvip_write(&xtpg->xvip, XTPG_STUCK_PIXEL_THRESH, ctrl->val);
+		return 0;
+	case V4L2_CID_XILINX_TPG_NOISE_GAIN:
+		xvip_write(&xtpg->xvip, XTPG_NOISE_GAIN, ctrl->val);
+		return 0;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops xtpg_ctrl_ops = {
+	.s_ctrl	= xtpg_s_ctrl,
+};
+
+static struct v4l2_subdev_core_ops xtpg_core_ops = {
+};
+
+static struct v4l2_subdev_video_ops xtpg_video_ops = {
+	.s_stream = xtpg_s_stream,
+};
+
+static struct v4l2_subdev_pad_ops xtpg_pad_ops = {
+	.enum_mbus_code		= xvip_enum_mbus_code,
+	.enum_frame_size	= xtpg_enum_frame_size,
+	.get_fmt		= xtpg_get_format,
+	.set_fmt		= xtpg_set_format,
+};
+
+static struct v4l2_subdev_ops xtpg_ops = {
+	.core   = &xtpg_core_ops,
+	.video  = &xtpg_video_ops,
+	.pad    = &xtpg_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops xtpg_internal_ops = {
+	.open	= xtpg_open,
+	.close	= xtpg_close,
+};
+
+/*
+ * Control Config
+ */
+
+static const char *const xtpg_pattern_strings[] = {
+	"Passthrough",
+	"Horizontal Ramp",
+	"Vertical Ramp",
+	"Temporal Ramp",
+	"Solid Red",
+	"Solid Green",
+	"Solid Blue",
+	"Solid Black",
+	"Solid White",
+	"Color Bars",
+	"Zone Plate",
+	"Tartan Color Bars",
+	"Cross Hatch",
+	"None",
+	"Vertical/Horizontal Ramps",
+	"Black/White Checker Board",
+};
+
+static struct v4l2_ctrl_config xtpg_ctrls[] = {
+	{
+		.ops	= &xtpg_ctrl_ops,
+		.id	= V4L2_CID_XILINX_TPG_CROSS_HAIRS,
+		.name	= "Test Pattern: Cross Hairs",
+		.type	= V4L2_CTRL_TYPE_BOOLEAN,
+		.min	= false,
+		.max	= true,
+		.step	= 1,
+		.def	= 0,
+	}, {
+		.ops	= &xtpg_ctrl_ops,
+		.id	= V4L2_CID_XILINX_TPG_MOVING_BOX,
+		.name	= "Test Pattern: Moving Box",
+		.type	= V4L2_CTRL_TYPE_BOOLEAN,
+		.min	= false,
+		.max	= true,
+		.step	= 1,
+		.def	= 0,
+	}, {
+		.ops	= &xtpg_ctrl_ops,
+		.id	= V4L2_CID_XILINX_TPG_COLOR_MASK,
+		.name	= "Test Pattern: Color Mask",
+		.type	= V4L2_CTRL_TYPE_BITMASK,
+		.min	= 0,
+		.max	= 0xf,
+		.def	= 0,
+	}, {
+		.ops	= &xtpg_ctrl_ops,
+		.id	= V4L2_CID_XILINX_TPG_STUCK_PIXEL,
+		.name	= "Test Pattern: Stuck Pixel",
+		.type	= V4L2_CTRL_TYPE_BOOLEAN,
+		.min	= false,
+		.max	= true,
+		.step	= 1,
+		.def	= 0,
+	}, {
+		.ops	= &xtpg_ctrl_ops,
+		.id	= V4L2_CID_XILINX_TPG_NOISE,
+		.name	= "Test Pattern: Noise",
+		.type	= V4L2_CTRL_TYPE_BOOLEAN,
+		.min	= false,
+		.max	= true,
+		.step	= 1,
+		.def	= 0,
+	}, {
+		.ops	= &xtpg_ctrl_ops,
+		.id	= V4L2_CID_XILINX_TPG_MOTION,
+		.name	= "Test Pattern: Motion",
+		.type	= V4L2_CTRL_TYPE_BOOLEAN,
+		.min	= false,
+		.max	= true,
+		.step	= 1,
+		.def	= 0,
+	}, {
+		.ops	= &xtpg_ctrl_ops,
+		.id	= V4L2_CID_XILINX_TPG_MOTION_SPEED,
+		.name	= "Test Pattern: Motion Speed",
+		.type	= V4L2_CTRL_TYPE_INTEGER,
+		.min	= 0,
+		.max	= (1 << 8) - 1,
+		.step	= 1,
+		.def	= 4,
+		.flags	= V4L2_CTRL_FLAG_SLIDER,
+	}, {
+		.ops	= &xtpg_ctrl_ops,
+		.id	= V4L2_CID_XILINX_TPG_CROSS_HAIR_ROW,
+		.name	= "Test Pattern: Cross Hairs Row",
+		.type	= V4L2_CTRL_TYPE_INTEGER,
+		.min	= 0,
+		.max	= (1 << 12) - 1,
+		.step	= 1,
+		.def	= 0x64,
+		.flags	= V4L2_CTRL_FLAG_SLIDER,
+	}, {
+		.ops	= &xtpg_ctrl_ops,
+		.id	= V4L2_CID_XILINX_TPG_CROSS_HAIR_COLUMN,
+		.name	= "Test Pattern: Cross Hairs Column",
+		.type	= V4L2_CTRL_TYPE_INTEGER,
+		.min	= 0,
+		.max	= (1 << 12) - 1,
+		.step	= 1,
+		.def	= 0x64,
+		.flags	= V4L2_CTRL_FLAG_SLIDER,
+	}, {
+		.ops	= &xtpg_ctrl_ops,
+		.id	= V4L2_CID_XILINX_TPG_ZPLATE_HOR_START,
+		.name	= "Test Pattern: Zplate Horizontal Start Pos",
+		.type	= V4L2_CTRL_TYPE_INTEGER,
+		.min	= 0,
+		.max	= (1 << 16) - 1,
+		.step	= 1,
+		.def	= 0x1e,
+		.flags	= V4L2_CTRL_FLAG_SLIDER,
+	}, {
+		.ops	= &xtpg_ctrl_ops,
+		.id	= V4L2_CID_XILINX_TPG_ZPLATE_HOR_SPEED,
+		.name	= "Test Pattern: Zplate Horizontal Speed",
+		.type	= V4L2_CTRL_TYPE_INTEGER,
+		.min	= 0,
+		.max	= (1 << 16) - 1,
+		.step	= 1,
+		.def	= 0,
+		.flags	= V4L2_CTRL_FLAG_SLIDER,
+	}, {
+		.ops	= &xtpg_ctrl_ops,
+		.id	= V4L2_CID_XILINX_TPG_ZPLATE_VER_START,
+		.name	= "Test Pattern: Zplate Vertical Start Pos",
+		.type	= V4L2_CTRL_TYPE_INTEGER,
+		.min	= 0,
+		.max	= (1 << 16) - 1,
+		.step	= 1,
+		.def	= 1,
+		.flags	= V4L2_CTRL_FLAG_SLIDER,
+	}, {
+		.ops	= &xtpg_ctrl_ops,
+		.id	= V4L2_CID_XILINX_TPG_ZPLATE_VER_SPEED,
+		.name	= "Test Pattern: Zplate Vertical Speed",
+		.type	= V4L2_CTRL_TYPE_INTEGER,
+		.min	= 0,
+		.max	= (1 << 16) - 1,
+		.step	= 1,
+		.def	= 0,
+		.flags	= V4L2_CTRL_FLAG_SLIDER,
+	}, {
+		.ops	= &xtpg_ctrl_ops,
+		.id	= V4L2_CID_XILINX_TPG_BOX_SIZE,
+		.name	= "Test Pattern: Box Size",
+		.type	= V4L2_CTRL_TYPE_INTEGER,
+		.min	= 0,
+		.max	= (1 << 12) - 1,
+		.step	= 1,
+		.def	= 0x32,
+		.flags	= V4L2_CTRL_FLAG_SLIDER,
+	}, {
+		.ops	= &xtpg_ctrl_ops,
+		.id	= V4L2_CID_XILINX_TPG_BOX_COLOR,
+		.name	= "Test Pattern: Box Color(RGB)",
+		.type	= V4L2_CTRL_TYPE_INTEGER,
+		.min	= 0,
+		.max	= (1 << 24) - 1,
+		.step	= 1,
+		.def	= 0,
+	}, {
+		.ops	= &xtpg_ctrl_ops,
+		.id	= V4L2_CID_XILINX_TPG_STUCK_PIXEL_THRESH,
+		.name	= "Test Pattern: Stuck Pixel threshold",
+		.type	= V4L2_CTRL_TYPE_INTEGER,
+		.min	= 0,
+		.max	= (1 << 16) - 1,
+		.step	= 1,
+		.def	= 0,
+		.flags	= V4L2_CTRL_FLAG_SLIDER,
+	}, {
+		.ops	= &xtpg_ctrl_ops,
+		.id	= V4L2_CID_XILINX_TPG_NOISE_GAIN,
+		.name	= "Test Pattern: Noise Gain",
+		.type	= V4L2_CTRL_TYPE_INTEGER,
+		.min	= 0,
+		.max	= (1 << 8) - 1,
+		.step	= 1,
+		.def	= 0,
+		.flags	= V4L2_CTRL_FLAG_SLIDER,
+	},
+};
+
+/* -----------------------------------------------------------------------------
+ * Media Operations
+ */
+
+static const struct media_entity_operations xtpg_media_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+/* -----------------------------------------------------------------------------
+ * Power Management
+ */
+
+static int __maybe_unused xtpg_pm_suspend(struct device *dev)
+{
+	struct xtpg_device *xtpg = dev_get_drvdata(dev);
+
+	xvip_suspend(&xtpg->xvip);
+
+	return 0;
+}
+
+static int __maybe_unused xtpg_pm_resume(struct device *dev)
+{
+	struct xtpg_device *xtpg = dev_get_drvdata(dev);
+
+	xvip_resume(&xtpg->xvip);
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * Platform Device Driver
+ */
+
+static int xtpg_parse_of(struct xtpg_device *xtpg)
+{
+	struct device *dev = xtpg->xvip.dev;
+	struct device_node *node = xtpg->xvip.dev->of_node;
+	struct device_node *ports;
+	struct device_node *port;
+	unsigned int nports = 0;
+	bool has_endpoint = false;
+
+	ports = of_get_child_by_name(node, "ports");
+	if (ports == NULL)
+		ports = node;
+
+	for_each_child_of_node(ports, port) {
+		const struct xvip_video_format *format;
+		struct device_node *endpoint;
+
+		if (!port->name || of_node_cmp(port->name, "port"))
+			continue;
+
+		format = xvip_of_get_format(port);
+		if (IS_ERR(format)) {
+			dev_err(dev, "invalid format in DT");
+			return PTR_ERR(format);
+		}
+
+		/* Get and check the format description */
+		if (!xtpg->vip_format) {
+			xtpg->vip_format = format;
+		} else if (xtpg->vip_format != format) {
+			dev_err(dev, "in/out format mismatch in DT");
+			return -EINVAL;
+		}
+
+		if (nports == 0) {
+			endpoint = of_get_next_child(port, NULL);
+			if (endpoint)
+				has_endpoint = true;
+			of_node_put(endpoint);
+		}
+
+		/* Count the number of ports. */
+		nports++;
+	}
+
+	if (nports != 1 && nports != 2) {
+		dev_err(dev, "invalid number of ports %u\n", nports);
+		return -EINVAL;
+	}
+
+	xtpg->npads = nports;
+	if (nports == 2 && has_endpoint)
+		xtpg->has_input = true;
+
+	return 0;
+}
+
+static int xtpg_probe(struct platform_device *pdev)
+{
+	struct v4l2_subdev *subdev;
+	struct xtpg_device *xtpg;
+	u32 i, bayer_phase;
+	int ret;
+
+	xtpg = devm_kzalloc(&pdev->dev, sizeof(*xtpg), GFP_KERNEL);
+	if (!xtpg)
+		return -ENOMEM;
+
+	xtpg->xvip.dev = &pdev->dev;
+
+	ret = xtpg_parse_of(xtpg);
+	if (ret < 0)
+		return ret;
+
+	ret = xvip_init_resources(&xtpg->xvip);
+	if (ret < 0)
+		return ret;
+
+	xtpg->vtmux_gpio = devm_gpiod_get_optional(&pdev->dev, "timing",
+						   GPIOD_OUT_HIGH);
+	if (IS_ERR(xtpg->vtmux_gpio)) {
+		ret = PTR_ERR(xtpg->vtmux_gpio);
+		goto error_resource;
+	}
+
+	xtpg->vtc = xvtc_of_get(pdev->dev.of_node);
+	if (IS_ERR(xtpg->vtc)) {
+		ret = PTR_ERR(xtpg->vtc);
+		goto error_resource;
+	}
+
+	/* Reset and initialize the core */
+	xvip_reset(&xtpg->xvip);
+
+	/* Initialize V4L2 subdevice and media entity. Pad numbers depend on the
+	 * number of pads.
+	 */
+	if (xtpg->npads == 2) {
+		xtpg->pads[0].flags = MEDIA_PAD_FL_SINK;
+		xtpg->pads[1].flags = MEDIA_PAD_FL_SOURCE;
+	} else {
+		xtpg->pads[0].flags = MEDIA_PAD_FL_SOURCE;
+	}
+
+	/* Initialize the default format */
+	xtpg->default_format.code = xtpg->vip_format->code;
+	xtpg->default_format.field = V4L2_FIELD_NONE;
+	xtpg->default_format.colorspace = V4L2_COLORSPACE_SRGB;
+	xvip_get_frame_size(&xtpg->xvip, &xtpg->default_format);
+
+	bayer_phase = xtpg_get_bayer_phase(xtpg->vip_format->code);
+	if (bayer_phase != XTPG_BAYER_PHASE_OFF)
+		xtpg->bayer = true;
+
+	xtpg->formats[0] = xtpg->default_format;
+	if (xtpg->npads == 2)
+		xtpg->formats[1] = xtpg->default_format;
+
+	/* Initialize V4L2 subdevice and media entity */
+	subdev = &xtpg->xvip.subdev;
+	v4l2_subdev_init(subdev, &xtpg_ops);
+	subdev->dev = &pdev->dev;
+	subdev->internal_ops = &xtpg_internal_ops;
+	strlcpy(subdev->name, dev_name(&pdev->dev), sizeof(subdev->name));
+	v4l2_set_subdevdata(subdev, xtpg);
+	subdev->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	subdev->entity.ops = &xtpg_media_ops;
+
+	ret = media_entity_init(&subdev->entity, xtpg->npads, xtpg->pads, 0);
+	if (ret < 0)
+		goto error;
+
+	v4l2_ctrl_handler_init(&xtpg->ctrl_handler, 3 + ARRAY_SIZE(xtpg_ctrls));
+
+	xtpg->vblank = v4l2_ctrl_new_std(&xtpg->ctrl_handler, &xtpg_ctrl_ops,
+					 V4L2_CID_VBLANK, XTPG_MIN_VBLANK,
+					 XTPG_MAX_VBLANK, 1, 100);
+	xtpg->hblank = v4l2_ctrl_new_std(&xtpg->ctrl_handler, &xtpg_ctrl_ops,
+					 V4L2_CID_HBLANK, XTPG_MIN_HBLANK,
+					 XTPG_MAX_HBLANK, 1, 100);
+	xtpg->pattern = v4l2_ctrl_new_std_menu_items(&xtpg->ctrl_handler,
+					&xtpg_ctrl_ops, V4L2_CID_TEST_PATTERN,
+					ARRAY_SIZE(xtpg_pattern_strings) - 1,
+					1, 9, xtpg_pattern_strings);
+
+	for (i = 0; i < ARRAY_SIZE(xtpg_ctrls); i++)
+		v4l2_ctrl_new_custom(&xtpg->ctrl_handler, &xtpg_ctrls[i], NULL);
+
+	if (xtpg->ctrl_handler.error) {
+		dev_err(&pdev->dev, "failed to add controls\n");
+		ret = xtpg->ctrl_handler.error;
+		goto error;
+	}
+	subdev->ctrl_handler = &xtpg->ctrl_handler;
+
+	xtpg_update_pattern_control(xtpg, true, true);
+
+	ret = v4l2_ctrl_handler_setup(&xtpg->ctrl_handler);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to set controls\n");
+		goto error;
+	}
+
+	platform_set_drvdata(pdev, xtpg);
+
+	xvip_print_version(&xtpg->xvip);
+
+	ret = v4l2_async_register_subdev(subdev);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to register subdev\n");
+		goto error;
+	}
+
+	return 0;
+
+error:
+	v4l2_ctrl_handler_free(&xtpg->ctrl_handler);
+	media_entity_cleanup(&subdev->entity);
+	xvtc_put(xtpg->vtc);
+error_resource:
+	xvip_cleanup_resources(&xtpg->xvip);
+	return ret;
+}
+
+static int xtpg_remove(struct platform_device *pdev)
+{
+	struct xtpg_device *xtpg = platform_get_drvdata(pdev);
+	struct v4l2_subdev *subdev = &xtpg->xvip.subdev;
+
+	v4l2_async_unregister_subdev(subdev);
+	v4l2_ctrl_handler_free(&xtpg->ctrl_handler);
+	media_entity_cleanup(&subdev->entity);
+
+	xvip_cleanup_resources(&xtpg->xvip);
+
+	return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(xtpg_pm_ops, xtpg_pm_suspend, xtpg_pm_resume);
+
+static const struct of_device_id xtpg_of_id_table[] = {
+	{ .compatible = "xlnx,v-tpg-5.0" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, xtpg_of_id_table);
+
+static struct platform_driver xtpg_driver = {
+	.driver = {
+		.name		= "xilinx-tpg",
+		.pm		= &xtpg_pm_ops,
+		.of_match_table	= xtpg_of_id_table,
+	},
+	.probe			= xtpg_probe,
+	.remove			= xtpg_remove,
+};
+
+module_platform_driver(xtpg_driver);
+
+MODULE_AUTHOR("Laurent Pinchart <laurent.pinchart@ideasonboard.com>");
+MODULE_DESCRIPTION("Xilinx Test Pattern Generator Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/platform/xilinx/xilinx-vip.c b/drivers/media/platform/xilinx/xilinx-vip.c
new file mode 100644
index 0000000..3112591
--- /dev/null
+++ b/drivers/media/platform/xilinx/xilinx-vip.c
@@ -0,0 +1,323 @@
+/*
+ * Xilinx Video IP Core
+ *
+ * Copyright (C) 2013-2015 Ideas on Board
+ * Copyright (C) 2013-2015 Xilinx, Inc.
+ *
+ * Contacts: Hyun Kwon <hyun.kwon@xilinx.com>
+ *           Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/clk.h>
+#include <linux/export.h>
+#include <linux/kernel.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+
+#include <dt-bindings/media/xilinx-vip.h>
+
+#include "xilinx-vip.h"
+
+/* -----------------------------------------------------------------------------
+ * Helper functions
+ */
+
+static const struct xvip_video_format xvip_video_formats[] = {
+	{ XVIP_VF_YUV_422, 8, NULL, MEDIA_BUS_FMT_UYVY8_1X16,
+	  2, V4L2_PIX_FMT_YUYV, "4:2:2, packed, YUYV" },
+	{ XVIP_VF_YUV_444, 8, NULL, MEDIA_BUS_FMT_VUY8_1X24,
+	  3, V4L2_PIX_FMT_YUV444, "4:4:4, packed, YUYV" },
+	{ XVIP_VF_RBG, 8, NULL, MEDIA_BUS_FMT_RBG888_1X24,
+	  3, 0, NULL },
+	{ XVIP_VF_MONO_SENSOR, 8, "mono", MEDIA_BUS_FMT_Y8_1X8,
+	  1, V4L2_PIX_FMT_GREY, "Greyscale 8-bit" },
+	{ XVIP_VF_MONO_SENSOR, 8, "rggb", MEDIA_BUS_FMT_SRGGB8_1X8,
+	  1, V4L2_PIX_FMT_SGRBG8, "Bayer 8-bit RGGB" },
+	{ XVIP_VF_MONO_SENSOR, 8, "grbg", MEDIA_BUS_FMT_SGRBG8_1X8,
+	  1, V4L2_PIX_FMT_SGRBG8, "Bayer 8-bit GRBG" },
+	{ XVIP_VF_MONO_SENSOR, 8, "gbrg", MEDIA_BUS_FMT_SGBRG8_1X8,
+	  1, V4L2_PIX_FMT_SGBRG8, "Bayer 8-bit GBRG" },
+	{ XVIP_VF_MONO_SENSOR, 8, "bggr", MEDIA_BUS_FMT_SBGGR8_1X8,
+	  1, V4L2_PIX_FMT_SBGGR8, "Bayer 8-bit BGGR" },
+};
+
+/**
+ * xvip_get_format_by_code - Retrieve format information for a media bus code
+ * @code: the format media bus code
+ *
+ * Return: a pointer to the format information structure corresponding to the
+ * given V4L2 media bus format @code, or ERR_PTR if no corresponding format can
+ * be found.
+ */
+const struct xvip_video_format *xvip_get_format_by_code(unsigned int code)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(xvip_video_formats); ++i) {
+		const struct xvip_video_format *format = &xvip_video_formats[i];
+
+		if (format->code == code)
+			return format;
+	}
+
+	return ERR_PTR(-EINVAL);
+}
+EXPORT_SYMBOL_GPL(xvip_get_format_by_code);
+
+/**
+ * xvip_get_format_by_fourcc - Retrieve format information for a 4CC
+ * @fourcc: the format 4CC
+ *
+ * Return: a pointer to the format information structure corresponding to the
+ * given V4L2 format @fourcc, or ERR_PTR if no corresponding format can be
+ * found.
+ */
+const struct xvip_video_format *xvip_get_format_by_fourcc(u32 fourcc)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(xvip_video_formats); ++i) {
+		const struct xvip_video_format *format = &xvip_video_formats[i];
+
+		if (format->fourcc == fourcc)
+			return format;
+	}
+
+	return ERR_PTR(-EINVAL);
+}
+EXPORT_SYMBOL_GPL(xvip_get_format_by_fourcc);
+
+/**
+ * xvip_of_get_format - Parse a device tree node and return format information
+ * @node: the device tree node
+ *
+ * Read the xlnx,video-format, xlnx,video-width and xlnx,cfa-pattern properties
+ * from the device tree @node passed as an argument and return the corresponding
+ * format information.
+ *
+ * Return: a pointer to the format information structure corresponding to the
+ * format name and width, or ERR_PTR if no corresponding format can be found.
+ */
+const struct xvip_video_format *xvip_of_get_format(struct device_node *node)
+{
+	const char *pattern = "mono";
+	unsigned int vf_code;
+	unsigned int i;
+	u32 width;
+	int ret;
+
+	ret = of_property_read_u32(node, "xlnx,video-format", &vf_code);
+	if (ret < 0)
+		return ERR_PTR(ret);
+
+	ret = of_property_read_u32(node, "xlnx,video-width", &width);
+	if (ret < 0)
+		return ERR_PTR(ret);
+
+	if (vf_code == XVIP_VF_MONO_SENSOR)
+		of_property_read_string(node, "xlnx,cfa-pattern", &pattern);
+
+	for (i = 0; i < ARRAY_SIZE(xvip_video_formats); ++i) {
+		const struct xvip_video_format *format = &xvip_video_formats[i];
+
+		if (format->vf_code != vf_code || format->width != width)
+			continue;
+
+		if (vf_code == XVIP_VF_MONO_SENSOR &&
+		    strcmp(pattern, format->pattern))
+			continue;
+
+		return format;
+	}
+
+	return ERR_PTR(-EINVAL);
+}
+EXPORT_SYMBOL_GPL(xvip_of_get_format);
+
+/**
+ * xvip_set_format_size - Set the media bus frame format size
+ * @format: V4L2 frame format on media bus
+ * @fmt: media bus format
+ *
+ * Set the media bus frame format size. The width / height from the subdevice
+ * format are set to the given media bus format. The new format size is stored
+ * in @format. The width and height are clamped using default min / max values.
+ */
+void xvip_set_format_size(struct v4l2_mbus_framefmt *format,
+			  const struct v4l2_subdev_format *fmt)
+{
+	format->width = clamp_t(unsigned int, fmt->format.width,
+				XVIP_MIN_WIDTH, XVIP_MAX_WIDTH);
+	format->height = clamp_t(unsigned int, fmt->format.height,
+			 XVIP_MIN_HEIGHT, XVIP_MAX_HEIGHT);
+}
+EXPORT_SYMBOL_GPL(xvip_set_format_size);
+
+/**
+ * xvip_clr_or_set - Clear or set the register with a bitmask
+ * @xvip: Xilinx Video IP device
+ * @addr: address of register
+ * @mask: bitmask to be set or cleared
+ * @set: boolean flag indicating whether to set or clear
+ *
+ * Clear or set the register at address @addr with a bitmask @mask depending on
+ * the boolean flag @set. When the flag @set is true, the bitmask is set in
+ * the register, otherwise the bitmask is cleared from the register
+ * when the flag @set is false.
+ *
+ * Fox eample, this function can be used to set a control with a boolean value
+ * requested by users. If the caller knows whether to set or clear in the first
+ * place, the caller should call xvip_clr() or xvip_set() directly instead of
+ * using this function.
+ */
+void xvip_clr_or_set(struct xvip_device *xvip, u32 addr, u32 mask, bool set)
+{
+	u32 reg;
+
+	reg = xvip_read(xvip, addr);
+	reg = set ? reg | mask : reg & ~mask;
+	xvip_write(xvip, addr, reg);
+}
+EXPORT_SYMBOL_GPL(xvip_clr_or_set);
+
+/**
+ * xvip_clr_and_set - Clear and set the register with a bitmask
+ * @xvip: Xilinx Video IP device
+ * @addr: address of register
+ * @clr: bitmask to be cleared
+ * @set: bitmask to be set
+ *
+ * Clear a bit(s) of mask @clr in the register at address @addr, then set
+ * a bit(s) of mask @set in the register after.
+ */
+void xvip_clr_and_set(struct xvip_device *xvip, u32 addr, u32 clr, u32 set)
+{
+	u32 reg;
+
+	reg = xvip_read(xvip, addr);
+	reg &= ~clr;
+	reg |= set;
+	xvip_write(xvip, addr, reg);
+}
+EXPORT_SYMBOL_GPL(xvip_clr_and_set);
+
+int xvip_init_resources(struct xvip_device *xvip)
+{
+	struct platform_device *pdev = to_platform_device(xvip->dev);
+	struct resource *res;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	xvip->iomem = devm_ioremap_resource(xvip->dev, res);
+	if (IS_ERR(xvip->iomem))
+		return PTR_ERR(xvip->iomem);
+
+	xvip->clk = devm_clk_get(xvip->dev, NULL);
+	if (IS_ERR(xvip->clk))
+		return PTR_ERR(xvip->clk);
+
+	clk_prepare_enable(xvip->clk);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(xvip_init_resources);
+
+void xvip_cleanup_resources(struct xvip_device *xvip)
+{
+	clk_disable_unprepare(xvip->clk);
+}
+EXPORT_SYMBOL_GPL(xvip_cleanup_resources);
+
+/* -----------------------------------------------------------------------------
+ * Subdev operations handlers
+ */
+
+/**
+ * xvip_enum_mbus_code - Enumerate the media format code
+ * @subdev: V4L2 subdevice
+ * @cfg: V4L2 subdev pad configuration
+ * @code: returning media bus code
+ *
+ * Enumerate the media bus code of the subdevice. Return the corresponding
+ * pad format code. This function only works for subdevices with fixed format
+ * on all pads. Subdevices with multiple format should have their own
+ * function to enumerate mbus codes.
+ *
+ * Return: 0 if the media bus code is found, or -EINVAL if the format index
+ * is not valid.
+ */
+int xvip_enum_mbus_code(struct v4l2_subdev *subdev,
+			struct v4l2_subdev_pad_config *cfg,
+			struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct v4l2_mbus_framefmt *format;
+
+	/* Enumerating frame sizes based on the active configuration isn't
+	 * supported yet.
+	 */
+	if (code->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		return -EINVAL;
+
+	if (code->index)
+		return -EINVAL;
+
+	format = v4l2_subdev_get_try_format(subdev, cfg, code->pad);
+
+	code->code = format->code;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(xvip_enum_mbus_code);
+
+/**
+ * xvip_enum_frame_size - Enumerate the media bus frame size
+ * @subdev: V4L2 subdevice
+ * @cfg: V4L2 subdev pad configuration
+ * @fse: returning media bus frame size
+ *
+ * This function is a drop-in implementation of the subdev enum_frame_size pad
+ * operation. It assumes that the subdevice has one sink pad and one source
+ * pad, and that the format on the source pad is always identical to the
+ * format on the sink pad. Entities with different requirements need to
+ * implement their own enum_frame_size handlers.
+ *
+ * Return: 0 if the media bus frame size is found, or -EINVAL
+ * if the index or the code is not valid.
+ */
+int xvip_enum_frame_size(struct v4l2_subdev *subdev,
+			 struct v4l2_subdev_pad_config *cfg,
+			 struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct v4l2_mbus_framefmt *format;
+
+	/* Enumerating frame sizes based on the active configuration isn't
+	 * supported yet.
+	 */
+	if (fse->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		return -EINVAL;
+
+	format = v4l2_subdev_get_try_format(subdev, cfg, fse->pad);
+
+	if (fse->index || fse->code != format->code)
+		return -EINVAL;
+
+	if (fse->pad == XVIP_PAD_SINK) {
+		fse->min_width = XVIP_MIN_WIDTH;
+		fse->max_width = XVIP_MAX_WIDTH;
+		fse->min_height = XVIP_MIN_HEIGHT;
+		fse->max_height = XVIP_MAX_HEIGHT;
+	} else {
+		/* The size on the source pad is fixed and always identical to
+		 * the size on the sink pad.
+		 */
+		fse->min_width = format->width;
+		fse->max_width = format->width;
+		fse->min_height = format->height;
+		fse->max_height = format->height;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(xvip_enum_frame_size);
diff --git a/drivers/media/platform/xilinx/xilinx-vip.h b/drivers/media/platform/xilinx/xilinx-vip.h
new file mode 100644
index 0000000..42fee20
--- /dev/null
+++ b/drivers/media/platform/xilinx/xilinx-vip.h
@@ -0,0 +1,238 @@
+/*
+ * Xilinx Video IP Core
+ *
+ * Copyright (C) 2013-2015 Ideas on Board
+ * Copyright (C) 2013-2015 Xilinx, Inc.
+ *
+ * Contacts: Hyun Kwon <hyun.kwon@xilinx.com>
+ *           Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef __XILINX_VIP_H__
+#define __XILINX_VIP_H__
+
+#include <linux/io.h>
+#include <media/v4l2-subdev.h>
+
+struct clk;
+
+/*
+ * Minimum and maximum width and height common to most video IP cores. IP
+ * cores with different requirements must define their own values.
+ */
+#define XVIP_MIN_WIDTH			32
+#define XVIP_MAX_WIDTH			7680
+#define XVIP_MIN_HEIGHT			32
+#define XVIP_MAX_HEIGHT			7680
+
+/*
+ * Pad IDs. IP cores with with multiple inputs or outputs should define
+ * their own values.
+ */
+#define XVIP_PAD_SINK			0
+#define XVIP_PAD_SOURCE			1
+
+/* Xilinx Video IP Control Registers */
+#define XVIP_CTRL_CONTROL			0x0000
+#define XVIP_CTRL_CONTROL_SW_ENABLE		(1 << 0)
+#define XVIP_CTRL_CONTROL_REG_UPDATE		(1 << 1)
+#define XVIP_CTRL_CONTROL_BYPASS		(1 << 4)
+#define XVIP_CTRL_CONTROL_TEST_PATTERN		(1 << 5)
+#define XVIP_CTRL_CONTROL_FRAME_SYNC_RESET	(1 << 30)
+#define XVIP_CTRL_CONTROL_SW_RESET		(1 << 31)
+#define XVIP_CTRL_STATUS			0x0004
+#define XVIP_CTRL_STATUS_PROC_STARTED		(1 << 0)
+#define XVIP_CTRL_STATUS_EOF			(1 << 1)
+#define XVIP_CTRL_ERROR				0x0008
+#define XVIP_CTRL_ERROR_SLAVE_EOL_EARLY		(1 << 0)
+#define XVIP_CTRL_ERROR_SLAVE_EOL_LATE		(1 << 1)
+#define XVIP_CTRL_ERROR_SLAVE_SOF_EARLY		(1 << 2)
+#define XVIP_CTRL_ERROR_SLAVE_SOF_LATE		(1 << 3)
+#define XVIP_CTRL_IRQ_ENABLE			0x000c
+#define XVIP_CTRL_IRQ_ENABLE_PROC_STARTED	(1 << 0)
+#define XVIP_CTRL_IRQ_EOF			(1 << 1)
+#define XVIP_CTRL_VERSION			0x0010
+#define XVIP_CTRL_VERSION_MAJOR_MASK		(0xff << 24)
+#define XVIP_CTRL_VERSION_MAJOR_SHIFT		24
+#define XVIP_CTRL_VERSION_MINOR_MASK		(0xff << 16)
+#define XVIP_CTRL_VERSION_MINOR_SHIFT		16
+#define XVIP_CTRL_VERSION_REVISION_MASK		(0xf << 12)
+#define XVIP_CTRL_VERSION_REVISION_SHIFT	12
+#define XVIP_CTRL_VERSION_PATCH_MASK		(0xf << 8)
+#define XVIP_CTRL_VERSION_PATCH_SHIFT		8
+#define XVIP_CTRL_VERSION_INTERNAL_MASK		(0xff << 0)
+#define XVIP_CTRL_VERSION_INTERNAL_SHIFT	0
+
+/* Xilinx Video IP Timing Registers */
+#define XVIP_ACTIVE_SIZE			0x0020
+#define XVIP_ACTIVE_VSIZE_MASK			(0x7ff << 16)
+#define XVIP_ACTIVE_VSIZE_SHIFT			16
+#define XVIP_ACTIVE_HSIZE_MASK			(0x7ff << 0)
+#define XVIP_ACTIVE_HSIZE_SHIFT			0
+#define XVIP_ENCODING				0x0028
+#define XVIP_ENCODING_NBITS_8			(0 << 4)
+#define XVIP_ENCODING_NBITS_10			(1 << 4)
+#define XVIP_ENCODING_NBITS_12			(2 << 4)
+#define XVIP_ENCODING_NBITS_16			(3 << 4)
+#define XVIP_ENCODING_NBITS_MASK		(3 << 4)
+#define XVIP_ENCODING_NBITS_SHIFT		4
+#define XVIP_ENCODING_VIDEO_FORMAT_YUV422	(0 << 0)
+#define XVIP_ENCODING_VIDEO_FORMAT_YUV444	(1 << 0)
+#define XVIP_ENCODING_VIDEO_FORMAT_RGB		(2 << 0)
+#define XVIP_ENCODING_VIDEO_FORMAT_YUV420	(3 << 0)
+#define XVIP_ENCODING_VIDEO_FORMAT_MASK		(3 << 0)
+#define XVIP_ENCODING_VIDEO_FORMAT_SHIFT	0
+
+/**
+ * struct xvip_device - Xilinx Video IP device structure
+ * @subdev: V4L2 subdevice
+ * @dev: (OF) device
+ * @iomem: device I/O register space remapped to kernel virtual memory
+ * @clk: video core clock
+ * @saved_ctrl: saved control register for resume / suspend
+ */
+struct xvip_device {
+	struct v4l2_subdev subdev;
+	struct device *dev;
+	void __iomem *iomem;
+	struct clk *clk;
+	u32 saved_ctrl;
+};
+
+/**
+ * struct xvip_video_format - Xilinx Video IP video format description
+ * @vf_code: AXI4 video format code
+ * @width: AXI4 format width in bits per component
+ * @pattern: CFA pattern for Mono/Sensor formats
+ * @code: media bus format code
+ * @bpp: bytes per pixel (when stored in memory)
+ * @fourcc: V4L2 pixel format FCC identifier
+ * @description: format description, suitable for userspace
+ */
+struct xvip_video_format {
+	unsigned int vf_code;
+	unsigned int width;
+	const char *pattern;
+	unsigned int code;
+	unsigned int bpp;
+	u32 fourcc;
+	const char *description;
+};
+
+const struct xvip_video_format *xvip_get_format_by_code(unsigned int code);
+const struct xvip_video_format *xvip_get_format_by_fourcc(u32 fourcc);
+const struct xvip_video_format *xvip_of_get_format(struct device_node *node);
+void xvip_set_format_size(struct v4l2_mbus_framefmt *format,
+			  const struct v4l2_subdev_format *fmt);
+int xvip_enum_mbus_code(struct v4l2_subdev *subdev,
+			struct v4l2_subdev_pad_config *cfg,
+			struct v4l2_subdev_mbus_code_enum *code);
+int xvip_enum_frame_size(struct v4l2_subdev *subdev,
+			 struct v4l2_subdev_pad_config *cfg,
+			 struct v4l2_subdev_frame_size_enum *fse);
+
+static inline u32 xvip_read(struct xvip_device *xvip, u32 addr)
+{
+	return ioread32(xvip->iomem + addr);
+}
+
+static inline void xvip_write(struct xvip_device *xvip, u32 addr, u32 value)
+{
+	iowrite32(value, xvip->iomem + addr);
+}
+
+static inline void xvip_clr(struct xvip_device *xvip, u32 addr, u32 clr)
+{
+	xvip_write(xvip, addr, xvip_read(xvip, addr) & ~clr);
+}
+
+static inline void xvip_set(struct xvip_device *xvip, u32 addr, u32 set)
+{
+	xvip_write(xvip, addr, xvip_read(xvip, addr) | set);
+}
+
+void xvip_clr_or_set(struct xvip_device *xvip, u32 addr, u32 mask, bool set);
+void xvip_clr_and_set(struct xvip_device *xvip, u32 addr, u32 clr, u32 set);
+
+int xvip_init_resources(struct xvip_device *xvip);
+void xvip_cleanup_resources(struct xvip_device *xvip);
+
+static inline void xvip_reset(struct xvip_device *xvip)
+{
+	xvip_write(xvip, XVIP_CTRL_CONTROL, XVIP_CTRL_CONTROL_SW_RESET);
+}
+
+static inline void xvip_start(struct xvip_device *xvip)
+{
+	xvip_set(xvip, XVIP_CTRL_CONTROL,
+		 XVIP_CTRL_CONTROL_SW_ENABLE | XVIP_CTRL_CONTROL_REG_UPDATE);
+}
+
+static inline void xvip_stop(struct xvip_device *xvip)
+{
+	xvip_clr(xvip, XVIP_CTRL_CONTROL, XVIP_CTRL_CONTROL_SW_ENABLE);
+}
+
+static inline void xvip_resume(struct xvip_device *xvip)
+{
+	xvip_write(xvip, XVIP_CTRL_CONTROL,
+		   xvip->saved_ctrl | XVIP_CTRL_CONTROL_SW_ENABLE);
+}
+
+static inline void xvip_suspend(struct xvip_device *xvip)
+{
+	xvip->saved_ctrl = xvip_read(xvip, XVIP_CTRL_CONTROL);
+	xvip_write(xvip, XVIP_CTRL_CONTROL,
+		   xvip->saved_ctrl & ~XVIP_CTRL_CONTROL_SW_ENABLE);
+}
+
+static inline void xvip_set_frame_size(struct xvip_device *xvip,
+				       const struct v4l2_mbus_framefmt *format)
+{
+	xvip_write(xvip, XVIP_ACTIVE_SIZE,
+		   (format->height << XVIP_ACTIVE_VSIZE_SHIFT) |
+		   (format->width << XVIP_ACTIVE_HSIZE_SHIFT));
+}
+
+static inline void xvip_get_frame_size(struct xvip_device *xvip,
+				       struct v4l2_mbus_framefmt *format)
+{
+	u32 reg;
+
+	reg = xvip_read(xvip, XVIP_ACTIVE_SIZE);
+	format->width = (reg & XVIP_ACTIVE_HSIZE_MASK) >>
+			XVIP_ACTIVE_HSIZE_SHIFT;
+	format->height = (reg & XVIP_ACTIVE_VSIZE_MASK) >>
+			 XVIP_ACTIVE_VSIZE_SHIFT;
+}
+
+static inline void xvip_enable_reg_update(struct xvip_device *xvip)
+{
+	xvip_set(xvip, XVIP_CTRL_CONTROL, XVIP_CTRL_CONTROL_REG_UPDATE);
+}
+
+static inline void xvip_disable_reg_update(struct xvip_device *xvip)
+{
+	xvip_clr(xvip, XVIP_CTRL_CONTROL, XVIP_CTRL_CONTROL_REG_UPDATE);
+}
+
+static inline void xvip_print_version(struct xvip_device *xvip)
+{
+	u32 version;
+
+	version = xvip_read(xvip, XVIP_CTRL_VERSION);
+
+	dev_info(xvip->dev, "device found, version %u.%02x%x\n",
+		 ((version & XVIP_CTRL_VERSION_MAJOR_MASK) >>
+		  XVIP_CTRL_VERSION_MAJOR_SHIFT),
+		 ((version & XVIP_CTRL_VERSION_MINOR_MASK) >>
+		  XVIP_CTRL_VERSION_MINOR_SHIFT),
+		 ((version & XVIP_CTRL_VERSION_REVISION_MASK) >>
+		  XVIP_CTRL_VERSION_REVISION_SHIFT));
+}
+
+#endif /* __XILINX_VIP_H__ */
diff --git a/drivers/media/platform/xilinx/xilinx-vipp.c b/drivers/media/platform/xilinx/xilinx-vipp.c
new file mode 100644
index 0000000..7b7cb9c
--- /dev/null
+++ b/drivers/media/platform/xilinx/xilinx-vipp.c
@@ -0,0 +1,669 @@
+/*
+ * Xilinx Video IP Composite Device
+ *
+ * Copyright (C) 2013-2015 Ideas on Board
+ * Copyright (C) 2013-2015 Xilinx, Inc.
+ *
+ * Contacts: Hyun Kwon <hyun.kwon@xilinx.com>
+ *           Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_graph.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#include <media/v4l2-async.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-of.h>
+
+#include "xilinx-dma.h"
+#include "xilinx-vipp.h"
+
+#define XVIPP_DMA_S2MM				0
+#define XVIPP_DMA_MM2S				1
+
+/**
+ * struct xvip_graph_entity - Entity in the video graph
+ * @list: list entry in a graph entities list
+ * @node: the entity's DT node
+ * @entity: media entity, from the corresponding V4L2 subdev
+ * @asd: subdev asynchronous registration information
+ * @subdev: V4L2 subdev
+ */
+struct xvip_graph_entity {
+	struct list_head list;
+	struct device_node *node;
+	struct media_entity *entity;
+
+	struct v4l2_async_subdev asd;
+	struct v4l2_subdev *subdev;
+};
+
+/* -----------------------------------------------------------------------------
+ * Graph Management
+ */
+
+static struct xvip_graph_entity *
+xvip_graph_find_entity(struct xvip_composite_device *xdev,
+		       const struct device_node *node)
+{
+	struct xvip_graph_entity *entity;
+
+	list_for_each_entry(entity, &xdev->entities, list) {
+		if (entity->node == node)
+			return entity;
+	}
+
+	return NULL;
+}
+
+static int xvip_graph_build_one(struct xvip_composite_device *xdev,
+				struct xvip_graph_entity *entity)
+{
+	u32 link_flags = MEDIA_LNK_FL_ENABLED;
+	struct media_entity *local = entity->entity;
+	struct media_entity *remote;
+	struct media_pad *local_pad;
+	struct media_pad *remote_pad;
+	struct xvip_graph_entity *ent;
+	struct v4l2_of_link link;
+	struct device_node *ep = NULL;
+	struct device_node *next;
+	int ret = 0;
+
+	dev_dbg(xdev->dev, "creating links for entity %s\n", local->name);
+
+	while (1) {
+		/* Get the next endpoint and parse its link. */
+		next = of_graph_get_next_endpoint(entity->node, ep);
+		if (next == NULL)
+			break;
+
+		of_node_put(ep);
+		ep = next;
+
+		dev_dbg(xdev->dev, "processing endpoint %s\n", ep->full_name);
+
+		ret = v4l2_of_parse_link(ep, &link);
+		if (ret < 0) {
+			dev_err(xdev->dev, "failed to parse link for %s\n",
+				ep->full_name);
+			continue;
+		}
+
+		/* Skip sink ports, they will be processed from the other end of
+		 * the link.
+		 */
+		if (link.local_port >= local->num_pads) {
+			dev_err(xdev->dev, "invalid port number %u on %s\n",
+				link.local_port, link.local_node->full_name);
+			v4l2_of_put_link(&link);
+			ret = -EINVAL;
+			break;
+		}
+
+		local_pad = &local->pads[link.local_port];
+
+		if (local_pad->flags & MEDIA_PAD_FL_SINK) {
+			dev_dbg(xdev->dev, "skipping sink port %s:%u\n",
+				link.local_node->full_name, link.local_port);
+			v4l2_of_put_link(&link);
+			continue;
+		}
+
+		/* Skip DMA engines, they will be processed separately. */
+		if (link.remote_node == xdev->dev->of_node) {
+			dev_dbg(xdev->dev, "skipping DMA port %s:%u\n",
+				link.local_node->full_name, link.local_port);
+			v4l2_of_put_link(&link);
+			continue;
+		}
+
+		/* Find the remote entity. */
+		ent = xvip_graph_find_entity(xdev, link.remote_node);
+		if (ent == NULL) {
+			dev_err(xdev->dev, "no entity found for %s\n",
+				link.remote_node->full_name);
+			v4l2_of_put_link(&link);
+			ret = -ENODEV;
+			break;
+		}
+
+		remote = ent->entity;
+
+		if (link.remote_port >= remote->num_pads) {
+			dev_err(xdev->dev, "invalid port number %u on %s\n",
+				link.remote_port, link.remote_node->full_name);
+			v4l2_of_put_link(&link);
+			ret = -EINVAL;
+			break;
+		}
+
+		remote_pad = &remote->pads[link.remote_port];
+
+		v4l2_of_put_link(&link);
+
+		/* Create the media link. */
+		dev_dbg(xdev->dev, "creating %s:%u -> %s:%u link\n",
+			local->name, local_pad->index,
+			remote->name, remote_pad->index);
+
+		ret = media_entity_create_link(local, local_pad->index,
+					       remote, remote_pad->index,
+					       link_flags);
+		if (ret < 0) {
+			dev_err(xdev->dev,
+				"failed to create %s:%u -> %s:%u link\n",
+				local->name, local_pad->index,
+				remote->name, remote_pad->index);
+			break;
+		}
+	}
+
+	of_node_put(ep);
+	return ret;
+}
+
+static struct xvip_dma *
+xvip_graph_find_dma(struct xvip_composite_device *xdev, unsigned int port)
+{
+	struct xvip_dma *dma;
+
+	list_for_each_entry(dma, &xdev->dmas, list) {
+		if (dma->port == port)
+			return dma;
+	}
+
+	return NULL;
+}
+
+static int xvip_graph_build_dma(struct xvip_composite_device *xdev)
+{
+	u32 link_flags = MEDIA_LNK_FL_ENABLED;
+	struct device_node *node = xdev->dev->of_node;
+	struct media_entity *source;
+	struct media_entity *sink;
+	struct media_pad *source_pad;
+	struct media_pad *sink_pad;
+	struct xvip_graph_entity *ent;
+	struct v4l2_of_link link;
+	struct device_node *ep = NULL;
+	struct device_node *next;
+	struct xvip_dma *dma;
+	int ret = 0;
+
+	dev_dbg(xdev->dev, "creating links for DMA engines\n");
+
+	while (1) {
+		/* Get the next endpoint and parse its link. */
+		next = of_graph_get_next_endpoint(node, ep);
+		if (next == NULL)
+			break;
+
+		of_node_put(ep);
+		ep = next;
+
+		dev_dbg(xdev->dev, "processing endpoint %s\n", ep->full_name);
+
+		ret = v4l2_of_parse_link(ep, &link);
+		if (ret < 0) {
+			dev_err(xdev->dev, "failed to parse link for %s\n",
+				ep->full_name);
+			continue;
+		}
+
+		/* Find the DMA engine. */
+		dma = xvip_graph_find_dma(xdev, link.local_port);
+		if (dma == NULL) {
+			dev_err(xdev->dev, "no DMA engine found for port %u\n",
+				link.local_port);
+			v4l2_of_put_link(&link);
+			ret = -EINVAL;
+			break;
+		}
+
+		dev_dbg(xdev->dev, "creating link for DMA engine %s\n",
+			dma->video.name);
+
+		/* Find the remote entity. */
+		ent = xvip_graph_find_entity(xdev, link.remote_node);
+		if (ent == NULL) {
+			dev_err(xdev->dev, "no entity found for %s\n",
+				link.remote_node->full_name);
+			v4l2_of_put_link(&link);
+			ret = -ENODEV;
+			break;
+		}
+
+		if (link.remote_port >= ent->entity->num_pads) {
+			dev_err(xdev->dev, "invalid port number %u on %s\n",
+				link.remote_port, link.remote_node->full_name);
+			v4l2_of_put_link(&link);
+			ret = -EINVAL;
+			break;
+		}
+
+		if (dma->pad.flags & MEDIA_PAD_FL_SOURCE) {
+			source = &dma->video.entity;
+			source_pad = &dma->pad;
+			sink = ent->entity;
+			sink_pad = &sink->pads[link.remote_port];
+		} else {
+			source = ent->entity;
+			source_pad = &source->pads[link.remote_port];
+			sink = &dma->video.entity;
+			sink_pad = &dma->pad;
+		}
+
+		v4l2_of_put_link(&link);
+
+		/* Create the media link. */
+		dev_dbg(xdev->dev, "creating %s:%u -> %s:%u link\n",
+			source->name, source_pad->index,
+			sink->name, sink_pad->index);
+
+		ret = media_entity_create_link(source, source_pad->index,
+					       sink, sink_pad->index,
+					       link_flags);
+		if (ret < 0) {
+			dev_err(xdev->dev,
+				"failed to create %s:%u -> %s:%u link\n",
+				source->name, source_pad->index,
+				sink->name, sink_pad->index);
+			break;
+		}
+	}
+
+	of_node_put(ep);
+	return ret;
+}
+
+static int xvip_graph_notify_complete(struct v4l2_async_notifier *notifier)
+{
+	struct xvip_composite_device *xdev =
+		container_of(notifier, struct xvip_composite_device, notifier);
+	struct xvip_graph_entity *entity;
+	int ret;
+
+	dev_dbg(xdev->dev, "notify complete, all subdevs registered\n");
+
+	/* Create links for every entity. */
+	list_for_each_entry(entity, &xdev->entities, list) {
+		ret = xvip_graph_build_one(xdev, entity);
+		if (ret < 0)
+			return ret;
+	}
+
+	/* Create links for DMA channels. */
+	ret = xvip_graph_build_dma(xdev);
+	if (ret < 0)
+		return ret;
+
+	ret = v4l2_device_register_subdev_nodes(&xdev->v4l2_dev);
+	if (ret < 0)
+		dev_err(xdev->dev, "failed to register subdev nodes\n");
+
+	return ret;
+}
+
+static int xvip_graph_notify_bound(struct v4l2_async_notifier *notifier,
+				   struct v4l2_subdev *subdev,
+				   struct v4l2_async_subdev *asd)
+{
+	struct xvip_composite_device *xdev =
+		container_of(notifier, struct xvip_composite_device, notifier);
+	struct xvip_graph_entity *entity;
+
+	/* Locate the entity corresponding to the bound subdev and store the
+	 * subdev pointer.
+	 */
+	list_for_each_entry(entity, &xdev->entities, list) {
+		if (entity->node != subdev->dev->of_node)
+			continue;
+
+		if (entity->subdev) {
+			dev_err(xdev->dev, "duplicate subdev for node %s\n",
+				entity->node->full_name);
+			return -EINVAL;
+		}
+
+		dev_dbg(xdev->dev, "subdev %s bound\n", subdev->name);
+		entity->entity = &subdev->entity;
+		entity->subdev = subdev;
+		return 0;
+	}
+
+	dev_err(xdev->dev, "no entity for subdev %s\n", subdev->name);
+	return -EINVAL;
+}
+
+static int xvip_graph_parse_one(struct xvip_composite_device *xdev,
+				struct device_node *node)
+{
+	struct xvip_graph_entity *entity;
+	struct device_node *remote;
+	struct device_node *ep = NULL;
+	struct device_node *next;
+	int ret = 0;
+
+	dev_dbg(xdev->dev, "parsing node %s\n", node->full_name);
+
+	while (1) {
+		next = of_graph_get_next_endpoint(node, ep);
+		if (next == NULL)
+			break;
+
+		of_node_put(ep);
+		ep = next;
+
+		dev_dbg(xdev->dev, "handling endpoint %s\n", ep->full_name);
+
+		remote = of_graph_get_remote_port_parent(ep);
+		if (remote == NULL) {
+			ret = -EINVAL;
+			break;
+		}
+
+		/* Skip entities that we have already processed. */
+		if (remote == xdev->dev->of_node ||
+		    xvip_graph_find_entity(xdev, remote)) {
+			of_node_put(remote);
+			continue;
+		}
+
+		entity = devm_kzalloc(xdev->dev, sizeof(*entity), GFP_KERNEL);
+		if (entity == NULL) {
+			of_node_put(remote);
+			ret = -ENOMEM;
+			break;
+		}
+
+		entity->node = remote;
+		entity->asd.match_type = V4L2_ASYNC_MATCH_OF;
+		entity->asd.match.of.node = remote;
+		list_add_tail(&entity->list, &xdev->entities);
+		xdev->num_subdevs++;
+	}
+
+	of_node_put(ep);
+	return ret;
+}
+
+static int xvip_graph_parse(struct xvip_composite_device *xdev)
+{
+	struct xvip_graph_entity *entity;
+	int ret;
+
+	/*
+	 * Walk the links to parse the full graph. Start by parsing the
+	 * composite node and then parse entities in turn. The list_for_each
+	 * loop will handle entities added at the end of the list while walking
+	 * the links.
+	 */
+	ret = xvip_graph_parse_one(xdev, xdev->dev->of_node);
+	if (ret < 0)
+		return 0;
+
+	list_for_each_entry(entity, &xdev->entities, list) {
+		ret = xvip_graph_parse_one(xdev, entity->node);
+		if (ret < 0)
+			break;
+	}
+
+	return ret;
+}
+
+static int xvip_graph_dma_init_one(struct xvip_composite_device *xdev,
+				   struct device_node *node)
+{
+	struct xvip_dma *dma;
+	enum v4l2_buf_type type;
+	const char *direction;
+	unsigned int index;
+	int ret;
+
+	ret = of_property_read_string(node, "direction", &direction);
+	if (ret < 0)
+		return ret;
+
+	if (strcmp(direction, "input") == 0)
+		type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	else if (strcmp(direction, "output") == 0)
+		type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	else
+		return -EINVAL;
+
+	of_property_read_u32(node, "reg", &index);
+
+	dma = devm_kzalloc(xdev->dev, sizeof(*dma), GFP_KERNEL);
+	if (dma == NULL)
+		return -ENOMEM;
+
+	ret = xvip_dma_init(xdev, dma, type, index);
+	if (ret < 0) {
+		dev_err(xdev->dev, "%s initialization failed\n",
+			node->full_name);
+		return ret;
+	}
+
+	list_add_tail(&dma->list, &xdev->dmas);
+
+	xdev->v4l2_caps |= type == V4L2_BUF_TYPE_VIDEO_CAPTURE
+			 ? V4L2_CAP_VIDEO_CAPTURE : V4L2_CAP_VIDEO_OUTPUT;
+
+	return 0;
+}
+
+static int xvip_graph_dma_init(struct xvip_composite_device *xdev)
+{
+	struct device_node *ports;
+	struct device_node *port;
+	int ret;
+
+	ports = of_get_child_by_name(xdev->dev->of_node, "ports");
+	if (ports == NULL) {
+		dev_err(xdev->dev, "ports node not present\n");
+		return -EINVAL;
+	}
+
+	for_each_child_of_node(ports, port) {
+		ret = xvip_graph_dma_init_one(xdev, port);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+static void xvip_graph_cleanup(struct xvip_composite_device *xdev)
+{
+	struct xvip_graph_entity *entityp;
+	struct xvip_graph_entity *entity;
+	struct xvip_dma *dmap;
+	struct xvip_dma *dma;
+
+	v4l2_async_notifier_unregister(&xdev->notifier);
+
+	list_for_each_entry_safe(entity, entityp, &xdev->entities, list) {
+		of_node_put(entity->node);
+		list_del(&entity->list);
+	}
+
+	list_for_each_entry_safe(dma, dmap, &xdev->dmas, list) {
+		xvip_dma_cleanup(dma);
+		list_del(&dma->list);
+	}
+}
+
+static int xvip_graph_init(struct xvip_composite_device *xdev)
+{
+	struct xvip_graph_entity *entity;
+	struct v4l2_async_subdev **subdevs = NULL;
+	unsigned int num_subdevs;
+	unsigned int i;
+	int ret;
+
+	/* Init the DMA channels. */
+	ret = xvip_graph_dma_init(xdev);
+	if (ret < 0) {
+		dev_err(xdev->dev, "DMA initialization failed\n");
+		goto done;
+	}
+
+	/* Parse the graph to extract a list of subdevice DT nodes. */
+	ret = xvip_graph_parse(xdev);
+	if (ret < 0) {
+		dev_err(xdev->dev, "graph parsing failed\n");
+		goto done;
+	}
+
+	if (!xdev->num_subdevs) {
+		dev_err(xdev->dev, "no subdev found in graph\n");
+		goto done;
+	}
+
+	/* Register the subdevices notifier. */
+	num_subdevs = xdev->num_subdevs;
+	subdevs = devm_kzalloc(xdev->dev, sizeof(*subdevs) * num_subdevs,
+			       GFP_KERNEL);
+	if (subdevs == NULL) {
+		ret = -ENOMEM;
+		goto done;
+	}
+
+	i = 0;
+	list_for_each_entry(entity, &xdev->entities, list)
+		subdevs[i++] = &entity->asd;
+
+	xdev->notifier.subdevs = subdevs;
+	xdev->notifier.num_subdevs = num_subdevs;
+	xdev->notifier.bound = xvip_graph_notify_bound;
+	xdev->notifier.complete = xvip_graph_notify_complete;
+
+	ret = v4l2_async_notifier_register(&xdev->v4l2_dev, &xdev->notifier);
+	if (ret < 0) {
+		dev_err(xdev->dev, "notifier registration failed\n");
+		goto done;
+	}
+
+	ret = 0;
+
+done:
+	if (ret < 0)
+		xvip_graph_cleanup(xdev);
+
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * Media Controller and V4L2
+ */
+
+static void xvip_composite_v4l2_cleanup(struct xvip_composite_device *xdev)
+{
+	v4l2_device_unregister(&xdev->v4l2_dev);
+	media_device_unregister(&xdev->media_dev);
+}
+
+static int xvip_composite_v4l2_init(struct xvip_composite_device *xdev)
+{
+	int ret;
+
+	xdev->media_dev.dev = xdev->dev;
+	strlcpy(xdev->media_dev.model, "Xilinx Video Composite Device",
+		sizeof(xdev->media_dev.model));
+	xdev->media_dev.hw_revision = 0;
+
+	ret = media_device_register(&xdev->media_dev);
+	if (ret < 0) {
+		dev_err(xdev->dev, "media device registration failed (%d)\n",
+			ret);
+		return ret;
+	}
+
+	xdev->v4l2_dev.mdev = &xdev->media_dev;
+	ret = v4l2_device_register(xdev->dev, &xdev->v4l2_dev);
+	if (ret < 0) {
+		dev_err(xdev->dev, "V4L2 device registration failed (%d)\n",
+			ret);
+		media_device_unregister(&xdev->media_dev);
+		return ret;
+	}
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * Platform Device Driver
+ */
+
+static int xvip_composite_probe(struct platform_device *pdev)
+{
+	struct xvip_composite_device *xdev;
+	int ret;
+
+	xdev = devm_kzalloc(&pdev->dev, sizeof(*xdev), GFP_KERNEL);
+	if (!xdev)
+		return -ENOMEM;
+
+	xdev->dev = &pdev->dev;
+	INIT_LIST_HEAD(&xdev->entities);
+	INIT_LIST_HEAD(&xdev->dmas);
+
+	ret = xvip_composite_v4l2_init(xdev);
+	if (ret < 0)
+		return ret;
+
+	ret = xvip_graph_init(xdev);
+	if (ret < 0)
+		goto error;
+
+	platform_set_drvdata(pdev, xdev);
+
+	dev_info(xdev->dev, "device registered\n");
+
+	return 0;
+
+error:
+	xvip_composite_v4l2_cleanup(xdev);
+	return ret;
+}
+
+static int xvip_composite_remove(struct platform_device *pdev)
+{
+	struct xvip_composite_device *xdev = platform_get_drvdata(pdev);
+
+	xvip_graph_cleanup(xdev);
+	xvip_composite_v4l2_cleanup(xdev);
+
+	return 0;
+}
+
+static const struct of_device_id xvip_composite_of_id_table[] = {
+	{ .compatible = "xlnx,video" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, xvip_composite_of_id_table);
+
+static struct platform_driver xvip_composite_driver = {
+	.driver = {
+		.name = "xilinx-video",
+		.of_match_table = xvip_composite_of_id_table,
+	},
+	.probe = xvip_composite_probe,
+	.remove = xvip_composite_remove,
+};
+
+module_platform_driver(xvip_composite_driver);
+
+MODULE_AUTHOR("Laurent Pinchart <laurent.pinchart@ideasonboard.com>");
+MODULE_DESCRIPTION("Xilinx Video IP Composite Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/platform/xilinx/xilinx-vipp.h b/drivers/media/platform/xilinx/xilinx-vipp.h
new file mode 100644
index 0000000..faf6b6e
--- /dev/null
+++ b/drivers/media/platform/xilinx/xilinx-vipp.h
@@ -0,0 +1,49 @@
+/*
+ * Xilinx Video IP Composite Device
+ *
+ * Copyright (C) 2013-2015 Ideas on Board
+ * Copyright (C) 2013-2015 Xilinx, Inc.
+ *
+ * Contacts: Hyun Kwon <hyun.kwon@xilinx.com>
+ *           Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef __XILINX_VIPP_H__
+#define __XILINX_VIPP_H__
+
+#include <linux/list.h>
+#include <linux/mutex.h>
+#include <media/media-device.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+
+/**
+ * struct xvip_composite_device - Xilinx Video IP device structure
+ * @v4l2_dev: V4L2 device
+ * @media_dev: media device
+ * @dev: (OF) device
+ * @notifier: V4L2 asynchronous subdevs notifier
+ * @entities: entities in the graph as a list of xvip_graph_entity
+ * @num_subdevs: number of subdevs in the pipeline
+ * @dmas: list of DMA channels at the pipeline output and input
+ * @v4l2_caps: V4L2 capabilities of the whole device (see VIDIOC_QUERYCAP)
+ */
+struct xvip_composite_device {
+	struct v4l2_device v4l2_dev;
+	struct media_device media_dev;
+	struct device *dev;
+
+	struct v4l2_async_notifier notifier;
+	struct list_head entities;
+	unsigned int num_subdevs;
+
+	struct list_head dmas;
+	u32 v4l2_caps;
+};
+
+#endif /* __XILINX_VIPP_H__ */
diff --git a/drivers/media/platform/xilinx/xilinx-vtc.c b/drivers/media/platform/xilinx/xilinx-vtc.c
new file mode 100644
index 0000000..01c750e
--- /dev/null
+++ b/drivers/media/platform/xilinx/xilinx-vtc.c
@@ -0,0 +1,380 @@
+/*
+ * Xilinx Video Timing Controller
+ *
+ * Copyright (C) 2013-2015 Ideas on Board
+ * Copyright (C) 2013-2015 Xilinx, Inc.
+ *
+ * Contacts: Hyun Kwon <hyun.kwon@xilinx.com>
+ *           Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/clk.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#include "xilinx-vip.h"
+#include "xilinx-vtc.h"
+
+#define XVTC_CONTROL_FIELD_ID_POL_SRC		(1 << 26)
+#define XVTC_CONTROL_ACTIVE_CHROMA_POL_SRC	(1 << 25)
+#define XVTC_CONTROL_ACTIVE_VIDEO_POL_SRC	(1 << 24)
+#define XVTC_CONTROL_HSYNC_POL_SRC		(1 << 23)
+#define XVTC_CONTROL_VSYNC_POL_SRC		(1 << 22)
+#define XVTC_CONTROL_HBLANK_POL_SRC		(1 << 21)
+#define XVTC_CONTROL_VBLANK_POL_SRC		(1 << 20)
+#define XVTC_CONTROL_CHROMA_SRC			(1 << 18)
+#define XVTC_CONTROL_VBLANK_HOFF_SRC		(1 << 17)
+#define XVTC_CONTROL_VSYNC_END_SRC		(1 << 16)
+#define XVTC_CONTROL_VSYNC_START_SRC		(1 << 15)
+#define XVTC_CONTROL_ACTIVE_VSIZE_SRC		(1 << 14)
+#define XVTC_CONTROL_FRAME_VSIZE_SRC		(1 << 13)
+#define XVTC_CONTROL_HSYNC_END_SRC		(1 << 11)
+#define XVTC_CONTROL_HSYNC_START_SRC		(1 << 10)
+#define XVTC_CONTROL_ACTIVE_HSIZE_SRC		(1 << 9)
+#define XVTC_CONTROL_FRAME_HSIZE_SRC		(1 << 8)
+#define XVTC_CONTROL_SYNC_ENABLE		(1 << 5)
+#define XVTC_CONTROL_DET_ENABLE			(1 << 3)
+#define XVTC_CONTROL_GEN_ENABLE			(1 << 2)
+
+#define XVTC_STATUS_FSYNC(n)			((n) << 16)
+#define XVTC_STATUS_GEN_ACTIVE_VIDEO		(1 << 13)
+#define XVTC_STATUS_GEN_VBLANK			(1 << 12)
+#define XVTC_STATUS_DET_ACTIVE_VIDEO		(1 << 11)
+#define XVTC_STATUS_DET_VBLANK			(1 << 10)
+#define XVTC_STATUS_LOCK_LOSS			(1 << 9)
+#define XVTC_STATUS_LOCK			(1 << 8)
+
+#define XVTC_ERROR_ACTIVE_CHROMA_LOCK		(1 << 21)
+#define XVTC_ERROR_ACTIVE_VIDEO_LOCK		(1 << 20)
+#define XVTC_ERROR_HSYNC_LOCK			(1 << 19)
+#define XVTC_ERROR_VSYNC_LOCK			(1 << 18)
+#define XVTC_ERROR_HBLANK_LOCK			(1 << 17)
+#define XVTC_ERROR_VBLANK_LOCK			(1 << 16)
+
+#define XVTC_IRQ_ENABLE_FSYNC(n)		((n) << 16)
+#define XVTC_IRQ_ENABLE_GEN_ACTIVE_VIDEO	(1 << 13)
+#define XVTC_IRQ_ENABLE_GEN_VBLANK		(1 << 12)
+#define XVTC_IRQ_ENABLE_DET_ACTIVE_VIDEO	(1 << 11)
+#define XVTC_IRQ_ENABLE_DET_VBLANK		(1 << 10)
+#define XVTC_IRQ_ENABLE_LOCK_LOSS		(1 << 9)
+#define XVTC_IRQ_ENABLE_LOCK			(1 << 8)
+
+/*
+ * The following registers exist in two blocks, one at 0x0020 for the detector
+ * and one at 0x0060 for the generator.
+ */
+
+#define XVTC_DETECTOR_OFFSET			0x0020
+#define XVTC_GENERATOR_OFFSET			0x0060
+
+#define XVTC_ACTIVE_SIZE			0x0000
+#define XVTC_ACTIVE_VSIZE_SHIFT			16
+#define XVTC_ACTIVE_VSIZE_MASK			(0x1fff << 16)
+#define XVTC_ACTIVE_HSIZE_SHIFT			0
+#define XVTC_ACTIVE_HSIZE_MASK			(0x1fff << 0)
+
+#define XVTC_TIMING_STATUS			0x0004
+#define XVTC_TIMING_STATUS_ACTIVE_VIDEO		(1 << 2)
+#define XVTC_TIMING_STATUS_VBLANK		(1 << 1)
+#define XVTC_TIMING_STATUS_LOCKED		(1 << 0)
+
+#define XVTC_ENCODING				0x0008
+#define XVTC_ENCODING_CHROMA_PARITY_SHIFT	8
+#define XVTC_ENCODING_CHROMA_PARITY_MASK	(3 << 8)
+#define XVTC_ENCODING_CHROMA_PARITY_EVEN_ALL	(0 << 8)
+#define XVTC_ENCODING_CHROMA_PARITY_ODD_ALL	(1 << 8)
+#define XVTC_ENCODING_CHROMA_PARITY_EVEN_EVEN	(2 << 8)
+#define XVTC_ENCODING_CHROMA_PARITY_ODD_EVEN	(3 << 8)
+#define XVTC_ENCODING_VIDEO_FORMAT_SHIFT	0
+#define XVTC_ENCODING_VIDEO_FORMAT_MASK		(0xf << 0)
+#define XVTC_ENCODING_VIDEO_FORMAT_YUV422	(0 << 0)
+#define XVTC_ENCODING_VIDEO_FORMAT_YUV444	(1 << 0)
+#define XVTC_ENCODING_VIDEO_FORMAT_RGB		(2 << 0)
+#define XVTC_ENCODING_VIDEO_FORMAT_YUV420	(3 << 0)
+
+#define XVTC_POLARITY				0x000c
+#define XVTC_POLARITY_ACTIVE_CHROMA_POL		(1 << 5)
+#define XVTC_POLARITY_ACTIVE_VIDEO_POL		(1 << 4)
+#define XVTC_POLARITY_HSYNC_POL			(1 << 3)
+#define XVTC_POLARITY_VSYNC_POL			(1 << 2)
+#define XVTC_POLARITY_HBLANK_POL		(1 << 1)
+#define XVTC_POLARITY_VBLANK_POL		(1 << 0)
+
+#define XVTC_HSIZE				0x0010
+#define XVTC_HSIZE_MASK				(0x1fff << 0)
+
+#define XVTC_VSIZE				0x0014
+#define XVTC_VSIZE_MASK				(0x1fff << 0)
+
+#define XVTC_HSYNC				0x0018
+#define XVTC_HSYNC_END_SHIFT			16
+#define XVTC_HSYNC_END_MASK			(0x1fff << 16)
+#define XVTC_HSYNC_START_SHIFT			0
+#define XVTC_HSYNC_START_MASK			(0x1fff << 0)
+
+#define XVTC_F0_VBLANK_H			0x001c
+#define XVTC_F0_VBLANK_HEND_SHIFT		16
+#define XVTC_F0_VBLANK_HEND_MASK		(0x1fff << 16)
+#define XVTC_F0_VBLANK_HSTART_SHIFT		0
+#define XVTC_F0_VBLANK_HSTART_MASK		(0x1fff << 0)
+
+#define XVTC_F0_VSYNC_V				0x0020
+#define XVTC_F0_VSYNC_VEND_SHIFT		16
+#define XVTC_F0_VSYNC_VEND_MASK			(0x1fff << 16)
+#define XVTC_F0_VSYNC_VSTART_SHIFT		0
+#define XVTC_F0_VSYNC_VSTART_MASK		(0x1fff << 0)
+
+#define XVTC_F0_VSYNC_H				0x0024
+#define XVTC_F0_VSYNC_HEND_SHIFT		16
+#define XVTC_F0_VSYNC_HEND_MASK			(0x1fff << 16)
+#define XVTC_F0_VSYNC_HSTART_SHIFT		0
+#define XVTC_F0_VSYNC_HSTART_MASK		(0x1fff << 0)
+
+#define XVTC_FRAME_SYNC_CONFIG(n)		(0x0100 + 4 * (n))
+#define XVTC_FRAME_SYNC_V_START_SHIFT		16
+#define XVTC_FRAME_SYNC_V_START_MASK		(0x1fff << 16)
+#define XVTC_FRAME_SYNC_H_START_SHIFT		0
+#define XVTC_FRAME_SYNC_H_START_MASK		(0x1fff << 0)
+
+#define XVTC_GENERATOR_GLOBAL_DELAY		0x0104
+
+/**
+ * struct xvtc_device - Xilinx Video Timing Controller device structure
+ * @xvip: Xilinx Video IP device
+ * @list: entry in the global VTC list
+ * @has_detector: the VTC has a timing detector
+ * @has_generator: the VTC has a timing generator
+ * @config: generator timings configuration
+ */
+struct xvtc_device {
+	struct xvip_device xvip;
+	struct list_head list;
+
+	bool has_detector;
+	bool has_generator;
+
+	struct xvtc_config config;
+};
+
+static LIST_HEAD(xvtc_list);
+static DEFINE_MUTEX(xvtc_lock);
+
+static inline void xvtc_gen_write(struct xvtc_device *xvtc, u32 addr, u32 value)
+{
+	xvip_write(&xvtc->xvip, XVTC_GENERATOR_OFFSET + addr, value);
+}
+
+/* -----------------------------------------------------------------------------
+ * Generator Operations
+ */
+
+int xvtc_generator_start(struct xvtc_device *xvtc,
+			 const struct xvtc_config *config)
+{
+	int ret;
+
+	if (!xvtc->has_generator)
+		return -ENXIO;
+
+	ret = clk_prepare_enable(xvtc->xvip.clk);
+	if (ret < 0)
+		return ret;
+
+	/* We don't care about the chroma active signal, encoding parameters are
+	 * not important for now.
+	 */
+	xvtc_gen_write(xvtc, XVTC_POLARITY,
+		       XVTC_POLARITY_ACTIVE_CHROMA_POL |
+		       XVTC_POLARITY_ACTIVE_VIDEO_POL |
+		       XVTC_POLARITY_HSYNC_POL | XVTC_POLARITY_VSYNC_POL |
+		       XVTC_POLARITY_HBLANK_POL | XVTC_POLARITY_VBLANK_POL);
+
+	/* Hardcode the polarity to active high, as required by the video in to
+	 * AXI4-stream core.
+	 */
+	xvtc_gen_write(xvtc, XVTC_ENCODING, 0);
+
+	/* Configure the timings. The VBLANK and VSYNC signals assertion and
+	 * deassertion are hardcoded to the first pixel of the line.
+	 */
+	xvtc_gen_write(xvtc, XVTC_ACTIVE_SIZE,
+		       (config->vblank_start << XVTC_ACTIVE_VSIZE_SHIFT) |
+		       (config->hblank_start << XVTC_ACTIVE_HSIZE_SHIFT));
+	xvtc_gen_write(xvtc, XVTC_HSIZE, config->hsize);
+	xvtc_gen_write(xvtc, XVTC_VSIZE, config->vsize);
+	xvtc_gen_write(xvtc, XVTC_HSYNC,
+		       (config->hsync_end << XVTC_HSYNC_END_SHIFT) |
+		       (config->hsync_start << XVTC_HSYNC_START_SHIFT));
+	xvtc_gen_write(xvtc, XVTC_F0_VBLANK_H, 0);
+	xvtc_gen_write(xvtc, XVTC_F0_VSYNC_V,
+		       (config->vsync_end << XVTC_F0_VSYNC_VEND_SHIFT) |
+		       (config->vsync_start << XVTC_F0_VSYNC_VSTART_SHIFT));
+	xvtc_gen_write(xvtc, XVTC_F0_VSYNC_H, 0);
+
+	/* Enable the generator. Set the source of all generator parameters to
+	 * generator registers.
+	 */
+	xvip_write(&xvtc->xvip, XVIP_CTRL_CONTROL,
+		   XVTC_CONTROL_ACTIVE_CHROMA_POL_SRC |
+		   XVTC_CONTROL_ACTIVE_VIDEO_POL_SRC |
+		   XVTC_CONTROL_HSYNC_POL_SRC | XVTC_CONTROL_VSYNC_POL_SRC |
+		   XVTC_CONTROL_HBLANK_POL_SRC | XVTC_CONTROL_VBLANK_POL_SRC |
+		   XVTC_CONTROL_CHROMA_SRC | XVTC_CONTROL_VBLANK_HOFF_SRC |
+		   XVTC_CONTROL_VSYNC_END_SRC | XVTC_CONTROL_VSYNC_START_SRC |
+		   XVTC_CONTROL_ACTIVE_VSIZE_SRC |
+		   XVTC_CONTROL_FRAME_VSIZE_SRC | XVTC_CONTROL_HSYNC_END_SRC |
+		   XVTC_CONTROL_HSYNC_START_SRC |
+		   XVTC_CONTROL_ACTIVE_HSIZE_SRC |
+		   XVTC_CONTROL_FRAME_HSIZE_SRC | XVTC_CONTROL_GEN_ENABLE |
+		   XVIP_CTRL_CONTROL_REG_UPDATE);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(xvtc_generator_start);
+
+int xvtc_generator_stop(struct xvtc_device *xvtc)
+{
+	if (!xvtc->has_generator)
+		return -ENXIO;
+
+	xvip_write(&xvtc->xvip, XVIP_CTRL_CONTROL, 0);
+
+	clk_disable_unprepare(xvtc->xvip.clk);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(xvtc_generator_stop);
+
+struct xvtc_device *xvtc_of_get(struct device_node *np)
+{
+	struct device_node *xvtc_node;
+	struct xvtc_device *found = NULL;
+	struct xvtc_device *xvtc;
+
+	if (!of_find_property(np, "xlnx,vtc", NULL))
+		return NULL;
+
+	xvtc_node = of_parse_phandle(np, "xlnx,vtc", 0);
+	if (xvtc_node == NULL)
+		return ERR_PTR(-EINVAL);
+
+	mutex_lock(&xvtc_lock);
+	list_for_each_entry(xvtc, &xvtc_list, list) {
+		if (xvtc->xvip.dev->of_node == xvtc_node) {
+			found = xvtc;
+			break;
+		}
+	}
+	mutex_unlock(&xvtc_lock);
+
+	of_node_put(xvtc_node);
+
+	if (!found)
+		return ERR_PTR(-EPROBE_DEFER);
+
+	return found;
+}
+EXPORT_SYMBOL_GPL(xvtc_of_get);
+
+void xvtc_put(struct xvtc_device *xvtc)
+{
+}
+EXPORT_SYMBOL_GPL(xvtc_put);
+
+/* -----------------------------------------------------------------------------
+ * Registration and Unregistration
+ */
+
+static void xvtc_register_device(struct xvtc_device *xvtc)
+{
+	mutex_lock(&xvtc_lock);
+	list_add_tail(&xvtc->list, &xvtc_list);
+	mutex_unlock(&xvtc_lock);
+}
+
+static void xvtc_unregister_device(struct xvtc_device *xvtc)
+{
+	mutex_lock(&xvtc_lock);
+	list_del(&xvtc->list);
+	mutex_unlock(&xvtc_lock);
+}
+
+/* -----------------------------------------------------------------------------
+ * Platform Device Driver
+ */
+
+static int xvtc_parse_of(struct xvtc_device *xvtc)
+{
+	struct device_node *node = xvtc->xvip.dev->of_node;
+
+	xvtc->has_detector = of_property_read_bool(node, "xlnx,detector");
+	xvtc->has_generator = of_property_read_bool(node, "xlnx,generator");
+
+	return 0;
+}
+
+static int xvtc_probe(struct platform_device *pdev)
+{
+	struct xvtc_device *xvtc;
+	int ret;
+
+	xvtc = devm_kzalloc(&pdev->dev, sizeof(*xvtc), GFP_KERNEL);
+	if (!xvtc)
+		return -ENOMEM;
+
+	xvtc->xvip.dev = &pdev->dev;
+
+	ret = xvtc_parse_of(xvtc);
+	if (ret < 0)
+		return ret;
+
+	ret = xvip_init_resources(&xvtc->xvip);
+	if (ret < 0)
+		return ret;
+
+	platform_set_drvdata(pdev, xvtc);
+
+	xvip_print_version(&xvtc->xvip);
+
+	xvtc_register_device(xvtc);
+
+	return 0;
+}
+
+static int xvtc_remove(struct platform_device *pdev)
+{
+	struct xvtc_device *xvtc = platform_get_drvdata(pdev);
+
+	xvtc_unregister_device(xvtc);
+
+	xvip_cleanup_resources(&xvtc->xvip);
+
+	return 0;
+}
+
+static const struct of_device_id xvtc_of_id_table[] = {
+	{ .compatible = "xlnx,v-tc-6.1" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, xvtc_of_id_table);
+
+static struct platform_driver xvtc_driver = {
+	.driver = {
+		.name = "xilinx-vtc",
+		.of_match_table = xvtc_of_id_table,
+	},
+	.probe = xvtc_probe,
+	.remove = xvtc_remove,
+};
+
+module_platform_driver(xvtc_driver);
+
+MODULE_AUTHOR("Laurent Pinchart <laurent.pinchart@ideasonboard.com>");
+MODULE_DESCRIPTION("Xilinx Video Timing Controller Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/platform/xilinx/xilinx-vtc.h b/drivers/media/platform/xilinx/xilinx-vtc.h
new file mode 100644
index 0000000..e1bb2cf
--- /dev/null
+++ b/drivers/media/platform/xilinx/xilinx-vtc.h
@@ -0,0 +1,42 @@
+/*
+ * Xilinx Video Timing Controller
+ *
+ * Copyright (C) 2013-2015 Ideas on Board
+ * Copyright (C) 2013-2015 Xilinx, Inc.
+ *
+ * Contacts: Hyun Kwon <hyun.kwon@xilinx.com>
+ *           Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef __XILINX_VTC_H__
+#define __XILINX_VTC_H__
+
+struct device_node;
+struct xvtc_device;
+
+#define XVTC_MAX_HSIZE			8191
+#define XVTC_MAX_VSIZE			8191
+
+struct xvtc_config {
+	unsigned int hblank_start;
+	unsigned int hsync_start;
+	unsigned int hsync_end;
+	unsigned int hsize;
+	unsigned int vblank_start;
+	unsigned int vsync_start;
+	unsigned int vsync_end;
+	unsigned int vsize;
+};
+
+struct xvtc_device *xvtc_of_get(struct device_node *np);
+void xvtc_put(struct xvtc_device *xvtc);
+
+int xvtc_generator_start(struct xvtc_device *xvtc,
+			 const struct xvtc_config *config);
+int xvtc_generator_stop(struct xvtc_device *xvtc);
+
+#endif /* __XILINX_VTC_H__ */