1 files changed, 40 insertions, 25 deletions

diff --git a/drivers/media/video/s5p-fimc/fimc-capture.c b/drivers/media/video/s5p-fimc/fimc-capture.c
index e6afe5f5e24a..287d099caf83 100644
--- a/drivers/media/video/s5p-fimc/fimc-capture.c
+++ b/drivers/media/video/s5p-fimc/fimc-capture.c
@@ -151,27 +151,11 @@ static int fimc_isp_subdev_init(struct fimc_dev *fimc, unsigned int index)
 	return ret;
 }
 
-static int fimc_stop_capture(struct fimc_dev *fimc)
+static void fimc_capture_state_cleanup(struct fimc_dev *fimc)
 {
-	unsigned long flags;
-	struct fimc_vid_cap *cap;
+	struct fimc_vid_cap *cap = &fimc->vid_cap;
 	struct fimc_vid_buffer *buf;
-
-	cap = &fimc->vid_cap;
-
-	if (!fimc_capture_active(fimc))
-		return 0;
-
-	spin_lock_irqsave(&fimc->slock, flags);
-	set_bit(ST_CAPT_SHUT, &fimc->state);
-	fimc_deactivate_capture(fimc);
-	spin_unlock_irqrestore(&fimc->slock, flags);
-
-	wait_event_timeout(fimc->irq_queue,
-			   !test_bit(ST_CAPT_SHUT, &fimc->state),
-			   FIMC_SHUTDOWN_TIMEOUT);
-
-	v4l2_subdev_call(cap->sd, video, s_stream, 0);
+	unsigned long flags;
 
 	spin_lock_irqsave(&fimc->slock, flags);
 	fimc->state &= ~(1 << ST_CAPT_RUN | 1 << ST_CAPT_PEND |
@@ -191,27 +175,50 @@ static int fimc_stop_capture(struct fimc_dev *fimc)
 	}
 
 	spin_unlock_irqrestore(&fimc->slock, flags);
+}
+
+static int fimc_stop_capture(struct fimc_dev *fimc)
+{
+	struct fimc_vid_cap *cap = &fimc->vid_cap;
+	unsigned long flags;
+
+	if (!fimc_capture_active(fimc))
+		return 0;
+
+	spin_lock_irqsave(&fimc->slock, flags);
+	set_bit(ST_CAPT_SHUT, &fimc->state);
+	fimc_deactivate_capture(fimc);
+	spin_unlock_irqrestore(&fimc->slock, flags);
+
+	wait_event_timeout(fimc->irq_queue,
+			   !test_bit(ST_CAPT_SHUT, &fimc->state),
+			   FIMC_SHUTDOWN_TIMEOUT);
 
+	v4l2_subdev_call(cap->sd, video, s_stream, 0);
+
+	fimc_capture_state_cleanup(fimc);
 	dbg("state: 0x%lx", fimc->state);
 	return 0;
 }
 
-static int start_streaming(struct vb2_queue *q)
+
+static int start_streaming(struct vb2_queue *q, unsigned int count)
 {
 	struct fimc_ctx *ctx = q->drv_priv;
 	struct fimc_dev *fimc = ctx->fimc_dev;
 	struct s5p_fimc_isp_info *isp_info;
+	int min_bufs;
 	int ret;
 
 	fimc_hw_reset(fimc);
 
 	ret = v4l2_subdev_call(fimc->vid_cap.sd, video, s_stream, 1);
 	if (ret && ret != -ENOIOCTLCMD)
-		return ret;
+		goto error;
 
 	ret = fimc_prepare_config(ctx, ctx->state);
 	if (ret)
-		return ret;
+		goto error;
 
 	isp_info = &fimc->pdata->isp_info[fimc->vid_cap.input_index];
 	fimc_hw_set_camera_type(fimc, isp_info);
@@ -222,7 +229,7 @@ static int start_streaming(struct vb2_queue *q)
 		ret = fimc_set_scaler_info(ctx);
 		if (ret) {
 			err("Scaler setup error");
-			return ret;
+			goto error;
 		}
 		fimc_hw_set_input_path(ctx);
 		fimc_hw_set_prescaler(ctx);
@@ -237,13 +244,20 @@ static int start_streaming(struct vb2_queue *q)
 
 	INIT_LIST_HEAD(&fimc->vid_cap.pending_buf_q);
 	INIT_LIST_HEAD(&fimc->vid_cap.active_buf_q);
-	fimc->vid_cap.active_buf_cnt = 0;
 	fimc->vid_cap.frame_count = 0;
 	fimc->vid_cap.buf_index = 0;
 
 	set_bit(ST_CAPT_PEND, &fimc->state);
 
+	min_bufs = fimc->vid_cap.reqbufs_count > 1 ? 2 : 1;
+
+	if (fimc->vid_cap.active_buf_cnt >= min_bufs)
+		fimc_activate_capture(ctx);
+
 	return 0;
+error:
+	fimc_capture_state_cleanup(fimc);
+	return ret;
 }
 
 static int stop_streaming(struct vb2_queue *q)
@@ -341,7 +355,8 @@ static void buffer_queue(struct vb2_buffer *vb)
 
 	min_bufs = vid_cap->reqbufs_count > 1 ? 2 : 1;
 
-	if (vid_cap->active_buf_cnt >= min_bufs &&
+	if (vb2_is_streaming(&vid_cap->vbq) &&
+	    vid_cap->active_buf_cnt >= min_bufs &&
 	    !test_and_set_bit(ST_CAPT_STREAM, &fimc->state))
 		fimc_activate_capture(ctx);