media: tegra_v4l2: add dual camera support

Add support for dual cameras from both CSI-A and CSI-B:
 - move all the CSI settings into video buffer struct
 - queue the video buffer struct to a dedicated queue
 - process one video buffer struct from the queue at one time

Bug 1369083

Change-Id: Ie64d69282ab991b66e97327e288a2bacde088bd6
Signed-off-by: Bryan Wu <pengw@nvidia.com>
Reviewed-on: http://git-master/r/246269
(cherry picked from commit 228b0c2d9ae3fa1121f88836626d654ae0fc4ff0)
Reviewed-on: http://git-master/r/279987
Reviewed-by: Matthew Pedro <mapedro@nvidia.com>
Tested-by: Matthew Pedro <mapedro@nvidia.com>

Conflicts:
	drivers/media/video/tegra_v4l2_camera.c

1 files changed, 395 insertions, 426 deletions

diff --git a/drivers/media/video/tegra_v4l2_camera.c b/drivers/media/video/tegra_v4l2_camera.c
index 3dd02b7eb795..1e74aea88cfa 100644
--- a/drivers/media/video/tegra_v4l2_camera.c
+++ b/drivers/media/video/tegra_v4l2_camera.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2012, NVIDIA CORPORATION.  All rights reserved.
+ * Copyright (c) 2013-2014 Antmicro Ltd <www.antmicro.com>
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
@@ -14,6 +15,7 @@
  * 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>
@@ -24,6 +26,7 @@
 #include <linux/kthread.h>
 
 #include <mach/iomap.h>
+#include <mach/powergate.h>
 
 #include <media/soc_camera.h>
 #include <media/soc_mediabus.h>
@@ -45,7 +48,7 @@ module_param(internal_sync, int, 0644);
 MODULE_PARM_DESC(internal_sync, "enable internal vsync and hsync decoded " \
 		"from data");
 
-#define TEGRA_SYNCPT_VI_WAIT_TIMEOUT                    25
+#define TEGRA_SYNCPT_VI_WAIT_TIMEOUT                    200
 #define TEGRA_SYNCPT_CSI_WAIT_TIMEOUT                   200
 
 #define TEGRA_SYNCPT_RETRY_COUNT			10
@@ -55,7 +58,8 @@ MODULE_PARM_DESC(internal_sync, "enable internal vsync and hsync decoded " \
 
 /* SYNCPTs 12-17 are reserved for VI. */
 #define TEGRA_VI_SYNCPT_VI                              NVSYNCPT_VI_ISP_2
-#define TEGRA_VI_SYNCPT_CSI                             NVSYNCPT_VI_ISP_3
+#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
@@ -266,6 +270,9 @@ MODULE_PARM_DESC(internal_sync, "enable internal vsync and hsync decoded " \
 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.
@@ -284,15 +291,17 @@ struct tegra_buffer {
 
 struct tegra_camera_dev {
 	struct soc_camera_host		ici;
-	struct soc_camera_device	*icd;
 	struct nvhost_device		*ndev;
-	struct tegra_camera_platform_data *pdata;
 
 	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;
@@ -300,21 +309,22 @@ struct tegra_camera_dev {
 	struct vb2_buffer		*active;
 	struct vb2_alloc_ctx		*alloc_ctx;
 	enum v4l2_field			field;
-	int				sequence;
+	int				sequence_a;
+	int				sequence_b;
 
 	struct work_struct		work;
 	struct mutex			work_mutex;
 
 	u32				syncpt_vi;
-	u32				syncpt_csi;
+	u32				syncpt_csi_a;
+	u32				syncpt_csi_b;
 
 	/* private buffer for non-interlaced frame */
 	struct vb2_dc_buf *internal_vbuf;
 
 	/* Debug */
 	int num_frames;
-
-	int output_channel;
+	int enable_refcnt;
 };
 
 static const struct soc_mbus_pixelfmt tegra_camera_formats[] = {
@@ -363,15 +373,15 @@ static const struct soc_mbus_pixelfmt tegra_camera_formats[] = {
 
 	/* For RAW8 and RAW10 output, we always output 16-bit (2 bytes). */
 	{
-		.fourcc			= V4L2_PIX_FMT_SGRBG8,
-		.name			= "Bayer 8 GRGR.. BGBG..",
+		.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_SGRBG10,
-		.name			= "Bayer 10 GRGR.. BGBG..",
+		.fourcc			= V4L2_PIX_FMT_SBGGR10,
+		.name			= "Bayer 10 BGBG.. GRGR..",
 		.bits_per_sample	= 16,
 		.packing		= SOC_MBUS_PACKING_EXTEND16,
 		.order			= SOC_MBUS_ORDER_LE,
@@ -400,7 +410,7 @@ void interlace_and_copy(void *dst, void *src, int width, int height)
 int make_interlaced(void *arg)
 {
 	struct thread_args *ta = arg;
-	struct soc_camera_device *icd = ta->pcdev->icd;
+	struct soc_camera_device *icd = ta->tb->icd;
 	void *src = ta->tb->internal_virtual_addr;
 	void *dst = ta->tb->virtual_addr;
 	int bytes_per_line;
@@ -429,9 +439,13 @@ static struct tegra_buffer *to_tegra_vb(struct vb2_buffer *vb)
 
 static void tegra_camera_save_syncpts(struct tegra_camera_dev *pcdev)
 {
-	pcdev->syncpt_csi =
+	pcdev->syncpt_csi_a =
 		nvhost_syncpt_read_ext(pcdev->ndev,
-				TEGRA_VI_SYNCPT_CSI);
+				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,
@@ -441,7 +455,10 @@ static void tegra_camera_save_syncpts(struct tegra_camera_dev *pcdev)
 static void tegra_camera_incr_syncpts(struct tegra_camera_dev *pcdev)
 {
 	nvhost_syncpt_cpu_incr_ext(pcdev->ndev,
-			TEGRA_VI_SYNCPT_CSI);
+			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);
@@ -449,25 +466,9 @@ static void tegra_camera_incr_syncpts(struct tegra_camera_dev *pcdev)
 
 static void tegra_camera_capture_clean(struct tegra_camera_dev *pcdev)
 {
-	/* CSI A */
 	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_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_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_CIL_COMMAND, 0x00000000);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_PHY_CILA_CONTROL0, 0x00000000);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_PHY_CILB_CONTROL0, 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);
@@ -476,14 +477,7 @@ static void tegra_camera_capture_clean(struct tegra_camera_dev *pcdev)
 	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_CILA_PAD_CONFIG0, 0x00000000);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_CILA_PAD_CONFIG1, 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_CIL_PAD_CONFIG, 0x00000000);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_CILA_MIPI_CAL_CONFIG, 0x00000000);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_CILB_MIPI_CAL_CONFIG, 0x00000000);
 	TC_VI_REG_WT(pcdev, TEGRA_CSI_CIL_MIPI_CAL_STATUS, 0x00000000);
 	TC_VI_REG_WT(pcdev, TEGRA_CSI_CLKEN_OVERRIDE, 0x00000000);
 
@@ -491,51 +485,29 @@ static void tegra_camera_capture_clean(struct tegra_camera_dev *pcdev)
 	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);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_EXPECTED_FRAME, 0x0);
-	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_EXPECTED_FRAME, 0x0);
-}
-
-static u32 tegra_camera_header_for_wrong_fmt(struct tegra_camera_dev *pcdev)
-{
-	struct soc_camera_device *icd = pcdev->icd;
-	const struct soc_camera_format_xlate *current_fmt = icd->current_fmt;
-	enum v4l2_mbus_pixelcode input_code = current_fmt->code;
-	u32 hdr;
-
-	switch (input_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:
-			hdr = 30;
-			break;
-		case V4L2_MBUS_FMT_SGRBG8_1X8:
-			hdr = 42;
-			break;
-		case V4L2_MBUS_FMT_SGRBG10_1X10:
-			hdr = 43;
-			break;
-		default:
-			BUG_ON(1);
-	}
-
-	return hdr;
 }
 
 static void tegra_camera_capture_setup_csi_a(struct tegra_camera_dev *pcdev,
-		u32 input_control)
+		struct soc_camera_device *icd,
+		u32 hdr)
 {
-	struct soc_camera_device *icd = pcdev->icd;
+	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);
-	u32 hdr = tegra_camera_header_for_wrong_fmt(pcdev);
 
-	TC_VI_REG_WT(pcdev, TEGRA_VI_VI_CORE_CONTROL, 0x02000000);
-
-	TC_VI_REG_WT(pcdev, TEGRA_VI_VI_INPUT_CONTROL, input_control);
+	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_H_DOWNSCALE_CONTROL, 0x00000004);
-	TC_VI_REG_WT(pcdev, TEGRA_VI_V_DOWNSCALE_CONTROL, 0x00000004);
+	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,
@@ -566,16 +538,16 @@ static void tegra_camera_capture_setup_csi_a(struct tegra_camera_dev *pcdev,
 
 	TC_VI_REG_WT(pcdev, TEGRA_CSI_INPUT_STREAM_A_CONTROL,
 			(0x3f << 16) | /* Skip packet threshold */
-			(pcdev->pdata->lanes - 1));
+			(pdata->lanes - 1));
 
 	/* Use 0x00000022 for continuous clock mode. */
 	TC_VI_REG_WT(pcdev, TEGRA_CSI_PHY_CILA_CONTROL0,
-			(pcdev->pdata->continuous_clk << 5) |
+			(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);
+			TEGRA_VI_SYNCPT_CSI_A);
 
 	TC_VI_REG_WT(pcdev, TEGRA_CSI_PHY_CIL_COMMAND, 0x00020001);
 
@@ -583,19 +555,26 @@ static void tegra_camera_capture_setup_csi_a(struct tegra_camera_dev *pcdev,
 }
 
 static void tegra_camera_capture_setup_csi_b(struct tegra_camera_dev *pcdev,
-		u32 input_control)
+		struct soc_camera_device *icd,
+		u32 hdr)
 {
-	struct soc_camera_device *icd = pcdev->icd;
+	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);
-	u32 hdr = tegra_camera_header_for_wrong_fmt(pcdev);
 
-	TC_VI_REG_WT(pcdev, TEGRA_VI_VI_CORE_CONTROL, 0x04000000);
-
-	TC_VI_REG_WT(pcdev, TEGRA_VI_VI_INPUT_CONTROL, input_control);
+	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_H_DOWNSCALE_CONTROL, 0x00000008);
-	TC_VI_REG_WT(pcdev, TEGRA_VI_V_DOWNSCALE_CONTROL, 0x00000008);
+	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,
@@ -627,16 +606,16 @@ static void tegra_camera_capture_setup_csi_b(struct tegra_camera_dev *pcdev,
 
 	TC_VI_REG_WT(pcdev, TEGRA_CSI_INPUT_STREAM_B_CONTROL,
 			(0x3f << 16) | /* Skip packet threshold */
-			(pcdev->pdata->lanes - 1));
+			(pdata->lanes - 1));
 
 	/* Use 0x00000022 for continuous clock mode. */
 	TC_VI_REG_WT(pcdev, TEGRA_CSI_PHY_CILB_CONTROL0,
-			(pcdev->pdata->continuous_clk << 5) |
+			(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);
+			TEGRA_VI_SYNCPT_CSI_B);
 
 	TC_VI_REG_WT(pcdev, TEGRA_CSI_PHY_CIL_COMMAND, 0x00010002);
 
@@ -644,9 +623,9 @@ static void tegra_camera_capture_setup_csi_b(struct tegra_camera_dev *pcdev,
 }
 
 static void tegra_camera_capture_setup_vip(struct tegra_camera_dev *pcdev,
+		struct soc_camera_device *icd,
 		u32 input_control)
 {
-	struct soc_camera_device *icd = pcdev->icd;
 
 
 	TC_VI_REG_WT(pcdev, TEGRA_VI_VI_CORE_CONTROL, 0x00000000);
@@ -657,7 +636,6 @@ static void tegra_camera_capture_setup_vip(struct tegra_camera_dev *pcdev,
 			((internal_sync == 1) << 25) |	/* 1 == hsync/vsync decoded
 											   internally from data
 											   (BT.656) */
-			/*		(yuv_input_format << 8) | */
 			(1 << 1) | /* VIP_INPUT_ENABLE */
 			input_control);
 
@@ -699,17 +677,19 @@ struct vb2_dc_buf {
 	struct nvmap_handle_ref		*nvmap_ref;
 };
 
-static void tegra_camera_capture_output_channel_setup(
-		struct tegra_camera_dev *pcdev)
+static int tegra_camera_capture_output_channel_setup(
+		struct tegra_camera_dev *pcdev,
+		struct soc_camera_device *icd)
 {
-	struct soc_camera_device *icd = pcdev->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;
-	int port = pcdev->pdata->port;
 	int frame_count;
+	struct tegra_buffer *buf = to_tegra_vb(pcdev->active);
 
 	switch (output_fourcc) {
 		case V4L2_PIX_FMT_UYVY:
@@ -731,7 +711,7 @@ static void tegra_camera_capture_output_channel_setup(
 		case V4L2_PIX_FMT_SBGGR8:
 		case V4L2_PIX_FMT_SBGGR10:
 			/* Use second output channel for RAW8/RAW10 */
-			pcdev->output_channel = 1;
+			buf->output_channel = 1;
 
 			if (port == TEGRA_CAMERA_PORT_CSI_A)
 				output_format = 0x7;
@@ -741,28 +721,21 @@ static void tegra_camera_capture_output_channel_setup(
 				output_format = 0x9;
 			break;
 		default:
-			BUG_ON(1);
-	}
-
-	/* prepare internal_vbuf in case of later interlacing */
-	if (IS_INTERLACED) {
-		if (output_fourcc == V4L2_PIX_FMT_YUV420 || output_fourcc == V4L2_PIX_FMT_YVU420)
-			pcdev->internal_vbuf = vb2_dma_nvmap_memops.alloc(pcdev->alloc_ctx, ((icd->user_height * icd->user_width) + (icd->user_height * icd->user_width)/2));
-		else
-			pcdev->internal_vbuf = vb2_dma_nvmap_memops.alloc(pcdev->alloc_ctx, (icd->user_height * bytes_per_line));
+			dev_err(&pcdev->ndev->dev, "Wrong output format %d\n",
+					output_fourcc);
+			return -EINVAL;
 	}
 
-	output_control = (pcdev->pdata->flip_v ? (0x1 << 20) : 0) |
-		(pcdev->pdata->flip_h ? (0x1 << 19) : 0) |
+	output_control = (pdata->flip_v ? (0x1 << 20) : 0) |
+		(pdata->flip_h ? (0x1 << 19) : 0) |
 		output_format;
 
 	/* if the video is interlaced, then take two frames */
 	frame_count = IS_INTERLACED ? 2 : 1;
 
-	if (pcdev->output_channel == 0) {
+	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.
@@ -793,8 +766,7 @@ static void tegra_camera_capture_output_channel_setup(
 				TEGRA_VI_SYNCPT_VI);
 
 		TC_VI_REG_WT(pcdev, TEGRA_VI_VI_ENABLE, 0x00000000);
-
-	} else if (pcdev->output_channel == 1) {
+	} else if (buf->output_channel == 1) {
 		TC_VI_REG_WT(pcdev, TEGRA_VI_VI_SECOND_OUTPUT_CONTROL,
 				output_control);
 
@@ -818,38 +790,54 @@ static void tegra_camera_capture_output_channel_setup(
 				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 void tegra_camera_capture_setup(struct tegra_camera_dev *pcdev)
+static int tegra_camera_capture_setup(struct tegra_camera_dev *pcdev)
 {
-	struct soc_camera_device *icd = pcdev->icd;
+	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 input_control = 0x0;
-	int port = pcdev->pdata->port;
-
-	BUG_ON(!tegra_camera_port_is_valid(port));
-
+	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_SGRBG8_1X8:
-		case V4L2_MBUS_FMT_SGRBG10_1X10:
+		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:
-			BUG_ON(1);
+			dev_err(&pcdev->ndev->dev, "Input format %d is not supported\n",
+					input_code);
+			return -EINVAL;
 	}
 
 	/*
@@ -867,18 +855,20 @@ static void tegra_camera_capture_setup(struct tegra_camera_dev *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, input_control);
+		tegra_camera_capture_setup_csi_a(pcdev, icd, hdr);
 	else if (port == TEGRA_CAMERA_PORT_CSI_B)
-		tegra_camera_capture_setup_csi_b(pcdev, input_control);
+		tegra_camera_capture_setup_csi_b(pcdev, icd, hdr);
 	else
-		tegra_camera_capture_setup_vip(pcdev, input_control);
-	tegra_camera_capture_output_channel_setup(pcdev);
+		tegra_camera_capture_setup_vip(pcdev, icd, input_control);
+
+	/* Setup registers for output channels */
+	return tegra_camera_capture_output_channel_setup(pcdev, icd);
 }
 
-static void tegra_camera_capture_buffer_setup(struct tegra_camera_dev *pcdev,
+static int tegra_camera_capture_buffer_setup(struct tegra_camera_dev *pcdev,
 		struct tegra_buffer *buf)
 {
-	struct soc_camera_device *icd = pcdev->icd;
+	struct soc_camera_device *icd = buf->icd;
 
 	switch (icd->current_fmt->host_fmt->fourcc) {
 		case V4L2_PIX_FMT_YUV420:
@@ -900,91 +890,86 @@ static void tegra_camera_capture_buffer_setup(struct tegra_camera_dev *pcdev,
 		case V4L2_PIX_FMT_SGRBG8:
 		case V4L2_PIX_FMT_SGRBG10:
 			/* output 1 */
-			if (!pcdev->output_channel) {
+			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 (pcdev->output_channel == 1) {
+			} 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:
-			BUG_ON(1);
+			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)
 {
-	int port = pcdev->pdata->port;
+	struct soc_camera_device *icd = buf->icd;
+	struct tegra_camera_platform_data *pdata = icd->link->priv;
+	int port = pdata->port;
 	int err;
-
+	int count = 1;
 	struct task_struct *interlace_task;
 	struct thread_args ta;
 
-	BUG_ON(!tegra_camera_port_is_valid(port));
-
-	pcdev->syncpt_csi++;
-	pcdev->syncpt_vi++;
-
-	tegra_camera_capture_buffer_setup(pcdev, buf);
-
-	if (port == TEGRA_CAMERA_PORT_CSI_A)
-		TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND,
-				0x0000f005);
-	else if (port == TEGRA_CAMERA_PORT_CSI_B)
-		TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND,
-				0x0000f005);
-	else
-		TC_VI_REG_WT(pcdev, TEGRA_VI_CAMERA_CONTROL,
-				0x00000005); /* start & stop */
-
-	/* start the interlacing task now */
-	if (IS_INTERLACED) {
-		ta.tb = buf;
-		ta.pcdev = pcdev;
-		interlace_task = kthread_run(make_interlaced, &ta, "interlacing thread");
-	}
-
+	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.
 	 */
-	pcdev->syncpt_vi++;
-	pcdev->syncpt_csi++;
-
-	if (tegra_camera_port_is_csi(port))
+	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,
-				pcdev->syncpt_csi,
+				TEGRA_VI_SYNCPT_CSI_A,
+				pcdev->syncpt_csi_a,
 				TEGRA_SYNCPT_CSI_WAIT_TIMEOUT,
 				NULL);
-	else
-		err = nvhost_syncpt_wait_timeout_ext(pcdev->ndev,
-				TEGRA_VI_SYNCPT_VI,
-				pcdev->syncpt_vi,
-				TEGRA_SYNCPT_VI_WAIT_TIMEOUT,
-				NULL);
-
-	if (IS_INTERLACED) {
-
-		TC_VI_REG_WT(pcdev, TEGRA_VI_CAMERA_CONTROL,
-				0x00000005); /* start & stop */
-
-		pcdev->syncpt_vi++;
-		pcdev->syncpt_csi++;
-
+	} 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_VI,
-				pcdev->syncpt_vi,
-				TEGRA_SYNCPT_VI_WAIT_TIMEOUT,
+				TEGRA_VI_SYNCPT_CSI_B,
+				pcdev->syncpt_csi_b,
+				TEGRA_SYNCPT_CSI_WAIT_TIMEOUT,
 				NULL);
+	} else {
+		if (IS_INTERLACED) {
+			count = 2; /* if interlaced - grab two frames */
+			ta.tb = buf;
+			ta.pcdev = pcdev;
+			interlace_task = kthread_run(make_interlaced, &ta, "interlacing thread");
+		}
+		while (count-- && !err) {
+			pcdev->syncpt_vi++;
+			TC_VI_REG_WT(pcdev, TEGRA_VI_CAMERA_CONTROL,
+					0x00000005);
+			err = nvhost_syncpt_wait_timeout_ext(pcdev->ndev,
+					TEGRA_VI_SYNCPT_VI,
+					pcdev->syncpt_vi,
+					TEGRA_SYNCPT_VI_WAIT_TIMEOUT,
+					NULL);
+		}
 	}
 
 	if (!err)
@@ -995,8 +980,8 @@ static int tegra_camera_capture_start(struct tegra_camera_dev *pcdev,
 		u32 cilstatus;
 		u32 rostatus;
 
-		dev_err(&pcdev->ndev->dev, "Timeout on CSI syncpt\n");
-		dev_err(&pcdev->ndev->dev, "buffer_addr = 0x%08x\n",
+		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,
@@ -1006,7 +991,7 @@ static int tegra_camera_capture_start(struct tegra_camera_dev *pcdev,
 		rostatus = TC_VI_REG_RD(pcdev,
 				TEGRA_CSI_CSI_READONLY_STATUS);
 
-		dev_err(&pcdev->ndev->dev,
+		dev_warn(&icd->vdev->dev,
 				"PPSTATUS = 0x%08x, "
 				"CILSTATUS = 0x%08x, "
 				"ROSTATUS = 0x%08x\n",
@@ -1014,26 +999,24 @@ static int tegra_camera_capture_start(struct tegra_camera_dev *pcdev,
 	} else {
 		u32 vip_input_status;
 
-		dev_err(&pcdev->ndev->dev, "Timeout on VI syncpt\n");
-		dev_err(&pcdev->ndev->dev, "buffer_addr = 0x%08x\n",
+		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_err(&pcdev->ndev->dev,
+		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)
+static int tegra_camera_capture_stop(struct tegra_camera_dev *pcdev, int port)
 {
-	int port = pcdev->pdata->port;
 	int err;
-
-	BUG_ON(!tegra_camera_port_is_valid(port));
+	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,
@@ -1059,22 +1042,28 @@ static int tegra_camera_capture_stop(struct tegra_camera_dev *pcdev)
 		u32 ppstatus;
 		u32 cilstatus;
 
-		dev_err(&pcdev->ndev->dev, "Timeout on VI syncpt\n");
+		dev_warn(&pcdev->ndev->dev, "Timeout on VI syncpt\n");
 
-		if (!pcdev->output_channel)
+		if (buf->output_channel == 0)
 			buffer_addr = TC_VI_REG_RD(pcdev,
 					TEGRA_VI_VB0_BASE_ADDRESS_FIRST);
-		else
+		else if (buf->output_channel == 1)
 			buffer_addr = TC_VI_REG_RD(pcdev,
 					TEGRA_VI_VB0_BASE_ADDRESS_SECOND);
-		dev_err(&pcdev->ndev->dev, "buffer_addr = 0x%08x\n",
+		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_err(&pcdev->ndev->dev,
+		dev_warn(&pcdev->ndev->dev,
 				"PPSTATUS = 0x%08x, CILSTATUS = 0x%08x\n",
 				ppstatus, cilstatus);
 	}
@@ -1089,27 +1078,21 @@ static void tegra_camera_activate(struct tegra_camera_dev *pcdev)
 	/* 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.
-	 */
-	tegra_unpowergate_partition(TEGRA_POWERGATE_DISA);
 	/* 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_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_enable(pcdev->clk_vi_sensor);
+	clk_enable(pcdev->clk_csi);
+	clk_enable(pcdev->clk_isp);
+	clk_enable(pcdev->clk_csus);
 	clk_set_rate(pcdev->clk_sclk, 80000000);
-	clk_prepare_enable(pcdev->clk_sclk);
+	clk_enable(pcdev->clk_sclk);
 	clk_set_rate(pcdev->clk_sclk, 375000000);
-	clk_prepare_enable(pcdev->clk_emc);
+	clk_enable(pcdev->clk_emc);
 
 	/* Save current syncpt values. */
 	tegra_camera_save_syncpts(pcdev);
@@ -1118,17 +1101,16 @@ static void tegra_camera_activate(struct tegra_camera_dev *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);
+	clk_disable(pcdev->clk_vi);
+	clk_disable(pcdev->clk_vi_sensor);
+	clk_disable(pcdev->clk_csi);
+	clk_disable(pcdev->clk_isp);
+	clk_disable(pcdev->clk_csus);
+	clk_disable(pcdev->clk_sclk);
+	clk_disable(pcdev->clk_emc);
 
 	/* Powergate VE */
 	tegra_powergate_partition(TEGRA_POWERGATE_VENC);
-	tegra_powergate_partition(TEGRA_POWERGATE_DISA);
 
 	/* Disable external power */
 	regulator_disable(pcdev->reg);
@@ -1138,83 +1120,80 @@ static void tegra_camera_deactivate(struct tegra_camera_dev *pcdev)
 
 static int tegra_camera_capture_frame(struct tegra_camera_dev *pcdev)
 {
-	struct vb2_buffer *vb;
-	struct tegra_buffer *buf;
+	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 port = pcdev->pdata->port;
-	int err = 0;
-
-	if (!pcdev->active)
-		return 0;
-
-	vb = pcdev->active;
-	buf = to_tegra_vb(vb);
-
-	while (retry--) {
+	int err;
+	while (retry) {
 		err = tegra_camera_capture_start(pcdev, buf);
-		if (err == 0) {
-			err = tegra_camera_capture_stop(pcdev);
-			if (err == 0) break;
-		}
+		/* 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);
+			}
 
-		/* 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);
 
-		tegra_camera_incr_syncpts(pcdev);
-		tegra_camera_save_syncpts(pcdev);
-		continue;
+			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);
-
-	/*
-	 * If vb->state is VB2_BUF_STATE_ERROR, then the vb has already been
-	 * removed, so we shouldn't remove it again.
-	 */
-	if (vb->state != VB2_BUF_STATE_ERROR)
-		list_del_init(&buf->queue);
-
-	if (!list_empty(&pcdev->capture))
-		pcdev->active = &list_entry(pcdev->capture.next,
-				struct tegra_buffer, queue)->vb;
-	else
-		pcdev->active = NULL;
-
 	do_gettimeofday(&vb->v4l2_buf.timestamp);
 	vb->v4l2_buf.field = pcdev->field;
-	vb->v4l2_buf.sequence = pcdev->sequence++;
+	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++;
 
-	if (IS_INTERLACED && pcdev->num_frames==0)
+	if (IS_INTERLACED && pcdev->num_frames == 0)
 		/* if we're dealing with interlaced frames, tell V4L to remove the frame from the queue */
 		vb2_buffer_done(vb, VB2_BUF_STATE_DEQUEUED);
 	else
 		vb2_buffer_done(vb, (err != 0) ? VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+	list_del_init(&buf->queue);
 
 	pcdev->num_frames++;
 
@@ -1227,78 +1206,40 @@ 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;
 
-	mutex_lock(&pcdev->work_mutex);
-
-	while (pcdev->active)
-		tegra_camera_capture_frame(pcdev);
-
-	mutex_unlock(&pcdev->work_mutex);
-}
-
-static void tegra_camera_activate(struct tegra_camera_dev *pcdev)
-{
-#ifdef CONFIG_ARCH_TEGRA_2x_SOC
-	u32 val;
-	void __iomem *apb_misc;
-#endif
-
-	nvhost_module_busy_ext(pcdev->ndev);
-
-#ifndef CONFIG_ARCH_TEGRA_2x_SOC
-	if (!tegra_powergate_is_powered(TEGRA_POWERGATE_VENC))
-		tegra_unpowergate_partition(TEGRA_POWERGATE_VENC);
-#endif
-
-	/* Turn on relevant clocks. */
-	clk_enable(pcdev->clk_vi);
-	clk_enable(pcdev->clk_vi_sensor);
-	clk_enable(pcdev->clk_csi);
-	clk_enable(pcdev->clk_isp);
-	clk_enable(pcdev->clk_csus);
+	while (1) {
+		mutex_lock(&pcdev->work_mutex);
 
-#ifdef CONFIG_ARCH_TEGRA_2x_SOC
-	apb_misc = IO_ADDRESS(TEGRA_APB_MISC_BASE);
-	val = readl(apb_misc + 0x42c);
-	writel(val | 0x1, apb_misc + 0x42c);
-#endif
+		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;
+		}
 
-	/* Save current syncpt values. */
-	tegra_camera_save_syncpts(pcdev);
-}
+		buf = list_entry(pcdev->capture.next, struct tegra_buffer,
+				queue);
+		pcdev->active = &buf->vb;
+		spin_unlock_irq(&pcdev->videobuf_queue_lock);
 
-static void tegra_camera_deactivate(struct tegra_camera_dev *pcdev)
-{
-	mutex_lock(&pcdev->work_mutex);
+		tegra_camera_capture_setup(pcdev);
+		tegra_camera_capture_frame(pcdev);
 
-	/* Cancel active buffer. */
-	if (pcdev->active) {
-		list_del_init(&to_tegra_vb(pcdev->active)->queue);
-		vb2_buffer_done(pcdev->active, VB2_BUF_STATE_ERROR);
-		pcdev->active = NULL;
+		mutex_unlock(&pcdev->work_mutex);
 	}
-
-	mutex_unlock(&pcdev->work_mutex);
-
-	/* Turn off relevant clocks. */
-	clk_disable(pcdev->clk_vi);
-	clk_disable(pcdev->clk_vi_sensor);
-	clk_disable(pcdev->clk_csi);
-	clk_disable(pcdev->clk_isp);
-	clk_disable(pcdev->clk_csus);
-
-	nvhost_module_idle_ext(pcdev->ndev);
-
 }
 
-static void tegra_camera_init_buffer(struct tegra_camera_dev *pcdev,
-		struct tegra_buffer *buf)
+static int tegra_camera_init_buffer(struct tegra_buffer *buf)
 {
-	struct soc_camera_device *icd = pcdev->icd;
+	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;
 
-
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct tegra_camera_dev *pcdev = ici->priv;
 	if (IS_INTERLACED) {
 		buf->internal_phys_addr = pcdev->internal_vbuf->paddr; /* physical addr of internal buffer */
 		buf->internal_virtual_addr = pcdev->internal_vbuf->vaddr; /* virtual address of internal buffer */
@@ -1316,11 +1257,11 @@ static void tegra_camera_init_buffer(struct tegra_camera_dev *pcdev,
 		case V4L2_PIX_FMT_SGRBG10:
 			buf->start_addr = buf->buffer_addr;
 
-			if (pcdev->pdata->flip_v)
+			if (pdata->flip_v)
 				buf->start_addr += bytes_per_line *
 					(icd->user_height-1);
 
-			if (pcdev->pdata->flip_h)
+			if (pdata->flip_h)
 				buf->start_addr += bytes_per_line - 1;
 
 			break;
@@ -1343,7 +1284,7 @@ static void tegra_camera_init_buffer(struct tegra_camera_dev *pcdev,
 			buf->start_addr_u = buf->buffer_addr_u;
 			buf->start_addr_v = buf->buffer_addr_v;
 
-			if (pcdev->pdata->flip_v) {
+			if (pdata->flip_v) {
 				buf->start_addr += icd->user_width *
 					(icd->user_height - 1);
 
@@ -1354,7 +1295,7 @@ static void tegra_camera_init_buffer(struct tegra_camera_dev *pcdev,
 						((icd->user_height/2) - 1));
 			}
 
-			if (pcdev->pdata->flip_h) {
+			if (pdata->flip_h) {
 				buf->start_addr += icd->user_width - 1;
 
 				buf->start_addr_u += (icd->user_width/2) - 1;
@@ -1365,8 +1306,12 @@ static void tegra_camera_init_buffer(struct tegra_camera_dev *pcdev,
 			break;
 
 		default:
-			BUG_ON(1);
+			dev_err(icd->parent, "Wrong host format %d\n",
+					icd->current_fmt->host_fmt->fourcc);
+			return -EINVAL;
 	}
+
+	return 0;
 }
 
 /*
@@ -1381,6 +1326,7 @@ static int tegra_camera_videobuf_setup(struct vb2_queue *vq,
 	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,
@@ -1392,20 +1338,18 @@ static int tegra_camera_videobuf_setup(struct vb2_queue *vq,
 		return bytes_per_line;
 
 	*num_planes = 1;
-
-	pcdev->sequence = 0;
+	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;
 
-	dev_dbg(icd->parent, "num_buffers=%u, size=%lu\n",
-			*num_buffers, sizes[0]);
-
-	tegra_camera_capture_setup(pcdev);
-
-	dev_dbg(icd->parent, "Finished tegra_camera_videobuf_setup()\n");
+	if (IS_INTERLACED)
+		pcdev->internal_vbuf = vb2_dma_nvmap_memops.alloc(pcdev->alloc_ctx, bytes_per_line * icd->user_height);
 
 	return 0;
 }
@@ -1415,9 +1359,8 @@ 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 *pcdev = ici->priv;
-	struct tegra_buffer *buf;
+	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;
@@ -1427,7 +1370,19 @@ static int tegra_camera_videobuf_prepare(struct vb2_buffer *vb)
 	if (bytes_per_line < 0)
 		return bytes_per_line;
 
-	buf = to_tegra_vb(vb);
+	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));
@@ -1441,7 +1396,10 @@ static int tegra_camera_videobuf_prepare(struct vb2_buffer *vb)
 		memset(vb2_plane_vaddr(vb, 0), 0xbd, vb2_plane_size(vb, 0));
 #endif
 
-	BUG_ON(NULL == icd->current_fmt);
+	if (!icd->current_fmt) {
+		dev_err(icd->parent, "%s NULL format point\n", __func__);
+		return -EINVAL;
+	}
 
 	size = icd->user_height * bytes_per_line;
 
@@ -1453,11 +1411,7 @@ static int tegra_camera_videobuf_prepare(struct vb2_buffer *vb)
 
 	vb2_set_plane_payload(vb, 0, size);
 
-	tegra_camera_init_buffer(pcdev, buf);
-
-	dev_dbg(icd->parent, "Finished tegra_camera_videobuf_prepare()\n");
-
-	return 0;
+	return tegra_camera_init_buffer(buf);
 }
 
 static void tegra_camera_videobuf_queue(struct vb2_buffer *vb)
@@ -1469,18 +1423,12 @@ static void tegra_camera_videobuf_queue(struct vb2_buffer *vb)
 	struct tegra_camera_dev *pcdev = ici->priv;
 	struct tegra_buffer *buf = to_tegra_vb(vb);
 
-	dev_dbg(icd->parent, "In tegra_camera_videobuf_queue()\n");
-
 	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);
-
-	if (!pcdev->active) {
-		pcdev->active = vb;
-		schedule_work(&pcdev->work);
-	}
+	schedule_work(&pcdev->work);
 	spin_unlock_irq(&pcdev->videobuf_queue_lock);
 
 	dev_dbg(icd->parent, "Finished tegra_camera_videobuf_queue()\n");
@@ -1527,7 +1475,6 @@ 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;
 }
 
@@ -1540,17 +1487,25 @@ static int tegra_camera_stop_streaming(struct vb2_queue *q)
 	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);
-
-	pcdev->active = NULL;
-
-	list_for_each_safe(buf_head, tmp, &pcdev->capture)
-		list_del_init(buf_head);
-
+	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;
@@ -1580,7 +1535,6 @@ static int tegra_camera_init_videobuf(struct vb2_queue *q,
 	q->drv_priv = icd;
 	q->ops = &tegra_camera_videobuf_ops;
 	q->mem_ops = &vb2_dma_nvmap_memops;
-	/*q->mem_ops = &am_vb2_dma_nvmap_memops; */
 	q->buf_struct_size = sizeof(struct tegra_buffer);
 
 	dev_dbg(icd->parent, "Finished tegra_camera_init_videobuf()\n");
@@ -1595,35 +1549,13 @@ 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;
-	int err;
-
-	pcdev->pdata = icd->link->priv;
-	if (!pcdev->pdata) {
-		dev_err(icd->parent, "No platform data!\n");
-		return -EINVAL;
-	}
-
-	if (!tegra_camera_port_is_valid(pcdev->pdata->port)) {
-		dev_err(icd->parent,
-				"Invalid camera port %d in platform data\n",
-				pcdev->pdata->port);
-		return -EINVAL;
-	}
 
-	pm_runtime_get_sync(ici->v4l2_dev.dev);
-
-	if (pcdev->pdata->enable_camera) {
-		err = pcdev->pdata->enable_camera(pcdev->ndev);
-		if (IS_ERR_VALUE(err))
-			return err;
+	if (!pcdev->enable_refcnt) {
+		pm_runtime_get_sync(ici->v4l2_dev.dev);
+		tegra_camera_activate(pcdev);
+		pcdev->num_frames = 0;
 	}
-
-	tegra_camera_activate(pcdev);
-
-	pcdev->icd = icd;
-
-	pcdev->num_frames = 0;
-	pcdev->output_channel = 0;
+	pcdev->enable_refcnt++;
 
 	dev_dbg(icd->parent, "TEGRA Camera host attached to camera %d\n",
 			icd->devnum);
@@ -1637,14 +1569,12 @@ 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;
 
-	tegra_camera_deactivate(pcdev);
-
-	pcdev->icd = NULL;
-
-	if (pcdev->pdata->disable_camera)
-		pcdev->pdata->disable_camera(pcdev->ndev);
-
-	pm_runtime_put_sync(ici->v4l2_dev.dev);
+	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);
 
@@ -1890,8 +1820,7 @@ static int __devinit tegra_camera_probe(struct nvhost_device *ndev,
 		goto exit;
 	}
 
-	pcdev->pdata			= ndev->dev.platform_data;
-	pcdev->ndev			= ndev;
+	pcdev->ndev = ndev;
 
 	pcdev->ici.priv		= pcdev;
 	pcdev->ici.v4l2_dev.dev	= &ndev->dev;
@@ -1904,8 +1833,6 @@ static int __devinit tegra_camera_probe(struct nvhost_device *ndev,
 	spin_lock_init(&pcdev->videobuf_queue_lock);
 	mutex_init(&pcdev->work_mutex);
 
-	nvhost_set_drvdata(ndev, pcdev);
-
 	pcdev->clk_vi = clk_get_sys("tegra_camera", "vi");
 	if (IS_ERR_OR_NULL(pcdev->clk_vi)) {
 		dev_err(&ndev->dev, "Failed to get vi clock.\n");
@@ -1935,16 +1862,51 @@ static int __devinit tegra_camera_probe(struct nvhost_device *ndev,
 		dev_err(&ndev->dev, "Failed to get csus clock.\n");
 		goto exit_put_clk_isp;
 	}
+
+	pcdev->clk_sclk = clk_get_sys("tegra_camera", "sclk");
+	if (IS_ERR_OR_NULL(pcdev->clk_sclk)) {
+		dev_err(&ndev->dev, "Failed to get sclk clock.\n");
+		goto exit_put_clk_csus;
+	}
+
+	pcdev->clk_emc = clk_get_sys("tegra_camera", "emc");
+	if (IS_ERR_OR_NULL(pcdev->clk_emc)) {
+		dev_err(&ndev->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(&ndev->dev, "vcsi");
+#else
+	pcdev->reg = regulator_get(&ndev->dev, "avdd_dsi_csi");
+#endif
+	if (IS_ERR_OR_NULL(pcdev->reg)) {
+		dev_err(&ndev->dev, "%s: couldn't get regulator\n",
+				__func__);
+		goto exit_put_clk_emc;
+	}
+
+	nvhost_set_drvdata(ndev, pcdev);
 	err = nvhost_client_device_get_resources(ndev);
-	if (err)
-		goto exit_put_clk_csus;
+	if (err) {
+		dev_err(&ndev->dev, "%s: nvhost get resources failed %d\n",
+				__func__, err);
+		goto exit_put_regulator;
+	}
 
 	/* initialize nvhost client device only the first time */
-	if (ndev->power_attrib == NULL)
-		nvhost_client_device_init(ndev);
+	if (ndev->power_attrib == NULL) {
+		err = nvhost_client_device_init(ndev);
+		if (err) {
+			dev_err(&ndev->dev, "%s: nvhost init failed %d\n",
+					__func__, err);
+			goto exit_put_regulator;
+		}
+	}
 
 	pcdev->vi_base = ndev->aperture;
 
@@ -1952,10 +1914,10 @@ static int __devinit tegra_camera_probe(struct nvhost_device *ndev,
 	pm_runtime_enable(&ndev->dev);
 	pm_runtime_resume(&ndev->dev);
 
-	pcdev->alloc_ctx = vb2_dma_nvmap_init_ctx(pcdev->ici.v4l2_dev.dev);
+	pcdev->alloc_ctx = vb2_dma_nvmap_init_ctx(&ndev->dev);
 	if (IS_ERR(pcdev->alloc_ctx)) {
 		err = PTR_ERR(pcdev->alloc_ctx);
-		goto exit_put_resources;
+		goto exit_pm_disable;
 	}
 
 	err = soc_camera_host_register(&pcdev->ici);
@@ -1968,9 +1930,15 @@ static int __devinit tegra_camera_probe(struct nvhost_device *ndev,
 
 exit_cleanup_alloc_ctx:
 	vb2_dma_nvmap_cleanup_ctx(pcdev->alloc_ctx);
-exit_put_resources:
+exit_pm_disable:
 	pm_runtime_disable(&ndev->dev);
 	nvhost_client_device_put_resources(ndev);
+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:
@@ -2048,7 +2016,8 @@ static int tegra_camera_resume(struct nvhost_device *ndev)
 	if (pcdev->icd) {
 		/* Resume the camera host. */
 		tegra_camera_save_syncpts(pcdev);
-		tegra_camera_capture_setup(pcdev);
+		if (pcdev->active)
+			tegra_camera_capture_setup(pcdev);
 
 		/* Resume the camera sensor. */
 		WARN_ON(!pcdev->icd->ops->resume);