media: tegra v4l2 camera: refactor driver code

Reconstruct the driver for Tegra V4L2 camera driver:
 - remove old driver file tegra_v4l2_camera.c
 - create driver file vi.c for VI/CSI (T20/T30/T114/T148)
 - create driver file vi2.c for VI2/CSI2 (T124)

Bug 1377330

Change-Id: If030cf98e700b9201caa60328be822bc10610e74
Signed-off-by: Bryan Wu <pengw@nvidia.com>
Reviewed-on: http://git-master/r/289329
GVS: Gerrit_Virtual_Submit
Reviewed-by: Allen Martin <amartin@nvidia.com>

11 files changed, 3002 insertions, 2015 deletions

diff --git a/drivers/media/platform/soc_camera/Kconfig b/drivers/media/platform/soc_camera/Kconfig
index 74eb64784a77..b51b274e9a7d 100644
--- a/drivers/media/platform/soc_camera/Kconfig
+++ b/drivers/media/platform/soc_camera/Kconfig
@@ -58,13 +58,6 @@ config VIDEO_SH_MOBILE_CEU
 	---help---
 	  This is a v4l2 driver for the SuperH Mobile CEU Interface
 
-config VIDEO_TEGRA
-	tristate "Tegra soc_camera host driver"
-	depends on VIDEO_DEV && ARCH_TEGRA && SOC_CAMERA && HAS_DMA && HAVE_CLK
-	select VIDEOBUF2_DMA_NVMAP
-	---help---
-	  This is a v4l2 driver for the Tegra camera interface
-
 config VIDEO_OMAP1
 	tristate "OMAP1 Camera Interface driver"
 	depends on VIDEO_DEV && ARCH_OMAP1 && SOC_CAMERA
@@ -92,9 +85,4 @@ config VIDEO_ATMEL_ISI
 	  This module makes the ATMEL Image Sensor Interface available
 	  as a v4l2 device.
 
-config VIDEO_TEGRA
-	tristate "Tegra soc_camera host driver"
-	depends on VIDEO_DEV && ARCH_TEGRA && SOC_CAMERA && HAS_DMA && HAVE_CLK
-	select VIDEOBUF2_DMA_CONTIG
-	---help---
-	  This is a v4l2 driver for the Tegra camera interface
+source "drivers/media/platform/soc_camera/tegra_camera/Kconfig"
diff --git a/drivers/media/platform/soc_camera/Makefile b/drivers/media/platform/soc_camera/Makefile
index a660d2889088..4146e1a33597 100644
--- a/drivers/media/platform/soc_camera/Makefile
+++ b/drivers/media/platform/soc_camera/Makefile
@@ -6,12 +6,10 @@ obj-$(CONFIG_VIDEO_ATMEL_ISI)		+= atmel-isi.o
 obj-$(CONFIG_VIDEO_MX1)			+= mx1_camera.o
 obj-$(CONFIG_VIDEO_MX2)			+= mx2_camera.o
 obj-$(CONFIG_VIDEO_MX3)			+= mx3_camera.o
-obj-$(CONFIG_VIDEO_TEGRA)		+= tegra_v4l2_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_TEGRA)		+= tegra_v4l2_camera.o
+obj-$(CONFIG_VIDEO_TEGRA)		+= tegra_camera/
 
 ccflags-y += -I$(srctree)/drivers/media/i2c/soc_camera
-ccflags-y += -I$(srctree)/drivers/video/tegra/host
diff --git a/drivers/media/platform/soc_camera/tegra_camera/Kconfig b/drivers/media/platform/soc_camera/tegra_camera/Kconfig
new file mode 100644
index 000000000000..9ebe344f2043
--- /dev/null
+++ b/drivers/media/platform/soc_camera/tegra_camera/Kconfig
@@ -0,0 +1,6 @@
+config VIDEO_TEGRA
+	tristate "Tegra soc_camera host driver"
+	depends on VIDEO_DEV && ARCH_TEGRA && SOC_CAMERA && HAS_DMA && HAVE_CLK
+	select VIDEOBUF2_DMA_CONTIG
+	---help---
+	  This is a v4l2 driver for the Tegra camera interface
diff --git a/drivers/media/platform/soc_camera/tegra_camera/Makefile b/drivers/media/platform/soc_camera/tegra_camera/Makefile
new file mode 100644
index 000000000000..552f489f6ed3
--- /dev/null
+++ b/drivers/media/platform/soc_camera/tegra_camera/Makefile
@@ -0,0 +1,7 @@
+# Makefile for Tegra SoC camera driver
+
+tegra_camera-objs += common.o vi.o vi2.o
+
+obj-$(CONFIG_VIDEO_TEGRA) += tegra_camera.o
+
+ccflags-y += -I$(srctree)/drivers/video/tegra/host
diff --git a/drivers/media/platform/soc_camera/tegra_camera/common.c b/drivers/media/platform/soc_camera/tegra_camera/common.c
new file mode 100644
index 000000000000..2db4627c07a8
--- /dev/null
+++ b/drivers/media/platform/soc_camera/tegra_camera/common.c
@@ -0,0 +1,968 @@
+/*
+ * Copyright (c) 2013, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/delay.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/nvhost.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_platform.h>
+
+#include <mach/powergate.h>
+#include <mach/pm_domains.h>
+
+#include <media/soc_camera.h>
+#include <media/soc_mediabus.h>
+#include <media/videobuf2-dma-contig.h>
+#include <media/tegra_v4l2_camera.h>
+
+#include "dev.h"
+#include "bus_client.h"
+#include "t114/t114.h"
+#include "t124/t124.h"
+
+#include "common.h"
+
+static int tpg_mode;
+module_param(tpg_mode, int, 0644);
+
+#define TEGRA_CAM_DRV_NAME "vi"
+#define TEGRA_CAM_VERSION_CODE KERNEL_VERSION(0, 0, 5)
+
+#define TEGRA_SYNCPT_RETRY_COUNT	10
+
+static const struct soc_mbus_pixelfmt tegra_camera_formats[] = {
+	{
+		.fourcc			= V4L2_PIX_FMT_UYVY,
+		.name			= "YUV422 (UYVY) packed",
+		.bits_per_sample	= 16,
+		.packing		= SOC_MBUS_PACKING_NONE,
+		.order			= SOC_MBUS_ORDER_LE,
+	},
+	{
+		.fourcc			= V4L2_PIX_FMT_VYUY,
+		.name			= "YUV422 (VYUY) packed",
+		.bits_per_sample	= 16,
+		.packing		= SOC_MBUS_PACKING_NONE,
+		.order			= SOC_MBUS_ORDER_LE,
+	},
+	{
+		.fourcc			= V4L2_PIX_FMT_YUYV,
+		.name			= "YUV422 (YUYV) packed",
+		.bits_per_sample	= 16,
+		.packing		= SOC_MBUS_PACKING_NONE,
+		.order			= SOC_MBUS_ORDER_LE,
+	},
+	{
+		.fourcc			= V4L2_PIX_FMT_YVYU,
+		.name			= "YUV422 (YVYU) packed",
+		.bits_per_sample	= 16,
+		.packing		= SOC_MBUS_PACKING_NONE,
+		.order			= SOC_MBUS_ORDER_LE,
+	},
+	{
+		.fourcc			= V4L2_PIX_FMT_YUV420,
+		.name			= "YUV420 (YU12) planar",
+		.bits_per_sample	= 12,
+		.packing		= SOC_MBUS_PACKING_NONE,
+		.order			= SOC_MBUS_ORDER_LE,
+	},
+	{
+		.fourcc			= V4L2_PIX_FMT_YVU420,
+		.name			= "YVU420 (YV12) planar",
+		.bits_per_sample	= 12,
+		.packing		= SOC_MBUS_PACKING_NONE,
+		.order			= SOC_MBUS_ORDER_LE,
+	},
+	{
+		.fourcc			= V4L2_PIX_FMT_SBGGR8,
+		.name			= "Bayer 8 BGBG.. GRGR..",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_NONE,
+		.order			= SOC_MBUS_ORDER_LE,
+	},
+	{
+		.fourcc			= V4L2_PIX_FMT_SGBRG8,
+		.name			= "Bayer 8 GBGB.. RGRG..",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_NONE,
+		.order			= SOC_MBUS_ORDER_LE,
+	},
+	{
+		.fourcc			= V4L2_PIX_FMT_SBGGR10,
+		.name			= "Bayer 10 BGBG.. GRGR..",
+		.bits_per_sample	= 16,
+		.packing		= SOC_MBUS_PACKING_EXTEND16,
+		.order			= SOC_MBUS_ORDER_LE,
+	},
+	{
+		.fourcc			= V4L2_PIX_FMT_RGB32,
+		.name			= "RGBA 8-8-8-8",
+		.bits_per_sample	= 32,
+		.packing		= SOC_MBUS_PACKING_NONE,
+		.order			= SOC_MBUS_ORDER_LE,
+	},
+};
+
+static void tegra_camera_activate(struct tegra_camera_dev *cam)
+{
+	struct tegra_camera_ops *cam_ops = cam->ops;
+
+	nvhost_module_busy_ext(cam->ndev);
+
+	/* Enable external power */
+	if (cam->reg)
+		regulator_enable(cam->reg);
+
+	if (cam_ops->activate)
+		cam_ops->activate(cam);
+
+	/* Unpowergate VE */
+	tegra_unpowergate_partition(TEGRA_POWERGATE_VENC);
+
+	if (cam_ops->clks_enable)
+		cam_ops->clks_enable(cam);
+
+	if (cam_ops->capture_clean)
+		cam_ops->capture_clean(cam);
+
+	if (cam_ops->save_syncpts)
+		cam_ops->save_syncpts(cam);
+}
+
+static void tegra_camera_deactivate(struct tegra_camera_dev *cam)
+{
+	struct tegra_camera_ops *cam_ops = cam->ops;
+
+	if (cam_ops->clks_disable)
+		cam_ops->clks_disable(cam);
+
+	if (cam_ops->deactivate)
+		cam_ops->deactivate(cam);
+
+	/* Powergate VE */
+	tegra_powergate_partition(TEGRA_POWERGATE_VENC);
+
+	/* Disable external power */
+	if (cam->reg)
+		regulator_disable(cam->reg);
+
+	nvhost_module_idle_ext(cam->ndev);
+}
+
+static int tegra_camera_capture_frame(struct tegra_camera_dev *cam)
+{
+	struct vb2_buffer *vb = cam->active;
+	struct tegra_camera_buffer *buf = to_tegra_vb(vb);
+	struct soc_camera_device *icd = buf->icd;
+	struct soc_camera_subdev_desc *ssdesc = &icd->sdesc->subdev_desc;
+	struct tegra_camera_platform_data *pdata = ssdesc->drv_priv;
+	int port = pdata->port;
+	int retry = TEGRA_SYNCPT_RETRY_COUNT;
+	int err;
+
+	/* Setup capture registers */
+	cam->ops->capture_setup(cam);
+
+	while (retry) {
+		err = cam->ops->capture_start(cam, buf);
+		/* Capturing succeed, stop capturing */
+		cam->ops->capture_stop(cam, port);
+		if (err) {
+			retry--;
+
+			cam->ops->incr_syncpts(cam);
+			cam->ops->save_syncpts(cam);
+
+			continue;
+		}
+		break;
+	}
+
+	/* Reset hardware for too many errors */
+	if (!retry) {
+		tegra_camera_deactivate(cam);
+		mdelay(5);
+		tegra_camera_activate(cam);
+		if (cam->active)
+			cam->ops->capture_setup(cam);
+	}
+
+	spin_lock_irq(&cam->videobuf_queue_lock);
+
+	vb = cam->active;
+	do_gettimeofday(&vb->v4l2_buf.timestamp);
+	vb->v4l2_buf.field = cam->field;
+	if (port == TEGRA_CAMERA_PORT_CSI_A)
+		vb->v4l2_buf.sequence = cam->sequence_a++;
+	else if (port == TEGRA_CAMERA_PORT_CSI_B)
+		vb->v4l2_buf.sequence = cam->sequence_b++;
+
+	vb2_buffer_done(vb, err < 0 ? VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+	list_del_init(&buf->queue);
+
+	cam->num_frames++;
+
+	spin_unlock_irq(&cam->videobuf_queue_lock);
+
+	return err;
+}
+
+static void tegra_camera_work(struct work_struct *work)
+{
+	struct tegra_camera_dev *cam =
+		container_of(work, struct tegra_camera_dev, work);
+	struct tegra_camera_buffer *buf;
+
+	while (1) {
+		mutex_lock(&cam->work_mutex);
+
+		spin_lock_irq(&cam->videobuf_queue_lock);
+		if (list_empty(&cam->capture)) {
+			cam->active = NULL;
+			spin_unlock_irq(&cam->videobuf_queue_lock);
+			mutex_unlock(&cam->work_mutex);
+			return;
+		}
+
+		buf = list_entry(cam->capture.next, struct tegra_camera_buffer,
+				queue);
+		cam->active = &buf->vb;
+		spin_unlock_irq(&cam->videobuf_queue_lock);
+
+		tegra_camera_capture_frame(cam);
+
+		mutex_unlock(&cam->work_mutex);
+	}
+}
+
+static int tegra_camera_init_buffer(struct tegra_camera_buffer *buf)
+{
+	struct soc_camera_device *icd = buf->icd;
+	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
+						icd->current_fmt->host_fmt);
+	struct soc_camera_subdev_desc *ssdesc = &icd->sdesc->subdev_desc;
+	struct tegra_camera_platform_data *pdata = ssdesc->drv_priv;
+
+	switch (icd->current_fmt->host_fmt->fourcc) {
+	case V4L2_PIX_FMT_UYVY:
+	case V4L2_PIX_FMT_VYUY:
+	case V4L2_PIX_FMT_YUYV:
+	case V4L2_PIX_FMT_YVYU:
+	case V4L2_PIX_FMT_SBGGR8:
+	case V4L2_PIX_FMT_SGBRG8:
+	case V4L2_PIX_FMT_SBGGR10:
+	case V4L2_PIX_FMT_RGB32:
+		buf->buffer_addr = vb2_dma_contig_plane_dma_addr(&buf->vb, 0);
+		buf->start_addr = buf->buffer_addr;
+
+		if (pdata->flip_v)
+			buf->start_addr += bytes_per_line *
+					   (icd->user_height-1);
+
+		if (pdata->flip_h)
+			buf->start_addr += bytes_per_line - 1;
+
+		break;
+
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_YVU420:
+		buf->buffer_addr = vb2_dma_contig_plane_dma_addr(&buf->vb, 0);
+		buf->buffer_addr_u = buf->buffer_addr +
+				     icd->user_width * icd->user_height;
+		buf->buffer_addr_v = buf->buffer_addr_u +
+				     (icd->user_width * icd->user_height) / 4;
+
+		/* For YVU420, we swap the locations of the U and V planes. */
+		if (icd->current_fmt->host_fmt->fourcc == V4L2_PIX_FMT_YVU420) {
+			dma_addr_t temp = buf->buffer_addr_u;
+			buf->buffer_addr_u = buf->buffer_addr_v;
+			buf->buffer_addr_v = temp;
+		}
+
+		buf->start_addr = buf->buffer_addr;
+		buf->start_addr_u = buf->buffer_addr_u;
+		buf->start_addr_v = buf->buffer_addr_v;
+
+		if (pdata->flip_v) {
+			buf->start_addr += icd->user_width *
+					   (icd->user_height - 1);
+
+			buf->start_addr_u += ((icd->user_width/2) *
+					      ((icd->user_height/2) - 1));
+
+			buf->start_addr_v += ((icd->user_width/2) *
+					      ((icd->user_height/2) - 1));
+		}
+
+		if (pdata->flip_h) {
+			buf->start_addr += icd->user_width - 1;
+
+			buf->start_addr_u += (icd->user_width/2) - 1;
+
+			buf->start_addr_v += (icd->user_width/2) - 1;
+		}
+
+		break;
+
+	default:
+		dev_err(icd->parent, "Wrong host format %d\n",
+			icd->current_fmt->host_fmt->fourcc);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/*
+ *  Videobuf operations
+ */
+static int tegra_camera_videobuf_setup(struct vb2_queue *vq,
+				       const struct v4l2_format *fmt,
+				       unsigned int *num_buffers,
+				       unsigned int *num_planes,
+				       unsigned int sizes[],
+				       void *alloc_ctxs[])
+{
+	struct soc_camera_device *icd = container_of(vq,
+						     struct soc_camera_device,
+						     vb2_vidq);
+	struct soc_camera_subdev_desc *ssdesc = &icd->sdesc->subdev_desc;
+	struct tegra_camera_platform_data *pdata = ssdesc->drv_priv;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct tegra_camera_dev *cam = ici->priv;
+	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
+						icd->current_fmt->host_fmt);
+	if (bytes_per_line < 0)
+		return bytes_per_line;
+
+	*num_planes = 1;
+
+	if (pdata->port == TEGRA_CAMERA_PORT_CSI_A)
+		cam->sequence_a = 0;
+	else if (pdata->port == TEGRA_CAMERA_PORT_CSI_B)
+		cam->sequence_b = 0;
+	sizes[0] = bytes_per_line * icd->user_height;
+	alloc_ctxs[0] = cam->alloc_ctx;
+
+	if (!*num_buffers)
+		*num_buffers = 2;
+
+	return 0;
+}
+
+static int tegra_camera_videobuf_prepare(struct vb2_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 tegra_camera_dev *cam = ici->priv;
+	struct tegra_camera_buffer *buf = to_tegra_vb(vb);
+	struct soc_camera_subdev_desc *ssdesc = &icd->sdesc->subdev_desc;
+	struct tegra_camera_platform_data *pdata = ssdesc->drv_priv;
+	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
+						icd->current_fmt->host_fmt);
+	unsigned long size;
+
+	if (bytes_per_line < 0)
+		return bytes_per_line;
+
+	buf->icd = icd;
+
+	if (!pdata) {
+		dev_err(icd->parent, "No platform data for this device!\n");
+		return -EINVAL;
+	}
+
+	if (!cam->ops->port_is_valid(pdata->port)) {
+		dev_err(icd->parent,
+			"Invalid camera port %d in platform data\n",
+			pdata->port);
+		return -EINVAL;
+	}
+
+#ifdef PREFILL_BUFFER
+	dev_info(icd->parent, "%s (vb=0x%p) 0x%p %lu\n", __func__,
+		vb, vb2_plane_vaddr(vb, 0), vb2_plane_size(vb, 0));
+
+	/*
+	 * 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), 0xbd, vb2_plane_size(vb, 0));
+#endif
+
+	if (!icd->current_fmt) {
+		dev_err(icd->parent, "%s NULL format point\n", __func__);
+		return -EINVAL;
+	}
+
+	size = icd->user_height * bytes_per_line;
+
+	if (vb2_plane_size(vb, 0) < size) {
+		dev_err(icd->parent, "Buffer too small (%lu < %lu)\n",
+			vb2_plane_size(vb, 0), size);
+		return -ENOBUFS;
+	}
+
+	vb2_set_plane_payload(vb, 0, size);
+
+	return tegra_camera_init_buffer(buf);
+}
+
+static void tegra_camera_videobuf_queue(struct vb2_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 tegra_camera_dev *cam = ici->priv;
+	struct tegra_camera_buffer *buf = to_tegra_vb(vb);
+
+	spin_lock_irq(&cam->videobuf_queue_lock);
+	list_add_tail(&buf->queue, &cam->capture);
+	schedule_work(&cam->work);
+	spin_unlock_irq(&cam->videobuf_queue_lock);
+}
+
+static void tegra_camera_videobuf_release(struct vb2_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 tegra_camera_buffer *buf = to_tegra_vb(vb);
+	struct tegra_camera_dev *cam = ici->priv;
+
+	mutex_lock(&cam->work_mutex);
+
+	spin_lock_irq(&cam->videobuf_queue_lock);
+
+	if (cam->active == vb)
+		cam->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);
+
+	spin_unlock_irq(&cam->videobuf_queue_lock);
+
+	mutex_unlock(&cam->work_mutex);
+}
+
+static int tegra_camera_videobuf_init(struct vb2_buffer *vb)
+{
+	/* This is for locking debugging only */
+	INIT_LIST_HEAD(&to_tegra_vb(vb)->queue);
+
+	return 0;
+}
+
+static int tegra_camera_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 tegra_camera_dev *cam = ici->priv;
+	struct list_head *buf_head, *tmp;
+
+
+	mutex_lock(&cam->work_mutex);
+
+	spin_lock_irq(&cam->videobuf_queue_lock);
+	list_for_each_safe(buf_head, tmp, &cam->capture) {
+		struct tegra_camera_buffer *buf = container_of(buf_head,
+				struct tegra_camera_buffer,
+				queue);
+		if (buf->icd == icd)
+			list_del_init(buf_head);
+	}
+	spin_unlock_irq(&cam->videobuf_queue_lock);
+
+	if (cam->active) {
+		struct tegra_camera_buffer *buf = to_tegra_vb(cam->active);
+		if (buf->icd == icd)
+			cam->active = NULL;
+	}
+
+	mutex_unlock(&cam->work_mutex);
+
+	return 0;
+}
+
+static struct vb2_ops tegra_camera_videobuf_ops = {
+	.queue_setup	= tegra_camera_videobuf_setup,
+	.buf_prepare	= tegra_camera_videobuf_prepare,
+	.buf_queue	= tegra_camera_videobuf_queue,
+	.buf_cleanup	= tegra_camera_videobuf_release,
+	.buf_init	= tegra_camera_videobuf_init,
+	.wait_prepare	= soc_camera_unlock,
+	.wait_finish	= soc_camera_lock,
+	.stop_streaming	= tegra_camera_stop_streaming,
+};
+
+/*
+ *  SOC camera host operations
+ */
+static int tegra_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 = &tegra_camera_videobuf_ops;
+	q->mem_ops = &vb2_dma_contig_memops;
+	q->buf_struct_size = sizeof(struct tegra_camera_buffer);
+	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+
+	return vb2_queue_init(q);
+}
+
+/*
+ * Called with .video_lock held
+ */
+static int tegra_camera_add_device(struct soc_camera_device *icd)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct tegra_camera_dev *cam = ici->priv;
+
+	if (!cam->enable_refcnt) {
+		pm_runtime_get_sync(ici->v4l2_dev.dev);
+		tegra_camera_activate(cam);
+		cam->num_frames = 0;
+	}
+	cam->enable_refcnt++;
+
+	return 0;
+}
+
+/* Called with .video_lock held */
+static void tegra_camera_remove_device(struct soc_camera_device *icd)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct tegra_camera_dev *cam = ici->priv;
+
+	cam->enable_refcnt--;
+	if (!cam->enable_refcnt) {
+		cancel_work_sync(&cam->work);
+		tegra_camera_deactivate(cam);
+		pm_runtime_put_sync(ici->v4l2_dev.dev);
+	}
+}
+
+static int tegra_camera_set_bus_param(struct soc_camera_device *icd)
+{
+	return 0;
+}
+
+static int tegra_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;
+	int ret;
+	enum v4l2_mbus_pixelcode code;
+	int k;
+
+	ret = v4l2_subdev_call(sd, video, enum_mbus_fmt, idx, &code);
+	if (ret != 0)
+		/* No more formats */
+		return 0;
+
+	switch (code) {
+	case V4L2_MBUS_FMT_UYVY8_2X8:
+	case V4L2_MBUS_FMT_VYUY8_2X8:
+	case V4L2_MBUS_FMT_YUYV8_2X8:
+	case V4L2_MBUS_FMT_YVYU8_2X8:
+	case V4L2_MBUS_FMT_SBGGR8_1X8:
+	case V4L2_MBUS_FMT_SGBRG8_1X8:
+	case V4L2_MBUS_FMT_SBGGR10_1X10:
+	case V4L2_MBUS_FMT_RGBA8888_4X8_LE:
+		formats += ARRAY_SIZE(tegra_camera_formats);
+		for (k = 0;
+		     xlate && (k < ARRAY_SIZE(tegra_camera_formats));
+		     k++) {
+			xlate->host_fmt	= &tegra_camera_formats[k];
+			xlate->code	= code;
+			xlate++;
+
+			dev_dbg(dev, "Providing format %s using code %d\n",
+				 tegra_camera_formats[k].name, code);
+		}
+		break;
+	default:
+		dev_err(dev, "Not supporting %d\n", code);
+		return 0;
+	}
+
+	return formats;
+}
+
+static void tegra_camera_put_formats(struct soc_camera_device *icd)
+{
+	kfree(icd->host_priv);
+	icd->host_priv = NULL;
+}
+
+static int tegra_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 tegra_camera_dev *cam = ici->priv;
+
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	const struct soc_camera_format_xlate *xlate = NULL;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct v4l2_mbus_framefmt mf;
+	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;
+	}
+
+	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, video, s_mbus_fmt, &mf);
+	if (IS_ERR_VALUE(ret)) {
+		dev_warn(dev, "Failed to configure for format %x\n",
+			 pix->pixelformat);
+		return ret;
+	}
+
+	if (mf.code != xlate->code) {
+		dev_warn(dev, "mf.code = %d, xlate->code = %d, mismatch\n",
+			mf.code, xlate->code);
+		return -EINVAL;
+	}
+
+	icd->user_width		= mf.width;
+	icd->user_height	= mf.height;
+	icd->current_fmt	= xlate;
+
+	cam->field = pix->field;
+
+	return ret;
+}
+
+static int tegra_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_mbus_framefmt mf;
+	__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;
+	}
+
+	pix->bytesperline = soc_mbus_bytes_per_line(pix->width,
+						    xlate->host_fmt);
+	if (pix->bytesperline < 0)
+		return pix->bytesperline;
+	pix->sizeimage = pix->height * pix->bytesperline;
+
+	/* 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, video, try_mbus_fmt, &mf);
+	if (IS_ERR_VALUE(ret))
+		return ret;
+
+	pix->width	= mf.width;
+	pix->height	= mf.height;
+	pix->colorspace	= mf.colorspace;
+	/*
+	 * width and height could have been changed, therefore update the
+	 * bytesperline and sizeimage here.
+	 */
+	pix->bytesperline = soc_mbus_bytes_per_line(pix->width,
+						    xlate->host_fmt);
+	pix->sizeimage = pix->height * pix->bytesperline;
+
+	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 tegra_camera_reqbufs(struct soc_camera_device *icd,
+				struct v4l2_requestbuffers *p)
+{
+	return 0;
+}
+
+static unsigned int tegra_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 tegra_camera_querycap(struct soc_camera_host *ici,
+				 struct v4l2_capability *cap)
+{
+	strlcpy(cap->card, TEGRA_CAM_DRV_NAME, sizeof(cap->card));
+	cap->version = TEGRA_CAM_VERSION_CODE;
+	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+
+	return 0;
+}
+
+static struct soc_camera_host_ops tegra_soc_camera_host_ops = {
+	.owner		= THIS_MODULE,
+	.init_videobuf2	= tegra_camera_init_videobuf,
+	.add		= tegra_camera_add_device,
+	.remove		= tegra_camera_remove_device,
+	.set_bus_param	= tegra_camera_set_bus_param,
+	.get_formats	= tegra_camera_get_formats,
+	.put_formats	= tegra_camera_put_formats,
+	.set_fmt	= tegra_camera_set_fmt,
+	.try_fmt	= tegra_camera_try_fmt,
+	.reqbufs	= tegra_camera_reqbufs,
+	.poll		= tegra_camera_poll,
+	.querycap	= tegra_camera_querycap,
+};
+
+static struct of_device_id tegra_vi_of_match[] = {
+#ifdef TEGRA_11X_OR_HIGHER_CONFIG
+	{ .compatible = "nvidia,tegra114-vi",
+		.data = (struct nvhost_device_data *)&t11_vi_info },
+#endif
+#ifdef TEGRA_14X_OR_HIGHER_CONFIG
+	{ .compatible = "nvidia,tegra148-vi",
+		.data = (struct nvhost_device_data *)&t14_vi_info },
+#endif
+#ifdef TEGRA_12X_OR_HIGHER_CONFIG
+	{ .compatible = "nvidia,tegra124-vi",
+		.data = (struct nvhost_device_data *)&t124_vi_info },
+#endif
+	{ },
+};
+
+static int tegra_camera_probe(struct platform_device *pdev)
+{
+	struct tegra_camera_dev *cam;
+	struct nvhost_device_data *ndata = NULL;
+	int err = 0;
+
+	if (pdev->dev.of_node) {
+		const struct of_device_id *match;
+
+		match = of_match_device(tegra_vi_of_match, &pdev->dev);
+		if (match)
+			ndata = (struct nvhost_device_data *) match->data;
+	} else
+		ndata = pdev->dev.platform_data;
+
+	if (!ndata) {
+		dev_err(&pdev->dev, "No nvhost device data!\n");
+		err = -EINVAL;
+		goto exit;
+	}
+
+	cam = kzalloc(sizeof(struct tegra_camera_dev), GFP_KERNEL);
+	if (!cam) {
+		dev_err(&pdev->dev, "couldn't allocate cam\n");
+		err = -ENOMEM;
+		goto exit;
+	}
+
+	cam->ndata = ndata;
+	cam->ndev = pdev;
+
+	cam->ici.priv = cam;
+	cam->ici.v4l2_dev.dev = &pdev->dev;
+	cam->ici.nr = pdev->id;
+	cam->ici.drv_name = dev_name(&pdev->dev);
+	cam->ici.ops = &tegra_soc_camera_host_ops;
+
+	cam->tpg_mode = tpg_mode;
+
+	INIT_LIST_HEAD(&cam->capture);
+	INIT_WORK(&cam->work, tegra_camera_work);
+	spin_lock_init(&cam->videobuf_queue_lock);
+	mutex_init(&cam->work_mutex);
+
+	if (pdev->dev.of_node) {
+		int cplen;
+		const char *compat;
+		compat = of_get_property(pdev->dev.of_node,
+					 "compatible", &cplen);
+
+		if (!strcmp(compat, "nvidia,tegra124-vi"))
+			vi2_register(cam);
+		else
+			vi_register(cam);
+	} else {
+#ifdef TEGRA_12X_OR_HIGHER_CONFIG
+	/* Register VI/CSI or VI2/CSI2 structs */
+		vi2_register(cam);
+#else
+		vi_register(cam);
+#endif
+	}
+
+	/* Init Clocks */
+	cam->ops->clks_init(cam);
+
+	/* Init Regulator */
+	cam->reg = devm_regulator_get(&pdev->dev, cam->regulator_name);
+	if (IS_ERR_OR_NULL(cam->reg)) {
+		dev_err(&pdev->dev, "%s: couldn't get regulator %s, err %ld\n",
+			__func__, cam->regulator_name, PTR_ERR(cam->reg));
+		cam->reg = NULL;
+		goto exit_deinit_clk;
+	}
+
+	platform_set_drvdata(pdev, ndata);
+	err = nvhost_client_device_get_resources(pdev);
+	if (err) {
+		dev_err(&pdev->dev, "%s: nvhost get resources failed %d\n",
+				__func__, err);
+		goto exit_deinit_clk;
+	}
+
+	err = nvhost_client_device_init(pdev);
+	if (err) {
+		dev_err(&pdev->dev, "%s: nvhost init failed %d\n",
+				__func__, err);
+		goto exit_deinit_clk;
+	}
+
+	cam->reg_base = ndata->aperture[0];
+
+	tegra_pd_add_device(&pdev->dev);
+	pm_runtime_use_autosuspend(&pdev->dev);
+	pm_runtime_set_autosuspend_delay(&pdev->dev, ndata->clockgate_delay);
+	pm_runtime_enable(&pdev->dev);
+
+	cam->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
+	if (IS_ERR(cam->alloc_ctx)) {
+		err = PTR_ERR(cam->alloc_ctx);
+		goto exit_pm_disable;
+	}
+
+	platform_set_drvdata(pdev, cam);
+	err = soc_camera_host_register(&cam->ici);
+	if (IS_ERR_VALUE(err))
+		goto exit_cleanup_alloc_ctx;
+
+	dev_notice(&pdev->dev, "Tegra camera driver loaded.\n");
+
+	return err;
+
+exit_cleanup_alloc_ctx:
+	platform_set_drvdata(pdev, cam->ndata);
+	vb2_dma_contig_cleanup_ctx(cam->alloc_ctx);
+exit_pm_disable:
+	pm_runtime_disable(&pdev->dev);
+exit_deinit_clk:
+	cam->ops->clks_deinit(cam);
+	kfree(cam);
+exit:
+	return err;
+}
+
+static int tegra_camera_remove(struct platform_device *pdev)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(&pdev->dev);
+	struct tegra_camera_dev *cam = container_of(ici,
+					struct tegra_camera_dev, ici);
+
+	soc_camera_host_unregister(ici);
+
+	platform_set_drvdata(pdev, cam->ndata);
+	nvhost_client_device_release(pdev);
+
+	vb2_dma_contig_cleanup_ctx(cam->alloc_ctx);
+
+	pm_runtime_disable(&pdev->dev);
+
+	if (cam->ops)
+		cam->ops->clks_deinit(cam);
+
+	kfree(cam);
+
+	dev_notice(&pdev->dev, "Tegra camera host driver unloaded\n");
+
+	return 0;
+}
+
+static struct platform_driver tegra_camera_driver = {
+	.driver	= {
+		.name	= TEGRA_CAM_DRV_NAME,
+		.owner	= THIS_MODULE,
+#ifdef CONFIG_OF
+		.of_match_table = tegra_vi_of_match,
+#endif
+	},
+	.probe		= tegra_camera_probe,
+	.remove		= tegra_camera_remove,
+};
+
+static int __init tegra_camera_init(void)
+{
+	return platform_driver_register(&tegra_camera_driver);
+}
+
+static void __exit tegra_camera_exit(void)
+{
+	platform_driver_unregister(&tegra_camera_driver);
+}
+
+module_init(tegra_camera_init);
+module_exit(tegra_camera_exit);
+
+MODULE_DESCRIPTION("TEGRA SoC Camera Host driver");
+MODULE_AUTHOR("Bryan Wu <pengw@nvidia.com>");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("nvhost:" TEGRA_CAM_DRV_NAME);
diff --git a/drivers/media/platform/soc_camera/tegra_camera/common.h b/drivers/media/platform/soc_camera/tegra_camera/common.h
new file mode 100644
index 000000000000..94a3f46bbf92
--- /dev/null
+++ b/drivers/media/platform/soc_camera/tegra_camera/common.h
@@ -0,0 +1,121 @@
+/*
+ * Copyright (c) 2012-2013, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef _TEGRA_CAMERA_COMMON_H_
+#define _TEGRA_CAMERA_COMMON_H_
+
+#include <linux/videodev2.h>
+
+#include <media/videobuf2-dma-contig.h>
+#include <media/soc_camera.h>
+
+/* Buffer for one video frame */
+struct tegra_camera_buffer {
+	struct vb2_buffer		vb; /* v4l buffer must be first */
+	struct list_head		queue;
+	struct soc_camera_device	*icd;
+	int				output_channel;
+
+	/*
+	 * Various buffer addresses shadowed so we don't have to recalculate
+	 * per frame. These are calculated during videobuf_prepare.
+	 */
+	dma_addr_t			buffer_addr;
+	dma_addr_t			buffer_addr_u;
+	dma_addr_t			buffer_addr_v;
+	dma_addr_t			start_addr;
+	dma_addr_t			start_addr_u;
+	dma_addr_t			start_addr_v;
+};
+
+static struct tegra_camera_buffer *to_tegra_vb(struct vb2_buffer *vb)
+{
+	return container_of(vb, struct tegra_camera_buffer, vb);
+}
+
+struct tegra_camera_dev;
+
+struct tegra_camera_clk {
+	const char	*name;
+	struct clk	*clk;
+	u32		freq;
+	int		use_devname;
+};
+
+struct tegra_camera_ops {
+	int (*clks_init)(struct tegra_camera_dev *cam);
+	void (*clks_deinit)(struct tegra_camera_dev *cam);
+	void (*clks_enable)(struct tegra_camera_dev *cam);
+	void (*clks_disable)(struct tegra_camera_dev *cam);
+
+	void (*capture_clean)(struct tegra_camera_dev *vi2_cam);
+	int (*capture_setup)(struct tegra_camera_dev *vi2_cam);
+	int (*capture_start)(struct tegra_camera_dev *vi2_cam,
+			     struct tegra_camera_buffer *buf);
+	int (*capture_stop)(struct tegra_camera_dev *vi2_cam, int port);
+
+	void (*incr_syncpts)(struct tegra_camera_dev *vi2_cam);
+	void (*save_syncpts)(struct tegra_camera_dev *vi2_cam);
+
+	void (*activate)(struct tegra_camera_dev *vi2_cam);
+	void (*deactivate)(struct tegra_camera_dev *vi2_cam);
+	int (*port_is_valid)(int port);
+};
+
+struct tegra_camera_dev {
+	struct soc_camera_host		ici;
+	struct platform_device		*ndev;
+	struct nvhost_device_data	*ndata;
+
+	struct regulator		*reg;
+	const char			*regulator_name;
+
+	struct tegra_camera_clk		*clks;
+	int				num_clks;
+
+	struct tegra_camera_ops		*ops;
+
+	void __iomem			*reg_base;
+	spinlock_t			videobuf_queue_lock;
+	struct list_head		capture;
+	struct vb2_buffer		*active;
+	struct vb2_alloc_ctx		*alloc_ctx;
+	enum v4l2_field			field;
+	int				sequence_a;
+	int				sequence_b;
+
+	struct work_struct		work;
+	struct mutex			work_mutex;
+
+	u32				syncpt_csi_a;
+	u32				syncpt_csi_b;
+	u32				syncpt_vip;
+
+	/* Debug */
+	int				num_frames;
+	int				enable_refcnt;
+
+	/* Test Pattern Generator mode */
+	int				tpg_mode;
+};
+
+#define TC_VI_REG_RD(dev, offset) readl(dev->reg_base + offset)
+#define TC_VI_REG_WT(dev, offset, val) writel(val, dev->reg_base + offset)
+
+int vi2_register(struct tegra_camera_dev *cam);
+int vi_register(struct tegra_camera_dev *cam);
+
+#endif
diff --git a/drivers/media/platform/soc_camera/tegra_camera/vi.c b/drivers/media/platform/soc_camera/tegra_camera/vi.c
new file mode 100644
index 000000000000..9792d1869538
--- /dev/null
+++ b/drivers/media/platform/soc_camera/tegra_camera/vi.c
@@ -0,0 +1,1075 @@
+/*
+ * Copyright (c) 2013, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/delay.h>
+#include <linux/clk.h>
+#include <linux/platform_device.h>
+
+#include <media/soc_camera.h>
+#include <media/soc_mediabus.h>
+#include <media/tegra_v4l2_camera.h>
+
+#include <mach/clk.h>
+
+#include "nvhost_syncpt.h"
+#include "common.h"
+
+#define TEGRA_SYNCPT_VI_WAIT_TIMEOUT                    200
+#define TEGRA_SYNCPT_CSI_WAIT_TIMEOUT                   200
+
+#define TEGRA_VIP_H_ACTIVE_START			0x98
+#define TEGRA_VIP_V_ACTIVE_START			0x10
+
+/* SYNCPTs 12-17 are reserved for VI. */
+#define TEGRA_VI_SYNCPT_VI                              NVSYNCPT_VI_ISP_2
+#define TEGRA_VI_SYNCPT_CSI_A                           NVSYNCPT_VI_ISP_3
+#define TEGRA_VI_SYNCPT_CSI_B                           NVSYNCPT_VI_ISP_4
+
+/* Tegra CSI-MIPI registers. */
+#define TEGRA_VI_OUT_1_INCR_SYNCPT			0x000
+#define TEGRA_VI_OUT_1_INCR_SYNCPT_CNTRL		0x004
+#define TEGRA_VI_OUT_1_INCR_SYNCPT_ERROR		0x008
+#define TEGRA_VI_OUT_2_INCR_SYNCPT			0x020
+#define TEGRA_VI_OUT_2_INCR_SYNCPT_CNTRL		0x024
+#define TEGRA_VI_OUT_2_INCR_SYNCPT_ERROR		0x028
+#define TEGRA_VI_MISC_INCR_SYNCPT			0x040
+#define TEGRA_VI_MISC_INCR_SYNCPT_CNTRL			0x044
+#define TEGRA_VI_MISC_INCR_SYNCPT_ERROR			0x048
+#define TEGRA_VI_CONT_SYNCPT_OUT_1			0x060
+#define TEGRA_VI_CONT_SYNCPT_OUT_2			0x064
+#define TEGRA_VI_CONT_SYNCPT_VIP_VSYNC			0x068
+#define TEGRA_VI_CONT_SYNCPT_VI2EPP			0x06c
+#define TEGRA_VI_CONT_SYNCPT_CSI_PPA_FRAME_START	0x070
+#define TEGRA_VI_CONT_SYNCPT_CSI_PPA_FRAME_END		0x074
+#define TEGRA_VI_CONT_SYNCPT_CSI_PPB_FRAME_START	0x078
+#define TEGRA_VI_CONT_SYNCPT_CSI_PPB_FRAME_END		0x07c
+#define TEGRA_VI_CTXSW					0x080
+#define TEGRA_VI_INTSTATUS				0x084
+#define TEGRA_VI_VI_INPUT_CONTROL			0x088
+#define TEGRA_VI_VI_CORE_CONTROL			0x08c
+#define TEGRA_VI_VI_FIRST_OUTPUT_CONTROL		0x090
+#define TEGRA_VI_VI_SECOND_OUTPUT_CONTROL		0x094
+#define TEGRA_VI_HOST_INPUT_FRAME_SIZE			0x098
+#define TEGRA_VI_HOST_H_ACTIVE				0x09c
+#define TEGRA_VI_HOST_V_ACTIVE				0x0a0
+#define TEGRA_VI_VIP_H_ACTIVE				0x0a4
+#define TEGRA_VI_VIP_V_ACTIVE				0x0a8
+#define TEGRA_VI_VI_PEER_CONTROL			0x0ac
+#define TEGRA_VI_VI_DMA_SELECT				0x0b0
+#define TEGRA_VI_HOST_DMA_WRITE_BUFFER			0x0b4
+#define TEGRA_VI_HOST_DMA_BASE_ADDRESS			0x0b8
+#define TEGRA_VI_HOST_DMA_WRITE_BUFFER_STATUS		0x0bc
+#define TEGRA_VI_HOST_DMA_WRITE_PEND_BUFCOUNT		0x0c0
+#define TEGRA_VI_VB0_START_ADDRESS_FIRST		0x0c4
+#define TEGRA_VI_VB0_BASE_ADDRESS_FIRST			0x0c8
+#define TEGRA_VI_VB0_START_ADDRESS_U			0x0cc
+#define TEGRA_VI_VB0_BASE_ADDRESS_U			0x0d0
+#define TEGRA_VI_VB0_START_ADDRESS_V			0x0d4
+#define TEGRA_VI_VB0_BASE_ADDRESS_V			0x0d8
+#define TEGRA_VI_VB_SCRATCH_ADDRESS_UV			0x0dc
+#define TEGRA_VI_FIRST_OUTPUT_FRAME_SIZE		0x0e0
+#define TEGRA_VI_VB0_COUNT_FIRST			0x0e4
+#define TEGRA_VI_VB0_SIZE_FIRST				0x0e8
+#define TEGRA_VI_VB0_BUFFER_STRIDE_FIRST		0x0ec
+#define TEGRA_VI_VB0_START_ADDRESS_SECOND		0x0f0
+#define TEGRA_VI_VB0_BASE_ADDRESS_SECOND		0x0f4
+#define TEGRA_VI_SECOND_OUTPUT_FRAME_SIZE		0x0f8
+#define TEGRA_VI_VB0_COUNT_SECOND			0x0fc
+#define TEGRA_VI_VB0_SIZE_SECOND			0x100
+#define TEGRA_VI_VB0_BUFFER_STRIDE_SECOND		0x104
+#define TEGRA_VI_H_LPF_CONTROL				0x108
+#define TEGRA_VI_H_DOWNSCALE_CONTROL			0x10c
+#define TEGRA_VI_V_DOWNSCALE_CONTROL			0x110
+#define TEGRA_VI_CSC_Y					0x114
+#define TEGRA_VI_CSC_UV_R				0x118
+#define TEGRA_VI_CSC_UV_G				0x11c
+#define TEGRA_VI_CSC_UV_B				0x120
+#define TEGRA_VI_CSC_ALPHA				0x124
+#define TEGRA_VI_HOST_VSYNC				0x128
+#define TEGRA_VI_COMMAND				0x12c
+#define TEGRA_VI_HOST_FIFO_STATUS			0x130
+#define TEGRA_VI_INTERRUPT_MASK				0x134
+#define TEGRA_VI_INTERRUPT_TYPE_SELECT			0x138
+#define TEGRA_VI_INTERRUPT_POLARITY_SELECT		0x13c
+#define TEGRA_VI_INTERRUPT_STATUS			0x140
+#define TEGRA_VI_VIP_INPUT_STATUS			0x144
+#define TEGRA_VI_VIDEO_BUFFER_STATUS			0x148
+#define TEGRA_VI_SYNC_OUTPUT				0x14c
+#define TEGRA_VI_VVS_OUTPUT_DELAY			0x150
+#define TEGRA_VI_PWM_CONTROL				0x154
+#define TEGRA_VI_PWM_SELECT_PULSE_A			0x158
+#define TEGRA_VI_PWM_SELECT_PULSE_B			0x15c
+#define TEGRA_VI_PWM_SELECT_PULSE_C			0x160
+#define TEGRA_VI_PWM_SELECT_PULSE_D			0x164
+#define TEGRA_VI_VI_DATA_INPUT_CONTROL			0x168
+#define TEGRA_VI_PIN_INPUT_ENABLE			0x16c
+#define TEGRA_VI_PIN_OUTPUT_ENABLE			0x170
+#define TEGRA_VI_PIN_INVERSION				0x174
+#define TEGRA_VI_PIN_INPUT_DATA				0x178
+#define TEGRA_VI_PIN_OUTPUT_DATA			0x17c
+#define TEGRA_VI_PIN_OUTPUT_SELECT			0x180
+#define TEGRA_VI_RAISE_VIP_BUFFER_FIRST_OUTPUT		0x184
+#define TEGRA_VI_RAISE_VIP_FRAME_FIRST_OUTPUT		0x188
+#define TEGRA_VI_RAISE_VIP_BUFFER_SECOND_OUTPUT		0x18c
+#define TEGRA_VI_RAISE_VIP_FRAME_SECOND_OUTPUT		0x190
+#define TEGRA_VI_RAISE_HOST_FIRST_OUTPUT		0x194
+#define TEGRA_VI_RAISE_HOST_SECOND_OUTPUT		0x198
+#define TEGRA_VI_RAISE_EPP				0x19c
+#define TEGRA_VI_CAMERA_CONTROL				0x1a0
+#define TEGRA_VI_VI_ENABLE				0x1a4
+#define TEGRA_VI_VI_ENABLE_2				0x1a8
+#define TEGRA_VI_VI_RAISE				0x1ac
+#define TEGRA_VI_Y_FIFO_WRITE				0x1b0
+#define TEGRA_VI_U_FIFO_WRITE				0x1b4
+#define TEGRA_VI_V_FIFO_WRITE				0x1b8
+#define TEGRA_VI_VI_MCCIF_FIFOCTRL			0x1bc
+#define TEGRA_VI_TIMEOUT_WCOAL_VI			0x1c0
+#define TEGRA_VI_MCCIF_VIRUV_HP				0x1c4
+#define TEGRA_VI_MCCIF_VIWSB_HP				0x1c8
+#define TEGRA_VI_MCCIF_VIWU_HP				0x1cc
+#define TEGRA_VI_MCCIF_VIWV_HP				0x1d0
+#define TEGRA_VI_MCCIF_VIWY_HP				0x1d4
+#define TEGRA_VI_CSI_PPA_RAISE_FRAME_START		0x1d8
+#define TEGRA_VI_CSI_PPA_RAISE_FRAME_END		0x1dc
+#define TEGRA_VI_CSI_PPB_RAISE_FRAME_START		0x1e0
+#define TEGRA_VI_CSI_PBB_RAISE_FRAME_END		0x1e4
+#define TEGRA_VI_CSI_PPA_H_ACTIVE			0x1e8
+#define TEGRA_VI_CSI_PPA_V_ACTIVE			0x1ec
+#define TEGRA_VI_CSI_PPB_H_ACTIVE			0x1f0
+#define TEGRA_VI_CSI_PPB_V_ACTIVE			0x1f4
+#define TEGRA_VI_ISP_H_ACTIVE				0x1f8
+#define TEGRA_VI_ISP_V_ACTIVE				0x1fc
+#define TEGRA_VI_STREAM_1_RESOURCE_DEFINE		0x200
+#define TEGRA_VI_STREAM_2_RESOURCE_DEFINE		0x204
+#define TEGRA_VI_RAISE_STREAM_1_DONE			0x208
+#define TEGRA_VI_RAISE_STREAM_2_DONE			0x20c
+#define TEGRA_VI_TS_MODE				0x210
+#define TEGRA_VI_TS_CONTROL				0x214
+#define TEGRA_VI_TS_PACKET_COUNT			0x218
+#define TEGRA_VI_TS_ERROR_COUNT				0x21c
+#define TEGRA_VI_TS_CPU_FLOW_CTL			0x220
+#define TEGRA_VI_VB0_CHROMA_BUFFER_STRIDE_FIRST		0x224
+#define TEGRA_VI_VB0_CHROMA_LINE_STRIDE_FIRST		0x228
+#define TEGRA_VI_EPP_LINES_PER_BUFFER			0x22c
+#define TEGRA_VI_BUFFER_RELEASE_OUTPUT1			0x230
+#define TEGRA_VI_BUFFER_RELEASE_OUTPUT2			0x234
+#define TEGRA_VI_DEBUG_FLOW_CONTROL_COUNTER_OUTPUT1	0x238
+#define TEGRA_VI_DEBUG_FLOW_CONTROL_COUNTER_OUTPUT2	0x23c
+#define TEGRA_VI_TERMINATE_BW_FIRST			0x240
+#define TEGRA_VI_TERMINATE_BW_SECOND			0x244
+#define TEGRA_VI_VB0_FIRST_BUFFER_ADDR_MODE		0x248
+#define TEGRA_VI_VB0_SECOND_BUFFER_ADDR_MODE		0x24c
+#define TEGRA_VI_RESERVE			0x250
+#define TEGRA_VI_RESERVE_1				0x254
+#define TEGRA_VI_RESERVE_2				0x258
+#define TEGRA_VI_RESERVE_3				0x25c
+#define TEGRA_VI_RESERVE_4				0x260
+#define TEGRA_VI_MCCIF_VIRUV_HYST			0x264
+#define TEGRA_VI_MCCIF_VIWSB_HYST			0x268
+#define TEGRA_VI_MCCIF_VIWU_HYST			0x26c
+#define TEGRA_VI_MCCIF_VIWV_HYST			0x270
+#define TEGRA_VI_MCCIF_VIWY_HYST			0x274
+
+#define TEGRA_CSI_VI_INPUT_STREAM_CONTROL		0x800
+#define TEGRA_CSI_HOST_INPUT_STREAM_CONTROL		0x808
+#define TEGRA_CSI_INPUT_STREAM_A_CONTROL		0x810
+#define TEGRA_CSI_PIXEL_STREAM_A_CONTROL0		0x818
+#define TEGRA_CSI_PIXEL_STREAM_A_CONTROL1		0x81c
+#define TEGRA_CSI_PIXEL_STREAM_A_WORD_COUNT		0x820
+#define TEGRA_CSI_PIXEL_STREAM_A_GAP			0x824
+#define TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND		0x828
+#define TEGRA_CSI_INPUT_STREAM_B_CONTROL		0x83c
+#define TEGRA_CSI_PIXEL_STREAM_B_CONTROL0		0x844
+#define TEGRA_CSI_PIXEL_STREAM_B_CONTROL1		0x848
+#define TEGRA_CSI_PIXEL_STREAM_B_WORD_COUNT		0x84c
+#define TEGRA_CSI_PIXEL_STREAM_B_GAP			0x850
+#define TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND		0x854
+#define TEGRA_CSI_PHY_CIL_COMMAND			0x868
+#define TEGRA_CSI_PHY_CILA_CONTROL0			0x86c
+#define TEGRA_CSI_PHY_CILB_CONTROL0			0x870
+#define TEGRA_CSI_CSI_PIXEL_PARSER_STATUS		0x878
+#define TEGRA_CSI_CSI_CIL_STATUS			0x87c
+#define TEGRA_CSI_CSI_PIXEL_PARSER_INTERRUPT_MASK	0x880
+#define TEGRA_CSI_CSI_CIL_INTERRUPT_MASK		0x884
+#define TEGRA_CSI_CSI_READONLY_STATUS			0x888
+#define TEGRA_CSI_ESCAPE_MODE_COMMAND			0x88c
+#define TEGRA_CSI_ESCAPE_MODE_DATA			0x890
+#define TEGRA_CSI_CILA_PAD_CONFIG0			0x894
+#define TEGRA_CSI_CILA_PAD_CONFIG1			0x898
+#define TEGRA_CSI_CILB_PAD_CONFIG0			0x89c
+#define TEGRA_CSI_CILB_PAD_CONFIG1			0x8a0
+#define TEGRA_CSI_CIL_PAD_CONFIG			0x8a4
+#define TEGRA_CSI_CILA_MIPI_CAL_CONFIG			0x8a8
+#define TEGRA_CSI_CILB_MIPI_CAL_CONFIG			0x8ac
+#define TEGRA_CSI_CIL_MIPI_CAL_STATUS			0x8b0
+#define TEGRA_CSI_CLKEN_OVERRIDE			0x8b4
+#define TEGRA_CSI_DEBUG_CONTROL				0x8b8
+#define TEGRA_CSI_DEBUG_COUNTER				0x8bc
+#define TEGRA_CSI_DEBUG_COUNTER_1			0x8c0
+#define TEGRA_CSI_DEBUG_COUNTER_2			0x8c4
+#define TEGRA_CSI_PIXEL_STREAM_A_EXPECTED_FRAME		0x8c8
+#define TEGRA_CSI_PIXEL_STREAM_B_EXPECTED_FRAME		0x8cc
+#define TEGRA_CSI_DSI_MIPI_CAL_CONFIG			0x8d0
+
+/* Test Pattern Generator of CSI */
+#define TEGRA_CSI_PATTERN_GENERATOR_CTRL_A		0x940
+#define TEGRA_CSI_PG_BLANK_A				0x944
+#define TEGRA_CSI_PG_PHASE_A				0x948
+#define TEGRA_CSI_PG_RED_FREQ_A				0x94c
+#define TEGRA_CSI_PG_RED_FREQ_RATE_A			0x950
+#define TEGRA_CSI_PG_GREEN_FREQ_A			0x954
+#define TEGRA_CSI_PG_GREEN_FREQ_RATE_A			0x958
+#define TEGRA_CSI_PG_BLUE_FREQ_A			0x95c
+#define TEGRA_CSI_PG_BLUE_FREQ_RATE_A			0x960
+#define TEGRA_CSI_PATTERN_GENERATOR_CTRL_B		0x974
+#define TEGRA_CSI_PG_BLANK_B				0x978
+#define TEGRA_CSI_PG_PHASE_B				0x97c
+#define TEGRA_CSI_PG_RED_FREQ_B				0x980
+#define TEGRA_CSI_PG_RED_FREQ_RATE_B			0x984
+#define TEGRA_CSI_PG_GREEN_FREQ_B			0x988
+#define TEGRA_CSI_PG_GREEN_FREQ_RATE_B			0x98c
+#define TEGRA_CSI_PG_BLUE_FREQ_B			0x990
+#define TEGRA_CSI_PG_BLUE_FREQ_RATE_B			0x994
+
+static int vi_port_is_valid(int port)
+{
+	return (((port) >= TEGRA_CAMERA_PORT_CSI_A) &&
+		((port) <= TEGRA_CAMERA_PORT_VIP));
+}
+
+static int vi_port_is_csi(int port)
+{
+	return (((port) == TEGRA_CAMERA_PORT_CSI_A) ||
+		((port) == TEGRA_CAMERA_PORT_CSI_B));
+}
+
+/* Clock settings for camera */
+static struct tegra_camera_clk vi_clks[] = {
+	{
+		.name = "vi",
+		.freq = 150000000,
+	},
+	{
+		.name = "vi_sensor",
+		.freq = 24000000,
+	},
+	{
+		.name = "csi",
+		.freq = 0,
+	},
+	{
+		.name = "isp",
+		.freq = 0,
+	},
+	{
+		.name = "csus",
+		.freq = 0,
+	},
+	{
+		.name = "sclk",
+		.freq = 80000000,
+	},
+	{
+		.name = "emc",
+		.freq = 375000000,
+	},
+#ifdef TEGRA_11X_OR_HIGHER_CONFIG
+	{
+		.name = "cilab",
+		.freq = 102000000,
+	},
+	{
+		.name = "cilcd",
+		.freq = 0,
+	},
+	{
+		.name = "cile",
+		.freq = 0,
+	},
+	/* Always put "p11_d2" at the end */
+	{
+		.name = "pll_d2",
+		.freq = 0,
+	},
+#endif
+};
+
+static int vi_clks_init(struct tegra_camera_dev *cam)
+{
+	struct platform_device *pdev = cam->ndev;
+	struct tegra_camera_clk *clks;
+	int i;
+
+	cam->num_clks = ARRAY_SIZE(vi_clks);
+	cam->clks = vi_clks;
+
+	for (i = 0; i < cam->num_clks; i++) {
+		clks = &cam->clks[i];
+		clks->clk = devm_clk_get(&pdev->dev, clks->name);
+		if (IS_ERR_OR_NULL(clks->clk)) {
+			clks->clk = NULL;
+			dev_err(&pdev->dev, "Failed to get clock %s.\n",
+				clks->name);
+			return PTR_ERR(clks->clk);
+		}
+
+		if (clks->freq > 0)
+			clk_set_rate(clks->clk, clks->freq);
+	}
+
+	return 0;
+}
+
+static void vi_clks_deinit(struct tegra_camera_dev *cam)
+{
+	/* We don't need cleanup for devm_clk_get() */
+	return;
+}
+
+static void vi_clks_enable(struct tegra_camera_dev *cam)
+{
+	struct tegra_camera_clk *clks;
+	int i;
+
+	for (i = 0; i < cam->num_clks - 1; i++) {
+		clks = &cam->clks[i];
+		if (clks->clk)
+			clk_prepare_enable(clks->clk);
+	}
+
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+#define TEGRA_APB_MISC_BASE		0x70000000
+	{
+		u32 val;
+		void __iomem *apb_misc = IO_ADDRESS(TEGRA_APB_MISC_BASE);
+
+		val = readl(apb_misc + 0x42c);
+		writel(val | 0x1, apb_misc + 0x42c);
+	}
+#endif
+
+	if (cam->tpg_mode) {
+		clks = &cam->clks[i];
+		if (clks->clk) {
+			clk_prepare_enable(clks->clk);
+#ifdef TEGRA_11X_OR_HIGHER_CONFIG
+			tegra_clk_cfg_ex(clks->clk,
+					 TEGRA_CLK_PLLD_CSI_OUT_ENB, 1);
+			tegra_clk_cfg_ex(clks->clk,
+					 TEGRA_CLK_PLLD_DSI_OUT_ENB, 1);
+#else
+			/*
+			 * bit 25: 0 = pd2vi_Clk,
+			 *         1 = vi_sensor_clk
+			 * bit 24: 0 = internal clock,
+			 *	   1 = external clock (pd2vi_clk)
+			 */
+			tegra_clk_cfg_ex(clks->clk, TEGRA_CLK_VI_INP_SEL, 2);
+#endif
+		}
+	}
+}
+
+static void vi_clks_disable(struct tegra_camera_dev *cam)
+{
+	struct tegra_camera_clk *clks;
+	int i;
+
+	for (i = 0; i < cam->num_clks - 1; i++) {
+		clks = &cam->clks[i];
+		if (clks->clk)
+			clk_disable_unprepare(clks->clk);
+	}
+
+	if (cam->tpg_mode) {
+		clks = &cam->clks[i];
+		if (clks->clk) {
+#ifdef TEGRA_11X_OR_HIGHER_CONFIG
+			tegra_clk_cfg_ex(clks->clk,
+					 TEGRA_CLK_PLLD_CSI_OUT_ENB, 0);
+			tegra_clk_cfg_ex(clks->clk,
+					 TEGRA_CLK_PLLD_DSI_OUT_ENB, 0);
+#endif
+			clk_disable_unprepare(clks->clk);
+		}
+	}
+}
+
+static void vi_save_syncpts(struct tegra_camera_dev *cam)
+{
+	cam->syncpt_csi_a =
+		nvhost_syncpt_read_ext(cam->ndev,
+				       TEGRA_VI_SYNCPT_CSI_A);
+
+	cam->syncpt_csi_b =
+		nvhost_syncpt_read_ext(cam->ndev,
+				       TEGRA_VI_SYNCPT_CSI_B);
+
+	cam->syncpt_vip =
+		nvhost_syncpt_read_ext(cam->ndev,
+				       TEGRA_VI_SYNCPT_VI);
+}
+
+static void vi_incr_syncpts(struct tegra_camera_dev *cam)
+{
+	nvhost_syncpt_cpu_incr_ext(cam->ndev,
+				   TEGRA_VI_SYNCPT_CSI_A);
+
+	nvhost_syncpt_cpu_incr_ext(cam->ndev,
+				   TEGRA_VI_SYNCPT_CSI_B);
+
+	nvhost_syncpt_cpu_incr_ext(cam->ndev,
+				   TEGRA_VI_SYNCPT_VI);
+}
+
+static void vi_capture_clean(struct tegra_camera_dev *cam)
+{
+	TC_VI_REG_WT(cam, TEGRA_CSI_VI_INPUT_STREAM_CONTROL, 0x00000000);
+	TC_VI_REG_WT(cam, TEGRA_CSI_HOST_INPUT_STREAM_CONTROL, 0x00000000);
+
+	TC_VI_REG_WT(cam, TEGRA_CSI_CSI_PIXEL_PARSER_STATUS, 0x0);
+	TC_VI_REG_WT(cam, TEGRA_CSI_CSI_CIL_STATUS, 0x0);
+	TC_VI_REG_WT(cam, TEGRA_CSI_CSI_PIXEL_PARSER_INTERRUPT_MASK, 0x0);
+	TC_VI_REG_WT(cam, TEGRA_CSI_CSI_CIL_INTERRUPT_MASK, 0x0);
+	TC_VI_REG_WT(cam, TEGRA_CSI_CSI_READONLY_STATUS, 0x0);
+	TC_VI_REG_WT(cam, TEGRA_CSI_ESCAPE_MODE_COMMAND, 0x0);
+	TC_VI_REG_WT(cam, TEGRA_CSI_ESCAPE_MODE_DATA, 0x0);
+
+	TC_VI_REG_WT(cam, TEGRA_CSI_CIL_PAD_CONFIG, 0x00000000);
+	TC_VI_REG_WT(cam, TEGRA_CSI_CIL_MIPI_CAL_STATUS, 0x00000000);
+	TC_VI_REG_WT(cam, TEGRA_CSI_CLKEN_OVERRIDE, 0x00000000);
+
+	TC_VI_REG_WT(cam, TEGRA_CSI_DEBUG_CONTROL, 0x0);
+	TC_VI_REG_WT(cam, TEGRA_CSI_DEBUG_COUNTER, 0x0);
+	TC_VI_REG_WT(cam, TEGRA_CSI_DEBUG_COUNTER_1, 0x0);
+	TC_VI_REG_WT(cam, TEGRA_CSI_DEBUG_COUNTER_2, 0x0);
+}
+
+static void vi_capture_setup_csi_a(struct tegra_camera_dev *cam,
+				   struct soc_camera_device *icd,
+				   u32 hdr)
+{
+	struct soc_camera_subdev_desc *ssdesc = &icd->sdesc->subdev_desc;
+	struct tegra_camera_platform_data *pdata = ssdesc->drv_priv;
+	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
+			icd->current_fmt->host_fmt);
+
+	TC_VI_REG_WT(cam, TEGRA_CSI_INPUT_STREAM_A_CONTROL, 0x00000000);
+	TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_A_CONTROL0, 0x00000000);
+	TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_A_CONTROL1, 0x00000000);
+	TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_A_WORD_COUNT, 0x00000000);
+	TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_A_GAP, 0x00000000);
+	TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND, 0x00000000);
+	TC_VI_REG_WT(cam, TEGRA_CSI_PHY_CILA_CONTROL0, 0x00000000);
+	TC_VI_REG_WT(cam, TEGRA_CSI_CILA_PAD_CONFIG0, 0x00000000);
+	TC_VI_REG_WT(cam, TEGRA_CSI_CILA_PAD_CONFIG1, 0x00000000);
+	TC_VI_REG_WT(cam, TEGRA_CSI_CILA_MIPI_CAL_CONFIG, 0x00000000);
+	TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_A_EXPECTED_FRAME, 0x0);
+
+	TC_VI_REG_WT(cam, TEGRA_VI_VI_CORE_CONTROL, 0x02000000);
+
+	/* CSI-A H_ACTIVE and V_ACTIVE */
+	TC_VI_REG_WT(cam, TEGRA_VI_CSI_PPA_H_ACTIVE,
+		     (icd->user_width << 16));
+	TC_VI_REG_WT(cam, TEGRA_VI_CSI_PPA_V_ACTIVE,
+		     (icd->user_height << 16));
+
+	TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_A_CONTROL1,
+		0x1); /* Frame # for top field detect for interlaced */
+
+	TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_A_WORD_COUNT,
+		bytes_per_line);
+	TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_A_GAP, 0x00140000);
+
+	TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_A_EXPECTED_FRAME,
+			((icd->user_height + cam->tpg_mode) << 16));
+
+	/* pad 0s enabled, virtual channel ID 00 */
+	TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_A_CONTROL0,
+		(0x1 << 16) | /* Output 1 pixel per clock */
+		(hdr << 8) | /* If hdr shows wrong fmt, use right value */
+		(0x1 << 7) | /* Check header CRC */
+		(0x1 << 6) | /* Use word count field in the header */
+		(0x1 << 5) | /* Look at data identifier byte in hdr */
+		(0x1 << 4));  /* Expect packet header */
+
+	TC_VI_REG_WT(cam, TEGRA_CSI_INPUT_STREAM_A_CONTROL,
+		     (0x3f << 16) | /* Skip packet threshold */
+		     (pdata->lanes - 1));
+
+	/* Use 0x00000022 for continuous clock mode. */
+	TC_VI_REG_WT(cam, TEGRA_CSI_PHY_CILA_CONTROL0,
+		(pdata->continuous_clk << 5) |
+		0x5); /* Clock settle time */
+
+	TC_VI_REG_WT(cam, TEGRA_VI_CONT_SYNCPT_CSI_PPA_FRAME_END,
+		(0x1 << 8) | /* Enable continuous syncpt */
+		TEGRA_VI_SYNCPT_CSI_A);
+
+	TC_VI_REG_WT(cam, TEGRA_CSI_PHY_CIL_COMMAND, 0x00020001);
+
+	TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND, 0x0000f002);
+
+	if (cam->tpg_mode) {
+		TC_VI_REG_WT(cam, TEGRA_CSI_PATTERN_GENERATOR_CTRL_A,
+				((cam->tpg_mode - 1) << 2) | 0x1);
+		TC_VI_REG_WT(cam, TEGRA_CSI_PG_PHASE_A, 0x0);
+		TC_VI_REG_WT(cam, TEGRA_CSI_PG_RED_FREQ_A, 0x00800080);
+		TC_VI_REG_WT(cam, TEGRA_CSI_PG_RED_FREQ_RATE_A, 0x0);
+		TC_VI_REG_WT(cam, TEGRA_CSI_PG_GREEN_FREQ_A, 0x00800080);
+		TC_VI_REG_WT(cam, TEGRA_CSI_PG_GREEN_FREQ_RATE_A, 0x0);
+		TC_VI_REG_WT(cam, TEGRA_CSI_PG_BLUE_FREQ_A, 0x00800080);
+		TC_VI_REG_WT(cam, TEGRA_CSI_PG_BLUE_FREQ_RATE_A, 0x0);
+		TC_VI_REG_WT(cam, TEGRA_CSI_PG_BLANK_A, 0x0000FFFF);
+	}
+
+}
+
+static void vi_capture_setup_csi_b(struct tegra_camera_dev *cam,
+				   struct soc_camera_device *icd,
+				   u32 hdr)
+{
+	struct soc_camera_subdev_desc *ssdesc = &icd->sdesc->subdev_desc;
+	struct tegra_camera_platform_data *pdata = ssdesc->drv_priv;
+	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
+						icd->current_fmt->host_fmt);
+
+	TC_VI_REG_WT(cam, TEGRA_CSI_INPUT_STREAM_B_CONTROL, 0x00000000);
+	TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_B_CONTROL0, 0x00000000);
+	TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_B_CONTROL1, 0x00000000);
+	TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_B_WORD_COUNT, 0x00000000);
+	TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_B_GAP, 0x00000000);
+	TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND, 0x00000000);
+	TC_VI_REG_WT(cam, TEGRA_CSI_PHY_CILB_CONTROL0, 0x00000000);
+	TC_VI_REG_WT(cam, TEGRA_CSI_CILB_PAD_CONFIG0, 0x00000000);
+	TC_VI_REG_WT(cam, TEGRA_CSI_CILB_PAD_CONFIG1, 0x00000000);
+	TC_VI_REG_WT(cam, TEGRA_CSI_CILB_MIPI_CAL_CONFIG, 0x00000000);
+	TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_B_EXPECTED_FRAME, 0x0);
+
+	TC_VI_REG_WT(cam, TEGRA_VI_VI_CORE_CONTROL, 0x04000000);
+
+	/* CSI-B H_ACTIVE and V_ACTIVE */
+	TC_VI_REG_WT(cam, TEGRA_VI_CSI_PPB_H_ACTIVE,
+		(icd->user_width << 16));
+	TC_VI_REG_WT(cam, TEGRA_VI_CSI_PPB_V_ACTIVE,
+		(icd->user_height << 16));
+
+	/* pad 0s enabled, virtual channel ID 00 */
+	TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_B_CONTROL0,
+		(0x1 << 16) | /* Output 1 pixel per clock */
+		(hdr << 8) | /* If hdr shows wrong fmt, use right value */
+		(0x1 << 7) | /* Check header CRC */
+		(0x1 << 6) | /* Use word count field in the header */
+		(0x1 << 5) | /* Look at data identifier byte in hdr */
+		(0x1 << 4) | /* Expect packet header */
+		0x1); /* Set PPB stream source to CSI B */
+
+	TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_B_CONTROL1,
+		0x1); /* Frame # for top field detect for interlaced */
+
+	TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_B_WORD_COUNT,
+		bytes_per_line);
+	TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_B_GAP, 0x00140000);
+
+	TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_B_EXPECTED_FRAME,
+		(icd->user_height << 16) |
+		(0x100 << 4) | /* Wait 0x100 vi clks for timeout */
+		0x1); /* Enable line timeout */
+
+	TC_VI_REG_WT(cam, TEGRA_CSI_INPUT_STREAM_B_CONTROL,
+		     (0x3f << 16) | /* Skip packet threshold */
+		     (pdata->lanes - 1));
+
+	/* Use 0x00000022 for continuous clock mode. */
+	TC_VI_REG_WT(cam, TEGRA_CSI_PHY_CILB_CONTROL0,
+		(pdata->continuous_clk << 5) |
+		0x5); /* Clock settle time */
+
+	TC_VI_REG_WT(cam, TEGRA_VI_CONT_SYNCPT_CSI_PPB_FRAME_END,
+		(0x1 << 8) | /* Enable continuous syncpt */
+		TEGRA_VI_SYNCPT_CSI_B);
+
+	TC_VI_REG_WT(cam, TEGRA_CSI_PHY_CIL_COMMAND, 0x00010002);
+
+	TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND, 0x0000f002);
+}
+
+static void vi_capture_setup_vip(struct tegra_camera_dev *cam,
+					   struct soc_camera_device *icd,
+					   u32 input_control)
+{
+	TC_VI_REG_WT(cam, TEGRA_VI_VI_CORE_CONTROL, 0x00000000);
+
+	TC_VI_REG_WT(cam, TEGRA_VI_VI_INPUT_CONTROL,
+		(1 << 27) | /* field detect */
+		(1 << 25) | /* hsync/vsync decoded from data (BT.656) */
+		(1 << 1) | /* VIP_INPUT_ENABLE */
+		input_control);
+
+	TC_VI_REG_WT(cam, TEGRA_VI_H_DOWNSCALE_CONTROL, 0x00000000);
+	TC_VI_REG_WT(cam, TEGRA_VI_V_DOWNSCALE_CONTROL, 0x00000000);
+
+	/* VIP H_ACTIVE and V_ACTIVE */
+	TC_VI_REG_WT(cam, TEGRA_VI_VIP_H_ACTIVE,
+		(icd->user_width << 16) |
+		TEGRA_VIP_H_ACTIVE_START);
+	TC_VI_REG_WT(cam, TEGRA_VI_VIP_V_ACTIVE,
+		(icd->user_height << 16) |
+		TEGRA_VIP_V_ACTIVE_START);
+
+	/*
+	 * For VIP, D9..D2 is mapped to the video decoder's P7..P0.
+	 * Disable/mask out the other Dn wires.
+	 */
+	TC_VI_REG_WT(cam, TEGRA_VI_PIN_INPUT_ENABLE, 0x000003fc);
+	TC_VI_REG_WT(cam, TEGRA_VI_VI_DATA_INPUT_CONTROL, 0x000003fc);
+	TC_VI_REG_WT(cam, TEGRA_VI_PIN_INVERSION, 0x00000000);
+
+	TC_VI_REG_WT(cam, TEGRA_VI_CONT_SYNCPT_VIP_VSYNC,
+		(0x1 << 8) | /* Enable continuous syncpt */
+		TEGRA_VI_SYNCPT_VI);
+
+	TC_VI_REG_WT(cam, TEGRA_VI_CAMERA_CONTROL, 0x00000004);
+}
+
+static int vi_capture_output_channel_setup(
+		struct tegra_camera_dev *cam,
+		struct soc_camera_device *icd)
+{
+	struct soc_camera_subdev_desc *ssdesc = &icd->sdesc->subdev_desc;
+	struct tegra_camera_platform_data *pdata = ssdesc->drv_priv;
+	int port = pdata->port;
+	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
+						icd->current_fmt->host_fmt);
+	const struct soc_camera_format_xlate *current_fmt = icd->current_fmt;
+	u32 output_fourcc = current_fmt->host_fmt->fourcc;
+	u32 output_format, output_control;
+	struct tegra_camera_buffer *buf = to_tegra_vb(cam->active);
+
+	switch (output_fourcc) {
+	case V4L2_PIX_FMT_UYVY:
+		output_format = 0x3; /* Default to YUV422 */
+		break;
+	case V4L2_PIX_FMT_VYUY:
+		output_format = (0x1 << 17) | 0x3;
+		break;
+	case V4L2_PIX_FMT_YUYV:
+		output_format = (0x2 << 17) | 0x3;
+		break;
+	case V4L2_PIX_FMT_YVYU:
+		output_format = (0x3 << 17) | 0x3;
+		break;
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_YVU420:
+		output_format = 0x6; /* YUV420 planar */
+		break;
+	case V4L2_PIX_FMT_SBGGR8:
+	case V4L2_PIX_FMT_SGBRG8:
+	case V4L2_PIX_FMT_SBGGR10:
+		/* Use second output channel for RAW8/RAW10 */
+		buf->output_channel = 1;
+
+		if (port == TEGRA_CAMERA_PORT_CSI_A)
+			output_format = 0x7;
+		else if (port == TEGRA_CAMERA_PORT_CSI_B)
+			output_format = 0x8;
+		else
+			output_format = 0x9;
+		break;
+	default:
+		dev_err(&cam->ndev->dev, "Wrong output format %d\n",
+			output_fourcc);
+		return -EINVAL;
+	}
+
+	output_control = (pdata->flip_v ? (0x1 << 20) : 0) |
+			(pdata->flip_h ? (0x1 << 19) : 0) |
+			output_format;
+
+	if (buf->output_channel == 0) {
+		TC_VI_REG_WT(cam, TEGRA_VI_VI_FIRST_OUTPUT_CONTROL,
+				output_control);
+		/*
+		 * Set up frame size.  Bits 31:16 are the number of lines, and
+		 * bits 15:0 are the number of pixels per line.
+		 */
+		TC_VI_REG_WT(cam, TEGRA_VI_FIRST_OUTPUT_FRAME_SIZE,
+				(icd->user_height << 16) | icd->user_width);
+
+		/* First output memory enabled */
+		TC_VI_REG_WT(cam, TEGRA_VI_VI_ENABLE, 0x00000000);
+
+		/* Set the number of frames in the buffer. */
+		TC_VI_REG_WT(cam, TEGRA_VI_VB0_COUNT_FIRST, 0x00000001);
+
+		/* Set up buffer frame size. */
+		TC_VI_REG_WT(cam, TEGRA_VI_VB0_SIZE_FIRST,
+				(icd->user_height << 16) | icd->user_width);
+
+		TC_VI_REG_WT(cam, TEGRA_VI_VB0_BUFFER_STRIDE_FIRST,
+				(icd->user_height * bytes_per_line));
+
+		TC_VI_REG_WT(cam, TEGRA_VI_CONT_SYNCPT_OUT_1,
+				(0x1 << 8) | /* Enable continuous syncpt */
+				TEGRA_VI_SYNCPT_VI);
+
+		TC_VI_REG_WT(cam, TEGRA_VI_VI_ENABLE, 0x00000000);
+	} else if (buf->output_channel == 1) {
+		TC_VI_REG_WT(cam, TEGRA_VI_VI_SECOND_OUTPUT_CONTROL,
+				output_control);
+
+		TC_VI_REG_WT(cam, TEGRA_VI_SECOND_OUTPUT_FRAME_SIZE,
+				(icd->user_height << 16) | icd->user_width);
+
+		TC_VI_REG_WT(cam, TEGRA_VI_VI_ENABLE_2, 0x00000000);
+
+		/* Set the number of frames in the buffer. */
+		TC_VI_REG_WT(cam, TEGRA_VI_VB0_COUNT_SECOND, 0x00000001);
+
+		/* Set up buffer frame size. */
+		TC_VI_REG_WT(cam, TEGRA_VI_VB0_SIZE_SECOND,
+				(icd->user_height << 16) | icd->user_width);
+
+		TC_VI_REG_WT(cam, TEGRA_VI_VB0_BUFFER_STRIDE_SECOND,
+				(icd->user_height * bytes_per_line));
+
+		TC_VI_REG_WT(cam, TEGRA_VI_CONT_SYNCPT_OUT_2,
+				(0x1 << 8) | /* Enable continuous syncpt */
+				TEGRA_VI_SYNCPT_VI);
+
+		TC_VI_REG_WT(cam, TEGRA_VI_VI_ENABLE_2, 0x00000000);
+	} else {
+		dev_err(&cam->ndev->dev, "Wrong output channel %d\n",
+			buf->output_channel);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+
+static int vi_capture_setup(struct tegra_camera_dev *cam)
+{
+	struct vb2_buffer *vb = cam->active;
+	struct tegra_camera_buffer *buf = to_tegra_vb(vb);
+	struct soc_camera_device *icd = buf->icd;
+	struct soc_camera_subdev_desc *ssdesc = &icd->sdesc->subdev_desc;
+	struct tegra_camera_platform_data *pdata = ssdesc->drv_priv;
+	int port = pdata->port;
+	const struct soc_camera_format_xlate *current_fmt = icd->current_fmt;
+	enum v4l2_mbus_pixelcode input_code = current_fmt->code;
+	u32 hdr, input_control = 0x0;
+
+	switch (input_code) {
+	case V4L2_MBUS_FMT_UYVY8_2X8:
+		input_control |= 0x2 << 8;
+		hdr = 30;
+		break;
+	case V4L2_MBUS_FMT_VYUY8_2X8:
+		input_control |= 0x3 << 8;
+		hdr = 30;
+		break;
+	case V4L2_MBUS_FMT_YUYV8_2X8:
+		input_control |= 0x0;
+		hdr = 30;
+		break;
+	case V4L2_MBUS_FMT_YVYU8_2X8:
+		input_control |= 0x1 << 8;
+		hdr = 30;
+		break;
+	case V4L2_MBUS_FMT_SBGGR8_1X8:
+	case V4L2_MBUS_FMT_SGBRG8_1X8:
+		input_control |= 0x2 << 2;	/* Input Format = Bayer */
+		hdr = 42;
+		break;
+	case V4L2_MBUS_FMT_SBGGR10_1X10:
+		input_control |= 0x2 << 2;	/* Input Format = Bayer */
+		hdr = 43;
+		break;
+	default:
+		dev_err(&cam->ndev->dev, "Input format %d is not supported\n",
+			input_code);
+		return -EINVAL;
+	}
+
+	/*
+	 * Set up low pass filter.  Use 0x240 for chromaticity and 0x240
+	 * for luminance, which is the default and means not to touch
+	 * anything.
+	 */
+	TC_VI_REG_WT(cam, TEGRA_VI_H_LPF_CONTROL, 0x02400240);
+
+	/* Set up raise-on-edge, so we get an interrupt on end of frame. */
+	TC_VI_REG_WT(cam, TEGRA_VI_VI_RAISE, 0x00000001);
+
+	/* Setup registers for CSI-A, CSI-B and VIP inputs */
+	if (port == TEGRA_CAMERA_PORT_CSI_A)
+		vi_capture_setup_csi_a(cam, icd, hdr);
+	else if (port == TEGRA_CAMERA_PORT_CSI_B)
+		vi_capture_setup_csi_b(cam, icd, hdr);
+	else
+		vi_capture_setup_vip(cam, icd, input_control);
+
+	/* Setup registers for output channels */
+	return vi_capture_output_channel_setup(cam, icd);
+}
+
+static int vi_capture_buffer_setup(struct tegra_camera_dev *cam,
+			struct tegra_camera_buffer *buf)
+{
+	struct soc_camera_device *icd = buf->icd;
+
+	switch (icd->current_fmt->host_fmt->fourcc) {
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_YVU420:
+		TC_VI_REG_WT(cam, TEGRA_VI_VB0_BASE_ADDRESS_U,
+			     buf->buffer_addr_u);
+		TC_VI_REG_WT(cam, TEGRA_VI_VB0_START_ADDRESS_U,
+			     buf->start_addr_u);
+
+		TC_VI_REG_WT(cam, TEGRA_VI_VB0_BASE_ADDRESS_V,
+			     buf->buffer_addr_v);
+		TC_VI_REG_WT(cam, TEGRA_VI_VB0_START_ADDRESS_V,
+			     buf->start_addr_v);
+
+	case V4L2_PIX_FMT_UYVY:
+	case V4L2_PIX_FMT_VYUY:
+	case V4L2_PIX_FMT_YUYV:
+	case V4L2_PIX_FMT_YVYU:
+	case V4L2_PIX_FMT_SBGGR8:
+	case V4L2_PIX_FMT_SGBRG8:
+	case V4L2_PIX_FMT_SBGGR10:
+	case V4L2_PIX_FMT_RGB32:
+		/* output 1 */
+		if (buf->output_channel == 0) {
+			TC_VI_REG_WT(cam, TEGRA_VI_VB0_BASE_ADDRESS_FIRST,
+					buf->buffer_addr);
+			TC_VI_REG_WT(cam, TEGRA_VI_VB0_START_ADDRESS_FIRST,
+					buf->start_addr);
+		/* output 2 */
+		} else if (buf->output_channel == 1) {
+			TC_VI_REG_WT(cam, TEGRA_VI_VB0_BASE_ADDRESS_SECOND,
+					buf->buffer_addr);
+			TC_VI_REG_WT(cam, TEGRA_VI_VB0_START_ADDRESS_SECOND,
+					buf->start_addr);
+		} else {
+			dev_err(&cam->ndev->dev, "Wrong output channel %d\n",
+				buf->output_channel);
+			return -EINVAL;
+		}
+	break;
+
+	default:
+		dev_err(&cam->ndev->dev, "Wrong host format %d\n",
+			icd->current_fmt->host_fmt->fourcc);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int vi_capture_start(struct tegra_camera_dev *cam,
+				      struct tegra_camera_buffer *buf)
+{
+	struct soc_camera_device *icd = buf->icd;
+	struct soc_camera_subdev_desc *ssdesc = &icd->sdesc->subdev_desc;
+	struct tegra_camera_platform_data *pdata = ssdesc->drv_priv;
+	int port = pdata->port;
+	int err;
+
+	err = vi_capture_buffer_setup(cam, buf);
+	if (err < 0)
+		return err;
+	/*
+	 * Only wait on CSI frame end syncpt if we're using CSI.  Otherwise,
+	 * wait on VIP VSYNC syncpt.
+	 */
+	if (port == TEGRA_CAMERA_PORT_CSI_A) {
+		cam->syncpt_csi_a++;
+		TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND,
+				0x0000f005);
+		err = nvhost_syncpt_wait_timeout_ext(cam->ndev,
+				TEGRA_VI_SYNCPT_CSI_A,
+				cam->syncpt_csi_a,
+				TEGRA_SYNCPT_CSI_WAIT_TIMEOUT,
+				NULL,
+				NULL);
+	} else if (port == TEGRA_CAMERA_PORT_CSI_B) {
+		cam->syncpt_csi_b++;
+		TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND,
+				0x0000f005);
+		err = nvhost_syncpt_wait_timeout_ext(cam->ndev,
+				TEGRA_VI_SYNCPT_CSI_B,
+				cam->syncpt_csi_b,
+				TEGRA_SYNCPT_CSI_WAIT_TIMEOUT,
+				NULL,
+				NULL);
+	} else {
+		cam->syncpt_vip++;
+		TC_VI_REG_WT(cam, TEGRA_VI_CAMERA_CONTROL,
+				0x00000001);
+		err = nvhost_syncpt_wait_timeout_ext(cam->ndev,
+				TEGRA_VI_SYNCPT_VI,
+				cam->syncpt_csi_a,
+				TEGRA_SYNCPT_VI_WAIT_TIMEOUT,
+				NULL,
+				NULL);
+	}
+
+	if (!err)
+		return 0;
+
+	if (vi_port_is_csi(port)) {
+		u32 ppstatus;
+		u32 cilstatus;
+		u32 rostatus;
+
+		dev_warn(&icd->vdev->dev, "Timeout on CSI syncpt\n");
+		dev_warn(&icd->vdev->dev, "buffer_addr = 0x%08x\n",
+			buf->buffer_addr);
+
+		ppstatus = TC_VI_REG_RD(cam,
+			TEGRA_CSI_CSI_PIXEL_PARSER_STATUS);
+		cilstatus = TC_VI_REG_RD(cam,
+			 TEGRA_CSI_CSI_CIL_STATUS);
+		rostatus = TC_VI_REG_RD(cam,
+			TEGRA_CSI_CSI_READONLY_STATUS);
+
+		dev_warn(&icd->vdev->dev,
+			"PPSTATUS = 0x%08x, "
+			"CILSTATUS = 0x%08x, "
+			"ROSTATUS = 0x%08x\n",
+			ppstatus, cilstatus, rostatus);
+	} else {
+		u32 vip_input_status;
+
+		dev_warn(&cam->ndev->dev, "Timeout on VI syncpt\n");
+		dev_warn(&cam->ndev->dev, "buffer_addr = 0x%08x\n",
+			buf->buffer_addr);
+
+		vip_input_status = TC_VI_REG_RD(cam,
+			TEGRA_VI_VIP_INPUT_STATUS);
+
+		dev_warn(&cam->ndev->dev,
+			"VIP_INPUT_STATUS = 0x%08x\n",
+			vip_input_status);
+	}
+
+	return err;
+}
+
+static int vi_capture_stop(struct tegra_camera_dev *cam, int port)
+{
+	struct tegra_camera_buffer *buf = to_tegra_vb(cam->active);
+	int err;
+
+	if (vi_port_is_csi(port))
+		err = nvhost_syncpt_wait_timeout_ext(cam->ndev,
+			TEGRA_VI_SYNCPT_VI,
+			cam->syncpt_vip,
+			TEGRA_SYNCPT_VI_WAIT_TIMEOUT,
+			NULL,
+			NULL);
+	else
+		err = 0;
+
+	if (err) {
+		u32 buffer_addr;
+		u32 ppstatus;
+		u32 cilstatus;
+
+		dev_warn(&cam->ndev->dev, "Timeout on VI syncpt\n");
+
+		if (buf->output_channel == 0)
+			buffer_addr = TC_VI_REG_RD(cam,
+					   TEGRA_VI_VB0_BASE_ADDRESS_FIRST);
+		else if (buf->output_channel == 1)
+			buffer_addr = TC_VI_REG_RD(cam,
+					   TEGRA_VI_VB0_BASE_ADDRESS_SECOND);
+		else {
+			dev_err(&cam->ndev->dev, "Wrong output channel %d\n",
+				buf->output_channel);
+			return -EINVAL;
+		}
+
+		dev_warn(&cam->ndev->dev, "buffer_addr = 0x%08x\n",
+			buffer_addr);
+
+		ppstatus = TC_VI_REG_RD(cam,
+					TEGRA_CSI_CSI_PIXEL_PARSER_STATUS);
+		cilstatus = TC_VI_REG_RD(cam,
+					 TEGRA_CSI_CSI_CIL_STATUS);
+		dev_warn(&cam->ndev->dev,
+			"PPSTATUS = 0x%08x, CILSTATUS = 0x%08x\n",
+			ppstatus, cilstatus);
+	}
+
+	return err;
+}
+
+static void vi_unpowergate(struct tegra_camera_dev *cam)
+{
+	/*
+	 * Powergating DIS must powergate VE partition. Camera
+	 * module needs to increase the ref-count of disa to
+	 * avoid itself powergated by DIS inadvertently.
+	 */
+#if defined(CONFIG_ARCH_TEGRA_11x_SOC) || defined(CONFIG_ARCH_TEGRA_14x_SOC)
+	tegra_unpowergate_partition(TEGRA_POWERGATE_DISA);
+#endif
+}
+
+static void vi_powergate(struct tegra_camera_dev *cam)
+{
+#if defined(CONFIG_ARCH_TEGRA_11x_SOC) || defined(CONFIG_ARCH_TEGRA_14x_SOC)
+	tegra_powergate_partition(TEGRA_POWERGATE_DISA);
+#endif
+}
+
+struct tegra_camera_ops vi_ops = {
+	.clks_init = vi_clks_init,
+	.clks_deinit = vi_clks_deinit,
+	.clks_enable = vi_clks_enable,
+	.clks_disable = vi_clks_disable,
+
+	.capture_clean = vi_capture_clean,
+	.capture_setup = vi_capture_setup,
+	.capture_start = vi_capture_start,
+	.capture_stop = vi_capture_stop,
+
+	.activate = vi_unpowergate,
+	.deactivate = vi_powergate,
+
+	.save_syncpts = vi_save_syncpts,
+	.incr_syncpts = vi_incr_syncpts,
+
+	.port_is_valid = vi_port_is_valid,
+};
+
+int vi_register(struct tegra_camera_dev *cam)
+{
+	/* Init regulator */
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+	cam->regulator_name = "vcsi";
+#else
+	cam->regulator_name = "avdd_dsi_csi";
+#endif
+
+	/* Init VI/CSI ops */
+	cam->ops = &vi_ops;
+
+	return 0;
+}
diff --git a/drivers/media/platform/soc_camera/tegra_camera/vi2.c b/drivers/media/platform/soc_camera/tegra_camera/vi2.c
new file mode 100644
index 000000000000..754a4bef9a71
--- /dev/null
+++ b/drivers/media/platform/soc_camera/tegra_camera/vi2.c
@@ -0,0 +1,817 @@
+/*
+ * Copyright (c) 2013, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/delay.h>
+#include <linux/clk.h>
+#include <linux/platform_device.h>
+
+#include <media/soc_camera.h>
+#include <media/soc_mediabus.h>
+#include <media/tegra_v4l2_camera.h>
+
+#include <mach/clk.h>
+
+#include "nvhost_syncpt.h"
+#include "common.h"
+
+#define TEGRA_SYNCPT_CSI_WAIT_TIMEOUT                   200
+
+#define TEGRA_VI_SYNCPT_CSI_A				NVSYNCPT_VI_0_3
+#define TEGRA_VI_SYNCPT_CSI_B				NVSYNCPT_VI_1_3
+
+#define TEGRA_VI_CFG_VI_INCR_SYNCPT			0x000
+#define TEGRA_VI_CFG_VI_INCR_SYNCPT_CNTRL		0x004
+#define TEGRA_VI_CFG_VI_INCR_SYNCPT_ERROR		0x008
+#define TEGRA_VI_CFG_CTXSW				0x020
+#define TEGRA_VI_CFG_INTSTATUS				0x024
+#define TEGRA_VI_CFG_PWM_CONTROL			0x038
+#define TEGRA_VI_CFG_PWM_HIGH_PULSE			0x03c
+#define TEGRA_VI_CFG_PWM_LOW_PULSE			0x040
+#define TEGRA_VI_CFG_PWM_SELECT_PULSE_A			0x044
+#define TEGRA_VI_CFG_PWM_SELECT_PULSE_B			0x048
+#define TEGRA_VI_CFG_PWM_SELECT_PULSE_C			0x04c
+#define TEGRA_VI_CFG_PWM_SELECT_PULSE_D			0x050
+#define TEGRA_VI_CFG_VGP1				0x064
+#define TEGRA_VI_CFG_VGP2				0x068
+#define TEGRA_VI_CFG_VGP3				0x06c
+#define TEGRA_VI_CFG_VGP4				0x070
+#define TEGRA_VI_CFG_VGP5				0x074
+#define TEGRA_VI_CFG_VGP6				0x078
+#define TEGRA_VI_CFG_INTERRUPT_MASK			0x08c
+#define TEGRA_VI_CFG_INTERRUPT_TYPE_SELECT		0x090
+#define TEGRA_VI_CFG_INTERRUPT_POLARITY_SELECT		0x094
+#define TEGRA_VI_CFG_INTERRUPT_STATUS			0x098
+#define TEGRA_VI_CFG_VGP_SYNCPT_CONFIG			0x0ac
+#define TEGRA_VI_CFG_VI_SW_RESET			0x0b4
+#define TEGRA_VI_CFG_CG_CTRL				0x0b8
+#define TEGRA_VI_CFG_VI_MCCIF_FIFOCTRL			0x0e4
+#define TEGRA_VI_CFG_TIMEOUT_WCOAL_VI			0x0e8
+#define TEGRA_VI_CFG_DVFS				0x0f0
+#define TEGRA_VI_CFG_RESERVE				0x0f4
+#define TEGRA_VI_CFG_RESERVE_1				0x0f8
+
+#define TEGRA_VI_CSI_0_SW_RESET				0x100
+#define TEGRA_VI_CSI_0_SINGLE_SHOT			0x104
+#define TEGRA_VI_CSI_0_SINGLE_SHOT_STATE_UPDATE		0x108
+#define TEGRA_VI_CSI_0_IMAGE_DEF			0x10c
+#define TEGRA_VI_CSI_0_RGB2Y_CTRL			0x110
+#define TEGRA_VI_CSI_0_MEM_TILING			0x114
+#define TEGRA_VI_CSI_0_CSI_IMAGE_SIZE			0x118
+#define TEGRA_VI_CSI_0_CSI_IMAGE_SIZE_WC		0x11c
+#define TEGRA_VI_CSI_0_CSI_IMAGE_DT			0x120
+#define TEGRA_VI_CSI_0_SURFACE0_OFFSET_MSB		0x124
+#define TEGRA_VI_CSI_0_SURFACE0_OFFSET_LSB		0x128
+#define TEGRA_VI_CSI_0_SURFACE1_OFFSET_MSB		0x12c
+#define TEGRA_VI_CSI_0_SURFACE1_OFFSET_LSB		0x130
+#define TEGRA_VI_CSI_0_SURFACE2_OFFSET_MSB		0x134
+#define TEGRA_VI_CSI_0_SURFACE2_OFFSET_LSB		0x138
+#define TEGRA_VI_CSI_0_SURFACE0_BF_OFFSET_MSB		0x13c
+#define TEGRA_VI_CSI_0_SURFACE0_BF_OFFSET_LSB		0x140
+#define TEGRA_VI_CSI_0_SURFACE1_BF_OFFSET_MSB		0x144
+#define TEGRA_VI_CSI_0_SURFACE1_BF_OFFSET_LSB		0x148
+#define TEGRA_VI_CSI_0_SURFACE2_BF_OFFSET_MSB		0x14c
+#define TEGRA_VI_CSI_0_SURFACE2_BF_OFFSET_LSB		0x150
+#define TEGRA_VI_CSI_0_SURFACE0_STRIDE			0x154
+#define TEGRA_VI_CSI_0_SURFACE1_STRIDE			0x158
+#define TEGRA_VI_CSI_0_SURFACE2_STRIDE			0x15c
+#define TEGRA_VI_CSI_0_SURFACE_HEIGHT0			0x160
+#define TEGRA_VI_CSI_0_ISPINTF_CONFIG			0x164
+#define TEGRA_VI_CSI_0_ERROR_STATUS			0x184
+#define TEGRA_VI_CSI_0_ERROR_INT_MASK			0x188
+#define TEGRA_VI_CSI_0_WD_CTRL				0x18c
+#define TEGRA_VI_CSI_0_WD_PERIOD			0x190
+
+#define TEGRA_VI_CSI_1_SW_RESET				0x200
+#define TEGRA_VI_CSI_1_SINGLE_SHOT			0x204
+#define TEGRA_VI_CSI_1_SINGLE_SHOT_STATE_UPDATE		0x208
+#define TEGRA_VI_CSI_1_IMAGE_DEF			0x20c
+#define TEGRA_VI_CSI_1_RGB2Y_CTRL			0x210
+#define TEGRA_VI_CSI_1_MEM_TILING			0x214
+#define TEGRA_VI_CSI_1_CSI_IMAGE_SIZE			0x218
+#define TEGRA_VI_CSI_1_CSI_IMAGE_SIZE_WC		0x21c
+#define TEGRA_VI_CSI_1_CSI_IMAGE_DT			0x220
+#define TEGRA_VI_CSI_1_SURFACE0_OFFSET_MSB		0x224
+#define TEGRA_VI_CSI_1_SURFACE0_OFFSET_LSB		0x228
+#define TEGRA_VI_CSI_1_SURFACE1_OFFSET_MSB		0x22c
+#define TEGRA_VI_CSI_1_SURFACE1_OFFSET_LSB		0x230
+#define TEGRA_VI_CSI_1_SURFACE2_OFFSET_MSB		0x234
+#define TEGRA_VI_CSI_1_SURFACE2_OFFSET_LSB		0x238
+#define TEGRA_VI_CSI_1_SURFACE0_BF_OFFSET_MSB		0x23c
+#define TEGRA_VI_CSI_1_SURFACE0_BF_OFFSET_LSB		0x240
+#define TEGRA_VI_CSI_1_SURFACE1_BF_OFFSET_MSB		0x244
+#define TEGRA_VI_CSI_1_SURFACE1_BF_OFFSET_LSB		0x248
+#define TEGRA_VI_CSI_1_SURFACE2_BF_OFFSET_MSB		0x24c
+#define TEGRA_VI_CSI_1_SURFACE2_BF_OFFSET_LSB		0x250
+#define TEGRA_VI_CSI_1_SURFACE0_STRIDE			0x254
+#define TEGRA_VI_CSI_1_SURFACE1_STRIDE			0x258
+#define TEGRA_VI_CSI_1_SURFACE2_STRIDE			0x25c
+#define TEGRA_VI_CSI_1_SURFACE_HEIGHT0			0x260
+#define TEGRA_VI_CSI_1_ISPINTF_CONFIG			0x264
+#define TEGRA_VI_CSI_1_ERROR_STATUS			0x284
+#define TEGRA_VI_CSI_1_ERROR_INT_MASK			0x288
+#define TEGRA_VI_CSI_1_WD_CTRL				0x28c
+#define TEGRA_VI_CSI_1_WD_PERIOD			0x290
+
+#define TEGRA_CSI_CSI_CAP_CIL				0x808
+#define TEGRA_CSI_CSI_CAP_CSI				0x818
+#define TEGRA_CSI_CSI_CAP_PP				0x828
+#define TEGRA_CSI_INPUT_STREAM_A_CONTROL		0x838
+#define TEGRA_CSI_PIXEL_STREAM_A_CONTROL0		0x83c
+#define TEGRA_CSI_PIXEL_STREAM_A_CONTROL1		0x840
+#define TEGRA_CSI_PIXEL_STREAM_A_GAP			0x844
+#define TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND		0x848
+#define TEGRA_CSI_PIXEL_STREAM_A_EXPECTED_FRAME		0x84c
+#define TEGRA_CSI_CSI_PIXEL_PARSER_A_INTERRUPT_MASK	0x850
+#define TEGRA_CSI_CSI_PIXEL_PARSER_A_STATUS		0x854
+#define TEGRA_CSI_CSI_SW_SENSOR_A_RESET			0x858
+#define TEGRA_CSI_INPUT_STREAM_B_CONTROL		0x86c
+#define TEGRA_CSI_PIXEL_STREAM_B_CONTROL0		0x870
+#define TEGRA_CSI_PIXEL_STREAM_B_CONTROL1		0x874
+#define TEGRA_CSI_PIXEL_STREAM_B_GAP			0x878
+#define TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND		0x87c
+#define TEGRA_CSI_PIXEL_STREAM_B_EXPECTED_FRAME		0x880
+#define TEGRA_CSI_CSI_PIXEL_PARSER_B_INTERRUPT_MASK	0x884
+#define TEGRA_CSI_CSI_PIXEL_PARSER_B_STATUS		0x888
+#define TEGRA_CSI_CSI_SW_SENSOR_B_RESET			0x88c
+#define TEGRA_CSI_PHY_CIL_COMMAND			0x908
+#define TEGRA_CSI_CIL_PAD_CONFIG0			0x90c
+
+#define TEGRA_CSI_CILA_PAD_CONFIG0			0x92c
+#define TEGRA_CSI_CILA_PAD_CONFIG1			0x930
+#define TEGRA_CSI_PHY_CILA_CONTROL0			0x934
+#define TEGRA_CSI_CSI_CIL_A_INTERRUPT_MASK		0x938
+#define TEGRA_CSI_CSI_CIL_A_STATUS			0x93c
+#define TEGRA_CSI_CSI_CILA_STATUS			0x940
+#define TEGRA_CSI_CIL_A_ESCAPE_MODE_COMMAND		0x944
+#define TEGRA_CSI_CIL_A_ESCAPE_MODE_DATA		0x948
+#define TEGRA_CSI_CSICIL_SW_SENSOR_A_RESET		0x94c
+
+#define TEGRA_CSI_CILB_PAD_CONFIG0			0x960
+#define TEGRA_CSI_CILB_PAD_CONFIG1			0x964
+#define TEGRA_CSI_PHY_CILB_CONTROL0			0x968
+#define TEGRA_CSI_CSI_CIL_B_INTERRUPT_MASK		0x96c
+#define TEGRA_CSI_CSI_CIL_B_STATUS			0x970
+#define TEGRA_CSI_CSI_CILB_STATUS			0x974
+#define TEGRA_CSI_CIL_B_ESCAPE_MODE_COMMAND		0x978
+#define TEGRA_CSI_CIL_B_ESCAPE_MODE_DATA		0x97c
+#define TEGRA_CSI_CSICIL_SW_SENSOR_B_RESET		0x980
+
+#define TEGRA_CSI_CILC_PAD_CONFIG0			0x994
+#define TEGRA_CSI_CILC_PAD_CONFIG1			0x998
+#define TEGRA_CSI_PHY_CILC_CONTROL0			0x99c
+#define TEGRA_CSI_CSI_CIL_C_INTERRUPT_MASK		0x9a0
+#define TEGRA_CSI_CSI_CIL_C_STATUS			0x9a4
+#define TEGRA_CSI_CSI_CILC_STATUS			0x9a8
+#define TEGRA_CSI_CIL_C_ESCAPE_MODE_COMMAND		0x9ac
+#define TEGRA_CSI_CIL_C_ESCAPE_MODE_DATA		0x9b0
+#define TEGRA_CSI_CSICIL_SW_SENSOR_C_RESET		0x9b4
+
+#define TEGRA_CSI_CILD_PAD_CONFIG0			0x9c8
+#define TEGRA_CSI_CILD_PAD_CONFIG1			0x9cc
+#define TEGRA_CSI_PHY_CILD_CONTROL0			0x9d0
+#define TEGRA_CSI_CSI_CIL_D_INTERRUPT_MASK		0x9d4
+#define TEGRA_CSI_CSI_CIL_D_STATUS			0x9d8
+#define TEGRA_CSI_CSI_CILD_STATUS			0x9dc
+#define TEGRA_CSI_CIL_D_ESCAPE_MODE_COMMAND		0x9ec
+#define TEGRA_CSI_CIL_D_ESCAPE_MODE_DATA		0x9f0
+#define TEGRA_CSI_CSICIL_SW_SENSOR_D_RESET		0x9f4
+
+#define TEGRA_CSI_CILE_PAD_CONFIG0			0xa08
+#define TEGRA_CSI_CILE_PAD_CONFIG1			0xa0c
+#define TEGRA_CSI_PHY_CILE_CONTROL0			0xa10
+#define TEGRA_CSI_CSI_CIL_E_INTERRUPT_MASK		0xa14
+#define TEGRA_CSI_CSI_CIL_E_STATUS			0xa18
+#define TEGRA_CSI_CIL_E_ESCAPE_MODE_COMMAND		0xa1c
+#define TEGRA_CSI_CIL_E_ESCAPE_MODE_DATA		0xa20
+#define TEGRA_CSI_CSICIL_SW_SENSOR_E_RESET		0xa24
+
+#define TEGRA_CSI_PATTERN_GENERATOR_CTRL_A		0xa68
+#define TEGRA_CSI_PG_BLANK_A				0xa6c
+#define TEGRA_CSI_PG_PHASE_A				0xa70
+#define TEGRA_CSI_PG_RED_FREQ_A				0xa74
+#define TEGRA_CSI_PG_RED_FREQ_RATE_A			0xa78
+#define TEGRA_CSI_PG_GREEN_FREQ_A			0xa7c
+#define TEGRA_CSI_PG_GREEN_FREQ_RATE_A			0xa80
+#define TEGRA_CSI_PG_BLUE_FREQ_A			0xa84
+#define TEGRA_CSI_PG_BLUE_FREQ_RATE_A			0xa88
+
+#define TEGRA_CSI_PATTERN_GENERATOR_CTRL_B		0xa9c
+#define TEGRA_CSI_PG_BLANK_B				0xaa0
+#define TEGRA_CSI_PG_PHASE_B				0xaa4
+#define TEGRA_CSI_PG_RED_FREQ_B				0xaa8
+#define TEGRA_CSI_PG_RED_FREQ_RATE_B			0xaac
+#define TEGRA_CSI_PG_GREEN_FREQ_B			0xab0
+#define TEGRA_CSI_PG_GREEN_FREQ_RATE_B			0xab4
+#define TEGRA_CSI_PG_BLUE_FREQ_B			0xab8
+#define TEGRA_CSI_PG_BLUE_FREQ_RATE_B			0xabc
+
+#define TEGRA_CSI_DPCM_CTRL_A				0xad0
+#define TEGRA_CSI_DPCM_CTRL_B				0xad4
+#define TEGRA_CSI_STALL_COUNTER				0xae8
+#define TEGRA_CSI_CSI_READONLY_STATUS			0xaec
+#define TEGRA_CSI_CSI_SW_STATUS_RESET			0xaf0
+#define TEGRA_CSI_CLKEN_OVERRIDE			0xaf4
+#define TEGRA_CSI_DEBUG_CONTROL				0xaf8
+#define TEGRA_CSI_DEBUG_COUNTER_0			0xafc
+#define TEGRA_CSI_DEBUG_COUNTER_1			0xb00
+#define TEGRA_CSI_DEBUG_COUNTER_2			0xb04
+
+static int vi2_port_is_valid(int port)
+{
+	return (((port) >= TEGRA_CAMERA_PORT_CSI_A) &&
+		((port) <= TEGRA_CAMERA_PORT_CSI_E));
+}
+
+/* Clock settings for camera */
+static struct tegra_camera_clk vi2_clks0[] = {
+	{
+		.name = "vi",
+		.freq = 150000000,
+		.use_devname = 1,
+	},
+	{
+		.name = "vi_sensor",
+		.freq = 24000000,
+	},
+	{
+		.name = "csi",
+		.freq = 0,
+		.use_devname = 1,
+	},
+	{
+		.name = "isp",
+		.freq = 0,
+	},
+	{
+		.name = "csus",
+		.freq = 0,
+		.use_devname = 1,
+	},
+	{
+		.name = "sclk",
+		.freq = 80000000,
+	},
+	{
+		.name = "emc",
+		.freq = 375000000,
+	},
+	{
+		.name = "cilab",
+		.freq = 102000000,
+		.use_devname = 1,
+	},
+	/* Always put "p11_d" at the end */
+	{
+		.name = "pll_d",
+		.freq = 0,
+	},
+};
+
+static struct tegra_camera_clk vi2_clks1[] = {
+	{
+		.name = "vi",
+		.freq = 150000000,
+		.use_devname = 1,
+	},
+	{
+		.name = "vi_sensor",
+		.freq = 24000000,
+	},
+	{
+		.name = "csi",
+		.freq = 0,
+		.use_devname = 1,
+	},
+	{
+		.name = "isp",
+		.freq = 0,
+	},
+	{
+		.name = "sclk",
+		.freq = 80000000,
+	},
+	{
+		.name = "emc",
+		.freq = 375000000,
+	},
+	{
+		.name = "cilcd",
+		.freq = 0,
+		.use_devname = 1,
+	},
+	{
+		.name = "cile",
+		.freq = 0,
+		.use_devname = 1,
+	},
+	/* Always put "p11_d" at the end */
+	{
+		.name = "pll_d",
+		.freq = 0,
+	},
+};
+
+#define MAX_DEVID_LENGTH	16
+
+static int vi2_clks_init(struct tegra_camera_dev *cam)
+{
+	struct platform_device *pdev = cam->ndev;
+	char devname[MAX_DEVID_LENGTH];
+	struct tegra_camera_clk *clks;
+	int i;
+
+	snprintf(devname, MAX_DEVID_LENGTH,
+		 (pdev->id <= 0) ? "tegra_%s" : "tegra_%s.%d",
+		 pdev->name, pdev->id);
+
+	switch (pdev->id) {
+	case 0:
+		cam->num_clks = ARRAY_SIZE(vi2_clks0);
+		cam->clks = vi2_clks0;
+		break;
+	case 1:
+		cam->num_clks = ARRAY_SIZE(vi2_clks1);
+		cam->clks = vi2_clks1;
+		break;
+	default:
+		dev_err(&pdev->dev, "Wrong device ID %d\n", pdev->id);
+		return -ENODEV;
+	}
+
+	for (i = 0; i < cam->num_clks; i++) {
+		clks = &cam->clks[i];
+
+		if (clks->use_devname)
+			clks->clk = clk_get_sys(devname, clks->name);
+		else
+			clks->clk = clk_get(&pdev->dev, clks->name);
+		if (IS_ERR_OR_NULL(clks->clk)) {
+			dev_err(&pdev->dev, "Failed to get clock %s.\n",
+				clks->name);
+			return PTR_ERR(clks->clk);
+		}
+
+		if (clks->freq > 0)
+			clk_set_rate(clks->clk, clks->freq);
+	}
+
+	return 0;
+}
+
+static void vi2_clks_deinit(struct tegra_camera_dev *cam)
+{
+	struct tegra_camera_clk *clks;
+	int i;
+
+	for (i = 0; i < cam->num_clks; i++) {
+		clks = &cam->clks[i];
+		if (clks->clk)
+			clk_put(clks->clk);
+	}
+}
+
+static void vi2_clks_enable(struct tegra_camera_dev *cam)
+{
+	struct tegra_camera_clk *clks;
+	int i;
+
+	for (i = 0; i < cam->num_clks - 1; i++) {
+		clks = &cam->clks[i];
+		if (clks->clk)
+			clk_prepare_enable(clks->clk);
+	}
+
+	if (cam->tpg_mode) {
+		clks = &cam->clks[i];
+		if (clks->clk) {
+			clk_prepare_enable(clks->clk);
+			tegra_clk_cfg_ex(clks->clk,
+					 TEGRA_CLK_PLLD_CSI_OUT_ENB, 1);
+			tegra_clk_cfg_ex(clks->clk,
+					 TEGRA_CLK_PLLD_DSI_OUT_ENB, 1);
+			tegra_clk_cfg_ex(clks->clk,
+					 TEGRA_CLK_MIPI_CSI_OUT_ENB, 0);
+		}
+	}
+}
+
+static void vi2_clks_disable(struct tegra_camera_dev *cam)
+{
+	struct tegra_camera_clk *clks;
+	int i;
+
+	for (i = 0; i < cam->num_clks - 1; i++) {
+		clks = &cam->clks[i];
+		if (clks->clk)
+			clk_disable_unprepare(clks->clk);
+	}
+
+	if (cam->tpg_mode) {
+		clks = &cam->clks[i];
+		if (clks->clk) {
+			tegra_clk_cfg_ex(clks->clk,
+					 TEGRA_CLK_MIPI_CSI_OUT_ENB, 1);
+			tegra_clk_cfg_ex(clks->clk,
+					 TEGRA_CLK_PLLD_CSI_OUT_ENB, 0);
+			tegra_clk_cfg_ex(clks->clk,
+					 TEGRA_CLK_PLLD_DSI_OUT_ENB, 0);
+			clk_disable_unprepare(clks->clk);
+		}
+	}
+}
+
+static void vi2_save_syncpts(struct tegra_camera_dev *cam)
+{
+	cam->syncpt_csi_a =
+		nvhost_syncpt_read_ext(cam->ndev,
+				       TEGRA_VI_SYNCPT_CSI_A);
+
+	cam->syncpt_csi_b =
+		nvhost_syncpt_read_ext(cam->ndev,
+				       TEGRA_VI_SYNCPT_CSI_B);
+}
+
+static void vi2_incr_syncpts(struct tegra_camera_dev *cam)
+{
+	nvhost_syncpt_cpu_incr_ext(cam->ndev,
+				   TEGRA_VI_SYNCPT_CSI_A);
+
+	nvhost_syncpt_cpu_incr_ext(cam->ndev,
+				   TEGRA_VI_SYNCPT_CSI_B);
+}
+
+static void vi2_capture_clean(struct tegra_camera_dev *cam)
+{
+	/* Clean up status */
+	TC_VI_REG_WT(cam, TEGRA_CSI_CSI_CIL_A_STATUS, 0xFFFFFFFF);
+	TC_VI_REG_WT(cam, TEGRA_CSI_CSI_CILA_STATUS, 0xFFFFFFFF);
+	TC_VI_REG_WT(cam, TEGRA_CSI_CSI_CIL_B_STATUS, 0xFFFFFFFF);
+	TC_VI_REG_WT(cam, TEGRA_CSI_CSI_CIL_C_STATUS, 0xFFFFFFFF);
+	TC_VI_REG_WT(cam, TEGRA_CSI_CSI_CIL_D_STATUS, 0xFFFFFFFF);
+	TC_VI_REG_WT(cam, TEGRA_CSI_CSI_CIL_E_STATUS, 0xFFFFFFFF);
+	TC_VI_REG_WT(cam, TEGRA_CSI_CSI_PIXEL_PARSER_A_STATUS, 0xFFFFFFFF);
+	TC_VI_REG_WT(cam, TEGRA_VI_CSI_0_ERROR_STATUS, 0xFFFFFFFF);
+}
+
+static int vi2_capture_setup_csi_0(struct tegra_camera_dev *cam,
+				    struct soc_camera_device *icd)
+{
+	struct soc_camera_subdev_desc *ssdesc = &icd->sdesc->subdev_desc;
+	struct tegra_camera_platform_data *pdata = ssdesc->drv_priv;
+
+	TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND, 0xf007);
+	TC_VI_REG_WT(cam, TEGRA_CSI_CSI_PIXEL_PARSER_A_INTERRUPT_MASK, 0x0);
+	TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_A_CONTROL0, 0x280301f0);
+	TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND, 0xf007);
+	TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_A_CONTROL1, 0x11);
+	TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_A_GAP, 0x140000);
+
+	TC_VI_REG_WT(cam, TEGRA_CSI_INPUT_STREAM_A_CONTROL,
+			0x3f0000 | (pdata->lanes - 1));
+	if (pdata->lanes == 4)
+		TC_VI_REG_WT(cam, TEGRA_CSI_PHY_CIL_COMMAND, 0x22020101);
+	else
+		TC_VI_REG_WT(cam, TEGRA_CSI_PHY_CIL_COMMAND, 0x22020201);
+
+	/* VI_MWA_REQ_DONE */
+	TC_VI_REG_WT(cam, TEGRA_VI_CFG_VI_INCR_SYNCPT,
+			(0x4 << 8) | TEGRA_VI_SYNCPT_CSI_A);
+
+	if (cam->tpg_mode) {
+		TC_VI_REG_WT(cam, TEGRA_CSI_PATTERN_GENERATOR_CTRL_A,
+				((cam->tpg_mode - 1) << 2) | 0x1);
+		TC_VI_REG_WT(cam, TEGRA_CSI_PG_PHASE_A, 0x0);
+		TC_VI_REG_WT(cam, TEGRA_CSI_PG_RED_FREQ_A, 0x100010);
+		TC_VI_REG_WT(cam, TEGRA_CSI_PG_RED_FREQ_RATE_A, 0x0);
+		TC_VI_REG_WT(cam, TEGRA_CSI_PG_GREEN_FREQ_A, 0x100010);
+		TC_VI_REG_WT(cam, TEGRA_CSI_PG_GREEN_FREQ_RATE_A, 0x0);
+		TC_VI_REG_WT(cam, TEGRA_CSI_PG_BLUE_FREQ_A, 0x100010);
+		TC_VI_REG_WT(cam, TEGRA_CSI_PG_BLUE_FREQ_RATE_A, 0x0);
+		TC_VI_REG_WT(cam, TEGRA_CSI_PHY_CIL_COMMAND, 0x22020202);
+
+		/* output format A8B8G8R8, only support direct to mem */
+		TC_VI_REG_WT(cam, TEGRA_VI_CSI_0_IMAGE_DEF, (64 << 16) | 0x1);
+		/* input format is RGB888 */
+		TC_VI_REG_WT(cam, TEGRA_VI_CSI_0_CSI_IMAGE_DT, 36);
+	}
+
+	TC_VI_REG_WT(cam, TEGRA_VI_CSI_0_CSI_IMAGE_SIZE,
+		     (icd->user_height << 16) | icd->user_width);
+
+	TC_VI_REG_WT(cam, TEGRA_VI_CSI_0_CSI_IMAGE_SIZE_WC,
+		     icd->user_width * 3);
+
+	return 0;
+}
+
+static int vi2_capture_setup_csi_1(struct tegra_camera_dev *cam,
+				     struct soc_camera_device *icd)
+{
+	struct soc_camera_subdev_desc *ssdesc = &icd->sdesc->subdev_desc;
+	struct tegra_camera_platform_data *pdata = ssdesc->drv_priv;
+
+	TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND, 0xf007);
+	TC_VI_REG_WT(cam, TEGRA_CSI_CSI_PIXEL_PARSER_B_INTERRUPT_MASK, 0x0);
+	TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_B_CONTROL0, 0x280301f0);
+	TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND, 0xf007);
+	TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_B_CONTROL1, 0x11);
+	TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_B_GAP, 0x140000);
+
+	TC_VI_REG_WT(cam, TEGRA_CSI_INPUT_STREAM_B_CONTROL,
+			0x3f0000 | (pdata->lanes - 1));
+	if (pdata->lanes == 4)
+		TC_VI_REG_WT(cam, TEGRA_CSI_PHY_CIL_COMMAND, 0x21010202);
+	else if (pdata->lanes == 1 && pdata->port == TEGRA_CAMERA_PORT_CSI_E)
+		TC_VI_REG_WT(cam, TEGRA_CSI_PHY_CIL_COMMAND, 0x12020202);
+	else
+		TC_VI_REG_WT(cam, TEGRA_CSI_PHY_CIL_COMMAND, 0x22010202);
+
+	/* VI_MWB_REQ_DONE */
+	TC_VI_REG_WT(cam, TEGRA_VI_CFG_VI_INCR_SYNCPT,
+			(0x5 << 8) | TEGRA_VI_SYNCPT_CSI_B);
+
+	if (cam->tpg_mode) {
+		TC_VI_REG_WT(cam, TEGRA_CSI_PATTERN_GENERATOR_CTRL_B,
+				((cam->tpg_mode - 1) << 2) | 0x1);
+		TC_VI_REG_WT(cam, TEGRA_CSI_PG_PHASE_B, 0x0);
+		TC_VI_REG_WT(cam, TEGRA_CSI_PG_RED_FREQ_B, 0x100010);
+		TC_VI_REG_WT(cam, TEGRA_CSI_PG_RED_FREQ_RATE_B, 0x0);
+		TC_VI_REG_WT(cam, TEGRA_CSI_PG_GREEN_FREQ_B, 0x100010);
+		TC_VI_REG_WT(cam, TEGRA_CSI_PG_GREEN_FREQ_RATE_B, 0x0);
+		TC_VI_REG_WT(cam, TEGRA_CSI_PG_BLUE_FREQ_B, 0x100010);
+		TC_VI_REG_WT(cam, TEGRA_CSI_PG_BLUE_FREQ_RATE_B, 0x0);
+		TC_VI_REG_WT(cam, TEGRA_CSI_PHY_CIL_COMMAND, 0x22020202);
+
+		/* output format A8B8G8R8, only support direct to mem */
+		TC_VI_REG_WT(cam, TEGRA_VI_CSI_1_IMAGE_DEF, (64 << 16) | 0x1);
+		/* input format is RGB888 */
+		TC_VI_REG_WT(cam, TEGRA_VI_CSI_1_CSI_IMAGE_DT, 36);
+	}
+
+	TC_VI_REG_WT(cam, TEGRA_VI_CSI_1_CSI_IMAGE_SIZE,
+		     (icd->user_height << 16) | icd->user_width);
+
+	TC_VI_REG_WT(cam, TEGRA_VI_CSI_1_CSI_IMAGE_SIZE_WC,
+		     icd->user_width * 3);
+
+	return 0;
+}
+
+static int vi2_capture_setup(struct tegra_camera_dev *cam)
+{
+	struct vb2_buffer *vb = cam->active;
+	struct tegra_camera_buffer *buf = to_tegra_vb(vb);
+	struct soc_camera_device *icd = buf->icd;
+	struct soc_camera_subdev_desc *ssdesc = &icd->sdesc->subdev_desc;
+	struct tegra_camera_platform_data *pdata = ssdesc->drv_priv;
+	int port = pdata->port;
+
+	/*
+	 * MIPI pad controls
+	 * MIPI_CAL_MIPI_BIAS_PAD_CFG0_0 MIPI_BIAS_PAD_E_VCLAMP_REF 1
+	 * MIPI_CAL_MIPI_BIAS_PAD_CFG2_0 PAD_PDVREG 0
+	 */
+
+	/*
+	 * PAD_CILA_PDVCLAMP 0, PAD_CILA_PDIO_CLK 0,
+	 * PAD_CILA_PDIO 0, PAD_AB_BK_MODE 1
+	 */
+	TC_VI_REG_WT(cam, TEGRA_CSI_CILA_PAD_CONFIG0, 0x10000);
+
+	/* PAD_CILB_PDVCLAMP 0, PAD_CILB_PDIO_CLK 0, PAD_CILB_PDIO 0 */
+	TC_VI_REG_WT(cam, TEGRA_CSI_CILB_PAD_CONFIG0, 0x0);
+
+	/*
+	 * PAD_CILC_PDVCLAMP 0, PAD_CILC_PDIO_CLK 0,
+	 * PAD_CILC_PDIO 0, PAD_CD_BK_MODE 1
+	 */
+	TC_VI_REG_WT(cam, TEGRA_CSI_CILC_PAD_CONFIG0, 0x10000);
+
+	/* PAD_CILD_PDVCLAMP 0, PAD_CILD_PDIO_CLK 0, PAD_CILD_PDIO 0 */
+	TC_VI_REG_WT(cam, TEGRA_CSI_CILD_PAD_CONFIG0, 0x0);
+
+	/* PAD_CILE_PDVCLAMP 0, PAD_CILE_PDIO_CLK 0, PAD_CILE_PDIO 0 */
+	TC_VI_REG_WT(cam, TEGRA_CSI_CILE_PAD_CONFIG0, 0x0);
+
+	/* Common programming set for any config */
+	TC_VI_REG_WT(cam, TEGRA_CSI_CLKEN_OVERRIDE, 0x0);
+	TC_VI_REG_WT(cam, TEGRA_CSI_PHY_CIL_COMMAND, 0x22020202);
+	TC_VI_REG_WT(cam, TEGRA_CSI_CSI_CIL_A_INTERRUPT_MASK, 0x0);
+	TC_VI_REG_WT(cam, TEGRA_CSI_CSI_CIL_B_INTERRUPT_MASK, 0x0);
+	TC_VI_REG_WT(cam, TEGRA_CSI_CSI_CIL_C_INTERRUPT_MASK, 0x0);
+	TC_VI_REG_WT(cam, TEGRA_CSI_CSI_CIL_D_INTERRUPT_MASK, 0x0);
+	TC_VI_REG_WT(cam, TEGRA_CSI_CSI_CIL_E_INTERRUPT_MASK, 0x0);
+
+	/*
+	 * TODO: these values should be different with different
+	 * sensor connected.
+	 * Hardcode THS settle value just for TPG testing
+	 */
+	TC_VI_REG_WT(cam, TEGRA_CSI_PHY_CILA_CONTROL0, 0x8);
+	TC_VI_REG_WT(cam, TEGRA_CSI_PHY_CILB_CONTROL0, 0x8);
+	TC_VI_REG_WT(cam, TEGRA_CSI_PHY_CILC_CONTROL0, 0xa);
+	TC_VI_REG_WT(cam, TEGRA_CSI_PHY_CILD_CONTROL0, 0xa);
+	TC_VI_REG_WT(cam, TEGRA_CSI_PHY_CILE_CONTROL0, 0xa);
+
+	/* Setup registers for CSI-A and CSI-B inputs */
+	if (port == TEGRA_CAMERA_PORT_CSI_A)
+		return vi2_capture_setup_csi_0(cam, icd);
+	else if (port == TEGRA_CAMERA_PORT_CSI_B)
+		return vi2_capture_setup_csi_1(cam, icd);
+	else
+		return -ENODEV;
+}
+
+static int vi2_capture_buffer_setup(struct tegra_camera_dev *cam,
+			struct tegra_camera_buffer *buf)
+{
+	struct soc_camera_device *icd = buf->icd;
+	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
+			icd->current_fmt->host_fmt);
+
+	switch (icd->current_fmt->host_fmt->fourcc) {
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_YVU420:
+		/* FIXME: Setup YUV buffer */
+
+	case V4L2_PIX_FMT_UYVY:
+	case V4L2_PIX_FMT_VYUY:
+	case V4L2_PIX_FMT_YUYV:
+	case V4L2_PIX_FMT_YVYU:
+	case V4L2_PIX_FMT_SBGGR8:
+	case V4L2_PIX_FMT_SGBRG8:
+	case V4L2_PIX_FMT_SBGGR10:
+	case V4L2_PIX_FMT_RGB32:
+		switch (buf->output_channel) {
+		case 0:
+			TC_VI_REG_WT(cam, TEGRA_VI_CSI_0_SURFACE0_OFFSET_MSB,
+				     0x0);
+			TC_VI_REG_WT(cam, TEGRA_VI_CSI_0_SURFACE0_OFFSET_LSB,
+				     buf->buffer_addr);
+			TC_VI_REG_WT(cam, TEGRA_VI_CSI_0_SURFACE0_STRIDE,
+				     bytes_per_line);
+			break;
+		case 1:
+			TC_VI_REG_WT(cam, TEGRA_VI_CSI_0_SURFACE1_OFFSET_MSB,
+				     0x0);
+			TC_VI_REG_WT(cam, TEGRA_VI_CSI_0_SURFACE1_OFFSET_LSB,
+				     buf->buffer_addr);
+			TC_VI_REG_WT(cam, TEGRA_VI_CSI_0_SURFACE1_STRIDE,
+				     bytes_per_line);
+			break;
+		case 2:
+			TC_VI_REG_WT(cam, TEGRA_VI_CSI_0_SURFACE2_OFFSET_MSB,
+				     0x0);
+			TC_VI_REG_WT(cam, TEGRA_VI_CSI_0_SURFACE2_OFFSET_LSB,
+				     buf->buffer_addr);
+			TC_VI_REG_WT(cam, TEGRA_VI_CSI_0_SURFACE2_STRIDE,
+				     bytes_per_line);
+			break;
+		}
+		break;
+
+	default:
+		dev_err(&cam->ndev->dev, "Wrong host format %d\n",
+			icd->current_fmt->host_fmt->fourcc);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int vi2_capture_start(struct tegra_camera_dev *cam,
+				      struct tegra_camera_buffer *buf)
+{
+	struct soc_camera_device *icd = buf->icd;
+	struct soc_camera_subdev_desc *ssdesc = &icd->sdesc->subdev_desc;
+	struct tegra_camera_platform_data *pdata = ssdesc->drv_priv;
+	int port = pdata->port;
+	int err;
+
+	err = vi2_capture_buffer_setup(cam, buf);
+	if (err < 0)
+		return err;
+
+	/* Only wait on CSI frame end syncpt if we're using CSI. */
+	if (port == TEGRA_CAMERA_PORT_CSI_A) {
+		cam->syncpt_csi_a++;
+		TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND,
+				0x0000f005);
+		TC_VI_REG_WT(cam, TEGRA_VI_CSI_0_SINGLE_SHOT, 0x1);
+		err = nvhost_syncpt_wait_timeout_ext(cam->ndev,
+				TEGRA_VI_SYNCPT_CSI_A,
+				cam->syncpt_csi_a,
+				TEGRA_SYNCPT_CSI_WAIT_TIMEOUT,
+				NULL,
+				NULL);
+	} else if (port == TEGRA_CAMERA_PORT_CSI_B) {
+		cam->syncpt_csi_b++;
+		TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND,
+				0x0000f005);
+		TC_VI_REG_WT(cam, TEGRA_VI_CSI_1_SINGLE_SHOT, 0x1);
+		err = nvhost_syncpt_wait_timeout_ext(cam->ndev,
+				TEGRA_VI_SYNCPT_CSI_B,
+				cam->syncpt_csi_b,
+				TEGRA_SYNCPT_CSI_WAIT_TIMEOUT,
+				NULL,
+				NULL);
+	}
+
+	if (!err)
+		return 0;
+
+	err = TC_VI_REG_RD(cam, TEGRA_CSI_CSI_CIL_A_STATUS);
+	if (err)
+		pr_err("TEGRA_CSI_CSI_CIL_A_STATUS 0x%08x\n", err);
+	err = TC_VI_REG_RD(cam, TEGRA_CSI_CSI_CILA_STATUS);
+	if (err)
+		pr_err("TEGRA_CSI_CSI_CILA_STATUS 0x%08x\n", err);
+	err = TC_VI_REG_RD(cam, TEGRA_CSI_CSI_CIL_B_STATUS);
+	if (err)
+		pr_err("TEGRA_CSI_CSI_CIL_B_STATUS 0x%08x\n", err);
+	err = TC_VI_REG_RD(cam, TEGRA_CSI_CSI_CIL_C_STATUS);
+	if (err)
+		pr_err("TEGRA_CSI_CSI_CIL_C_STATUS 0x%08x\n", err);
+	err = TC_VI_REG_RD(cam, TEGRA_CSI_CSI_CIL_D_STATUS);
+	if (err)
+		pr_err("TEGRA_CSI_CSI_CIL_D_STATUS 0x%08x\n", err);
+	err = TC_VI_REG_RD(cam, TEGRA_CSI_CSI_CIL_E_STATUS);
+	if (err)
+		pr_err("TEGRA_CSI_CSI_CIL_E_STATUS 0x%08x\n", err);
+
+	err = TC_VI_REG_RD(cam, TEGRA_CSI_CSI_PIXEL_PARSER_A_STATUS);
+	if (err)
+		pr_err("TEGRA_CSI_CSI_PIXEL_PARSER_A_STATUS 0x%08x\n", err);
+
+	err = TC_VI_REG_RD(cam, TEGRA_VI_CSI_0_ERROR_STATUS);
+	if (err)
+		pr_err("TEGRA_VI_CSI_0_ERROR_STATUS 0x%08x\n", err);
+
+	return err;
+}
+
+static int vi2_capture_stop(struct tegra_camera_dev *cam, int port)
+{
+	if (port == TEGRA_CAMERA_PORT_CSI_A)
+		TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND,
+			     0x0000f002);
+	else if (port == TEGRA_CAMERA_PORT_CSI_B)
+		TC_VI_REG_WT(cam, TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND,
+			     0x0000f002);
+
+	return 0;
+}
+
+/* Reset VI2/CSI2 when activating, no sepecial ops for deactiving  */
+static void vi2_sw_reset(struct tegra_camera_dev *cam)
+{
+	/* T12_CG_2ND_LEVEL_EN */
+	TC_VI_REG_WT(cam, TEGRA_VI_CFG_CG_CTRL, 1);
+	TC_VI_REG_WT(cam, TEGRA_VI_CSI_0_SW_RESET, 0x1F);
+	TC_VI_REG_WT(cam, TEGRA_VI_CSI_1_SW_RESET, 0x1F);
+
+	udelay(10);
+}
+
+struct tegra_camera_ops vi2_ops = {
+	.clks_init = vi2_clks_init,
+	.clks_deinit = vi2_clks_deinit,
+	.clks_enable = vi2_clks_enable,
+	.clks_disable = vi2_clks_disable,
+
+	.capture_clean = vi2_capture_clean,
+	.capture_setup = vi2_capture_setup,
+	.capture_start = vi2_capture_start,
+	.capture_stop = vi2_capture_stop,
+
+	.activate = vi2_sw_reset,
+
+	.save_syncpts = vi2_save_syncpts,
+	.incr_syncpts = vi2_incr_syncpts,
+
+	.port_is_valid = vi2_port_is_valid,
+};
+
+int vi2_register(struct tegra_camera_dev *cam)
+{
+	/* Init regulator */
+	cam->regulator_name = "avdd_dsi_csi";
+
+	/* Init VI2/CSI2 ops */
+	cam->ops = &vi2_ops;
+
+	return 0;
+}
diff --git a/drivers/media/platform/soc_camera/tegra_v4l2_camera.c b/drivers/media/platform/soc_camera/tegra_v4l2_camera.c
deleted file mode 100644
index a551af56d889..000000000000
--- a/drivers/media/platform/soc_camera/tegra_v4l2_camera.c
+++ /dev/null
@@ -1,1998 +0,0 @@
-/*
- * Copyright (c) 2012-2013, NVIDIA CORPORATION.  All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms and conditions of the GNU General Public License,
- * version 2, as published by the Free Software Foundation.
- *
- * This program is distributed in the hope it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include <linux/delay.h>
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/platform_device.h>
-#include <linux/clk.h>
-#include <linux/pm_runtime.h>
-#include <linux/nvhost.h>
-#include <linux/of.h>
-#include <linux/of_device.h>
-#include <linux/of_platform.h>
-#include <linux/tegra-powergate.h>
-
-#include <mach/pm_domains.h>
-
-#include <media/soc_camera.h>
-#include <media/soc_mediabus.h>
-#include <media/videobuf2-dma-contig.h>
-#include <media/tegra_v4l2_camera.h>
-
-#include "dev.h"
-#include "bus_client.h"
-#include "nvhost_syncpt.h"
-#include "t20/t20.h"
-#include "t30/t30.h"
-#include "t114/t114.h"
-
-#define TEGRA_CAM_DRV_NAME "vi"
-#define TEGRA_CAM_VERSION_CODE KERNEL_VERSION(0, 0, 5)
-
-#define TEGRA_SYNCPT_VI_WAIT_TIMEOUT                    200
-#define TEGRA_SYNCPT_CSI_WAIT_TIMEOUT                   200
-
-#define TEGRA_SYNCPT_RETRY_COUNT			10
-
-#define TEGRA_VIP_H_ACTIVE_START			0x98
-#define TEGRA_VIP_V_ACTIVE_START			0x10
-
-/* SYNCPTs 12-17 are reserved for VI. */
-#define TEGRA_VI_SYNCPT_VI                              NVSYNCPT_VI_ISP_2
-#define TEGRA_VI_SYNCPT_CSI_A                           NVSYNCPT_VI_ISP_3
-#define TEGRA_VI_SYNCPT_CSI_B                           NVSYNCPT_VI_ISP_4
-
-/* Tegra CSI-MIPI registers. */
-#define TEGRA_VI_OUT_1_INCR_SYNCPT			0x0000
-#define TEGRA_VI_OUT_1_INCR_SYNCPT_CNTRL		0x0004
-#define TEGRA_VI_OUT_1_INCR_SYNCPT_ERROR		0x0008
-#define TEGRA_VI_OUT_2_INCR_SYNCPT			0x0020
-#define TEGRA_VI_OUT_2_INCR_SYNCPT_CNTRL		0x0024
-#define TEGRA_VI_OUT_2_INCR_SYNCPT_ERROR		0x0028
-#define TEGRA_VI_MISC_INCR_SYNCPT			0x0040
-#define TEGRA_VI_MISC_INCR_SYNCPT_CNTRL			0x0044
-#define TEGRA_VI_MISC_INCR_SYNCPT_ERROR			0x0048
-#define TEGRA_VI_CONT_SYNCPT_OUT_1			0x0060
-#define TEGRA_VI_CONT_SYNCPT_OUT_2			0x0064
-#define TEGRA_VI_CONT_SYNCPT_VIP_VSYNC			0x0068
-#define TEGRA_VI_CONT_SYNCPT_VI2EPP			0x006c
-#define TEGRA_VI_CONT_SYNCPT_CSI_PPA_FRAME_START	0x0070
-#define TEGRA_VI_CONT_SYNCPT_CSI_PPA_FRAME_END		0x0074
-#define TEGRA_VI_CONT_SYNCPT_CSI_PPB_FRAME_START	0x0078
-#define TEGRA_VI_CONT_SYNCPT_CSI_PPB_FRAME_END		0x007c
-#define TEGRA_VI_CTXSW					0x0080
-#define TEGRA_VI_INTSTATUS				0x0084
-#define TEGRA_VI_VI_INPUT_CONTROL			0x0088
-#define TEGRA_VI_VI_CORE_CONTROL			0x008c
-#define TEGRA_VI_VI_FIRST_OUTPUT_CONTROL		0x0090
-#define TEGRA_VI_VI_SECOND_OUTPUT_CONTROL		0x0094
-#define TEGRA_VI_HOST_INPUT_FRAME_SIZE			0x0098
-#define TEGRA_VI_HOST_H_ACTIVE				0x009c
-#define TEGRA_VI_HOST_V_ACTIVE				0x00a0
-#define TEGRA_VI_VIP_H_ACTIVE				0x00a4
-#define TEGRA_VI_VIP_V_ACTIVE				0x00a8
-#define TEGRA_VI_VI_PEER_CONTROL			0x00ac
-#define TEGRA_VI_VI_DMA_SELECT				0x00b0
-#define TEGRA_VI_HOST_DMA_WRITE_BUFFER			0x00b4
-#define TEGRA_VI_HOST_DMA_BASE_ADDRESS			0x00b8
-#define TEGRA_VI_HOST_DMA_WRITE_BUFFER_STATUS		0x00bc
-#define TEGRA_VI_HOST_DMA_WRITE_PEND_BUFCOUNT		0x00c0
-#define TEGRA_VI_VB0_START_ADDRESS_FIRST		0x00c4
-#define TEGRA_VI_VB0_BASE_ADDRESS_FIRST			0x00c8
-#define TEGRA_VI_VB0_START_ADDRESS_U			0x00cc
-#define TEGRA_VI_VB0_BASE_ADDRESS_U			0x00d0
-#define TEGRA_VI_VB0_START_ADDRESS_V			0x00d4
-#define TEGRA_VI_VB0_BASE_ADDRESS_V			0x00d8
-#define TEGRA_VI_VB_SCRATCH_ADDRESS_UV			0x00dc
-#define TEGRA_VI_FIRST_OUTPUT_FRAME_SIZE		0x00e0
-#define TEGRA_VI_VB0_COUNT_FIRST			0x00e4
-#define TEGRA_VI_VB0_SIZE_FIRST				0x00e8
-#define TEGRA_VI_VB0_BUFFER_STRIDE_FIRST		0x00ec
-#define TEGRA_VI_VB0_START_ADDRESS_SECOND		0x00f0
-#define TEGRA_VI_VB0_BASE_ADDRESS_SECOND		0x00f4
-#define TEGRA_VI_SECOND_OUTPUT_FRAME_SIZE		0x00f8
-#define TEGRA_VI_VB0_COUNT_SECOND			0x00fc
-#define TEGRA_VI_VB0_SIZE_SECOND			0x0100
-#define TEGRA_VI_VB0_BUFFER_STRIDE_SECOND		0x0104
-#define TEGRA_VI_H_LPF_CONTROL				0x0108
-#define TEGRA_VI_H_DOWNSCALE_CONTROL			0x010c
-#define TEGRA_VI_V_DOWNSCALE_CONTROL			0x0110
-#define TEGRA_VI_CSC_Y					0x0114
-#define TEGRA_VI_CSC_UV_R				0x0118
-#define TEGRA_VI_CSC_UV_G				0x011c
-#define TEGRA_VI_CSC_UV_B				0x0120
-#define TEGRA_VI_CSC_ALPHA				0x0124
-#define TEGRA_VI_HOST_VSYNC				0x0128
-#define TEGRA_VI_COMMAND				0x012c
-#define TEGRA_VI_HOST_FIFO_STATUS			0x0130
-#define TEGRA_VI_INTERRUPT_MASK				0x0134
-#define TEGRA_VI_INTERRUPT_TYPE_SELECT			0x0138
-#define TEGRA_VI_INTERRUPT_POLARITY_SELECT		0x013c
-#define TEGRA_VI_INTERRUPT_STATUS			0x0140
-#define TEGRA_VI_VIP_INPUT_STATUS			0x0144
-#define TEGRA_VI_VIDEO_BUFFER_STATUS			0x0148
-#define TEGRA_VI_SYNC_OUTPUT				0x014c
-#define TEGRA_VI_VVS_OUTPUT_DELAY			0x0150
-#define TEGRA_VI_PWM_CONTROL				0x0154
-#define TEGRA_VI_PWM_SELECT_PULSE_A			0x0158
-#define TEGRA_VI_PWM_SELECT_PULSE_B			0x015c
-#define TEGRA_VI_PWM_SELECT_PULSE_C			0x0160
-#define TEGRA_VI_PWM_SELECT_PULSE_D			0x0164
-#define TEGRA_VI_VI_DATA_INPUT_CONTROL			0x0168
-#define TEGRA_VI_PIN_INPUT_ENABLE			0x016c
-#define TEGRA_VI_PIN_OUTPUT_ENABLE			0x0170
-#define TEGRA_VI_PIN_INVERSION				0x0174
-#define TEGRA_VI_PIN_INPUT_DATA				0x0178
-#define TEGRA_VI_PIN_OUTPUT_DATA			0x017c
-#define TEGRA_VI_PIN_OUTPUT_SELECT			0x0180
-#define TEGRA_VI_RAISE_VIP_BUFFER_FIRST_OUTPUT		0x0184
-#define TEGRA_VI_RAISE_VIP_FRAME_FIRST_OUTPUT		0x0188
-#define TEGRA_VI_RAISE_VIP_BUFFER_SECOND_OUTPUT		0x018c
-#define TEGRA_VI_RAISE_VIP_FRAME_SECOND_OUTPUT		0x0190
-#define TEGRA_VI_RAISE_HOST_FIRST_OUTPUT		0x0194
-#define TEGRA_VI_RAISE_HOST_SECOND_OUTPUT		0x0198
-#define TEGRA_VI_RAISE_EPP				0x019c
-#define TEGRA_VI_CAMERA_CONTROL				0x01a0
-#define TEGRA_VI_VI_ENABLE				0x01a4
-#define TEGRA_VI_VI_ENABLE_2				0x01a8
-#define TEGRA_VI_VI_RAISE				0x01ac
-#define TEGRA_VI_Y_FIFO_WRITE				0x01b0
-#define TEGRA_VI_U_FIFO_WRITE				0x01b4
-#define TEGRA_VI_V_FIFO_WRITE				0x01b8
-#define TEGRA_VI_VI_MCCIF_FIFOCTRL			0x01bc
-#define TEGRA_VI_TIMEOUT_WCOAL_VI			0x01c0
-#define TEGRA_VI_MCCIF_VIRUV_HP				0x01c4
-#define TEGRA_VI_MCCIF_VIWSB_HP				0x01c8
-#define TEGRA_VI_MCCIF_VIWU_HP				0x01cc
-#define TEGRA_VI_MCCIF_VIWV_HP				0x01d0
-#define TEGRA_VI_MCCIF_VIWY_HP				0x01d4
-#define TEGRA_VI_CSI_PPA_RAISE_FRAME_START		0x01d8
-#define TEGRA_VI_CSI_PPA_RAISE_FRAME_END		0x01dc
-#define TEGRA_VI_CSI_PPB_RAISE_FRAME_START		0x01e0
-#define TEGRA_VI_CSI_PBB_RAISE_FRAME_END		0x01e4
-#define TEGRA_VI_CSI_PPA_H_ACTIVE			0x01e8
-#define TEGRA_VI_CSI_PPA_V_ACTIVE			0x01ec
-#define TEGRA_VI_CSI_PPB_H_ACTIVE			0x01f0
-#define TEGRA_VI_CSI_PPB_V_ACTIVE			0x01f4
-#define TEGRA_VI_ISP_H_ACTIVE				0x01f8
-#define TEGRA_VI_ISP_V_ACTIVE				0x01fc
-#define TEGRA_VI_STREAM_1_RESOURCE_DEFINE		0x0200
-#define TEGRA_VI_STREAM_2_RESOURCE_DEFINE		0x0204
-#define TEGRA_VI_RAISE_STREAM_1_DONE			0x0208
-#define TEGRA_VI_RAISE_STREAM_2_DONE			0x020c
-#define TEGRA_VI_TS_MODE				0x0210
-#define TEGRA_VI_TS_CONTROL				0x0214
-#define TEGRA_VI_TS_PACKET_COUNT			0x0218
-#define TEGRA_VI_TS_ERROR_COUNT				0x021c
-#define TEGRA_VI_TS_CPU_FLOW_CTL			0x0220
-#define TEGRA_VI_VB0_CHROMA_BUFFER_STRIDE_FIRST		0x0224
-#define TEGRA_VI_VB0_CHROMA_LINE_STRIDE_FIRST		0x0228
-#define TEGRA_VI_EPP_LINES_PER_BUFFER			0x022c
-#define TEGRA_VI_BUFFER_RELEASE_OUTPUT1			0x0230
-#define TEGRA_VI_BUFFER_RELEASE_OUTPUT2			0x0234
-#define TEGRA_VI_DEBUG_FLOW_CONTROL_COUNTER_OUTPUT1	0x0238
-#define TEGRA_VI_DEBUG_FLOW_CONTROL_COUNTER_OUTPUT2	0x023c
-#define TEGRA_VI_TERMINATE_BW_FIRST			0x0240
-#define TEGRA_VI_TERMINATE_BW_SECOND			0x0244
-#define TEGRA_VI_VB0_FIRST_BUFFER_ADDR_MODE		0x0248
-#define TEGRA_VI_VB0_SECOND_BUFFER_ADDR_MODE		0x024c
-#define TEGRA_VI_RESERVE_0				0x0250
-#define TEGRA_VI_RESERVE_1				0x0254
-#define TEGRA_VI_RESERVE_2				0x0258
-#define TEGRA_VI_RESERVE_3				0x025c
-#define TEGRA_VI_RESERVE_4				0x0260
-#define TEGRA_VI_MCCIF_VIRUV_HYST			0x0264
-#define TEGRA_VI_MCCIF_VIWSB_HYST			0x0268
-#define TEGRA_VI_MCCIF_VIWU_HYST			0x026c
-#define TEGRA_VI_MCCIF_VIWV_HYST			0x0270
-#define TEGRA_VI_MCCIF_VIWY_HYST			0x0274
-
-#define TEGRA_CSI_VI_INPUT_STREAM_CONTROL		0x0800
-#define TEGRA_CSI_HOST_INPUT_STREAM_CONTROL		0x0808
-#define TEGRA_CSI_INPUT_STREAM_A_CONTROL		0x0810
-#define TEGRA_CSI_PIXEL_STREAM_A_CONTROL0		0x0818
-#define TEGRA_CSI_PIXEL_STREAM_A_CONTROL1		0x081c
-#define TEGRA_CSI_PIXEL_STREAM_A_WORD_COUNT		0x0820
-#define TEGRA_CSI_PIXEL_STREAM_A_GAP			0x0824
-#define TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND		0x0828
-#define TEGRA_CSI_INPUT_STREAM_B_CONTROL		0x083c
-#define TEGRA_CSI_PIXEL_STREAM_B_CONTROL0		0x0844
-#define TEGRA_CSI_PIXEL_STREAM_B_CONTROL1		0x0848
-#define TEGRA_CSI_PIXEL_STREAM_B_WORD_COUNT		0x084c
-#define TEGRA_CSI_PIXEL_STREAM_B_GAP			0x0850
-#define TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND		0x0854
-#define TEGRA_CSI_PHY_CIL_COMMAND			0x0868
-#define TEGRA_CSI_PHY_CILA_CONTROL0			0x086c
-#define TEGRA_CSI_PHY_CILB_CONTROL0			0x0870
-#define TEGRA_CSI_CSI_PIXEL_PARSER_STATUS		0x0878
-#define TEGRA_CSI_CSI_CIL_STATUS			0x087c
-#define TEGRA_CSI_CSI_PIXEL_PARSER_INTERRUPT_MASK	0x0880
-#define TEGRA_CSI_CSI_CIL_INTERRUPT_MASK		0x0884
-#define TEGRA_CSI_CSI_READONLY_STATUS			0x0888
-#define TEGRA_CSI_ESCAPE_MODE_COMMAND			0x088c
-#define TEGRA_CSI_ESCAPE_MODE_DATA			0x0890
-#define TEGRA_CSI_CILA_PAD_CONFIG0			0x0894
-#define TEGRA_CSI_CILA_PAD_CONFIG1			0x0898
-#define TEGRA_CSI_CILB_PAD_CONFIG0			0x089c
-#define TEGRA_CSI_CILB_PAD_CONFIG1			0x08a0
-#define TEGRA_CSI_CIL_PAD_CONFIG			0x08a4
-#define TEGRA_CSI_CILA_MIPI_CAL_CONFIG			0x08a8
-#define TEGRA_CSI_CILB_MIPI_CAL_CONFIG			0x08ac
-#define TEGRA_CSI_CIL_MIPI_CAL_STATUS			0x08b0
-#define TEGRA_CSI_CLKEN_OVERRIDE			0x08b4
-#define TEGRA_CSI_DEBUG_CONTROL				0x08b8
-#define TEGRA_CSI_DEBUG_COUNTER_0			0x08bc
-#define TEGRA_CSI_DEBUG_COUNTER_1			0x08c0
-#define TEGRA_CSI_DEBUG_COUNTER_2			0x08c4
-#define TEGRA_CSI_PIXEL_STREAM_A_EXPECTED_FRAME		0x08c8
-#define TEGRA_CSI_PIXEL_STREAM_B_EXPECTED_FRAME		0x08cc
-#define TEGRA_CSI_DSI_MIPI_CAL_CONFIG			0x08d0
-
-#define TC_VI_REG_RD(DEV, REG) readl(DEV->vi_base + REG)
-#define TC_VI_REG_WT(DEV, REG, VAL) writel(VAL, DEV->vi_base + REG)
-
-#define tegra_camera_port_is_valid(port) \
-	(((port) >= TEGRA_CAMERA_PORT_CSI_A) && \
-	 ((port) <= TEGRA_CAMERA_PORT_VIP))
-
-#define tegra_camera_port_is_csi(port) \
-	(((port) == TEGRA_CAMERA_PORT_CSI_A) || \
-	 ((port) == TEGRA_CAMERA_PORT_CSI_B))
-
-/*
- * Structures
- */
-
-/* buffer for one video frame */
-struct tegra_buffer {
-	struct vb2_buffer		vb; /* v4l buffer must be first */
-	struct list_head		queue;
-	struct soc_camera_device	*icd;
-	int				output_channel;
-
-	/*
-	 * Various buffer addresses shadowed so we don't have to recalculate
-	 * per frame.  These are calculated during videobuf_prepare.
-	 */
-	dma_addr_t			buffer_addr;
-	dma_addr_t			buffer_addr_u;
-	dma_addr_t			buffer_addr_v;
-	dma_addr_t			start_addr;
-	dma_addr_t			start_addr_u;
-	dma_addr_t			start_addr_v;
-};
-
-struct tegra_camera_dev {
-	struct soc_camera_host		ici;
-	struct platform_device		*ndev;
-	struct nvhost_device_data	*ndata;
-
-	struct clk			*clk_vi;
-	struct clk			*clk_vi_sensor;
-	struct clk			*clk_csi;
-	struct clk			*clk_isp;
-	struct clk			*clk_csus;
-	struct clk			*clk_sclk;
-	struct clk			*clk_emc;
-
-	struct regulator		*reg;
-
-	void __iomem			*vi_base;
-	spinlock_t			videobuf_queue_lock;
-	struct list_head		capture;
-	struct vb2_buffer		*active;
-	struct vb2_alloc_ctx		*alloc_ctx;
-	enum v4l2_field			field;
-	int				sequence_a;
-	int				sequence_b;
-
-	struct work_struct		work;
-	struct mutex			work_mutex;
-
-	u32				syncpt_vi;
-	u32				syncpt_csi_a;
-	u32				syncpt_csi_b;
-
-	/* Debug */
-	int num_frames;
-	int enable_refcnt;
-};
-
-static const struct soc_mbus_pixelfmt tegra_camera_formats[] = {
-	{
-		.fourcc			= V4L2_PIX_FMT_UYVY,
-		.name			= "YUV422 (UYVY) packed",
-		.bits_per_sample	= 16,
-		.packing		= SOC_MBUS_PACKING_NONE,
-		.order			= SOC_MBUS_ORDER_LE,
-	},
-	{
-		.fourcc			= V4L2_PIX_FMT_VYUY,
-		.name			= "YUV422 (VYUY) packed",
-		.bits_per_sample	= 16,
-		.packing		= SOC_MBUS_PACKING_NONE,
-		.order			= SOC_MBUS_ORDER_LE,
-	},
-	{
-		.fourcc			= V4L2_PIX_FMT_YUYV,
-		.name			= "YUV422 (YUYV) packed",
-		.bits_per_sample	= 16,
-		.packing		= SOC_MBUS_PACKING_NONE,
-		.order			= SOC_MBUS_ORDER_LE,
-	},
-	{
-		.fourcc			= V4L2_PIX_FMT_YVYU,
-		.name			= "YUV422 (YVYU) packed",
-		.bits_per_sample	= 16,
-		.packing		= SOC_MBUS_PACKING_NONE,
-		.order			= SOC_MBUS_ORDER_LE,
-	},
-	{
-		.fourcc			= V4L2_PIX_FMT_YUV420,
-		.name			= "YUV420 (YU12) planar",
-		.bits_per_sample	= 12,
-		.packing		= SOC_MBUS_PACKING_NONE,
-		.order			= SOC_MBUS_ORDER_LE,
-	},
-	{
-		.fourcc			= V4L2_PIX_FMT_YVU420,
-		.name			= "YVU420 (YV12) planar",
-		.bits_per_sample	= 12,
-		.packing		= SOC_MBUS_PACKING_NONE,
-		.order			= SOC_MBUS_ORDER_LE,
-	},
-
-	/* For RAW8 and RAW10 output, we always output 16-bit (2 bytes). */
-	{
-		.fourcc			= V4L2_PIX_FMT_SBGGR8,
-		.name			= "Bayer 8 BGBG.. GRGR..",
-		.bits_per_sample	= 16,
-		.packing		= SOC_MBUS_PACKING_EXTEND16,
-		.order			= SOC_MBUS_ORDER_LE,
-	},
-	{
-		.fourcc			= V4L2_PIX_FMT_SBGGR10,
-		.name			= "Bayer 10 BGBG.. GRGR..",
-		.bits_per_sample	= 16,
-		.packing		= SOC_MBUS_PACKING_EXTEND16,
-		.order			= SOC_MBUS_ORDER_LE,
-	},
-
-};
-
-static struct tegra_buffer *to_tegra_vb(struct vb2_buffer *vb)
-{
-	return container_of(vb, struct tegra_buffer, vb);
-}
-
-static void tegra_camera_save_syncpts(struct tegra_camera_dev *pcdev)
-{
-	pcdev->syncpt_csi_a =
-		nvhost_syncpt_read_ext(pcdev->ndev,
-				       TEGRA_VI_SYNCPT_CSI_A);
-
-	pcdev->syncpt_csi_b =
-		nvhost_syncpt_read_ext(pcdev->ndev,
-				       TEGRA_VI_SYNCPT_CSI_B);
-
-	pcdev->syncpt_vi =
-		nvhost_syncpt_read_ext(pcdev->ndev,
-				       TEGRA_VI_SYNCPT_VI);
-}
-
-static void tegra_camera_incr_syncpts(struct tegra_camera_dev *pcdev)
-{
-	nvhost_syncpt_cpu_incr_ext(pcdev->ndev,
-				   TEGRA_VI_SYNCPT_CSI_A);
-
-	nvhost_syncpt_cpu_incr_ext(pcdev->ndev,
-				   TEGRA_VI_SYNCPT_CSI_B);
-
-	nvhost_syncpt_cpu_incr_ext(pcdev->ndev,
-				   TEGRA_VI_SYNCPT_VI);
-}
-
-static void tegra_camera_capture_clean(struct tegra_camera_dev *pcdev)
-{
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_VI_INPUT_STREAM_CONTROL, 0x00000000);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_HOST_INPUT_STREAM_CONTROL, 0x00000000);
-
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_CSI_PIXEL_PARSER_STATUS, 0x0);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_CSI_CIL_STATUS, 0x0);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_CSI_PIXEL_PARSER_INTERRUPT_MASK, 0x0);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_CSI_CIL_INTERRUPT_MASK, 0x0);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_CSI_READONLY_STATUS, 0x0);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_ESCAPE_MODE_COMMAND, 0x0);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_ESCAPE_MODE_DATA, 0x0);
-
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_CIL_PAD_CONFIG, 0x00000000);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_CIL_MIPI_CAL_STATUS, 0x00000000);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_CLKEN_OVERRIDE, 0x00000000);
-
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_DEBUG_CONTROL, 0x0);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_DEBUG_COUNTER_0, 0x0);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_DEBUG_COUNTER_1, 0x0);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_DEBUG_COUNTER_2, 0x0);
-}
-
-static void tegra_camera_capture_setup_csi_a(struct tegra_camera_dev *pcdev,
-					     struct soc_camera_device *icd,
-					     u32 hdr)
-{
-	struct tegra_camera_platform_data *pdata = icd->link->priv;
-	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
-			icd->current_fmt->host_fmt);
-
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_INPUT_STREAM_A_CONTROL, 0x00000000);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_CONTROL0, 0x00000000);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_CONTROL1, 0x00000000);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_WORD_COUNT, 0x00000000);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_GAP, 0x00000000);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND, 0x00000000);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_PHY_CILA_CONTROL0, 0x00000000);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_CILA_PAD_CONFIG0, 0x00000000);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_CILA_PAD_CONFIG1, 0x00000000);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_CILA_MIPI_CAL_CONFIG, 0x00000000);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_EXPECTED_FRAME, 0x0);
-
-	TC_VI_REG_WT(pcdev, TEGRA_VI_VI_CORE_CONTROL, 0x02000000);
-
-	/* CSI-A H_ACTIVE and V_ACTIVE */
-	TC_VI_REG_WT(pcdev, TEGRA_VI_CSI_PPA_H_ACTIVE,
-		     (icd->user_width << 16));
-	TC_VI_REG_WT(pcdev, TEGRA_VI_CSI_PPA_V_ACTIVE,
-		     (icd->user_height << 16));
-
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_CONTROL1,
-		0x1); /* Frame # for top field detect for interlaced */
-
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_WORD_COUNT,
-		bytes_per_line);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_GAP, 0x00140000);
-
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_EXPECTED_FRAME,
-		(icd->user_height << 16) |
-		(0x100 << 4) | /* Wait 0x100 vi clks for timeout */
-		0x1); /* Enable line timeout */
-
-	/* pad 0s enabled, virtual channel ID 00 */
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_CONTROL0,
-		(0x1 << 16) | /* Output 1 pixel per clock */
-		(hdr << 8) | /* If hdr shows wrong fmt, use right value */
-		(0x1 << 7) | /* Check header CRC */
-		(0x1 << 6) | /* Use word count field in the header */
-		(0x1 << 5) | /* Look at data identifier byte in hdr */
-		(0x1 << 4));  /* Expect packet header */
-
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_INPUT_STREAM_A_CONTROL,
-		     (0x3f << 16) | /* Skip packet threshold */
-		     (pdata->lanes - 1));
-
-	/* Use 0x00000022 for continuous clock mode. */
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_PHY_CILA_CONTROL0,
-		(pdata->continuous_clk << 5) |
-		0x5); /* Clock settle time */
-
-	TC_VI_REG_WT(pcdev, TEGRA_VI_CONT_SYNCPT_CSI_PPA_FRAME_END,
-		(0x1 << 8) | /* Enable continuous syncpt */
-		TEGRA_VI_SYNCPT_CSI_A);
-
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_PHY_CIL_COMMAND, 0x00020001);
-
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND, 0x0000f002);
-}
-
-static void tegra_camera_capture_setup_csi_b(struct tegra_camera_dev *pcdev,
-					     struct soc_camera_device *icd,
-					     u32 hdr)
-{
-	struct tegra_camera_platform_data *pdata = icd->link->priv;
-	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
-						icd->current_fmt->host_fmt);
-
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_INPUT_STREAM_B_CONTROL, 0x00000000);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_CONTROL0, 0x00000000);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_CONTROL1, 0x00000000);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_WORD_COUNT, 0x00000000);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_GAP, 0x00000000);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND, 0x00000000);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_PHY_CILB_CONTROL0, 0x00000000);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_CILB_PAD_CONFIG0, 0x00000000);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_CILB_PAD_CONFIG1, 0x00000000);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_CILB_MIPI_CAL_CONFIG, 0x00000000);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_EXPECTED_FRAME, 0x0);
-
-	TC_VI_REG_WT(pcdev, TEGRA_VI_VI_CORE_CONTROL, 0x04000000);
-
-	/* CSI-B H_ACTIVE and V_ACTIVE */
-	TC_VI_REG_WT(pcdev, TEGRA_VI_CSI_PPB_H_ACTIVE,
-		(icd->user_width << 16));
-	TC_VI_REG_WT(pcdev, TEGRA_VI_CSI_PPB_V_ACTIVE,
-		(icd->user_height << 16));
-
-	/* pad 0s enabled, virtual channel ID 00 */
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_CONTROL0,
-		(0x1 << 16) | /* Output 1 pixel per clock */
-		(hdr << 8) | /* If hdr shows wrong fmt, use right value */
-		(0x1 << 7) | /* Check header CRC */
-		(0x1 << 6) | /* Use word count field in the header */
-		(0x1 << 5) | /* Look at data identifier byte in hdr */
-		(0x1 << 4) | /* Expect packet header */
-		0x1); /* Set PPB stream source to CSI B */
-
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_CONTROL1,
-		0x1); /* Frame # for top field detect for interlaced */
-
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_WORD_COUNT,
-		bytes_per_line);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_GAP, 0x00140000);
-
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_EXPECTED_FRAME,
-		(icd->user_height << 16) |
-		(0x100 << 4) | /* Wait 0x100 vi clks for timeout */
-		0x1); /* Enable line timeout */
-
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_INPUT_STREAM_B_CONTROL,
-		     (0x3f << 16) | /* Skip packet threshold */
-		     (pdata->lanes - 1));
-
-	/* Use 0x00000022 for continuous clock mode. */
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_PHY_CILB_CONTROL0,
-		(pdata->continuous_clk << 5) |
-		0x5); /* Clock settle time */
-
-	TC_VI_REG_WT(pcdev, TEGRA_VI_CONT_SYNCPT_CSI_PPB_FRAME_END,
-		(0x1 << 8) | /* Enable continuous syncpt */
-		TEGRA_VI_SYNCPT_CSI_B);
-
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_PHY_CIL_COMMAND, 0x00010002);
-
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND, 0x0000f002);
-}
-
-static void tegra_camera_capture_setup_vip(struct tegra_camera_dev *pcdev,
-					   struct soc_camera_device *icd,
-					   u32 input_control)
-{
-
-	TC_VI_REG_WT(pcdev, TEGRA_VI_VI_CORE_CONTROL, 0x00000000);
-
-	TC_VI_REG_WT(pcdev, TEGRA_VI_VI_INPUT_CONTROL,
-		(1 << 27) | /* field detect */
-		(1 << 25) | /* hsync/vsync decoded from data (BT.656) */
-		(1 << 1) | /* VIP_INPUT_ENABLE */
-		input_control);
-
-	TC_VI_REG_WT(pcdev, TEGRA_VI_H_DOWNSCALE_CONTROL, 0x00000000);
-	TC_VI_REG_WT(pcdev, TEGRA_VI_V_DOWNSCALE_CONTROL, 0x00000000);
-
-	/* VIP H_ACTIVE and V_ACTIVE */
-	TC_VI_REG_WT(pcdev, TEGRA_VI_VIP_H_ACTIVE,
-		(icd->user_width << 16) |
-		TEGRA_VIP_H_ACTIVE_START);
-	TC_VI_REG_WT(pcdev, TEGRA_VI_VIP_V_ACTIVE,
-		(icd->user_height << 16) |
-		TEGRA_VIP_V_ACTIVE_START);
-
-	/*
-	 * For VIP, D9..D2 is mapped to the video decoder's P7..P0.
-	 * Disable/mask out the other Dn wires.
-	 */
-	TC_VI_REG_WT(pcdev, TEGRA_VI_PIN_INPUT_ENABLE, 0x000003fc);
-	TC_VI_REG_WT(pcdev, TEGRA_VI_VI_DATA_INPUT_CONTROL, 0x000003fc);
-	TC_VI_REG_WT(pcdev, TEGRA_VI_PIN_INVERSION, 0x00000000);
-
-	TC_VI_REG_WT(pcdev, TEGRA_VI_CONT_SYNCPT_VIP_VSYNC,
-		(0x1 << 8) | /* Enable continuous syncpt */
-		TEGRA_VI_SYNCPT_VI);
-
-	TC_VI_REG_WT(pcdev, TEGRA_VI_CAMERA_CONTROL, 0x00000004);
-}
-
-static int tegra_camera_capture_output_channel_setup(
-		struct tegra_camera_dev *pcdev,
-		struct soc_camera_device *icd)
-{
-	struct tegra_camera_platform_data *pdata = icd->link->priv;
-	int port = pdata->port;
-	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
-						icd->current_fmt->host_fmt);
-	const struct soc_camera_format_xlate *current_fmt = icd->current_fmt;
-	u32 output_fourcc = current_fmt->host_fmt->fourcc;
-	u32 output_format, output_control;
-	struct tegra_buffer *buf = to_tegra_vb(pcdev->active);
-
-	switch (output_fourcc) {
-	case V4L2_PIX_FMT_UYVY:
-		output_format = 0x3; /* Default to YUV422 */
-		break;
-	case V4L2_PIX_FMT_VYUY:
-		output_format = (0x1 << 17) | 0x3;
-		break;
-	case V4L2_PIX_FMT_YUYV:
-		output_format = (0x2 << 17) | 0x3;
-		break;
-	case V4L2_PIX_FMT_YVYU:
-		output_format = (0x3 << 17) | 0x3;
-		break;
-	case V4L2_PIX_FMT_YUV420:
-	case V4L2_PIX_FMT_YVU420:
-		output_format = 0x6; /* YUV420 planar */
-		break;
-	case V4L2_PIX_FMT_SBGGR8:
-	case V4L2_PIX_FMT_SBGGR10:
-		/* Use second output channel for RAW8/RAW10 */
-		buf->output_channel = 1;
-
-		if (port == TEGRA_CAMERA_PORT_CSI_A)
-			output_format = 0x7;
-		else if (port == TEGRA_CAMERA_PORT_CSI_B)
-			output_format = 0x8;
-		else
-			output_format = 0x9;
-		break;
-	default:
-		dev_err(&pcdev->ndev->dev, "Wrong output format %d\n",
-			output_fourcc);
-		return -EINVAL;
-	}
-
-	output_control = (pdata->flip_v ? (0x1 << 20) : 0) |
-			(pdata->flip_h ? (0x1 << 19) : 0) |
-			output_format;
-
-	if (buf->output_channel == 0) {
-		TC_VI_REG_WT(pcdev, TEGRA_VI_VI_FIRST_OUTPUT_CONTROL,
-				output_control);
-		/*
-		 * Set up frame size.  Bits 31:16 are the number of lines, and
-		 * bits 15:0 are the number of pixels per line.
-		 */
-		TC_VI_REG_WT(pcdev, TEGRA_VI_FIRST_OUTPUT_FRAME_SIZE,
-				(icd->user_height << 16) | icd->user_width);
-
-		/* First output memory enabled */
-		TC_VI_REG_WT(pcdev, TEGRA_VI_VI_ENABLE, 0x00000000);
-
-		/* Set the number of frames in the buffer. */
-		TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_COUNT_FIRST, 0x00000001);
-
-		/* Set up buffer frame size. */
-		TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_SIZE_FIRST,
-				(icd->user_height << 16) | icd->user_width);
-
-		TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_BUFFER_STRIDE_FIRST,
-				(icd->user_height * bytes_per_line));
-
-		TC_VI_REG_WT(pcdev, TEGRA_VI_CONT_SYNCPT_OUT_1,
-				(0x1 << 8) | /* Enable continuous syncpt */
-				TEGRA_VI_SYNCPT_VI);
-
-		TC_VI_REG_WT(pcdev, TEGRA_VI_VI_ENABLE, 0x00000000);
-	} else if (buf->output_channel == 1) {
-		TC_VI_REG_WT(pcdev, TEGRA_VI_VI_SECOND_OUTPUT_CONTROL,
-				output_control);
-
-		TC_VI_REG_WT(pcdev, TEGRA_VI_SECOND_OUTPUT_FRAME_SIZE,
-				(icd->user_height << 16) | icd->user_width);
-
-		TC_VI_REG_WT(pcdev, TEGRA_VI_VI_ENABLE_2, 0x00000000);
-
-		/* Set the number of frames in the buffer. */
-		TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_COUNT_SECOND, 0x00000001);
-
-		/* Set up buffer frame size. */
-		TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_SIZE_SECOND,
-				(icd->user_height << 16) | icd->user_width);
-
-		TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_BUFFER_STRIDE_SECOND,
-				(icd->user_height * bytes_per_line));
-
-		TC_VI_REG_WT(pcdev, TEGRA_VI_CONT_SYNCPT_OUT_2,
-				(0x1 << 8) | /* Enable continuous syncpt */
-				TEGRA_VI_SYNCPT_VI);
-
-		TC_VI_REG_WT(pcdev, TEGRA_VI_VI_ENABLE_2, 0x00000000);
-	} else {
-		dev_err(&pcdev->ndev->dev, "Wrong output channel %d\n",
-			buf->output_channel);
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-static int tegra_camera_capture_setup(struct tegra_camera_dev *pcdev)
-{
-	struct vb2_buffer *vb = pcdev->active;
-	struct tegra_buffer *buf = to_tegra_vb(vb);
-	struct soc_camera_device *icd = buf->icd;
-	struct tegra_camera_platform_data *pdata = icd->link->priv;
-	int port = pdata->port;
-	const struct soc_camera_format_xlate *current_fmt = icd->current_fmt;
-	enum v4l2_mbus_pixelcode input_code = current_fmt->code;
-	u32 hdr, input_control = 0x0;
-
-	switch (input_code) {
-	case V4L2_MBUS_FMT_UYVY8_2X8:
-		input_control |= 0x2 << 8;
-		hdr = 30;
-		break;
-	case V4L2_MBUS_FMT_VYUY8_2X8:
-		input_control |= 0x3 << 8;
-		hdr = 30;
-		break;
-	case V4L2_MBUS_FMT_YUYV8_2X8:
-		input_control |= 0x0;
-		hdr = 30;
-		break;
-	case V4L2_MBUS_FMT_YVYU8_2X8:
-		input_control |= 0x1 << 8;
-		hdr = 30;
-		break;
-	case V4L2_MBUS_FMT_SBGGR8_1X8:
-		input_control |= 0x2 << 2;	/* Input Format = Bayer */
-		hdr = 42;
-		break;
-	case V4L2_MBUS_FMT_SBGGR10_1X10:
-		input_control |= 0x2 << 2;	/* Input Format = Bayer */
-		hdr = 43;
-		break;
-	default:
-		dev_err(&pcdev->ndev->dev, "Input format %d is not supported\n",
-			input_code);
-		return -EINVAL;
-	}
-
-	/*
-	 * Set up low pass filter.  Use 0x240 for chromaticity and 0x240
-	 * for luminance, which is the default and means not to touch
-	 * anything.
-	 */
-	TC_VI_REG_WT(pcdev, TEGRA_VI_H_LPF_CONTROL, 0x02400240);
-
-	/* Set up raise-on-edge, so we get an interrupt on end of frame. */
-	TC_VI_REG_WT(pcdev, TEGRA_VI_VI_RAISE, 0x00000001);
-
-	/* Cleanup registers */
-	tegra_camera_capture_clean(pcdev);
-
-	/* Setup registers for CSI-A, CSI-B and VIP inputs */
-	if (port == TEGRA_CAMERA_PORT_CSI_A)
-		tegra_camera_capture_setup_csi_a(pcdev, icd, hdr);
-	else if (port == TEGRA_CAMERA_PORT_CSI_B)
-		tegra_camera_capture_setup_csi_b(pcdev, icd, hdr);
-	else
-		tegra_camera_capture_setup_vip(pcdev, icd, input_control);
-
-	/* Setup registers for output channels */
-	return tegra_camera_capture_output_channel_setup(pcdev, icd);
-}
-
-static int tegra_camera_capture_buffer_setup(struct tegra_camera_dev *pcdev,
-			struct tegra_buffer *buf)
-{
-	struct soc_camera_device *icd = buf->icd;
-
-	switch (icd->current_fmt->host_fmt->fourcc) {
-	case V4L2_PIX_FMT_YUV420:
-	case V4L2_PIX_FMT_YVU420:
-		TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_BASE_ADDRESS_U,
-			     buf->buffer_addr_u);
-		TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_START_ADDRESS_U,
-			     buf->start_addr_u);
-
-		TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_BASE_ADDRESS_V,
-			     buf->buffer_addr_v);
-		TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_START_ADDRESS_V,
-			     buf->start_addr_v);
-
-	case V4L2_PIX_FMT_UYVY:
-	case V4L2_PIX_FMT_VYUY:
-	case V4L2_PIX_FMT_YUYV:
-	case V4L2_PIX_FMT_YVYU:
-	case V4L2_PIX_FMT_SBGGR8:
-	case V4L2_PIX_FMT_SBGGR10:
-		/* output 1 */
-		if (buf->output_channel == 0) {
-			TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_BASE_ADDRESS_FIRST,
-					buf->buffer_addr);
-			TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_START_ADDRESS_FIRST,
-					buf->start_addr);
-		/* output 2 */
-		} else if (buf->output_channel == 1) {
-			TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_BASE_ADDRESS_SECOND,
-					buf->buffer_addr);
-			TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_START_ADDRESS_SECOND,
-					buf->start_addr);
-		} else {
-			dev_err(&pcdev->ndev->dev, "Wrong output channel %d\n",
-				buf->output_channel);
-			return -EINVAL;
-		}
-	break;
-
-	default:
-		dev_err(&pcdev->ndev->dev, "Wrong host format %d\n",
-			icd->current_fmt->host_fmt->fourcc);
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-static int tegra_camera_capture_start(struct tegra_camera_dev *pcdev,
-				      struct tegra_buffer *buf)
-{
-	struct soc_camera_device *icd = buf->icd;
-	struct tegra_camera_platform_data *pdata = icd->link->priv;
-	int port = pdata->port;
-	int err;
-
-	err = tegra_camera_capture_buffer_setup(pcdev, buf);
-	if (err < 0)
-		return err;
-	/*
-	 * Only wait on CSI frame end syncpt if we're using CSI.  Otherwise,
-	 * wait on VIP VSYNC syncpt.
-	 */
-	if (port == TEGRA_CAMERA_PORT_CSI_A) {
-		pcdev->syncpt_csi_a++;
-		TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND,
-				0x0000f005);
-		err = nvhost_syncpt_wait_timeout_ext(pcdev->ndev,
-				TEGRA_VI_SYNCPT_CSI_A,
-				pcdev->syncpt_csi_a,
-				TEGRA_SYNCPT_CSI_WAIT_TIMEOUT,
-				NULL,
-				NULL);
-	} else if (port == TEGRA_CAMERA_PORT_CSI_B) {
-		pcdev->syncpt_csi_b++;
-		TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND,
-				0x0000f005);
-		err = nvhost_syncpt_wait_timeout_ext(pcdev->ndev,
-				TEGRA_VI_SYNCPT_CSI_B,
-				pcdev->syncpt_csi_b,
-				TEGRA_SYNCPT_CSI_WAIT_TIMEOUT,
-				NULL,
-				NULL);
-	} else {
-		pcdev->syncpt_vi++;
-		TC_VI_REG_WT(pcdev, TEGRA_VI_CAMERA_CONTROL,
-				0x00000001);
-		err = nvhost_syncpt_wait_timeout_ext(pcdev->ndev,
-				TEGRA_VI_SYNCPT_VI,
-				pcdev->syncpt_csi_a,
-				TEGRA_SYNCPT_VI_WAIT_TIMEOUT,
-				NULL,
-				NULL);
-	}
-
-	if (!err)
-		return 0;
-
-	if (tegra_camera_port_is_csi(port)) {
-		u32 ppstatus;
-		u32 cilstatus;
-		u32 rostatus;
-
-		dev_warn(&icd->vdev->dev, "Timeout on CSI syncpt\n");
-		dev_warn(&icd->vdev->dev, "buffer_addr = 0x%08x\n",
-			buf->buffer_addr);
-
-		ppstatus = TC_VI_REG_RD(pcdev,
-			TEGRA_CSI_CSI_PIXEL_PARSER_STATUS);
-		cilstatus = TC_VI_REG_RD(pcdev,
-			 TEGRA_CSI_CSI_CIL_STATUS);
-		rostatus = TC_VI_REG_RD(pcdev,
-			TEGRA_CSI_CSI_READONLY_STATUS);
-
-		dev_warn(&icd->vdev->dev,
-			"PPSTATUS = 0x%08x, "
-			"CILSTATUS = 0x%08x, "
-			"ROSTATUS = 0x%08x\n",
-			ppstatus, cilstatus, rostatus);
-	} else {
-		u32 vip_input_status;
-
-		dev_warn(&pcdev->ndev->dev, "Timeout on VI syncpt\n");
-		dev_warn(&pcdev->ndev->dev, "buffer_addr = 0x%08x\n",
-			buf->buffer_addr);
-
-		vip_input_status = TC_VI_REG_RD(pcdev,
-			TEGRA_VI_VIP_INPUT_STATUS);
-
-		dev_warn(&pcdev->ndev->dev,
-			"VIP_INPUT_STATUS = 0x%08x\n",
-			vip_input_status);
-	}
-
-	return err;
-}
-
-static int tegra_camera_capture_stop(struct tegra_camera_dev *pcdev, int port)
-{
-	int err;
-	struct tegra_buffer *buf = to_tegra_vb(pcdev->active);
-
-	if (port == TEGRA_CAMERA_PORT_CSI_A)
-		TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND,
-			     0x0000f002);
-	else if (port == TEGRA_CAMERA_PORT_CSI_B)
-		TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND,
-			     0x0000f002);
-	else
-		TC_VI_REG_WT(pcdev, TEGRA_VI_CAMERA_CONTROL,
-			     0x00000005);
-
-	if (tegra_camera_port_is_csi(port))
-		err = nvhost_syncpt_wait_timeout_ext(pcdev->ndev,
-			TEGRA_VI_SYNCPT_VI,
-			pcdev->syncpt_vi,
-			TEGRA_SYNCPT_VI_WAIT_TIMEOUT,
-			NULL,
-			NULL);
-	else
-		err = 0;
-
-	if (err) {
-		u32 buffer_addr;
-		u32 ppstatus;
-		u32 cilstatus;
-
-		dev_warn(&pcdev->ndev->dev, "Timeout on VI syncpt\n");
-
-		if (buf->output_channel == 0)
-			buffer_addr = TC_VI_REG_RD(pcdev,
-					   TEGRA_VI_VB0_BASE_ADDRESS_FIRST);
-		else if (buf->output_channel == 1)
-			buffer_addr = TC_VI_REG_RD(pcdev,
-					   TEGRA_VI_VB0_BASE_ADDRESS_SECOND);
-		else {
-			dev_err(&pcdev->ndev->dev, "Wrong output channel %d\n",
-				buf->output_channel);
-			return -EINVAL;
-		}
-
-		dev_warn(&pcdev->ndev->dev, "buffer_addr = 0x%08x\n",
-			buffer_addr);
-
-		ppstatus = TC_VI_REG_RD(pcdev,
-					TEGRA_CSI_CSI_PIXEL_PARSER_STATUS);
-		cilstatus = TC_VI_REG_RD(pcdev,
-					 TEGRA_CSI_CSI_CIL_STATUS);
-		dev_warn(&pcdev->ndev->dev,
-			"PPSTATUS = 0x%08x, CILSTATUS = 0x%08x\n",
-			ppstatus, cilstatus);
-	}
-
-	return err;
-}
-
-static void tegra_camera_activate(struct tegra_camera_dev *pcdev)
-{
-		nvhost_module_busy_ext(pcdev->ndev);
-
-	/* Enable external power */
-	regulator_enable(pcdev->reg);
-
-	/*
-	 * Powergating DIS must powergate VE partition. Camera
-	 * module needs to increase the ref-count of disa to
-	 * avoid itself powergated by DIS inadvertently.
-	 */
-#if defined(CONFIG_ARCH_TEGRA_11x_SOC) || defined(CONFIG_ARCH_TEGRA_14x_SOC)
-	tegra_unpowergate_partition(TEGRA_POWERGATE_DISA);
-#endif
-	/* Unpowergate VE */
-	tegra_unpowergate_partition(TEGRA_POWERGATE_VENC);
-
-	/* Turn on relevant clocks. */
-	clk_set_rate(pcdev->clk_vi, 150000000);
-	clk_prepare_enable(pcdev->clk_vi);
-	clk_set_rate(pcdev->clk_vi_sensor, 24000000);
-	clk_prepare_enable(pcdev->clk_vi_sensor);
-	clk_prepare_enable(pcdev->clk_csi);
-	clk_prepare_enable(pcdev->clk_isp);
-	clk_prepare_enable(pcdev->clk_csus);
-	clk_set_rate(pcdev->clk_sclk, 80000000);
-	clk_prepare_enable(pcdev->clk_sclk);
-	clk_set_rate(pcdev->clk_sclk, 375000000);
-	clk_prepare_enable(pcdev->clk_emc);
-
-	/* Save current syncpt values. */
-	tegra_camera_save_syncpts(pcdev);
-}
-
-static void tegra_camera_deactivate(struct tegra_camera_dev *pcdev)
-{
-	/* Turn off relevant clocks. */
-	clk_disable_unprepare(pcdev->clk_vi);
-	clk_disable_unprepare(pcdev->clk_vi_sensor);
-	clk_disable_unprepare(pcdev->clk_csi);
-	clk_disable_unprepare(pcdev->clk_isp);
-	clk_disable_unprepare(pcdev->clk_csus);
-	clk_disable_unprepare(pcdev->clk_sclk);
-	clk_disable_unprepare(pcdev->clk_emc);
-
-	/* Powergate VE */
-	tegra_powergate_partition(TEGRA_POWERGATE_VENC);
-#if defined(CONFIG_ARCH_TEGRA_11x_SOC) || defined(CONFIG_ARCH_TEGRA_14x_SOC)
-	tegra_powergate_partition(TEGRA_POWERGATE_DISA);
-#endif
-
-	/* Disable external power */
-	regulator_disable(pcdev->reg);
-
-	nvhost_module_idle_ext(pcdev->ndev);
-}
-
-static int tegra_camera_capture_frame(struct tegra_camera_dev *pcdev)
-{
-	struct vb2_buffer *vb = pcdev->active;
-	struct tegra_buffer *buf = to_tegra_vb(vb);
-	struct soc_camera_device *icd = buf->icd;
-	struct tegra_camera_platform_data *pdata = icd->link->priv;
-	int port = pdata->port;
-	int retry = TEGRA_SYNCPT_RETRY_COUNT;
-	int err;
-
-	while (retry) {
-		err = tegra_camera_capture_start(pcdev, buf);
-		/* Capturing succeed, stop capturing */
-		if (!err)
-			err = tegra_camera_capture_stop(pcdev, port);
-		/* Capturing failed, stop and retry */
-		else {
-			retry--;
-
-			/* Stop streaming. */
-			if (port == TEGRA_CAMERA_PORT_CSI_A) {
-				TC_VI_REG_WT(pcdev,
-					     TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND,
-					     0x0000f002);
-				/* Clear status registers. */
-				TC_VI_REG_WT(pcdev,
-					     TEGRA_CSI_CSI_PIXEL_PARSER_STATUS,
-					     0xffffffff);
-				TC_VI_REG_WT(pcdev,
-					     TEGRA_CSI_CSI_CIL_STATUS,
-					     0xffffffff);
-			} else if (port == TEGRA_CAMERA_PORT_CSI_B) {
-				TC_VI_REG_WT(pcdev,
-					     TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND,
-					     0x0000f002);
-				/* Clear status registers. */
-				TC_VI_REG_WT(pcdev,
-					     TEGRA_CSI_CSI_PIXEL_PARSER_STATUS,
-					     0xffffffff);
-				TC_VI_REG_WT(pcdev,
-					     TEGRA_CSI_CSI_CIL_STATUS,
-					     0xffffffff);
-			} else {
-				TC_VI_REG_WT(pcdev,
-					     TEGRA_VI_CAMERA_CONTROL,
-					     0x00000005);
-			}
-
-			tegra_camera_incr_syncpts(pcdev);
-			tegra_camera_save_syncpts(pcdev);
-
-			continue;
-		}
-
-		break;
-	}
-
-	/* Reset hardware for too many errors */
-	if (!retry) {
-		tegra_camera_deactivate(pcdev);
-		mdelay(5);
-		tegra_camera_activate(pcdev);
-		if (pcdev->active)
-			tegra_camera_capture_setup(pcdev);
-	}
-
-	spin_lock_irq(&pcdev->videobuf_queue_lock);
-
-	do_gettimeofday(&vb->v4l2_buf.timestamp);
-	vb->v4l2_buf.field = pcdev->field;
-	if (port == TEGRA_CAMERA_PORT_CSI_A)
-		vb->v4l2_buf.sequence = pcdev->sequence_a++;
-	else if (port == TEGRA_CAMERA_PORT_CSI_B)
-		vb->v4l2_buf.sequence = pcdev->sequence_b++;
-
-	vb2_buffer_done(vb, err < 0 ? VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
-	list_del_init(&buf->queue);
-
-	pcdev->num_frames++;
-
-	spin_unlock_irq(&pcdev->videobuf_queue_lock);
-
-	return err;
-}
-
-static void tegra_camera_work(struct work_struct *work)
-{
-	struct tegra_camera_dev *pcdev =
-		container_of(work, struct tegra_camera_dev, work);
-	struct tegra_buffer *buf;
-
-	while (1) {
-		mutex_lock(&pcdev->work_mutex);
-
-		spin_lock_irq(&pcdev->videobuf_queue_lock);
-		if (list_empty(&pcdev->capture)) {
-			pcdev->active = NULL;
-			spin_unlock_irq(&pcdev->videobuf_queue_lock);
-			mutex_unlock(&pcdev->work_mutex);
-			return;
-		}
-
-		buf = list_entry(pcdev->capture.next, struct tegra_buffer,
-				queue);
-		pcdev->active = &buf->vb;
-		spin_unlock_irq(&pcdev->videobuf_queue_lock);
-
-		tegra_camera_capture_setup(pcdev);
-		tegra_camera_capture_frame(pcdev);
-
-		mutex_unlock(&pcdev->work_mutex);
-	}
-}
-
-static int tegra_camera_init_buffer(struct tegra_buffer *buf)
-{
-	struct soc_camera_device *icd = buf->icd;
-	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
-						icd->current_fmt->host_fmt);
-	struct tegra_camera_platform_data *pdata = icd->link->priv;
-
-	switch (icd->current_fmt->host_fmt->fourcc) {
-	case V4L2_PIX_FMT_UYVY:
-	case V4L2_PIX_FMT_VYUY:
-	case V4L2_PIX_FMT_YUYV:
-	case V4L2_PIX_FMT_YVYU:
-	case V4L2_PIX_FMT_SBGGR8:
-	case V4L2_PIX_FMT_SBGGR10:
-		buf->buffer_addr = vb2_dma_contig_plane_dma_addr(&buf->vb, 0);
-		buf->start_addr = buf->buffer_addr;
-
-		if (pdata->flip_v)
-			buf->start_addr += bytes_per_line *
-					   (icd->user_height-1);
-
-		if (pdata->flip_h)
-			buf->start_addr += bytes_per_line - 1;
-
-		break;
-
-	case V4L2_PIX_FMT_YUV420:
-	case V4L2_PIX_FMT_YVU420:
-		buf->buffer_addr = vb2_dma_contig_plane_dma_addr(&buf->vb, 0);
-		buf->buffer_addr_u = buf->buffer_addr +
-				     icd->user_width * icd->user_height;
-		buf->buffer_addr_v = buf->buffer_addr_u +
-				     (icd->user_width * icd->user_height) / 4;
-
-		/* For YVU420, we swap the locations of the U and V planes. */
-		if (icd->current_fmt->host_fmt->fourcc == V4L2_PIX_FMT_YVU420) {
-			dma_addr_t temp = buf->buffer_addr_u;
-			buf->buffer_addr_u = buf->buffer_addr_v;
-			buf->buffer_addr_v = temp;
-		}
-
-		buf->start_addr = buf->buffer_addr;
-		buf->start_addr_u = buf->buffer_addr_u;
-		buf->start_addr_v = buf->buffer_addr_v;
-
-		if (pdata->flip_v) {
-			buf->start_addr += icd->user_width *
-					   (icd->user_height - 1);
-
-			buf->start_addr_u += ((icd->user_width/2) *
-					      ((icd->user_height/2) - 1));
-
-			buf->start_addr_v += ((icd->user_width/2) *
-					      ((icd->user_height/2) - 1));
-		}
-
-		if (pdata->flip_h) {
-			buf->start_addr += icd->user_width - 1;
-
-			buf->start_addr_u += (icd->user_width/2) - 1;
-
-			buf->start_addr_v += (icd->user_width/2) - 1;
-		}
-
-		break;
-
-	default:
-		dev_err(icd->parent, "Wrong host format %d\n",
-			icd->current_fmt->host_fmt->fourcc);
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-/*
- *  Videobuf operations
- */
-static int tegra_camera_videobuf_setup(struct vb2_queue *vq,
-				       const struct v4l2_format *fmt,
-				       unsigned int *num_buffers,
-				       unsigned int *num_planes,
-				       unsigned int sizes[],
-				       void *alloc_ctxs[])
-{
-	struct soc_camera_device *icd = container_of(vq,
-						     struct soc_camera_device,
-						     vb2_vidq);
-	struct tegra_camera_platform_data *pdata = icd->link->priv;
-	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-	struct tegra_camera_dev *pcdev = ici->priv;
-	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
-						icd->current_fmt->host_fmt);
-	if (bytes_per_line < 0)
-		return bytes_per_line;
-
-	*num_planes = 1;
-
-	if (pdata->port == TEGRA_CAMERA_PORT_CSI_A)
-		pcdev->sequence_a = 0;
-	else if (pdata->port == TEGRA_CAMERA_PORT_CSI_B)
-		pcdev->sequence_b = 0;
-	sizes[0] = bytes_per_line * icd->user_height;
-	alloc_ctxs[0] = pcdev->alloc_ctx;
-
-	if (!*num_buffers)
-		*num_buffers = 2;
-
-	return 0;
-}
-
-static int tegra_camera_videobuf_prepare(struct vb2_buffer *vb)
-{
-	struct soc_camera_device *icd = container_of(vb->vb2_queue,
-						     struct soc_camera_device,
-						     vb2_vidq);
-	struct tegra_buffer *buf = to_tegra_vb(vb);
-	struct tegra_camera_platform_data *pdata = icd->link->priv;
-	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
-						icd->current_fmt->host_fmt);
-	unsigned long size;
-
-	if (bytes_per_line < 0)
-		return bytes_per_line;
-
-	buf->icd = icd;
-
-	if (!pdata) {
-		dev_err(icd->parent, "No platform data for this device!\n");
-		return -EINVAL;
-	}
-
-	if (!tegra_camera_port_is_valid(pdata->port)) {
-		dev_err(icd->parent,
-			"Invalid camera port %d in platform data\n",
-			pdata->port);
-		return -EINVAL;
-	}
-
-	dev_dbg(icd->parent, "%s (vb=0x%p) 0x%p %lu\n", __func__,
-		vb, vb2_plane_vaddr(vb, 0), vb2_plane_size(vb, 0));
-
-#ifdef PREFILL_BUFFER
-	/*
-	 * 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), 0xbd, vb2_plane_size(vb, 0));
-#endif
-
-	if (!icd->current_fmt) {
-		dev_err(icd->parent, "%s NULL format point\n", __func__);
-		return -EINVAL;
-	}
-
-	size = icd->user_height * bytes_per_line;
-
-	if (vb2_plane_size(vb, 0) < size) {
-		dev_err(icd->parent, "Buffer too small (%lu < %lu)\n",
-			vb2_plane_size(vb, 0), size);
-		return -ENOBUFS;
-	}
-
-	vb2_set_plane_payload(vb, 0, size);
-
-	return tegra_camera_init_buffer(buf);
-}
-
-static void tegra_camera_videobuf_queue(struct vb2_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 tegra_camera_dev *pcdev = ici->priv;
-	struct tegra_buffer *buf = to_tegra_vb(vb);
-
-	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(&pcdev->videobuf_queue_lock);
-	list_add_tail(&buf->queue, &pcdev->capture);
-	schedule_work(&pcdev->work);
-	spin_unlock_irq(&pcdev->videobuf_queue_lock);
-
-	dev_dbg(icd->parent, "Finished tegra_camera_videobuf_queue()\n");
-}
-
-static void tegra_camera_videobuf_release(struct vb2_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 tegra_buffer *buf = to_tegra_vb(vb);
-	struct tegra_camera_dev *pcdev = ici->priv;
-
-	dev_dbg(icd->parent, "In tegra_camera_videobuf_release()\n");
-
-	mutex_lock(&pcdev->work_mutex);
-
-	spin_lock_irq(&pcdev->videobuf_queue_lock);
-
-	if (pcdev->active == vb)
-		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);
-
-	spin_unlock_irq(&pcdev->videobuf_queue_lock);
-
-	mutex_unlock(&pcdev->work_mutex);
-
-	dev_dbg(icd->parent, "Finished tegra_camera_videobuf_release()\n");
-}
-
-static int tegra_camera_videobuf_init(struct vb2_buffer *vb)
-{
-	/* This is for locking debugging only */
-	INIT_LIST_HEAD(&to_tegra_vb(vb)->queue);
-
-	return 0;
-}
-
-static int tegra_camera_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 tegra_camera_dev *pcdev = ici->priv;
-	struct list_head *buf_head, *tmp;
-
-
-	mutex_lock(&pcdev->work_mutex);
-
-	spin_lock_irq(&pcdev->videobuf_queue_lock);
-	list_for_each_safe(buf_head, tmp, &pcdev->capture) {
-		struct tegra_buffer *buf = container_of(buf_head,
-				struct tegra_buffer,
-				queue);
-		if (buf->icd == icd)
-			list_del_init(buf_head);
-	}
-	spin_unlock_irq(&pcdev->videobuf_queue_lock);
-
-	if (pcdev->active) {
-		struct tegra_buffer *buf = to_tegra_vb(pcdev->active);
-		if (buf->icd == icd)
-			pcdev->active = NULL;
-	}
-
-	mutex_unlock(&pcdev->work_mutex);
-
-	return 0;
-}
-
-static struct vb2_ops tegra_camera_videobuf_ops = {
-	.queue_setup	= tegra_camera_videobuf_setup,
-	.buf_prepare	= tegra_camera_videobuf_prepare,
-	.buf_queue	= tegra_camera_videobuf_queue,
-	.buf_cleanup	= tegra_camera_videobuf_release,
-	.buf_init	= tegra_camera_videobuf_init,
-	.wait_prepare	= soc_camera_unlock,
-	.wait_finish	= soc_camera_lock,
-	.stop_streaming	= tegra_camera_stop_streaming,
-};
-
-/*
- *  SOC camera host operations
- */
-static int tegra_camera_init_videobuf(struct vb2_queue *q,
-				      struct soc_camera_device *icd)
-{
-	dev_dbg(icd->parent, "In tegra_camera_init_videobuf()\n");
-
-	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	q->io_modes = VB2_MMAP | VB2_USERPTR;
-	q->drv_priv = icd;
-	q->ops = &tegra_camera_videobuf_ops;
-	q->mem_ops = &vb2_dma_contig_memops;
-	q->buf_struct_size = sizeof(struct tegra_buffer);
-
-	dev_dbg(icd->parent, "Finished tegra_camera_init_videobuf()\n");
-
-	return vb2_queue_init(q);
-}
-
-/*
- * Called with .video_lock held
- */
-static int tegra_camera_add_device(struct soc_camera_device *icd)
-{
-	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-	struct tegra_camera_dev *pcdev = ici->priv;
-
-	if (!pcdev->enable_refcnt) {
-		pm_runtime_get_sync(ici->v4l2_dev.dev);
-		tegra_camera_activate(pcdev);
-		pcdev->num_frames = 0;
-	}
-	pcdev->enable_refcnt++;
-
-	dev_dbg(icd->parent, "TEGRA Camera host attached to camera %d\n",
-		icd->devnum);
-
-	return 0;
-}
-
-/* Called with .video_lock held */
-static void tegra_camera_remove_device(struct soc_camera_device *icd)
-{
-	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
-	struct tegra_camera_dev *pcdev = ici->priv;
-
-	pcdev->enable_refcnt--;
-	if (!pcdev->enable_refcnt) {
-		cancel_work_sync(&pcdev->work);
-		tegra_camera_deactivate(pcdev);
-		pm_runtime_put_sync(ici->v4l2_dev.dev);
-	}
-
-	dev_dbg(icd->parent, "Frames captured: %d\n", pcdev->num_frames);
-
-	dev_dbg(icd->parent, "TEGRA camera host detached from camera %d\n",
-		icd->devnum);
-}
-
-static int tegra_camera_set_bus_param(struct soc_camera_device *icd)
-{
-	return 0;
-}
-
-static int tegra_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;
-	int ret;
-	enum v4l2_mbus_pixelcode code;
-	const struct soc_mbus_pixelfmt *fmt;
-	int k;
-
-	ret = v4l2_subdev_call(sd, video, enum_mbus_fmt, idx, &code);
-	if (ret != 0)
-		/* No more formats */
-		return 0;
-
-	fmt = soc_mbus_get_fmtdesc(code);
-	if (!fmt) {
-		dev_err(dev, "Invalid format code #%u: %d\n", idx, code);
-		return 0;
-	}
-
-	switch (code) {
-	case V4L2_MBUS_FMT_UYVY8_2X8:
-	case V4L2_MBUS_FMT_VYUY8_2X8:
-	case V4L2_MBUS_FMT_YUYV8_2X8:
-	case V4L2_MBUS_FMT_YVYU8_2X8:
-	case V4L2_MBUS_FMT_SBGGR8_1X8:
-	case V4L2_MBUS_FMT_SBGGR10_1X10:
-		formats += ARRAY_SIZE(tegra_camera_formats);
-		for (k = 0;
-		     xlate && (k < ARRAY_SIZE(tegra_camera_formats));
-		     k++) {
-			xlate->host_fmt	= &tegra_camera_formats[k];
-			xlate->code	= code;
-			xlate++;
-
-			dev_info(dev, "Providing format %s using code %d\n",
-				 tegra_camera_formats[k].name, code);
-		}
-		break;
-	default:
-		dev_info(dev, "Not supporting %s\n", fmt->name);
-		return 0;
-	}
-
-	return formats;
-}
-
-static void tegra_camera_put_formats(struct soc_camera_device *icd)
-{
-	kfree(icd->host_priv);
-	icd->host_priv = NULL;
-}
-
-static int tegra_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 tegra_camera_dev *pcdev = ici->priv;
-
-	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-	const struct soc_camera_format_xlate *xlate = NULL;
-	struct v4l2_pix_format *pix = &f->fmt.pix;
-	struct v4l2_mbus_framefmt mf;
-	int ret;
-
-	dev_dbg(dev, "In tegra_camera_set_fmt()\n");
-
-	xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
-	if (!xlate) {
-		dev_warn(dev, "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, video, s_mbus_fmt, &mf);
-	if (IS_ERR_VALUE(ret)) {
-		dev_warn(dev, "Failed to configure for format %x\n",
-			 pix->pixelformat);
-		return ret;
-	}
-
-	if (mf.code != xlate->code) {
-		dev_warn(dev, "mf.code = %d, xlate->code = %d, mismatch\n",
-			mf.code, xlate->code);
-		return -EINVAL;
-	}
-
-	icd->user_width		= mf.width;
-	icd->user_height	= mf.height;
-	icd->current_fmt	= xlate;
-
-	pcdev->field = pix->field;
-
-	dev_dbg(dev, "Finished tegra_camera_set_fmt(), returning %d\n", ret);
-
-	return ret;
-}
-
-static int tegra_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_mbus_framefmt mf;
-	__u32 pixfmt = pix->pixelformat;
-	int ret;
-
-	dev_dbg(icd->parent, "In tegra_camera_try_fmt()\n");
-
-	xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
-	if (!xlate) {
-		dev_warn(icd->parent, "Format %x not found\n", pixfmt);
-		return -EINVAL;
-	}
-
-	pix->bytesperline = soc_mbus_bytes_per_line(pix->width,
-						    xlate->host_fmt);
-	if (pix->bytesperline < 0)
-		return pix->bytesperline;
-	pix->sizeimage = pix->height * pix->bytesperline;
-
-	/* 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, video, try_mbus_fmt, &mf);
-	if (IS_ERR_VALUE(ret))
-		return ret;
-
-	pix->width	= mf.width;
-	pix->height	= mf.height;
-	pix->colorspace	= mf.colorspace;
-	/*
-	 * width and height could have been changed, therefore update the
-	 * bytesperline and sizeimage here.
-	 */
-	pix->bytesperline = soc_mbus_bytes_per_line(pix->width,
-						    xlate->host_fmt);
-	pix->sizeimage = pix->height * pix->bytesperline;
-
-	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;
-	}
-
-	dev_dbg(icd->parent,
-		"Finished tegra_camera_try_fmt(), returning %d\n", ret);
-
-	return ret;
-}
-
-static int tegra_camera_reqbufs(struct soc_camera_device *icd,
-				struct v4l2_requestbuffers *p)
-{
-	return 0;
-}
-
-static unsigned int tegra_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 tegra_camera_querycap(struct soc_camera_host *ici,
-				 struct v4l2_capability *cap)
-{
-	strlcpy(cap->card, TEGRA_CAM_DRV_NAME, sizeof(cap->card));
-	cap->version = TEGRA_CAM_VERSION_CODE;
-	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
-
-	return 0;
-}
-
-static struct soc_camera_host_ops tegra_soc_camera_host_ops = {
-	.owner		= THIS_MODULE,
-	.init_videobuf2	= tegra_camera_init_videobuf,
-	.add		= tegra_camera_add_device,
-	.remove		= tegra_camera_remove_device,
-	.set_bus_param	= tegra_camera_set_bus_param,
-	.get_formats	= tegra_camera_get_formats,
-	.put_formats	= tegra_camera_put_formats,
-	.set_fmt	= tegra_camera_set_fmt,
-	.try_fmt	= tegra_camera_try_fmt,
-	.reqbufs	= tegra_camera_reqbufs,
-	.poll		= tegra_camera_poll,
-	.querycap	= tegra_camera_querycap,
-};
-
-static struct of_device_id tegra_vi_of_match[] = {
-#ifdef TEGRA_2X_OR_HIGHER_CONFIG
-	{ .compatible = "nvidia,tegra20-vi",
-		.data = (struct nvhost_device_data *)&t20_vi_info },
-#endif
-#ifdef TEGRA_3X_OR_HIGHER_CONFIG
-	{ .compatible = "nvidia,tegra30-vi",
-		.data = (struct nvhost_device_data *)&t30_vi_info },
-#endif
-#ifdef TEGRA_11X_OR_HIGHER_CONFIG
-	{ .compatible = "nvidia,tegra114-vi",
-		.data = (struct nvhost_device_data *)&t11_vi_info },
-#endif
-#ifdef TEGRA_14X_OR_HIGHER_CONFIG
-	{ .compatible = "nvidia,tegra148-vi",
-		.data = (struct nvhost_device_data *)&t14_vi_info },
-#endif
-	{ },
-};
-
-static int tegra_camera_probe(struct platform_device *pdev)
-{
-	struct tegra_camera_dev *pcdev;
-	struct nvhost_device_data *ndata = NULL;
-	int err = 0;
-
-	if (pdev->dev.of_node) {
-		const struct of_device_id *match;
-
-		match = of_match_device(tegra_vi_of_match, &pdev->dev);
-		if (match)
-			ndata = match->data;
-	} else
-		ndata = pdev->dev.platform_data;
-
-	if (!ndata) {
-		dev_err(&pdev->dev, "No nvhost device data!\n");
-		err = -EINVAL;
-		goto exit;
-	}
-
-	pcdev = kzalloc(sizeof(struct tegra_camera_dev), GFP_KERNEL);
-	if (!pcdev) {
-		dev_err(&pdev->dev, "Could not allocate pcdev\n");
-		err = -ENOMEM;
-		goto exit;
-	}
-
-	pcdev->ndata = ndata;
-	pcdev->ndev = pdev;
-
-	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 = &tegra_soc_camera_host_ops;
-
-	INIT_LIST_HEAD(&pcdev->capture);
-	INIT_WORK(&pcdev->work, tegra_camera_work);
-	spin_lock_init(&pcdev->videobuf_queue_lock);
-	mutex_init(&pcdev->work_mutex);
-
-	pcdev->clk_vi = clk_get(&pdev->dev, "vi");
-	if (IS_ERR_OR_NULL(pcdev->clk_vi)) {
-		dev_err(&pdev->dev, "Failed to get vi clock.\n");
-		goto exit_free_pcdev;
-	}
-
-	pcdev->clk_vi_sensor = clk_get(&pdev->dev, "vi_sensor");
-	if (IS_ERR_OR_NULL(pcdev->clk_vi_sensor)) {
-		dev_err(&pdev->dev, "Failed to get vi_sensor clock.\n");
-		goto exit_put_clk_vi;
-	}
-
-	pcdev->clk_csi = clk_get(&pdev->dev, "csi");
-	if (IS_ERR_OR_NULL(pcdev->clk_csi)) {
-		dev_err(&pdev->dev, "Failed to get csi clock.\n");
-		goto exit_put_clk_vi_sensor;
-	}
-
-	pcdev->clk_isp = clk_get(&pdev->dev, "isp");
-	if (IS_ERR_OR_NULL(pcdev->clk_isp)) {
-		dev_err(&pdev->dev, "Failed to get isp clock.\n");
-		goto exit_put_clk_csi;
-	}
-
-	pcdev->clk_csus = clk_get(&pdev->dev, "csus");
-	if (IS_ERR_OR_NULL(pcdev->clk_csus)) {
-		dev_err(&pdev->dev, "Failed to get csus clock.\n");
-		goto exit_put_clk_isp;
-	}
-
-	pcdev->clk_sclk = clk_get(&pdev->dev, "sclk");
-	if (IS_ERR_OR_NULL(pcdev->clk_sclk)) {
-		dev_err(&pdev->dev, "Failed to get sclk clock.\n");
-		goto exit_put_clk_csus;
-	}
-
-	pcdev->clk_emc = clk_get(&pdev->dev, "emc");
-	if (IS_ERR_OR_NULL(pcdev->clk_emc)) {
-		dev_err(&pdev->dev, "Failed to get emc clock.\n");
-		goto exit_put_clk_sclk;
-	}
-
-	clk_set_rate(pcdev->clk_vi, 150000000);
-	clk_set_rate(pcdev->clk_vi_sensor, 24000000);
-
-	/* Get regulator pointer */
-#ifdef CONFIG_ARCH_TEGRA_2x_SOC
-	pcdev->reg = regulator_get(&pdev->dev, "vcsi");
-#else
-	pcdev->reg = regulator_get(&pdev->dev, "avdd_dsi_csi");
-#endif
-	if (IS_ERR_OR_NULL(pcdev->reg)) {
-		dev_err(&pdev->dev, "%s: couldn't get regulator\n",
-				__func__);
-		goto exit_put_clk_emc;
-	}
-
-	platform_set_drvdata(pdev, ndata);
-	err = nvhost_client_device_get_resources(pdev);
-	if (err) {
-		dev_err(&pdev->dev, "%s: nvhost get resources failed %d\n",
-				__func__, err);
-		goto exit_put_regulator;
-	}
-
-	err = nvhost_client_device_init(pdev);
-	if (err) {
-		dev_err(&pdev->dev, "%s: nvhost init failed %d\n",
-				__func__, err);
-		goto exit_put_regulator;
-	}
-
-	pcdev->vi_base = ndata->aperture[0];
-
-	tegra_pd_add_device(&pdev->dev);
-	pm_runtime_use_autosuspend(&pdev->dev);
-	pm_runtime_set_autosuspend_delay(&pdev->dev, ndata->clockgate_delay);
-	pm_runtime_enable(&pdev->dev);
-
-	pcdev->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
-	if (IS_ERR(pcdev->alloc_ctx)) {
-		err = PTR_ERR(pcdev->alloc_ctx);
-		goto exit_pm_disable;
-	}
-
-	platform_set_drvdata(pdev, pcdev);
-	err = soc_camera_host_register(&pcdev->ici);
-	if (IS_ERR_VALUE(err))
-		goto exit_cleanup_alloc_ctx;
-
-	dev_notice(&pdev->dev, "Tegra camera driver loaded.\n");
-
-	return err;
-
-exit_cleanup_alloc_ctx:
-	platform_set_drvdata(pdev, pcdev->ndata);
-	vb2_dma_contig_cleanup_ctx(pcdev->alloc_ctx);
-exit_pm_disable:
-	pm_runtime_disable(&pdev->dev);
-exit_put_regulator:
-	regulator_put(pcdev->reg);
-exit_put_clk_emc:
-	clk_put(pcdev->clk_emc);
-exit_put_clk_sclk:
-	clk_put(pcdev->clk_sclk);
-exit_put_clk_csus:
-	clk_put(pcdev->clk_csus);
-exit_put_clk_isp:
-	clk_put(pcdev->clk_isp);
-exit_put_clk_csi:
-	clk_put(pcdev->clk_csi);
-exit_put_clk_vi_sensor:
-	clk_put(pcdev->clk_vi_sensor);
-exit_put_clk_vi:
-	clk_put(pcdev->clk_vi);
-exit_free_pcdev:
-	kfree(pcdev);
-exit:
-	return err;
-}
-
-static int tegra_camera_remove(struct platform_device *pdev)
-{
-	struct soc_camera_host *ici = to_soc_camera_host(&pdev->dev);
-	struct tegra_camera_dev *pcdev = container_of(ici,
-					struct tegra_camera_dev, ici);
-
-	soc_camera_host_unregister(ici);
-
-	platform_set_drvdata(pdev, pcdev->ndata);
-	nvhost_client_device_release(pdev);
-
-	vb2_dma_contig_cleanup_ctx(pcdev->alloc_ctx);
-
-	pm_runtime_disable(&pdev->dev);
-
-	regulator_put(pcdev->reg);
-
-	clk_put(pcdev->clk_emc);
-	clk_put(pcdev->clk_sclk);
-	clk_put(pcdev->clk_csus);
-	clk_put(pcdev->clk_isp);
-	clk_put(pcdev->clk_csi);
-	clk_put(pcdev->clk_vi_sensor);
-	clk_put(pcdev->clk_vi);
-
-	kfree(pcdev);
-
-	dev_notice(&pdev->dev, "Tegra camera host driver unloaded\n");
-
-	return 0;
-}
-
-#ifdef CONFIG_PM_FISH
-static int tegra_camera_suspend(struct platform_device *pdev,
-		pm_message_t state)
-{
-	struct soc_camera_host *ici = to_soc_camera_host(&pdev->dev);
-	struct tegra_camera_dev *pcdev = container_of(ici,
-					struct tegra_camera_dev, ici);
-
-	mutex_lock(&pcdev->work_mutex);
-
-	/* We only need to do something if a camera sensor is attached. */
-	if (pcdev->icd) {
-		/* Suspend the camera sensor. */
-		WARN_ON(!pcdev->icd->ops->suspend);
-		pcdev->icd->ops->suspend(pcdev->icd, state);
-	}
-
-	return 0;
-}
-
-static int tegra_camera_resume(struct platform_device *pdev)
-{
-	struct soc_camera_host *ici = to_soc_camera_host(&pdev->dev);
-	struct tegra_camera_dev *pcdev = container_of(ici,
-					struct tegra_camera_dev, ici);
-
-	/* We only need to do something if a camera sensor is attached. */
-	if (pcdev->icd) {
-		/* Resume the camera host. */
-		tegra_camera_save_syncpts(pcdev);
-		if (pcdev->active)
-			tegra_camera_capture_setup(pcdev);
-
-		/* Resume the camera sensor. */
-		WARN_ON(!pcdev->icd->ops->resume);
-		pcdev->icd->ops->resume(pcdev->icd);
-	}
-
-	mutex_unlock(&pcdev->work_mutex);
-
-	return 0;
-}
-#endif
-
-static struct platform_driver tegra_camera_driver = {
-	.driver	= {
-		.name	= TEGRA_CAM_DRV_NAME,
-		.owner	= THIS_MODULE,
-#ifdef CONFIG_OF
-		.of_match_table = tegra_vi_of_match,
-#endif
-	},
-	.probe		= tegra_camera_probe,
-	.remove		= tegra_camera_remove,
-#ifdef CONFIG_PM_FISH
-	.suspend	= tegra_camera_suspend,
-	.resume		= tegra_camera_resume,
-#endif
-};
-
-
-static int __init tegra_camera_init(void)
-{
-	return platform_driver_register(&tegra_camera_driver);
-}
-
-static void __exit tegra_camera_exit(void)
-{
-	platform_driver_unregister(&tegra_camera_driver);
-}
-
-module_init(tegra_camera_init);
-module_exit(tegra_camera_exit);
-
-MODULE_DESCRIPTION("TEGRA SoC Camera Host driver");
-MODULE_AUTHOR("Andrew Chew <achew@nvidia.com>");
-MODULE_AUTHOR("Bryan Wu <pengw@nvidia.com>");
-MODULE_LICENSE("GPL v2");
-MODULE_ALIAS("nvhost:" TEGRA_CAM_DRV_NAME);
diff --git a/include/media/tegra_v4l2_camera.h b/include/media/tegra_v4l2_camera.h
index 9e4b65c362b3..7e25f97531ee 100644
--- a/include/media/tegra_v4l2_camera.h
+++ b/include/media/tegra_v4l2_camera.h
@@ -23,6 +23,9 @@
 enum tegra_camera_port {
 	TEGRA_CAMERA_PORT_CSI_A = 1,
 	TEGRA_CAMERA_PORT_CSI_B,
+	TEGRA_CAMERA_PORT_CSI_C,
+	TEGRA_CAMERA_PORT_CSI_D,
+	TEGRA_CAMERA_PORT_CSI_E,
 	TEGRA_CAMERA_PORT_VIP,
 };
 
diff --git a/include/uapi/linux/v4l2-mediabus.h b/include/uapi/linux/v4l2-mediabus.h
index 6ee63d09b32d..b1fedf8a5409 100644
--- a/include/uapi/linux/v4l2-mediabus.h
+++ b/include/uapi/linux/v4l2-mediabus.h
@@ -37,7 +37,7 @@
 enum v4l2_mbus_pixelcode {
 	V4L2_MBUS_FMT_FIXED = 0x0001,
 
-	/* RGB - next is 0x100d */
+	/* RGB - next is 0x100f */
 	V4L2_MBUS_FMT_RGB444_2X8_PADHI_BE = 0x1001,
 	V4L2_MBUS_FMT_RGB444_2X8_PADHI_LE = 0x1002,
 	V4L2_MBUS_FMT_RGB555_2X8_PADHI_BE = 0x1003,
@@ -50,6 +50,8 @@ enum v4l2_mbus_pixelcode {
 	V4L2_MBUS_FMT_RGB888_1X24 = 0x100a,
 	V4L2_MBUS_FMT_RGB888_2X12_BE = 0x100b,
 	V4L2_MBUS_FMT_RGB888_2X12_LE = 0x100c,
+	V4L2_MBUS_FMT_RGBA8888_4X8_BE = 0x100d,
+	V4L2_MBUS_FMT_RGBA8888_4X8_LE = 0x100e,
 
 	/* YUV (including grey) - next is 0x2017 */
 	V4L2_MBUS_FMT_Y8_1X8 = 0x2001,