ENGR00320056-1 v4l2 capture: imx6sx: Add dual camera support

on imx6sx, there are two CSI ports which supports dual camera.
The patch changes the framework for this support, including:
- introduce a new struct csi_soc for multiple instances.
- add csi_read and csi_write API with data type 'struct csi_soc *' to
  differentiate register access for each CSI.
- add a new parameter 'cam_data *' for the csi_* API (except
  csi_dmareq_rff_enable/disable) for dual instances.
- move 'input_fmt' and 'bswapenable' to struct _cam_data.
- initialize cam->bswapenable' to false.
- allocate dynamically memory for struce v4l2_int_device thus to
  differentiate each CSI port.

Signed-off-by: Robby Cai <r63905@freescale.com>
(cherry picked from commit 997d1e896eae21fb4c78495df1e01a126913cb53)

4 files changed, 276 insertions, 202 deletions

diff --git a/drivers/media/platform/mxc/capture/csi_v4l2_capture.c b/drivers/media/platform/mxc/capture/csi_v4l2_capture.c
index 3da162860205..3f759b635f8b 100644
--- a/drivers/media/platform/mxc/capture/csi_v4l2_capture.c
+++ b/drivers/media/platform/mxc/capture/csi_v4l2_capture.c
@@ -48,8 +48,6 @@ static int req_buf_number;
 static int csi_v4l2_master_attach(struct v4l2_int_device *slave);
 static void csi_v4l2_master_detach(struct v4l2_int_device *slave);
 static u8 camera_power(cam_data *cam, bool cameraOn);
-static struct v4l2_format cam_input_fmt;
-static bool bswapenable;
 
 /*! Information about this driver. */
 static struct v4l2_int_master csi_v4l2_master = {
@@ -57,15 +55,6 @@ static struct v4l2_int_master csi_v4l2_master = {
         .detach = csi_v4l2_master_detach,
 };
 
-static struct v4l2_int_device csi_v4l2_int_device = {
-        .module = THIS_MODULE,
-        .name = "csi_v4l2_cap",
-        .type = v4l2_int_type_master,
-        .u = {
-              .master = &csi_v4l2_master,
-              },
-};
-
 static struct v4l2_queryctrl pxp_controls[] = {
         {
                 .id             = V4L2_CID_HFLIP,
@@ -307,11 +296,12 @@ static int pxp_process_update(cam_data *cam)
         /*
          * Configure PxP for processing of new v4l2 buf
          */
-        pxp_conf->s0_param.pixel_fmt = v4l2_fmt_2_pxp_fmt(cam_input_fmt.fmt.pix.pixelformat);
+        pxp_conf->s0_param.pixel_fmt =
+                        v4l2_fmt_2_pxp_fmt(cam->input_fmt.fmt.pix.pixelformat);
         pxp_conf->s0_param.color_key = -1;
         pxp_conf->s0_param.color_key_enable = false;
-        pxp_conf->s0_param.width = cam_input_fmt.fmt.pix.width;
-        pxp_conf->s0_param.height = cam_input_fmt.fmt.pix.height;
+        pxp_conf->s0_param.width = cam->input_fmt.fmt.pix.width;
+        pxp_conf->s0_param.height = cam->input_fmt.fmt.pix.height;
 
         pxp_conf->ol_param[0].combine_enable = false;
 
@@ -462,12 +452,12 @@ next:
                 list_del(cam->ready_q.next);
                 list_add_tail(&ready_frame->queue, &cam->working_q);
 
-                __raw_writel(ready_frame->paddress,
+                csi_write(cam->csi_soc, ready_frame->paddress,
                         cam->ping_pong_csi == 1 ? CSI_CSIDMASA_FB1 :
                                                   CSI_CSIDMASA_FB2);
                 ready_frame->csi_buf_num = cam->ping_pong_csi;
         } else {
-                __raw_writel(cam->dummy_frame.paddress,
+                csi_write(cam->csi_soc, cam->dummy_frame.paddress,
                         cam->ping_pong_csi == 1 ? CSI_CSIDMASA_FB1 :
                                                   CSI_CSIDMASA_FB2);
         }
@@ -488,10 +478,10 @@ static int csi_cap_image(cam_data *cam)
 {
         unsigned int value;
 
-        value = __raw_readl(CSI_CSICR3);
-        __raw_writel(value | BIT_FRMCNT_RST, CSI_CSICR3);
-        value = __raw_readl(CSI_CSISR);
-        __raw_writel(value, CSI_CSISR);
+        value = csi_read(cam->csi_soc, CSI_CSICR3);
+        csi_write(cam->csi_soc, value | BIT_FRMCNT_RST, CSI_CSICR3);
+        value = csi_read(cam->csi_soc, CSI_CSISR);
+        csi_write(cam->csi_soc, value, CSI_CSISR);
 
         return 0;
 }
@@ -711,7 +701,7 @@ static int csi_streamon(cam_data *cam)
         frame = list_entry(cam->ready_q.next, struct mxc_v4l_frame, queue);
         list_del(cam->ready_q.next);
         list_add_tail(&frame->queue, &cam->working_q);
-        __raw_writel(frame->paddress, CSI_CSIDMASA_FB1);
+        csi_write(cam->csi_soc, frame->paddress, CSI_CSIDMASA_FB1);
         frame->csi_buf_num = 1;
 
         if (list_empty(&cam->ready_q)) {
@@ -723,7 +713,7 @@ static int csi_streamon(cam_data *cam)
         frame = list_entry(cam->ready_q.next, struct mxc_v4l_frame, queue);
         list_del(cam->ready_q.next);
         list_add_tail(&frame->queue, &cam->working_q);
-        __raw_writel(frame->paddress, CSI_CSIDMASA_FB2);
+        csi_write(cam->csi_soc, frame->paddress, CSI_CSIDMASA_FB2);
         frame->csi_buf_num = 2;
         spin_unlock_irqrestore(&cam->queue_int_lock, flags);
 
@@ -733,12 +723,13 @@ static int csi_streamon(cam_data *cam)
 
         local_irq_save(flags);
         for (timeout = 1000000; timeout > 0; timeout--) {
-                if (__raw_readl(CSI_CSISR) & BIT_SOF_INT) {
-                        val = __raw_readl(CSI_CSICR3);
-                        __raw_writel(val | BIT_DMA_REFLASH_RFF, CSI_CSICR3);
+                if (csi_read(cam->csi_soc, CSI_CSISR) & BIT_SOF_INT) {
+                        val = csi_read(cam->csi_soc, CSI_CSICR3);
+                        csi_write(cam->csi_soc, val | BIT_DMA_REFLASH_RFF,
+                                        CSI_CSICR3);
                         /* Wait DMA reflash done */
                         for (timeout2 = 1000000; timeout2 > 0; timeout2--) {
-                                if (__raw_readl(CSI_CSICR3) &
+                                if (csi_read(cam->csi_soc, CSI_CSICR3) &
                                         BIT_DMA_REFLASH_RFF)
                                         cpu_relax();
                                 else
@@ -750,9 +741,9 @@ static int csi_streamon(cam_data *cam)
                                 return -ETIME;
                         }
 
-                        csi_dmareq_rff_enable();
-                        csi_enable_int(1);
-                        csi_enable(1);
+                        csi_dmareq_rff_enable(cam->csi_soc);
+                        csi_enable_int(cam, 1);
+                        csi_enable(cam, 1);
                         break;
                 } else
                         cpu_relax();
@@ -781,21 +772,21 @@ static int csi_streamoff(cam_data *cam)
         if (cam->capture_on == false)
                 return 0;
 
-        csi_dmareq_rff_disable();
-        csi_disable_int();
+        csi_dmareq_rff_disable(cam->csi_soc);
+        csi_disable_int(cam);
         cam->capture_on = false;
 
         /* set CSI_CSIDMASA_FB1 and CSI_CSIDMASA_FB2 to default value */
-        __raw_writel(0, CSI_CSIDMASA_FB1);
-        __raw_writel(0, CSI_CSIDMASA_FB2);
+        csi_write(cam->csi_soc, 0, CSI_CSIDMASA_FB1);
+        csi_write(cam->csi_soc, 0, CSI_CSIDMASA_FB2);
 
         if (strcmp(csi_capture_inputs[cam->current_input].name,
                    "Vadc") == 0) {
-                csi_buf_stride_set(0);
-                csi_deinterlace_enable(false);
-                csi_tvdec_enable(false);
+                csi_buf_stride_set(cam, 0);
+                csi_deinterlace_enable(cam, false);
+                csi_tvdec_enable(cam, false);
         }
-        csi_enable(0);
+        csi_enable(cam, 0);
 
         csi_free_frames(cam);
         csi_free_frame_buf(cam);
@@ -814,11 +805,13 @@ static int start_preview(cam_data *cam)
 {
         unsigned long fb_addr = (unsigned long)cam->v4l2_fb.base;
 
-        __raw_writel(fb_addr, CSI_CSIDMASA_FB1);
-        __raw_writel(fb_addr, CSI_CSIDMASA_FB2);
-        __raw_writel(__raw_readl(CSI_CSICR3) | BIT_DMA_REFLASH_RFF, CSI_CSICR3);
+        csi_write(cam->csi_soc, fb_addr, CSI_CSIDMASA_FB1);
+        csi_write(cam->csi_soc, fb_addr, CSI_CSIDMASA_FB2);
+        csi_write(cam->csi_soc,
+                csi_read(cam->csi_soc, CSI_CSICR3) | BIT_DMA_REFLASH_RFF,
+                CSI_CSICR3);
 
-        csi_enable_int(0);
+        csi_enable_int(cam, 0);
 
         return 0;
 }
@@ -832,12 +825,14 @@ static int start_preview(cam_data *cam)
  */
 static int stop_preview(cam_data *cam)
 {
-        csi_disable_int();
+        csi_disable_int(cam);
 
         /* set CSI_CSIDMASA_FB1 and CSI_CSIDMASA_FB2 to default value */
-        __raw_writel(0, CSI_CSIDMASA_FB1);
-        __raw_writel(0, CSI_CSIDMASA_FB2);
-        __raw_writel(__raw_readl(CSI_CSICR3) | BIT_DMA_REFLASH_RFF, CSI_CSICR3);
+        csi_write(cam->csi_soc, 0, CSI_CSIDMASA_FB1);
+        csi_write(cam->csi_soc, 0, CSI_CSIDMASA_FB2);
+        csi_write(cam->csi_soc,
+                csi_read(cam->csi_soc, CSI_CSICR3) | BIT_DMA_REFLASH_RFF,
+                CSI_CSICR3);
 
         return 0;
 }
@@ -948,7 +943,8 @@ static int csi_v4l2_s_fmt(cam_data *cam, struct v4l2_format *f)
                 }
 
                 /* disable swap function */
-                csi_format_swap16(false);
+                csi_format_swap16(cam, false);
+                cam->bswapenable = false;
 
                 switch (f->fmt.pix.pixelformat) {
                 case V4L2_PIX_FMT_RGB565:
@@ -962,17 +958,17 @@ static int csi_v4l2_s_fmt(cam_data *cam, struct v4l2_format *f)
                 case V4L2_PIX_FMT_UYVY:
                         size = f->fmt.pix.width * f->fmt.pix.height * 2;
                         bytesperline = f->fmt.pix.width * 2;
-                        if (cam_input_fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_YUYV) {
-                                csi_format_swap16(true);
-                                bswapenable = true;
+                        if (cam->input_fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_YUYV) {
+                                csi_format_swap16(cam, true);
+                                cam->bswapenable = true;
                         }
                         break;
                 case V4L2_PIX_FMT_YUYV:
                         size = f->fmt.pix.width * f->fmt.pix.height * 2;
                         bytesperline = f->fmt.pix.width * 2;
-                        if (cam_input_fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_UYVY) {
-                                csi_format_swap16(true);
-                                bswapenable = true;
+                        if (cam->input_fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_UYVY) {
+                                csi_format_swap16(cam, true);
+                                cam->bswapenable = true;
                         }
                         break;
                 case V4L2_PIX_FMT_YUV420:
@@ -1000,8 +996,8 @@ static int csi_v4l2_s_fmt(cam_data *cam, struct v4l2_format *f)
                 else
                         size = f->fmt.pix.sizeimage;
 
-                if (cam_input_fmt.fmt.pix.sizeimage > f->fmt.pix.sizeimage)
-                         f->fmt.pix.sizeimage = cam_input_fmt.fmt.pix.sizeimage;
+                if (cam->input_fmt.fmt.pix.sizeimage > f->fmt.pix.sizeimage)
+                         f->fmt.pix.sizeimage = cam->input_fmt.fmt.pix.sizeimage;
 
                 cam->v2f.fmt.pix = f->fmt.pix;
 
@@ -1084,32 +1080,36 @@ static int csi_v4l2_s_param(cam_data *cam, struct v4l2_streamparm *parm)
         }
 
         vidioc_int_g_ifparm(cam->sensor, &ifparm);
-        cam_input_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-        vidioc_int_g_fmt_cap(cam->sensor, &cam_input_fmt);
+        cam->input_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+        vidioc_int_g_fmt_cap(cam->sensor, &cam->input_fmt);
 
         pr_debug("   g_fmt_cap returns widthxheight of input as %d x %d\n",
-                 cam_input_fmt.fmt.pix.width, cam_input_fmt.fmt.pix.height);
+                 cam->input_fmt.fmt.pix.width, cam->input_fmt.fmt.pix.height);
 
-        f = &cam_input_fmt;
+        f = &cam->input_fmt;
         switch (f->fmt.pix.pixelformat) {
         case V4L2_PIX_FMT_YUV444:
                 size = f->fmt.pix.width * f->fmt.pix.height * 4;
-                csi_set_32bit_imagpara(f->fmt.pix.width,
+                csi_set_32bit_imagpara(cam,
+                                       f->fmt.pix.width,
                                        f->fmt.pix.height);
                 break;
         case V4L2_PIX_FMT_UYVY:
                 size = f->fmt.pix.width * f->fmt.pix.height * 2;
-                csi_set_16bit_imagpara(f->fmt.pix.width,
+                csi_set_16bit_imagpara(cam,
+                                       f->fmt.pix.width,
                                        f->fmt.pix.height);
                 break;
         case V4L2_PIX_FMT_YUYV:
                 size = f->fmt.pix.width * f->fmt.pix.height * 2;
-                csi_set_16bit_imagpara(f->fmt.pix.width,
+                csi_set_16bit_imagpara(cam,
+                                       f->fmt.pix.width,
                                        f->fmt.pix.height);
                 break;
         case V4L2_PIX_FMT_YUV420:
                 size = f->fmt.pix.width * f->fmt.pix.height * 3 / 2;
-                csi_set_12bit_imagpara(f->fmt.pix.width,
+                csi_set_12bit_imagpara(cam,
+                                       f->fmt.pix.width,
                                        f->fmt.pix.height);
                 break;
         case V4L2_PIX_FMT_YUV422P:
@@ -1125,8 +1125,8 @@ static int csi_v4l2_s_param(cam_data *cam, struct v4l2_streamparm *parm)
         f->fmt.pix.sizeimage = size;
 
         cam->crop_bounds.top = cam->crop_bounds.left = 0;
-        cam->crop_bounds.width = cam_input_fmt.fmt.pix.width;
-        cam->crop_bounds.height = cam_input_fmt.fmt.pix.height;
+        cam->crop_bounds.width = cam->input_fmt.fmt.pix.width;
+        cam->crop_bounds.height = cam->input_fmt.fmt.pix.height;
         cam->crop_current.width = cam->crop_bounds.width;
         cam->crop_current.height = cam->crop_bounds.height;
 
@@ -1201,9 +1201,9 @@ static int csi_v4l_s_std(cam_data *cam, v4l2_std_id e)
         strcpy(cam->standard.name, video_fmts[video_index].name);
 
         /* Enable csi PAL/NTSC deinterlace mode */
-        csi_buf_stride_set(video_fmts[video_index].active_width);
-        csi_deinterlace_mode(cam->standard.id);
-        csi_deinterlace_enable(true);
+        csi_buf_stride_set(cam, video_fmts[video_index].active_width);
+        csi_deinterlace_mode(cam, cam->standard.id);
+        csi_deinterlace_enable(cam, true);
 
         /* crop will overwrite */
         cam->crop_bounds.width = video_fmts[video_index].active_width;
@@ -1231,7 +1231,7 @@ static int csi_v4l_g_std(cam_data *cam, v4l2_std_id *e)
 {
         struct v4l2_format tv_fmt;
 
-        pr_debug("In csi_v4l2_g_std\n");
+        pr_debug("In csi_v4l2_g_std, cam->csi %d\n", cam->csi);
 
         if (cam->device_type == 1) {
                 /* Use this function to get what the TV-In device detects the
@@ -1256,13 +1256,13 @@ static int csi_v4l_g_std(cam_data *cam, v4l2_std_id *e)
         return 0;
 }
 
-static void csi_input_select(int input_select)
+static void csi_input_select(cam_data *cam)
 {
-        if (strcmp(csi_capture_inputs[input_select].name, "Vadc") == 0)
+        if (strcmp(csi_capture_inputs[cam->current_input].name, "Vadc") == 0)
                 /* Enable csi tvdec */
-                csi_tvdec_enable(true);
+                csi_tvdec_enable(cam, true);
         else
-                csi_tvdec_enable(false);
+                csi_tvdec_enable(cam, false);
 }
 
 /*!
@@ -1332,8 +1332,8 @@ static int csi_v4l_dqueue(cam_data *cam, struct v4l2_buffer *buf)
          * If want to do preview on LCD, use PxP CSC to convert from UYVY
          * to RGB565; but for encoding, usually we don't use RGB format.
          */
-        if (cam->v2f.fmt.pix.pixelformat != cam_input_fmt.fmt.pix.pixelformat
-                        && !bswapenable) {
+        if (cam->v2f.fmt.pix.pixelformat != cam->input_fmt.fmt.pix.pixelformat
+                        && !cam->bswapenable) {
                 sg_dma_address(&cam->sg[0]) = buf->m.offset;
                 /* last frame buffer as pxp output buffer  */
                 sg_dma_address(&cam->sg[1]) =
@@ -1467,7 +1467,6 @@ static int csi_v4l_close(struct file *file)
                 file->private_data = NULL;
                 vidioc_int_s_power(cam->sensor, 0);
                 clk_disable_unprepare(sensor->sensor_clk);
-                csi_clk_disable();
         }
 
         return err;
@@ -1510,19 +1509,23 @@ static ssize_t csi_v4l_read(struct file *file, char *buf, size_t count,
                         return -ENOMEM;
                 }
                 cam->still_counter = 0;
-                __raw_writel(cam->still_buf[0], CSI_CSIDMASA_FB2);
-                __raw_writel(cam->still_buf[0], CSI_CSIDMASA_FB1);
-                __raw_writel(__raw_readl(CSI_CSICR3) | BIT_DMA_REFLASH_RFF,
-                             CSI_CSICR3);
-                __raw_writel(__raw_readl(CSI_CSISR), CSI_CSISR);
-                __raw_writel(__raw_readl(CSI_CSICR3) | BIT_FRMCNT_RST,
-                             CSI_CSICR3);
-                csi_enable_int(1);
-                csi_enable(1);
+                csi_write(cam->csi_soc, cam->still_buf[0], CSI_CSIDMASA_FB2);
+                csi_write(cam->csi_soc, cam->still_buf[0], CSI_CSIDMASA_FB1);
+                csi_write(cam->csi_soc,
+                        csi_read(cam->csi_soc, CSI_CSICR3) |
+                                BIT_DMA_REFLASH_RFF,
+                        CSI_CSICR3);
+                csi_write(cam->csi_soc, csi_read(cam->csi_soc, CSI_CSISR),
+                        CSI_CSISR);
+                csi_write(cam->csi_soc,
+                        csi_read(cam->csi_soc, CSI_CSICR3) | BIT_FRMCNT_RST,
+                        CSI_CSICR3);
+                csi_enable_int(cam, 1);
+                csi_enable(cam, 1);
         }
 
         wait_event_interruptible(cam->still_queue, cam->still_counter);
-        csi_disable_int();
+        csi_disable_int(cam);
         err = copy_to_user(buf, cam->still_buf_vaddr,
                            cam->v2f.fmt.pix.sizeimage);
 
@@ -1965,7 +1968,7 @@ static long csi_v4l_do_ioctl(struct file *file,
 
                 cam->current_input = *index;
 
-                csi_input_select(cam->current_input);
+                csi_input_select(cam);
                 break;
         }
         case VIDIOC_G_OUTPUT:
@@ -2066,11 +2069,20 @@ static struct video_device csi_v4l_template = {
  *
  * @return status  0 Success
  */
-static void init_camera_struct(cam_data *cam)
+static void init_camera_struct(cam_data *cam, struct platform_device *pdev)
 {
         struct pxp_proc_data *proc_data = &cam->pxp_conf.proc_data;
+        struct device_node *np = pdev->dev.of_node;
+        int ret = 0;
+        int csi_id;
         pr_debug("In MVC: %s\n", __func__);
 
+        ret = of_property_read_u32(np, "csi_id", &csi_id);
+        if (ret) {
+                dev_err(&pdev->dev, "csi_id missing or invalid\n");
+                return;
+        }
+
         proc_data->hflip = 0;
         proc_data->vflip = 0;
         proc_data->rotate = 0;
@@ -2082,6 +2094,9 @@ static void init_camera_struct(cam_data *cam)
         sema_init(&cam->param_lock, 1);
         sema_init(&cam->busy_lock, 1);
 
+        /* TODO sanity check */
+        cam->csi_soc = csi_get_soc(csi_id);
+
         cam->video_dev = video_device_alloc();
         if (cam->video_dev == NULL)
                 return;
@@ -2126,11 +2141,18 @@ static void init_camera_struct(cam_data *cam)
         cam->crop_bounds.height = 480;
         cam->crop_current = cam->crop_defrect = cam->crop_bounds;
 
+        cam->csi = csi_id;
         cam->enc_callback = camera_callback;
         csi_start_callback(cam);
         init_waitqueue_head(&cam->power_queue);
         spin_lock_init(&cam->queue_int_lock);
         spin_lock_init(&cam->dqueue_int_lock);
+
+        cam->self = kmalloc(sizeof(struct v4l2_int_device), GFP_KERNEL);
+        cam->self->module = THIS_MODULE;
+        sprintf(cam->self->name, "csi_v4l2_cap%d", cam->csi);
+        cam->self->type = v4l2_int_type_master;
+        cam->self->u.master = &csi_v4l2_master;
 }
 
 /*!
@@ -2172,14 +2194,13 @@ static int csi_v4l2_probe(struct platform_device *pdev)
                 err = -ENOMEM;
                 goto out;
         }
-        memset(&cam_input_fmt, 0, sizeof(cam_input_fmt));
-        init_camera_struct(g_cam);
+        memset(&g_cam->input_fmt, 0, sizeof(g_cam->input_fmt));
+        init_camera_struct(g_cam, pdev);
         platform_set_drvdata(pdev, (void *)g_cam);
 
         /* Set up the v4l2 device and register it */
-        csi_v4l2_int_device.priv = g_cam;
-        /* This function contains a bug that won't let this be rmmod'd. */
-        v4l2_int_device_register(&csi_v4l2_int_device);
+        g_cam->self->priv = g_cam;
+        v4l2_int_device_register(g_cam->self);
 
         /* register v4l video device */
         if (video_register_device(g_cam->video_dev, VFL_TYPE_GRABBER, video_nr)
@@ -2209,7 +2230,7 @@ static int csi_v4l2_remove(struct platform_device *pdev)
                        "-- setting ops to NULL\n");
         } else {
                 pr_info("V4L2 freeing image input device\n");
-                v4l2_int_device_unregister(&csi_v4l2_int_device);
+                v4l2_int_device_unregister(g_cam->self);
                 csi_stop_callback(g_cam);
                 video_unregister_device(g_cam->video_dev);
                 platform_set_drvdata(pdev, NULL);
@@ -2304,17 +2325,23 @@ static struct platform_driver csi_v4l2_driver = {
 static int csi_v4l2_master_attach(struct v4l2_int_device *slave)
 {
         cam_data *cam = slave->u.slave->master->priv;
+        struct sensor_data *sdata = slave->priv;
         struct v4l2_format cam_fmt;
 
         pr_debug("In MVC: %s\n", __func__);
         pr_debug("   slave.name = %s\n", slave->name);
         pr_debug("   master.name = %s\n", slave->u.slave->master->name);
 
-        cam->sensor = slave;
         if (slave == NULL) {
                 pr_err("ERROR: v4l2 capture: slave parameter not valid.\n");
                 return -1;
         }
+        if (sdata->csi != cam->csi) {
+                pr_debug("%s: csi doesn't match\n", __func__);
+                return -1;
+        }
+
+        cam->sensor = slave;
 
         cam_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
         vidioc_int_g_fmt_cap(cam->sensor, &cam_fmt);
diff --git a/drivers/media/platform/mxc/capture/fsl_csi.c b/drivers/media/platform/mxc/capture/fsl_csi.c
index 2839e9fa0d8e..602899d70ce0 100644
--- a/drivers/media/platform/mxc/capture/fsl_csi.c
+++ b/drivers/media/platform/mxc/capture/fsl_csi.c
@@ -33,9 +33,9 @@
 #include "mxc_v4l2_capture.h"
 #include "fsl_csi.h"
 
-void __iomem *csi_regbase;
-EXPORT_SYMBOL(csi_regbase);
-static int irq_nr;
+#define CSI_MAX_NUM     2
+struct csi_soc csi_array[CSI_MAX_NUM], *csi;
+
 static csi_irq_callback_t g_callback;
 static void *g_callback_data;
 static struct clk *disp_axi_clk;
@@ -61,9 +61,10 @@ EXPORT_SYMBOL(csi_clk_disable);
 static irqreturn_t csi_irq_handler(int irq, void *data)
 {
         cam_data *cam = (cam_data *) data;
-        unsigned long status = __raw_readl(CSI_CSISR);
+        struct csi_soc *csi = &csi_array[cam->csi];
+        unsigned long status = __raw_readl(csi->regbase + CSI_CSISR);
 
-        __raw_writel(status, CSI_CSISR);
+        __raw_writel(status, csi->regbase + CSI_CSISR);
 
         if (status & BIT_HRESP_ERR_INT)
                 pr_warning("Hresponse error is detected.\n");
@@ -100,24 +101,25 @@ static irqreturn_t csi_irq_handler(int irq, void *data)
         return IRQ_HANDLED;
 }
 
-static void csihw_reset_frame_count(void)
+static void csihw_reset_frame_count(struct csi_soc *csi)
 {
-        __raw_writel(__raw_readl(CSI_CSICR3) | BIT_FRMCNT_RST, CSI_CSICR3);
+        __raw_writel(__raw_readl(csi->regbase + CSI_CSICR3) | BIT_FRMCNT_RST,
+                        csi->regbase + CSI_CSICR3);
 }
 
-static void csihw_reset(void)
+static void csihw_reset(struct csi_soc *csi)
 {
-        csihw_reset_frame_count();
-        __raw_writel(CSICR1_RESET_VAL, CSI_CSICR1);
-        __raw_writel(CSICR2_RESET_VAL, CSI_CSICR2);
-        __raw_writel(CSICR3_RESET_VAL, CSI_CSICR3);
+        csihw_reset_frame_count(csi);
+        __raw_writel(CSICR1_RESET_VAL, csi->regbase + CSI_CSICR1);
+        __raw_writel(CSICR2_RESET_VAL, csi->regbase + CSI_CSICR2);
+        __raw_writel(CSICR3_RESET_VAL, csi->regbase + CSI_CSICR3);
 }
 
 /*!
  * csi_init_interface
  *    Init csi interface
  */
-void csi_init_interface(void)
+static void csi_init_interface(struct csi_soc *csi)
 {
         unsigned int val = 0;
         unsigned int imag_para;
@@ -129,22 +131,22 @@ void csi_init_interface(void)
         val |= BIT_FCC;
         val |= 1 << SHIFT_MCLKDIV;
         val |= BIT_MCLKEN;
-        __raw_writel(val, CSI_CSICR1);
+        __raw_writel(val, csi->regbase + CSI_CSICR1);
 
         imag_para = (640 << 16) | 960;
-        __raw_writel(imag_para, CSI_CSIIMAG_PARA);
+        __raw_writel(imag_para, csi->regbase + CSI_CSIIMAG_PARA);
 
         val = 0x1010;
         val |= BIT_DMA_REFLASH_RFF;
-        __raw_writel(val, CSI_CSICR3);
+        __raw_writel(val, csi->regbase + CSI_CSICR3);
 }
-EXPORT_SYMBOL(csi_init_interface);
 
-void csi_format_swap16(bool enable)
+void csi_format_swap16(cam_data *cam, bool enable)
 {
+        struct csi_soc *csi = &csi_array[cam->csi];
         unsigned int val;
 
-        val = __raw_readl(CSI_CSICR1);
+        val = __raw_readl(csi->regbase + CSI_CSICR1);
         if (enable) {
                 val |= BIT_PACK_DIR;
                 val |= BIT_SWAP16_EN;
@@ -153,7 +155,7 @@ void csi_format_swap16(bool enable)
                 val &= ~BIT_SWAP16_EN;
         }
 
-        __raw_writel(val, CSI_CSICR1);
+        __raw_writel(val, csi->regbase + CSI_CSICR1);
 }
 EXPORT_SYMBOL(csi_format_swap16);
 
@@ -172,7 +174,8 @@ void csi_start_callback(void *data)
 {
         cam_data *cam = (cam_data *) data;
 
-        if (request_irq(irq_nr, csi_irq_handler, 0, "csi", cam) < 0)
+        if (request_irq(csi_array[cam->csi].irq_nr, csi_irq_handler, 0, "csi",
+                        cam) < 0)
                 pr_debug("CSI error: irq request fail\n");
 
 }
@@ -182,13 +185,14 @@ void csi_stop_callback(void *data)
 {
         cam_data *cam = (cam_data *) data;
 
-        free_irq(irq_nr, cam);
+        free_irq(csi_array[cam->csi].irq_nr, cam);
 }
 EXPORT_SYMBOL(csi_stop_callback);
 
-void csi_enable_int(int arg)
+void csi_enable_int(cam_data *cam, int arg)
 {
-        unsigned long cr1 = __raw_readl(CSI_CSICR1);
+        struct csi_soc *csi = &csi_array[cam->csi];
+        unsigned long cr1 = __raw_readl(csi->regbase + CSI_CSICR1);
 
         cr1 |= BIT_SOF_INTEN;
         if (arg == 1) {
@@ -196,69 +200,76 @@ void csi_enable_int(int arg)
                 cr1 |= BIT_FB1_DMA_DONE_INTEN;
                 cr1 |= BIT_FB2_DMA_DONE_INTEN;
         }
-        __raw_writel(cr1, CSI_CSICR1);
+        __raw_writel(cr1, csi->regbase + CSI_CSICR1);
 }
 EXPORT_SYMBOL(csi_enable_int);
 
-void csi_disable_int(void)
+void csi_disable_int(cam_data *cam)
 {
-        unsigned long cr1 = __raw_readl(CSI_CSICR1);
+        struct csi_soc *csi = &csi_array[cam->csi];
+        unsigned long cr1 = __raw_readl(csi->regbase + CSI_CSICR1);
 
         cr1 &= ~BIT_SOF_INTEN;
         cr1 &= ~BIT_FB1_DMA_DONE_INTEN;
         cr1 &= ~BIT_FB2_DMA_DONE_INTEN;
-        __raw_writel(cr1, CSI_CSICR1);
+        __raw_writel(cr1, csi->regbase + CSI_CSICR1);
 }
 EXPORT_SYMBOL(csi_disable_int);
 
-void csi_enable(int arg)
+void csi_enable(cam_data *cam, int arg)
 {
-        unsigned long cr = __raw_readl(CSI_CSICR18);
+        struct csi_soc *csi = &csi_array[cam->csi];
+        unsigned long cr = __raw_readl(csi->regbase + CSI_CSICR18);
 
         if (arg == 1)
                 cr |= BIT_CSI_ENABLE;
         else
                 cr &= ~BIT_CSI_ENABLE;
-        __raw_writel(cr, CSI_CSICR18);
+        __raw_writel(cr, csi->regbase + CSI_CSICR18);
 }
 EXPORT_SYMBOL(csi_enable);
 
-void csi_buf_stride_set(u32 stride)
+void csi_buf_stride_set(cam_data *cam, u32 stride)
 {
-        __raw_writel(stride, CSI_CSIFBUF_PARA);
+        struct csi_soc *csi = &csi_array[cam->csi];
+
+        __raw_writel(stride, csi->regbase + CSI_CSIFBUF_PARA);
 }
 EXPORT_SYMBOL(csi_buf_stride_set);
 
-void csi_deinterlace_enable(bool enable)
+void csi_deinterlace_enable(cam_data *cam, bool enable)
 {
-        unsigned long cr18 = __raw_readl(CSI_CSICR18);
+        struct csi_soc *csi = &csi_array[cam->csi];
+        unsigned long cr18 = __raw_readl(csi->regbase + CSI_CSICR18);
 
         if (enable == true)
                 cr18 |= BIT_DEINTERLACE_EN;
         else
                 cr18 &= ~BIT_DEINTERLACE_EN;
 
-        __raw_writel(cr18, CSI_CSICR18);
+        __raw_writel(cr18, csi->regbase + CSI_CSICR18);
 }
 EXPORT_SYMBOL(csi_deinterlace_enable);
 
-void csi_deinterlace_mode(int mode)
+void csi_deinterlace_mode(cam_data *cam, int mode)
 {
-        unsigned long cr18 = __raw_readl(CSI_CSICR18);
+        struct csi_soc *csi = &csi_array[cam->csi];
+        unsigned long cr18 = __raw_readl(csi->regbase + CSI_CSICR18);
 
         if (mode == V4L2_STD_NTSC)
                 cr18 |= BIT_NTSC_EN;
         else
                 cr18 &= ~BIT_NTSC_EN;
 
-        __raw_writel(cr18, CSI_CSICR18);
+        __raw_writel(cr18, csi->regbase + CSI_CSICR18);
 }
 EXPORT_SYMBOL(csi_deinterlace_mode);
 
-void csi_tvdec_enable(bool enable)
+void csi_tvdec_enable(cam_data *cam, bool enable)
 {
-        unsigned long cr18 = __raw_readl(CSI_CSICR18);
-        unsigned long cr1 = __raw_readl(CSI_CSICR1);
+        struct csi_soc *csi = &csi_array[cam->csi];
+        unsigned long cr18 = __raw_readl(csi->regbase + CSI_CSICR18);
+        unsigned long cr1 = __raw_readl(csi->regbase + CSI_CSICR1);
 
         if (enable == true) {
                 cr18 |= (BIT_TVDECODER_IN_EN | BIT_BASEADDR_SWITCH_EN);
@@ -270,71 +281,85 @@ void csi_tvdec_enable(bool enable)
                 cr1 |= BIT_SOF_POL | BIT_REDGE;
         }
 
-        __raw_writel(cr18, CSI_CSICR18);
-        __raw_writel(cr1, CSI_CSICR1);
+        __raw_writel(cr18, csi->regbase + CSI_CSICR18);
+        __raw_writel(cr1, csi->regbase + CSI_CSICR1);
 }
 EXPORT_SYMBOL(csi_tvdec_enable);
 
-void csi_set_32bit_imagpara(int width, int height)
+void csi_set_32bit_imagpara(cam_data *cam, int width, int height)
 {
+        struct csi_soc *csi = &csi_array[cam->csi];
         int imag_para = 0;
-        unsigned long cr3 = __raw_readl(CSI_CSICR3);
+        unsigned long cr3 = __raw_readl(csi->regbase + CSI_CSICR3);
 
         imag_para = (width << 16) | height;
-        __raw_writel(imag_para, CSI_CSIIMAG_PARA);
+        __raw_writel(imag_para, csi->regbase + CSI_CSIIMAG_PARA);
 
 
         /* reflash the embeded DMA controller */
-        __raw_writel(cr3 | BIT_DMA_REFLASH_RFF, CSI_CSICR3);
+        __raw_writel(cr3 | BIT_DMA_REFLASH_RFF, csi->regbase + CSI_CSICR3);
 }
 EXPORT_SYMBOL(csi_set_32bit_imagpara);
 
-void csi_set_16bit_imagpara(int width, int height)
+void csi_set_16bit_imagpara(cam_data *cam, int width, int height)
 {
+        struct csi_soc *csi = &csi_array[cam->csi];
         int imag_para = 0;
-        unsigned long cr3 = __raw_readl(CSI_CSICR3);
+        unsigned long cr3 = __raw_readl(csi->regbase + CSI_CSICR3);
 
         imag_para = (width << 16) | (height * 2);
-        __raw_writel(imag_para, CSI_CSIIMAG_PARA);
+        __raw_writel(imag_para, csi->regbase + CSI_CSIIMAG_PARA);
 
         /* reflash the embeded DMA controller */
-        __raw_writel(cr3 | BIT_DMA_REFLASH_RFF, CSI_CSICR3);
+        __raw_writel(cr3 | BIT_DMA_REFLASH_RFF, csi->regbase + CSI_CSICR3);
 }
 EXPORT_SYMBOL(csi_set_16bit_imagpara);
 
-void csi_set_12bit_imagpara(int width, int height)
+void csi_set_12bit_imagpara(cam_data *cam, int width, int height)
 {
+        struct csi_soc *csi = &csi_array[cam->csi];
         int imag_para = 0;
-        unsigned long cr3 = __raw_readl(CSI_CSICR3);
+        unsigned long cr3 = __raw_readl(csi->regbase + CSI_CSICR3);
 
         imag_para = (width << 16) | (height * 3 / 2);
-        __raw_writel(imag_para, CSI_CSIIMAG_PARA);
+        __raw_writel(imag_para, csi->regbase + CSI_CSIIMAG_PARA);
 
         /* reflash the embeded DMA controller */
-        __raw_writel(cr3 | BIT_DMA_REFLASH_RFF, CSI_CSICR3);
+        __raw_writel(cr3 | BIT_DMA_REFLASH_RFF, csi->regbase + CSI_CSICR3);
 }
 EXPORT_SYMBOL(csi_set_12bit_imagpara);
 
-void csi_dmareq_rff_enable(void)
+void csi_dmareq_rff_enable(struct csi_soc *csi)
 {
-        unsigned long cr3 = __raw_readl(CSI_CSICR3);
+        unsigned long cr3 = __raw_readl(csi->regbase + CSI_CSICR3);
 
         cr3 |= BIT_DMA_REQ_EN_RFF;
         cr3 |= BIT_HRESP_ERR_EN;
-        __raw_writel(cr3, CSI_CSICR3);
+        __raw_writel(cr3, csi->regbase + CSI_CSICR3);
 }
 EXPORT_SYMBOL(csi_dmareq_rff_enable);
 
-void csi_dmareq_rff_disable(void)
+void csi_dmareq_rff_disable(struct csi_soc *csi)
 {
-        unsigned long cr3 = __raw_readl(CSI_CSICR3);
+        unsigned long cr3 = __raw_readl(csi->regbase + CSI_CSICR3);
 
         cr3 &= ~BIT_DMA_REQ_EN_RFF;
         cr3 &= ~BIT_HRESP_ERR_EN;
-        __raw_writel(cr3, CSI_CSICR3);
+        __raw_writel(cr3, csi->regbase + CSI_CSICR3);
 }
 EXPORT_SYMBOL(csi_dmareq_rff_disable);
 
+struct csi_soc *csi_get_soc(int id)
+{
+        if (id >= CSI_MAX_NUM)
+                return ERR_PTR(-ENODEV);
+        else if (!csi_array[id].online)
+                return ERR_PTR(-ENODEV);
+        else
+                return &(csi_array[id]);
+}
+EXPORT_SYMBOL_GPL(csi_get_soc);
+
 static const struct of_device_id fsl_csi_dt_ids[] = {
         { .compatible = "fsl,imx6sl-csi", },
         { /* sentinel */ }
@@ -345,6 +370,13 @@ static int csi_probe(struct platform_device *pdev)
 {
         int ret = 0;
         struct resource *res;
+        int id;
+
+        id = of_alias_get_id(pdev->dev.of_node, "csi");
+        if (id < 0) {
+                dev_dbg(&pdev->dev, "can not get alias id\n");
+                return id;
+        }
 
         res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
         if (!res) {
@@ -352,7 +384,10 @@ static int csi_probe(struct platform_device *pdev)
                 ret = -ENODEV;
                 goto err;
         }
-        irq_nr = res->start;
+
+        csi = &csi_array[id];
+        csi->irq_nr = res->start;
+        csi->online = false;
 
         res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
         if (!res) {
@@ -360,8 +395,8 @@ static int csi_probe(struct platform_device *pdev)
                 ret = -ENODEV;
                 goto err;
         }
-        csi_regbase = devm_ioremap(&pdev->dev, res->start, resource_size(res));
-        if (!csi_regbase) {
+        csi->regbase = devm_ioremap(&pdev->dev, res->start, resource_size(res));
+        if (!csi->regbase) {
                 dev_err(&pdev->dev, "ioremap failed with csi base\n");
                 ret = -ENOMEM;
                 goto err;
@@ -385,11 +420,11 @@ static int csi_probe(struct platform_device *pdev)
         }
 
         csi_clk_enable();
-        csihw_reset();
-        csi_init_interface();
-        csi_dmareq_rff_disable();
-        csi_clk_disable();
+        csihw_reset(csi);
+        csi_init_interface(csi);
+        csi_dmareq_rff_disable(csi);
 
+        csi->online = true;
 err:
         return ret;
 }
diff --git a/drivers/media/platform/mxc/capture/fsl_csi.h b/drivers/media/platform/mxc/capture/fsl_csi.h
index 78b393ef9151..2823ef4e2e50 100644
--- a/drivers/media/platform/mxc/capture/fsl_csi.h
+++ b/drivers/media/platform/mxc/capture/fsl_csi.h
@@ -110,30 +110,24 @@
 #define CSI_MCLK_I2C            8
 #endif
 
-extern void __iomem *csi_regbase;
-#define CSI_CSICR1              (csi_regbase)
-#define CSI_CSICR2              (csi_regbase + 0x4)
-#define CSI_CSICR3              (csi_regbase + 0x8)
-#define CSI_STATFIFO            (csi_regbase + 0xC)
-#define CSI_CSIRXFIFO           (csi_regbase + 0x10)
-#define CSI_CSIRXCNT            (csi_regbase + 0x14)
-#define CSI_CSISR               (csi_regbase + 0x18)
-
-#define CSI_CSIDBG              (csi_regbase + 0x1C)
-#define CSI_CSIDMASA_STATFIFO   (csi_regbase + 0x20)
-#define CSI_CSIDMATS_STATFIFO   (csi_regbase + 0x24)
-#define CSI_CSIDMASA_FB1        (csi_regbase + 0x28)
-#define CSI_CSIDMASA_FB2        (csi_regbase + 0x2C)
-#define CSI_CSIFBUF_PARA        (csi_regbase + 0x30)
-#define CSI_CSIIMAG_PARA        (csi_regbase + 0x34)
-
-#define CSI_CSICR18             (csi_regbase + 0x48)
-#define CSI_CSICR19             (csi_regbase + 0x4c)
-
-static inline void csi_clear_status(unsigned long status)
-{
-        __raw_writel(status, CSI_CSISR);
-}
+#define CSI_CSICR1              0x0
+#define CSI_CSICR2              0x4
+#define CSI_CSICR3              0x8
+#define CSI_STATFIFO            0xC
+#define CSI_CSIRXFIFO           0x10
+#define CSI_CSIRXCNT            0x14
+#define CSI_CSISR               0x18
+
+#define CSI_CSIDBG              0x1C
+#define CSI_CSIDMASA_STATFIFO   0x20
+#define CSI_CSIDMATS_STATFIFO   0x24
+#define CSI_CSIDMASA_FB1        0x28
+#define CSI_CSIDMASA_FB2        0x2C
+#define CSI_CSIFBUF_PARA        0x30
+#define CSI_CSIIMAG_PARA        0x34
+
+#define CSI_CSICR18             0x48
+#define CSI_CSICR19             0x4c
 
 struct csi_signal_cfg_t {
         unsigned data_width:3;
@@ -193,24 +187,39 @@ struct csi_config_t {
         unsigned int rxcnt;
 };
 
+struct csi_soc {
+        bool online;
+        int irq_nr;
+        void __iomem *regbase;
+};
+
 typedef void (*csi_irq_callback_t) (void *data, unsigned long status);
 
-void csi_init_interface(void);
-void csi_set_32bit_imagpara(int width, int height);
-void csi_set_16bit_imagpara(int width, int height);
-void csi_set_12bit_imagpara(int width, int height);
-void csi_format_swap16(bool enable);
+void csi_set_32bit_imagpara(cam_data *cam, int width, int height);
+void csi_set_16bit_imagpara(cam_data *cam, int width, int height);
+void csi_set_12bit_imagpara(cam_data *cam, int width, int height);
+void csi_format_swap16(cam_data *cam, bool enable);
 int csi_read_mclk_flag(void);
 void csi_start_callback(void *data);
 void csi_stop_callback(void *data);
-void csi_enable_int(int arg);
-void csi_buf_stride_set(u32 stride);
-void csi_deinterlace_mode(int mode);
-void csi_deinterlace_enable(bool enable);
-void csi_tvdec_enable(bool enable);
-void csi_enable(int arg);
-void csi_disable_int(void);
+void csi_enable_int(cam_data *cam, int arg);
+void csi_buf_stride_set(cam_data *cam, u32 stride);
+void csi_deinterlace_mode(cam_data *cam, int mode);
+void csi_deinterlace_enable(cam_data *cam, bool enable);
+void csi_tvdec_enable(cam_data *cam, bool enable);
+void csi_enable(cam_data *cam, int arg);
+void csi_disable_int(cam_data *cam);
 void csi_clk_enable(void);
 void csi_clk_disable(void);
-void csi_dmareq_rff_enable(void);
-void csi_dmareq_rff_disable(void);
+void csi_dmareq_rff_enable(struct csi_soc *csi);
+void csi_dmareq_rff_disable(struct csi_soc *csi);
+static inline int csi_read(struct csi_soc *csi, unsigned int offset)
+{
+        return __raw_readl(csi->regbase + offset);
+}
+void csi_write(struct csi_soc *csi, unsigned int value, unsigned int offset)
+{
+        __raw_writel(value, csi->regbase + offset);
+}
+
+struct csi_soc *csi_get_soc(int id);
diff --git a/drivers/media/platform/mxc/capture/mxc_v4l2_capture.h b/drivers/media/platform/mxc/capture/mxc_v4l2_capture.h
index 09a421f20f7e..b8ea5b9b2d27 100644
--- a/drivers/media/platform/mxc/capture/mxc_v4l2_capture.h
+++ b/drivers/media/platform/mxc/capture/mxc_v4l2_capture.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2004-2013 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2004-2014 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -157,6 +157,8 @@ typedef struct _cam_data {
 
         /* v4l2 format */
         struct v4l2_format v2f;
+        struct v4l2_format input_fmt;   /* camera in */
+        bool bswapenable;
         int rotation;   /* for IPUv1 and IPUv3, this means encoder rotation */
         int vf_rotation; /* viewfinder rotation only for IPUv1 and IPUv3 */
         struct v4l2_mxc_offset offset;
@@ -219,6 +221,7 @@ typedef struct _cam_data {
         struct v4l2_int_device *self;
         int sensor_index;
         void *ipu;
+        void *csi_soc;
         enum imx_v4l2_devtype devtype;
 
         /* v4l2 buf elements related to PxP DMA */