media: tegra_v4l2: add dual camera support
Bryan Wu [Mon, 24 Jun 2013 21:32:10 +0000 (14:32 -0700)]
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>

drivers/media/video/tegra_v4l2_camera.c

index 678e765..c96dae1 100644 (file)
@@ -14,6 +14,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>
@@ -22,6 +23,7 @@
 #include <linux/nvhost.h>
 
 #include <mach/iomap.h>
+#include <mach/powergate.h>
 
 #include <media/soc_camera.h>
 #include <media/soc_mediabus.h>
@@ -45,7 +47,8 @@
 
 /* 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
 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
@@ -267,15 +272,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;
@@ -283,18 +290,19 @@ 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;
 
        /* Debug */
        int num_frames;
-
-       int output_channel;
+       int enable_refcnt;
 };
 
 static const struct soc_mbus_pixelfmt tegra_camera_formats[] = {
@@ -344,14 +352,14 @@ 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_SBGGR8,
-               .name                   = "Bayer 8 GRGR.. BGBG..",
+               .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 GRGR.. BGBG..",
+               .name                   = "Bayer 10 BGBG.. GRGR..",
                .bits_per_sample        = 16,
                .packing                = SOC_MBUS_PACKING_EXTEND16,
                .order                  = SOC_MBUS_ORDER_LE,
@@ -366,9 +374,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,
@@ -378,7 +390,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);
@@ -386,25 +401,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);
@@ -413,14 +412,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);
 
@@ -428,51 +420,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_SBGGR8_1X8:
-               hdr = 42;
-               break;
-       case V4L2_MBUS_FMT_SBGGR10_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,
@@ -503,16 +473,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);
 
@@ -520,19 +490,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,
@@ -564,16 +541,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);
 
@@ -581,9 +558,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,
-                                            u32 input_control)
+                                          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);
 
@@ -619,16 +596,18 @@ static void tegra_camera_capture_setup_vip(struct tegra_camera_dev *pcdev,
        TC_VI_REG_WT(pcdev, TEGRA_VI_CAMERA_CONTROL, 0x00000004);
 }
 
-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;
+       struct tegra_buffer *buf = to_tegra_vb(pcdev->active);
 
        switch (output_fourcc) {
        case V4L2_PIX_FMT_UYVY:
@@ -650,7 +629,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;
@@ -660,17 +639,18 @@ static void tegra_camera_capture_output_channel_setup(
                        output_format = 0x9;
                break;
        default:
-               BUG_ON(1);
+               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 (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.
@@ -696,8 +676,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);
 
@@ -721,38 +700,55 @@ 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_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;
        }
 
        /*
@@ -770,20 +766,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_setup_vip(pcdev, icd, input_control);
 
        /* Setup registers for output channels */
-       tegra_camera_capture_output_channel_setup(pcdev);
+       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:
@@ -805,64 +801,76 @@ static void tegra_camera_capture_buffer_setup(struct tegra_camera_dev *pcdev,
        case V4L2_PIX_FMT_SBGGR8:
        case V4L2_PIX_FMT_SBGGR10:
                /* 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;
 
-       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,
-                            0x00000001);
-
+       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 (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_SYNCPT_CSI_WAIT_TIMEOUT,
-                       NULL);
-       else
+                               TEGRA_VI_SYNCPT_CSI_A,
+                               pcdev->syncpt_csi_a,
+                               TEGRA_SYNCPT_CSI_WAIT_TIMEOUT,
+                               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_VI,
-                       pcdev->syncpt_csi,
-                       TEGRA_SYNCPT_VI_WAIT_TIMEOUT,
-                       NULL);
+                               TEGRA_VI_SYNCPT_CSI_B,
+                               pcdev->syncpt_csi_b,
+                               TEGRA_SYNCPT_CSI_WAIT_TIMEOUT,
+                               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);
+       }
 
        if (!err)
                return 0;
@@ -872,8 +880,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,
@@ -883,7 +891,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",
@@ -891,14 +899,14 @@ 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);
        }
@@ -906,12 +914,10 @@ static int tegra_camera_capture_start(struct tegra_camera_dev *pcdev,
        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,
@@ -937,22 +943,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);
        }
@@ -962,32 +974,26 @@ static int tegra_camera_capture_stop(struct tegra_camera_dev *pcdev)
 
 static void tegra_camera_activate(struct tegra_camera_dev *pcdev)
 {
-       nvhost_module_busy_ext(pcdev->ndev);
+               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.
-        */
-       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);
@@ -996,17 +1002,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);
@@ -1016,24 +1021,21 @@ 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;
 
-       if (!pcdev->active)
-               return 0;
-
-       vb = pcdev->active;
-       buf = to_tegra_vb(vb);
-
        while (retry) {
                err = tegra_camera_capture_start(pcdev, buf);
+               /* Capturing succeed, stop capturing */
                if (!err)
-                       err = tegra_camera_capture_stop(pcdev);
-
-               if (err != 0) {
+                       err = tegra_camera_capture_stop(pcdev, port);
+               /* Capturing failed, stop and retry */
+               else {
                        retry--;
 
                        /* Stop streaming. */
@@ -1065,7 +1067,6 @@ static int tegra_camera_capture_frame(struct tegra_camera_dev *pcdev)
                                             0x00000005);
                        }
 
-
                        tegra_camera_incr_syncpts(pcdev);
                        tegra_camera_save_syncpts(pcdev);
 
@@ -1075,26 +1076,26 @@ static int tegra_camera_capture_frame(struct tegra_camera_dev *pcdev)
                break;
        }
 
-       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);
+       /* 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);
+       }
 
-       if (!list_empty(&pcdev->capture))
-               pcdev->active = &list_entry(pcdev->capture.next,
-                                           struct tegra_buffer, queue)->vb;
-       else
-               pcdev->active = NULL;
+       spin_lock_irq(&pcdev->videobuf_queue_lock);
 
        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++;
 
        vb2_buffer_done(vb, err < 0 ? VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+       list_del_init(&buf->queue);
 
        pcdev->num_frames++;
 
@@ -1107,70 +1108,37 @@ 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);
+       while (1) {
+               mutex_lock(&pcdev->work_mutex);
 
-       /* 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);
-
-#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;
 
        switch (icd->current_fmt->host_fmt->fourcc) {
        case V4L2_PIX_FMT_UYVY:
@@ -1182,11 +1150,11 @@ static void tegra_camera_init_buffer(struct tegra_camera_dev *pcdev,
                buf->buffer_addr = vb2_dma_nvmap_plane_paddr(&buf->vb, 0);
                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;
@@ -1210,7 +1178,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);
 
@@ -1221,7 +1189,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;
@@ -1232,8 +1200,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;
 }
 
 /*
@@ -1248,6 +1220,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,
@@ -1260,20 +1233,16 @@ static int tegra_camera_videobuf_setup(struct vb2_queue *vq,
 
        *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");
-
        return 0;
 }
 
@@ -1282,9 +1251,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;
@@ -1294,7 +1262,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));
@@ -1308,7 +1288,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;
 
@@ -1320,11 +1303,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)
@@ -1336,18 +1315,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");
@@ -1402,17 +1375,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;
@@ -1456,35 +1437,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 (!pcdev->enable_refcnt) {
+               pm_runtime_get_sync(ici->v4l2_dev.dev);
+               tegra_camera_activate(pcdev);
+               pcdev->num_frames = 0;
        }
-
-       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;
-       }
-
-       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);
@@ -1498,14 +1457,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);
 
@@ -1741,8 +1698,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;
@@ -1755,8 +1711,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");
@@ -1787,14 +1741,47 @@ static int __devinit tegra_camera_probe(struct nvhost_device *ndev,
                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;
+       }
 
-       nvhost_client_device_init(ndev);
+       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;
 
@@ -1802,10 +1789,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(NULL);
+       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);
@@ -1818,9 +1805,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:
@@ -1898,7 +1891,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);
@@ -1940,5 +1934,6 @@ 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);