3 files changed, 105 insertions, 80 deletions

diff --git a/drivers/media/video/mxc/capture/csi_v4l2_capture.c b/drivers/media/video/mxc/capture/csi_v4l2_capture.c
index 53f35d487b21..bcfbe09d18b4 100644
--- a/drivers/media/video/mxc/capture/csi_v4l2_capture.c
+++ b/drivers/media/video/mxc/capture/csi_v4l2_capture.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2009-2012 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2009-2013 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -274,44 +274,43 @@ static void camera_callback(u32 mask, void *dev)
 
 	spin_lock(&cam->queue_int_lock);
 	spin_lock(&cam->dqueue_int_lock);
-	if (list_empty(&cam->working_q)) {
-		pr_debug("v4l2 capture: %s: "
-				"working queue empty\n", __func__);
-		spin_unlock(&cam->dqueue_int_lock);
-		spin_unlock(&cam->queue_int_lock);
-		return;
-	}
-
-	done_frame =
-		list_entry(cam->working_q.next, struct mxc_v4l_frame, queue);
-	if (done_frame->buffer.flags & V4L2_BUF_FLAG_QUEUED) {
-		done_frame->buffer.flags |= V4L2_BUF_FLAG_DONE;
-		done_frame->buffer.flags &= ~V4L2_BUF_FLAG_QUEUED;
-		if (list_empty(&cam->ready_q)) {
-			cam->skip_frame++;
+	if (!list_empty(&cam->working_q)) {
+		done_frame = list_entry(cam->working_q.next,
+				struct mxc_v4l_frame, queue);
+
+		if (done_frame->csi_buf_num != cam->ping_pong_csi)
+			goto next;
+
+		if (done_frame->buffer.flags & V4L2_BUF_FLAG_QUEUED) {
+			done_frame->buffer.flags |= V4L2_BUF_FLAG_DONE;
+			done_frame->buffer.flags &= ~V4L2_BUF_FLAG_QUEUED;
+
+			/* Added to the done queue */
+			list_del(cam->working_q.next);
+			list_add_tail(&done_frame->queue, &cam->done_q);
+			cam->enc_counter++;
+			wake_up_interruptible(&cam->enc_queue);
 		} else {
-			ready_frame = list_entry(cam->ready_q.next,
-						 struct mxc_v4l_frame, queue);
-			list_del(cam->ready_q.next);
-			list_add_tail(&ready_frame->queue, &cam->working_q);
-
-			if (cam->ping_pong_csi == 1) {
-				__raw_writel(cam->frame[ready_frame->index].
-					     paddress, CSI_CSIDMASA_FB1);
-			} else {
-				__raw_writel(cam->frame[ready_frame->index].
-					     paddress, CSI_CSIDMASA_FB2);
-			}
+			pr_err("ERROR: v4l2 capture: %s: "
+					"buffer not queued\n", __func__);
 		}
+	}
 
-		/* Added to the done queue */
-		list_del(cam->working_q.next);
-		list_add_tail(&done_frame->queue, &cam->done_q);
-		cam->enc_counter++;
-		wake_up_interruptible(&cam->enc_queue);
+next:
+	if (!list_empty(&cam->ready_q)) {
+		ready_frame = list_entry(cam->ready_q.next,
+					 struct mxc_v4l_frame, queue);
+		list_del(cam->ready_q.next);
+		list_add_tail(&ready_frame->queue, &cam->working_q);
+
+		__raw_writel(ready_frame->paddress,
+			cam->ping_pong_csi == 1 ? CSI_CSIDMASA_FB1 :
+						  CSI_CSIDMASA_FB2);
+		ready_frame->csi_buf_num = cam->ping_pong_csi;
 	} else {
-		pr_err("ERROR: v4l2 capture: %s: "
-				"buffer not queued\n", __func__);
+		__raw_writel(cam->dummy_frame.paddress,
+			cam->ping_pong_csi == 1 ? CSI_CSIDMASA_FB1 :
+						  CSI_CSIDMASA_FB2);
 	}
 	spin_unlock(&cam->dqueue_int_lock);
 	spin_unlock(&cam->queue_int_lock);
@@ -331,7 +330,7 @@ static int csi_cap_image(cam_data *cam)
 	unsigned int value;
 
 	value = __raw_readl(CSI_CSICR3);
-	__raw_writel(value | BIT_DMA_REFLASH_RFF | BIT_FRMCNT_RST, CSI_CSICR3);
+	__raw_writel(value | BIT_FRMCNT_RST, CSI_CSICR3);
 	value = __raw_readl(CSI_CSISR);
 	__raw_writel(value, CSI_CSISR);
 
@@ -364,6 +363,13 @@ static int csi_free_frame_buf(cam_data *cam)
 		}
 	}
 
+	if (cam->dummy_frame.vaddress != 0) {
+		dma_free_coherent(0, cam->dummy_frame.buffer.length,
+				  cam->dummy_frame.vaddress,
+				  cam->dummy_frame.paddress);
+		cam->dummy_frame.vaddress = 0;
+	}
+
 	return 0;
 }
 
@@ -403,6 +409,7 @@ static int csi_allocate_frame_buf(cam_data *cam, int count)
 		cam->frame[i].buffer.memory = V4L2_MEMORY_MMAP;
 		cam->frame[i].buffer.m.offset = cam->frame[i].paddress;
 		cam->frame[i].index = i;
+		cam->frame[i].csi_buf_num = 0;
 	}
 
 	return 0;
@@ -424,7 +431,6 @@ static void csi_free_frames(cam_data *cam)
 	for (i = 0; i < FRAME_NUM; i++)
 		cam->frame[i].buffer.flags = V4L2_BUF_FLAG_MAPPED;
 
-	cam->skip_frame = 0;
 	INIT_LIST_HEAD(&cam->ready_q);
 	INIT_LIST_HEAD(&cam->working_q);
 	INIT_LIST_HEAD(&cam->done_q);
@@ -508,7 +514,9 @@ static inline int valid_mode(u32 palette)
 static int csi_streamon(cam_data *cam)
 {
 	struct mxc_v4l_frame *frame;
-	unsigned long lock_flags;
+	unsigned long flags;
+	unsigned long val;
+	int timeout, timeout2;
 
 	pr_debug("In MVC: %s\n", __func__);
 
@@ -517,37 +525,81 @@ static int csi_streamon(cam_data *cam)
 				__func__);
 		return -1;
 	}
+	cam->dummy_frame.vaddress = dma_alloc_coherent(0,
+			       PAGE_ALIGN(cam->v2f.fmt.pix.sizeimage),
+			       &cam->dummy_frame.paddress,
+			       GFP_DMA | GFP_KERNEL);
+	if (cam->dummy_frame.vaddress == 0) {
+		pr_err("ERROR: v4l2 capture: Allocate dummy frame "
+		       "failed.\n");
+		return -ENOBUFS;
+	}
+	cam->dummy_frame.buffer.type = V4L2_BUF_TYPE_PRIVATE;
+	cam->dummy_frame.buffer.length =
+	    PAGE_ALIGN(cam->v2f.fmt.pix.sizeimage);
+	cam->dummy_frame.buffer.m.offset = cam->dummy_frame.paddress;
 
-	spin_lock_irqsave(&cam->queue_int_lock, lock_flags);
+	spin_lock_irqsave(&cam->queue_int_lock, flags);
 	/* move the frame from readyq to workingq */
 	if (list_empty(&cam->ready_q)) {
 		pr_err("ERROR: v4l2 capture: %s: "
 				"ready_q queue empty\n", __func__);
-		spin_unlock_irqrestore(&cam->queue_int_lock, lock_flags);
+		spin_unlock_irqrestore(&cam->queue_int_lock, flags);
 		return -1;
 	}
 	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(cam->frame[frame->index].paddress, CSI_CSIDMASA_FB1);
+	__raw_writel(frame->paddress, CSI_CSIDMASA_FB1);
+	frame->csi_buf_num = 1;
 
 	if (list_empty(&cam->ready_q)) {
 		pr_err("ERROR: v4l2 capture: %s: "
 				"ready_q queue empty\n", __func__);
-		spin_unlock_irqrestore(&cam->queue_int_lock, lock_flags);
+		spin_unlock_irqrestore(&cam->queue_int_lock, flags);
 		return -1;
 	}
 	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(cam->frame[frame->index].paddress, CSI_CSIDMASA_FB2);
-	spin_unlock_irqrestore(&cam->queue_int_lock, lock_flags);
+	__raw_writel(frame->paddress, CSI_CSIDMASA_FB2);
+	frame->csi_buf_num = 2;
+	spin_unlock_irqrestore(&cam->queue_int_lock, flags);
 
 	cam->capture_pid = current->pid;
 	cam->capture_on = true;
 	csi_cap_image(cam);
-	csi_enable_int(1);
-	csi_dmareq_rff_enable();
+
+	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);
+			for (timeout2 = 1000000; timeout2 > 0; timeout2--) {
+				if (__raw_readl(CSI_CSICR3) &
+					BIT_DMA_REFLASH_RFF)
+					cpu_relax();
+				else
+					break;
+			}
+			if (timeout2 <= 0) {
+				pr_err("timeout when wait for reflash done.\n");
+				local_irq_restore(flags);
+				return -ETIME;
+			}
+
+			csi_dmareq_rff_enable();
+			csi_enable_int(1);
+			break;
+		} else
+			cpu_relax();
+	}
+	if (timeout <= 0) {
+		pr_err("timeout when wait for SOF\n");
+		local_irq_restore(flags);
+		return -ETIME;
+	}
+	local_irq_restore(flags);
 
 	return 0;
 }
@@ -561,8 +613,6 @@ static int csi_streamon(cam_data *cam)
  */
 static int csi_streamoff(cam_data *cam)
 {
-	unsigned int cr3;
-
 	pr_debug("In MVC: %s\n", __func__);
 
 	if (cam->capture_on == false)
@@ -575,8 +625,6 @@ static int csi_streamoff(cam_data *cam)
 	/* set CSI_CSIDMASA_FB1 and CSI_CSIDMASA_FB2 to default value */
 	__raw_writel(0, CSI_CSIDMASA_FB1);
 	__raw_writel(0, CSI_CSIDMASA_FB2);
-	cr3 = __raw_readl(CSI_CSICR3);
-	__raw_writel(cr3 | BIT_DMA_REFLASH_RFF, CSI_CSICR3);
 
 	csi_free_frames(cam);
 	csi_free_frame_buf(cam);
@@ -960,7 +1008,6 @@ static int csi_v4l_open(struct file *file)
 					 cam->low_power == false);
 
 		cam->enc_counter = 0;
-		cam->skip_frame = 0;
 		INIT_LIST_HEAD(&cam->ready_q);
 		INIT_LIST_HEAD(&cam->working_q);
 		INIT_LIST_HEAD(&cam->done_q);
@@ -1263,22 +1310,7 @@ static long csi_v4l_do_ioctl(struct file *file,
 		if ((cam->frame[index].buffer.flags & 0x7) ==
 				V4L2_BUF_FLAG_MAPPED) {
 			cam->frame[index].buffer.flags |= V4L2_BUF_FLAG_QUEUED;
-			if (cam->skip_frame > 0) {
-				list_add_tail(&cam->frame[index].queue,
-					      &cam->working_q);
-				cam->skip_frame = 0;
-
-				if (cam->ping_pong_csi == 1) {
-					__raw_writel(cam->frame[index].paddress,
-						     CSI_CSIDMASA_FB1);
-				} else {
-					__raw_writel(cam->frame[index].paddress,
-						     CSI_CSIDMASA_FB2);
-				}
-			} else {
-				list_add_tail(&cam->frame[index].queue,
-					      &cam->ready_q);
-			}
+			list_add_tail(&cam->frame[index].queue, &cam->ready_q);
 		} else if (cam->frame[index].buffer.flags &
 				V4L2_BUF_FLAG_QUEUED) {
 			pr_err("ERROR: v4l2 capture: VIDIOC_QBUF: "
@@ -1507,7 +1539,6 @@ static void init_camera_struct(cam_data *cam)
 	cam->streamparm.parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
 	cam->overlay_on = false;
 	cam->capture_on = false;
-	cam->skip_frame = 0;
 	cam->v4l2_fb.flags = V4L2_FBUF_FLAG_OVERLAY;
 
 	cam->v2f.fmt.pix.sizeimage = 480 * 640 * 2;
diff --git a/drivers/media/video/mxc/capture/fsl_csi.c b/drivers/media/video/mxc/capture/fsl_csi.c
index ede69b814b1b..5368b57c7a61 100644
--- a/drivers/media/video/mxc/capture/fsl_csi.c
+++ b/drivers/media/video/mxc/capture/fsl_csi.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2009-2012 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2009-2013 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -45,17 +45,9 @@ static irqreturn_t csi_irq_handler(int irq, void *data)
 {
 	cam_data *cam = (cam_data *) data;
 	unsigned long status = __raw_readl(CSI_CSISR);
-	unsigned long cr3 = __raw_readl(CSI_CSICR3);
-	unsigned int frame_count = (cr3 >> 16) & 0xFFFF;
 
 	__raw_writel(status, CSI_CSISR);
 
-	if (status & BIT_SOF_INT) {
-		/* reflash the embeded DMA controller */
-		if (frame_count % 2 == 1)
-			__raw_writel(cr3 | BIT_DMA_REFLASH_RFF, CSI_CSICR3);
-	}
-
 	if (status & BIT_HRESP_ERR_INT)
 		pr_warning("Hresponse error is detected.\n");
 
diff --git a/drivers/media/video/mxc/capture/mxc_v4l2_capture.h b/drivers/media/video/mxc/capture/mxc_v4l2_capture.h
index d2efbe8453b2..1431ef919a1c 100644
--- a/drivers/media/video/mxc/capture/mxc_v4l2_capture.h
+++ b/drivers/media/video/mxc/capture/mxc_v4l2_capture.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2004-2012 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2004-2013 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -56,7 +56,10 @@ struct mxc_v4l_frame {
 	struct v4l2_buffer buffer;
 	struct list_head queue;
 	int index;
-	int ipu_buf_num;
+	union {
+		int ipu_buf_num;
+		int csi_buf_num;
+	};
 };
 
 /* Only for old version.  Will go away soon. */
@@ -119,7 +122,6 @@ typedef struct _cam_data {
 	spinlock_t dqueue_int_lock;
 	struct mxc_v4l_frame frame[FRAME_NUM];
 	struct mxc_v4l_frame dummy_frame;
-	int skip_frame;
 	wait_queue_head_t enc_queue;
 	int enc_counter;
 	dma_addr_t rot_enc_bufs[2];